本文主要參考了
http://www.cnblogs.com/yin52133/archive/2012/07/21/2602562.html
http://blog.sina.com.cn/s/blog_8c84cf510100yv36.html
http://blog.csdn.net/wangjie0377/article/details/7098446
ICP（Iterative Closest Point迭代最近點）算法是一種基於輪廓特徵或點集對點集的點配準方法如下圖
這裏有兩個點集，紅色部分和藍色部分。
ICP算法就是計算怎麼把PB平移旋轉，使PB和PR儘量重疊， 並建立模型。ICP是改進自對應點集配准算法的一種優化算法。
對應點集配准算法是假設一個理想狀況，將一個模型點雲數據X（如藍色點集）利用四元數旋轉，並平移
得到點雲P（如紅色點集）。而對應點集配准算法主要就是怎麼計算出qR和qT的，知道這兩個就可以匹配點雲了。但是對應點集配准算法的前提條件是計算中的兩個點雲數據的元素一一對應，這個條件在現實裏因誤差等問題，不太可能實線，所以就有了ICP算法。
ICP算法是先計算出從源點雲上的（藍色部分）每個點到目標點雲（紅色部分）的每個點的距離，使每個點和目標雲的最近點匹配，（記得這種映射方式叫滿射吧）。
這樣滿足了對應點集配准算法的前提條件、每個點都有了對應的映射點，則可以按照對應點集配准算法計算，但因爲這個是假設，所以需要重複迭代運行上述過程，直到均方差誤差小於某個閥值。
也就是說 每次迭代，整個模型是靠近一點，每次都重新找最近點，然後再根據對應點集批准算法算一次，比較均方差誤差，如果不滿足就繼續迭代。
這個算法的主要思想如下所示：
基準點在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，重複上述步驟。