計算機視覺中的ICP算法

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

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

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

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

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

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

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

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

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

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

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

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

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

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