ICP（Iterative Closest Point迭代最近點）算法是一種基于輪廓特征或點集對點集的點配準方法如下圖

這里有兩個點集，紅色部分和藍色部分。

ICP算法就是計算怎么把PB平移旋轉，使PB和PR盡量重疊， 并建立模型。ICP是改進自對應點集配準算法的一種優(yōu)化算法。

對應點集配準算法是假設一個理想狀況，將一個模型點云數據X（如藍色點集）利用四元數旋轉，并平移得到點云P（如紅色點集）。而對應點集配準算法主要就是怎么計算出qR和qT的，知道這兩個就可以匹配點云了。但是對應點集配準算法的前提條件是計算中的兩個點云數據的元素一一對應，這個條件在現實里因誤差等問題，不太可能實線，所以就有了ICP算法。

ICP算法是先計算出從源點云上的（藍色部分）每個點到目標點云（紅色部分）的每個點的距離，使每個點和目標云的最近點匹配，（記得這種映射方式叫滿射吧）。

這樣滿足了對應點集配準算法的前提條件、每個點都有了對應的映射點，則可以按照對應點集配準算法計算，但因為這個是假設，所以需要重復迭代運行上述過程，直到均方差誤差小于某個閥值。

也就是說每次迭代，整個模型是靠近一點，每次都重新找最近點，然后再根據對應點集批準算法算一次，比較均方差誤差，如果不滿足就繼續(xù)迭代。

這個算法的主要思想如下所示：

基準點在CT圖像坐標系及世界坐標系下的坐標點集P = {Pi, i = 0,1, 2,…,k}及U = {Ui,i=0,1,2,…,n}。其中，U與P元素間不必存在一一對應關系，元素數目亦不必相同，設k ≥ n。配準過程就是求取 2 個坐標系間的旋轉和平移變換矩陣，使得來自U與P的同源點間距離最小。其過程如下：

(1)計算最近點，即對于集合U中的每一個點，在集合P中都找出距該點最近的對應點，設集合P中由這些對應點組成的新點集為Q = {qi,i = 0,1,2,…,n}。

(2)計算兩個點集Q和U的重心位置坐標，并進行點集中心化生成新的點集； 由新的點集計算正定矩陣N，并計算N的最大特征值及其最大特征向量；

(3)由于最大特征向量等價于殘差平方和最小時的旋轉四元數，將四元數轉換為旋轉矩陣R；在旋轉矩陣R被確定后，由平移向量t僅僅是兩個點集的重心差異，可以通過兩個坐標系中的重心點和旋轉矩陣確定；其中R是 3×3 的旋轉矩陣，T 是 3×1 的平移矩陣。

(3)計算坐標變換，即對于集合U，用配準變換矩陣R，T進行坐標變換，得到新的點集U1，即U1 = RU + T

(4)計算U1與Q之間的均方根誤差，如小于預設的極限值ε，則結束，否則，以點集U1替換U，重復上述步驟。