一、ICP 求解過程
ICP 有好多變種，ICP求解步驟可以分爲以下部分：
1、選點
在模型或者場景中選取匹配點
2、匹配
將選取的一組點匹配另一組點
3、分配權重
給匹配的點對分配權重
4、去除錯誤匹配
根據單個匹配點對或者根據總體匹配情況去除錯誤匹配點
5、給定誤差標準
可以是匹配點對的誤差二模或者一模等形式
6、最小化匹配誤差
通過最小化匹配誤差求解位姿轉換矩陣
下面分別從六個方面介紹ICP的求解過程。
二、選點
選點有以下方式
1、利用全部點進行迭代匹配
2、對所有點進行均勻採樣
3、對所有點進行隨機採樣
4、利用可見光圖像採樣梯度值大的點
5、對所有點按照法向量分佈採樣，使得采樣點法向量分佈均勻
6、對於所有上述採樣算法，可以只從模型採樣，或者只從場景採樣，或者雙向採樣
7、按照對點雲配準的貢獻進行採樣，算法見論文《Geometrically StableSampling for the ICP Algorithm》，見附錄 C。
利用方法 1 – 4 採樣缺點，對於下圖所示的配準會產生誤差，下圖配準靠十字形凹槽進行配準。
三、匹配
1、closest point 在一組點雲中找另一組點雲的最近點，可以用 k-d tree等算法加速最近點搜索
2、normal shooting 一組點雲中的一個點，沿着點的法向量的光線和另一組點雲的交點最爲匹配點
3、project將場景點，從模型點的視角下，投影到模型點雲
4、project and walk 如 3 方法，在投影到目標點雲後，再在目標點雲處搜索，搜索可以根據兩點之間的距離，點到光線的距離，或者灰度值的相近程度等
5、可以上述的所有算法，前提是在兩個點能在給定的衡量標準下可以匹配上，衡量標準比如顏色、兩個點之間法向量的角度等，利用最速下降法搜素
優缺點：
closestpoint對於 (b) 場景不收斂，對於噪聲比較敏感
對比closest point 和 project
對於場景 (c) 上述算法只有最近點算法是收斂的
利用 project 或者 project walk 算法收斂速度塊好多（甚至一個數量級）
四、分配權重
1、權重是常量
2、對於距離遠的點降低其權重
3、根據點法向量的相近程度分配權重
4、根據曲率分配權重
5、根據點顏色的相近程度分配權重
6、根據測量點的不確定性分配權重（由點雲測量傳感器決定）
優缺點：
實驗表明上述算法的效果相當。
五、去除錯誤匹配
1、將匹配點距離超過一定值的去除
2、將匹配點中，以一定衡量標準衡量的（通常點到點距離） n% 最壞的匹配點對去掉
3、將匹配點中，點到點距離大於一定乘機值的去掉（例如 2.5 倍的標準差）
4、去掉和周圍點對不一樣的匹配
5、去掉在mesh邊界的點
作者推薦去除mesh邊界上的點，使用起來計算代價低，對於，當兩組點雲不是完全重合匹配時，可以避免好多無匹配。
實驗表明去掉 10% 最壞的匹配、2.5倍的標準差去除，對於算法的收斂性沒有改善，反而兩組匹配點相隔較遠時，收斂速度較慢。
6、Picky ICP 對於一個模型點被幾個場景中點作爲匹配點
如果，一個模型中的點，被幾個場景中的點，選擇作爲匹配點，則按照模型點和場景點匹配的距離，選擇距離最小的場景點作爲模型點的匹配點。算法在兩組點雲只是部分重合時很有效，會使得收斂速度變慢，同時對於原始ICP算法收斂性的證明也不再適用於 Picky ICP。
7、對於一個模型中的點在場景中有幾個匹配點的問題，還可以參考論文《Robust ICP Registrationusing Biunique Correspondence》的解決方法。
六、給定誤差標準
1、匹配點距離的平方和
對於匹配點距離的平方和有閉式解，求解時，不需要迭代，算法有 SVD 等算法，求解步驟見附錄 A。匹配點的誤差平方和最爲目標函數，求解的方法還有Quaternions、Orthonoraml matrices和Dual quaternions。
2、上面點到點距離平方和再加上點的顏色的差別
3、點到包含另一個點平面的距離的平方和
如圖：
點到平面距離表達式：
沒有閉式解，可以用非線性最小化算法例如 LM 算法求解，或者將問題線性化求解，令 sin a = a，cosa = 1，將求解問題線性化適合兩幀之間變化小的時候求解，利用線性化求解，求解步驟見附錄 B。
4、其它衡量標準，例如 L1 模
七、加速算法
如下圖：
表示位姿求解的結果
令
當兩次變化的角度滿足以下關係時，對於最後三次位姿的移動可以有很好的近似。
令爲位姿求解後帶入能量函數的殘差， 爲近似的弧線的長度，則利用和可以對上述的曲線做近似爲：
對於直線近似在和 X 軸交點處，可以給出線性的更新，對於二次曲線在最小值處，給出二次的更新，如下；
附錄A
參考《Evaluation of surface registrationalgorithms for PET motion correction》2010
利用 SVD分解求解點到點距離平方和最小
待求解的能量函數
對於匹配點距離的平方和有閉式解，求解時，不需要迭代，算法有 SVD 等算法。
利用SVD分解求解算法：
p 和 q 最爲匹配點對
計算匹配點對的中心座標
點到中心點的偏差
找變換 T 使得下面目標函數最小化
將上式帶入可得
因爲要最小化誤差，需要將兩組點雲數據的中心點移到一起，滿足下式
目標函數簡化爲
定義
目標函數最小化轉換爲下式最小化
由柯西不等式得
由 R 是單位正交矩陣
對於 N 奇異值分解
當 R 爲
RN trace 變爲
即 R 取上式時目標函數達到最小。
附錄B
參考文獻《Linear Least-Squares Optimization for Point-to-Plane ICP SurfaceRegistration》
點到平面距離平方和表達式爲：
其中 M 是歐式變換
其中
其中
由上式可以看出，目標函數是三個旋轉角 和三個位移量 的二次函數，只能用迭代求解的方法（例如LM）。
線性近似
當旋轉角 近似爲 0 度，可以讓、，由近似可以得到以下表達式：
變換矩陣 M 變爲：
目標函數：
對於 N 對匹配點，將目標函數變爲：
變量
其中
目標函數變爲
位姿最優解
注意：
1、雖然近似的時候，角度不能太大，實際應用的時候，近似的角度甚至可以得到30度（角度的大小和兩組點雲的幾何形狀，和兩組點雲重疊的範圍有關）。
附錄C
按照點對配準的貢獻進行採樣，如圖，採樣如下
算法如下：
利用點到平面距離作爲目標函數最小化，表達式如下：
