迭代最近點 Iterative Closest Point （ICP）
ICP算法本質上是基於最小二乘法的最優配準方法。該算法重複進行選擇對應關係點對，計算最優剛體變換這一過程，直到滿足正確配準的收斂精度要求。ICP是一個廣泛使用的配准算法，主要目的就是找到旋轉和平移參數，將兩個不同座標系下的點雲，以其中一個點雲座標系爲全局座標系，另一個點雲經過旋轉和平移後兩組點雲重合部分完全重疊。ICP算法最早由Chen and Medioni，and Besl and McKay提出。
算法的輸入：參考點雲和目標點雲，停止迭代的標準。
算法的輸出：旋轉和平移矩陣，即轉換矩陣。
基本過程
對於目標點雲中的每個點，匹配參考點雲（或選定集合）中的最近點。
求得使上述對應點對計算均方根（root mean square，RMS）最小的剛體變換，求得平移參數和旋轉參數。
使用獲得的轉換矩陣來轉換目標點雲。
迭代（重新關聯點），直到滿足終止迭代的條件（迭代次數或誤差小於閾值）。這裏的誤差最小，可以是相鄰兩次均方根差的絕對值小於某一限差。
可能存在的問題
法收斂於局部最小誤差。
噪聲或異常數據可能導致算法無法收斂或錯誤。
在進行ICP算法第一步要確定一個迭代初值，選取的初值將對最後配準結果產生重要的影響，如果初值選擇不合適，算法可能就會限入局部最優，使得迭代不能收斂到正確的配準結果。
剛性（rigid）變換矩陣
旋轉矩陣是歐式空間中執行旋轉變換的矩陣，ICP算法在做點雲配準時，變換矩陣包含了平移和旋轉，因此，這裏有必要研究一下座標系的平移和旋轉過程。三維變換與二維變換相似，採用齊次座標技術來描述空間的各點座標及其變換，描述空間三維變換的變換矩陣是4×4的形式。由此，一系列變換可以用單個矩陣來表示。
以座標系的三個座標軸X,Y,ZX,Y,Z分別作爲旋轉軸，則點實際上只在垂直座標軸的平面上作二維旋轉。此時用二維旋轉公式就可以直接推出三維旋轉變換矩陣。規定在右手座標系中，物體旋轉的正方向是右手螺旋方向，即從該軸正半軸向原點看是逆時針方向。
繞三軸的基本旋轉矩陣，選裝角度分別位θ,α,βθ,α,β:
基本原理
假設用Qi,i=1,2,3,⋯Qi,i=1,2,3,⋯表示第一個點集，Pi,i=1,2,3,⋯Pi,i=1,2,3,⋯表示第二個點集。兩個點集的對齊配準轉換爲使下列目標函數最小。
其中，旋轉矩陣R和平移矩陣T，就是找到的待配準點雲數據與參考點雲數據之間的旋轉參數和平移參數,使得兩點集數據之間滿足某種度量準則下的最優匹配。
ICP配準過程：
1. 計算P中的每一個點在Q點集中的對應最近點
2. 求得使上述對應點對平均距離最小的剛體變換，求得平移參數和旋轉參數
3. 對PP使用上一步求得的平移和旋轉參數，得到新的變換點集P′P′
4. 如果新的變換點集與參考點集滿足上面目標函數要求，即兩點集的平均距離小於某一給定閾值，則停止迭代計算，否則新的變換點集P′P′作爲新的PP繼續迭代，直到達到目標函數的要求。
提示：這裏的目標函數的要求，也可以是迭代次數。