三維空間R3存在兩組含有n個坐標點的點集，分別為： PL和PR。三維空間點集PL中各點經過三維空間變換後與點集PR中點一一對應，其單點變換關係式為：
(0-1)
在ICP配準方法中，空間變換參數向量X可表示為[9] 。參數向量中四元數參數滿足約束條件為：
(0-2)
根據疊代的初值X0，由式(0-1)計算新點集Pi為：
(0-3)
式中，P表示原始未修改過的點集，Pi的下標i表示疊代次數，參數向量X的初始值X0為 。
根據以上數據處理方法，ICP配準算法可以概括為以下七個步驟：
1） 根據點集Plk中的點坐標，在曲面S上搜尋相應就近點點集Prk；
2） 計算兩個點集的重心位置坐標，並進行點集中心化生成新的點集；
3） 由新的點集計算正定矩陣N，並計算N的最大特徵值及其最大特徵向量；
4） 由於最大特徵向量等價於殘差平方和最小時的旋轉四元數，將四元數轉換為旋轉矩陣R；
5） 在旋轉矩陣R被確定後，由平移向量t僅僅是兩個點集的重心差異，可以通過兩個坐標系中的重心點和旋轉矩陣確定；
6） 根據式(0-3)，由點集Plk計算旋轉後的點集P’lk。通過Plk與P’lk計算距離平方和值為fk+1。以連續兩次距離平方和之差絕對值 作為疊代判斷數值；
7） 當 時，ICP配準算法就停止疊代，否則重複1至6步，直到滿足條件 後停止疊代
基本介紹
- 中文名：ICP算法
- 外文名：Iterative Closest Point
- 搜尋方法：最近點搜尋法
- 變換關係式：(0-1)
- 算法步驟：7步
- 主要用於：解決基於自由形態曲面
- 屬性：算法
疊代就近點算法,ICP搜尋方法,Point to Point,Point to Plane,Point to Projection,就近點搜尋圖,三維點雲算法,
疊代就近點算法
在20世紀80年代中期，很多學者開始對點集數據的配準進行了大量研究。1987年，Horn[1]、Arun[2]等人用四元數法提出點集對點集配準方法。這種點集與點集坐標系匹配算法通過實踐證明是一個解決複雜配準問題的關鍵方法。1992年，計算機視覺研究者Besl和Mckay[3]介紹了一種高層次的基於自由形態曲面的配準方法，也稱為疊代就近點法ICP（Iterative Closest Point）。以點集對點集（PSTPS）配準方法為基礎，他們闡述了一種曲面擬合算法，該算法是基於四元數的點集到點集配準方法。從測量點集中確定其對應的就近點點集後，運用Faugera和Hebert提出的方法計算新的就近點點集。用該方法進行疊代計算，直到殘差平方和所構成的目標函式值不變，結束疊代過程。ICP配準法主要用於解決基於自由形態曲面的配準問題。
疊代就近點法ICP就近點法經過十幾年的發展，不斷地得到了完善和補充。Chen和Medioni[4]及Bergevin等人[5]提出了point-to-plane搜尋就近點的精確配準方法。Rusinkiewicz和Levoy提出了point-to-p rojection搜尋就近點的快速配準方法。Soon-Yong和Murali提出了Contractive-projection-point搜尋就近點的配準方法。此外，Andrew和Sing[6]提取了基於彩色三維掃描數據點紋理信息的數據配準方法，主要在ICP算法中考慮三維掃描點的紋理色彩信息進行搜尋就近點。Natasha等人[7]分析了ICP算法中的點雲數據配準質量問題。
基本原理
(0-1)
(0-2)
根據疊代的初值X0，由式(0-1)計算新點集Pi為：
(0-3)
式中，P表示原始未修改過的點集，Pi的下標i表示疊代次數，參數向量X的初始值X0為 。
根據以上數據處理方法，ICP配準算法可以概括為以下七個步驟：
1） 根據點集Plk中的點坐標，在曲面S上搜尋相應就近點點集Prk；
2） 計算兩個點集的重心位置坐標，並進行點集中心化生成新的點集；
7） 當 時，ICP配準算法就停止疊代，否則重複1至6步，直到滿足條件 後停止疊代。
ICP搜尋方法
ICP搜尋就近點的主要方法
Point to Point
1. Point to Point就近點搜尋法
Point to Point就近點搜尋法是ICP算法中最經典的一種方法。如圖1a所示, Point to Point法根據源曲面上的一個點p，在目標曲面上找出對應於p點距離最近的q點。在這個方法中通常運用kd-tree的方法實現就近點搜尋。如圖1b所示，pi是源曲麵點雲數據中的一個點，Vi是生成目標曲面點雲數據中距Pi最近的點。根據Vi點搜尋出在曲面上與Vi點相鄰的點構成的三角形格網，計算pi點投影到每個三角形平面上的投影點qi的坐標。對於每個三角形來說，當投影點qi位於三角形內部，則距離最近點是搜尋的最近點，當投影點qi位於三角形外部，搜尋的就近點應位於三角形的兩條邊界上，Vi是該三角形到pi點的就近距離點。將每個三角形確定的就近距離點進行比較可獲得一個最近點。
Point to Plane
2. Point to Plane就近點搜尋算法
如圖1c所示，Point to Plane法是根據源曲面上的一個點p，在目標曲面上找出對應於p點一個最近的q點。搜尋算法是根據源曲面上p點的切平面的法線，確定發現於目標曲面的交點q’。根據目標曲面上q’點求出的過q’點切平面，然後求源曲面上p點到過q’點切平面的垂線的交點q。
Point to Projection
3. Point to Projection就近點搜尋算法
Point to Projection就近點搜尋法是一種快速的配準方法。如圖1-d所示，圖中Oq是掃描目標曲面的透視點的位置。Point to Projection法是根據源曲面上的一個點p和透視點Oq，在目標曲面上找出q點作為對應於p點的就近點。通過確定Oq點向p點方向的投影線與目標曲面的交點q，作為搜尋的就近點。
就近點搜尋圖
圖1 ICP算法就近點搜尋圖
[1]Horn B K P. Closed-form solution of absolute orientation using unit quaternions[J]. Journal of the Optical Society of America A, 1987, 4(4):629-642.
[2] K. S. Arun, T. S Huang, S. D. Blostein. Least-squares fitting of two 3-D point sets[J]. IEEE Transactions on Pattern Analysis and Machine Intelligence, 1987. 9(5): 698-700.
[3] Paul J. Besl, Neil D. McKay. A method for registration of 3-D shapes[J]. IEEE Transactions on Pattern Analysis and Machine Intelligence, 1992. 14(2): 239-256.
[4] Y. Chen, G. Medioni. Object Modeling by Registration of Multiple Range Images[J]. Image and Vision Computing, 1992. 10: 145-155.
[5] R. Bergevin, M Soucy, H. Gagnon, et al. Towards a general multi-view registration technique[J]. IEEE Transactions on Pattern Analysis and Machine Intelligence, 1996. 18(5): 540-547.
[6] Andrew Edie Johnson, Sing Bing Kang. Registration and Integration of Textured 3D Data[J]. Image and Vision Computing, 1999. 17: 135-147.
[7] Natasha Gelfand, Leslie Ikemoto, Szymon Rusinkiewicz, et al. Geometrically Stable Sampling for the ICP Algorithm[A]//. Proceedings of Fourth International Conference on 3-D Digital Imaging and Modeling[C]. Stanford University, CA, USA, 2003: 260-267.
[9] P.J. Besl, H.D. McKay. A Method for Registration of 3D Shape[J]. IEEE Transactions on Pattern Analysis and Machine Intelligence, 1992. 14(2): 239-256.
