版權聲明：本文爲博主原創文章，歡迎轉載。轉載請註明出處http://blog.csdn.net/jsgaobiao https://blog.csdn.net/jsgaobiao/article/details/78873718
ICP迭代最近點算法
代碼下載：http://download.csdn.net/download/jsgaobiao/10169426
一、實驗原理
點雲配準指將兩個不同視點的點羣三維數據整合到一個統一的座標系的過程。
迭代最近點法（ICP, Iterative Closest Points）的思路爲：根據某種幾何特性對數據進行匹配，並設這些匹配點爲假想的對應點，然後根據這種對應關係求解運動參數。再利用這些運動參數對數據進行變換。並利用同一幾何特徵，確定新的對應關係，重複上述過程進行迭代，使得數據中的重疊部分充分吻合。ICP方法中使用的幾何特性即點在空間中的距離。
（下面一段公式太多直接貼圖了。。好懶）
二、具體實現
本算法基於Python語言實現，使用Geomagic Studio或者matplotlib作爲可視化工具，具體實現的步驟如下：
- 使用Geomagic Studio將點雲文件進行粗配準；
- 在點集中隨機選取指定數量（用戶可自定義）的點作爲控制點，用於調節ICP算法的計算效率和配準精度；
- 在另一個點集中根據歐幾里得距離找到每個點的對應點；
- 利用控制點和其對應點計算旋轉矩陣和位移向量；
- 將計算出來的旋轉位移量應用到點集上，並迭代進行2、3、4步驟。
優化方法：
ICP進行匹配的過程中有一些可以優化的細節：
刪除點雲數據中在採集時產生的噪聲，例如明顯偏離物體很遠的點，減少異常值產生的影響；
使用KD-Tree這一數據結構對查找最近點對的過程進行優化，可以節省很多時間。
三、實驗結果
圖 1和圖 3分別爲點雲的粗配準和ICP配準後的結果，可以看到，圖 1原始的點雲兩種顏色分別佔據了大塊的面積，尤其從圖 2的視角可以看出，粗配準的結果下兩個點集依然有較大的距離。
圖 3兩種顏色互相融合，說明配準取得了一定效果。
圖 1 點雲粗配準結果
圖 2 點雲粗配準（誤差較大）
圖 3 配準結果
四、總結
ICP算法作爲點雲配準的經典方法，實現簡單，但有諸多細節需要考慮，如控制點的選取，異常值的處理，距離的度量方法等。而且ICP方法需要有較好的初值，否則也難以得到好的結果。