文章目錄
根本思想
ICP算法由Besl等人於1992年提出，文獻可以參考：A Method for Registration of 3D Shapes，另外還可以參考：Least-Squares Fitting of Two 3-D Point Sets。前者使用的是四元數方法來求解旋轉矩陣，而後者則是通過對協方差矩陣求解SVD來得到最終的旋轉矩陣。流程大體上一致，後面就主要列出前者的流程。
ICP 算法的直觀想法如下：
如果我們知道兩幅點雲上點的對應關係，那麼我們可以用 Least Squares 來求解 R, t 參數；
怎麼知道點的對應關係呢？如果我們已經知道了一個大概靠譜的 R, t 參數，那麼我們可以通過貪心的方式找兩幅點雲上點的對應關係（直接找距離最近的點作爲對應點）。
ICP 算法實際上就是交替進行上述兩個步驟，迭代進行計算，直到收斂。
經典流程
ICP算法有較多的數學公式和概念，數學公式總歸看起來費勁，這裏只簡要的理解下其算法步驟：
兩個點集P1，P2，每一步迭代，都朝着距離最小的目標進行。
a. 篩選點對：由P1中的點，在P2中搜索出其最近的點，組成一個點對；找出兩個點集中所有的點對。點對集合相當於進行有效計算的兩個新點集。
b. 根據點集對，即兩個新點集，計算兩個重心。
c. 由新點集，計算出下一步計算的旋轉矩陣R，和平移矩陣t(其實來源於重心的差異)。
d. 得到旋轉矩陣和平移矩陣Rt，就可以計算點集P2進行剛體變換之後的新點集P2，由計算P2到P2的距離平方和，以連續兩次距離平方和之差絕對值，作爲是否收斂的依據。若小於閾值，就收斂，停止迭代。
e. 重複a-e，直到收斂或達到既定的迭代次數。
其中，計算旋轉矩陣R時，需要矩陣方面的運算。
由新的點集，每個點到重心的距離關係，計算正定矩陣N，並計算N的最大特徵值及其最大特徵向量；其特徵向量等價於旋轉的四元數（且是殘差和最小的旋轉四元數），將四元數就可以轉換爲旋轉矩陣。
數學概念：
四元數：http://baike.soso.com/v113161.htm?ch=ch.bk.innerlink
正定矩陣：特徵值都大於0的矩陣。
這些是矩陣理論，最優化原理方面的一些概念。
Efficient Variants of the ICPalgorithm一般流程
Fast ICP 一般算法流程爲：
-
點雲預處理
- 濾波、清理數據等
-
匹配
- 應用上一步求解出的變換，找最近點
-
加權
- 調整一些對應點對的權重
-
剔除不合理的對應點對
-
計算 loss
-
最小化 loss，求解當前最優變換
-
回到步驟 2. 進行迭代，直到收斂
整體上來看，ICP 把點雲配準問題拆分成了兩個子問題：
- 找最近點
- 找最優變換
Fast ICP分爲6個步驟：
a. 篩選：點集或曲面的篩選（濾波）
b. 匹配：兩個點集之間的點進行配對
c. 權重：給每個匹配的點對分配權重
d. 去除：去除不符合條件的點對
e. 誤差度量：基於以上點對，給出每個點對的誤差計算方法
f. 最小化：最小化誤差度量
資源
鏈接：https://blog.csdn.net/viewcode/article/details/8426846
ICP（Iterative closest points）算法是點集配準的經典算法，算法基本原理是在A method for registration of 3-D shapes文章中提出。後人也在此基礎上不斷和補充ICP算法。
- 下面是介紹ICP算法的一些網絡資源：
ICP算法基本介紹：
http://en.wikipedia.org/wiki/Iterative_closest_point
http://plum.eecs.jacobs-university.de/download/3dim2007/node2.html
http://baike.baidu.com/view/1954001.htm
經典ICP算法論文：
http://eecs.vanderbilt.edu/courses/cs359/other_links/papers/1992_besl_mckay_ICP.pdf
Fast ICP論文地址：
http://docs.happycoders.org/orgadoc/graphics/imaging/fasticp_paper.pdf
fast icp source code C++源碼：
http://gfx.cs.princeton.edu/proj/trimesh2/， 大家不容易找到，是因爲工程的名字不叫fasticp，而是trimesh2
算法的一些實現總結：
http://www.mrpt.org/Iterative_Closest_Point_(ICP)_and_other_matching_algorithms
流程中的細節問題
《論文Efficient Variants of the ICPalgorithm》
爲測試以上階段中不同算法的性能和結果，論文提供了三個測試場景，並在這些點集（曲面）上加上噪聲
比較平滑的波紋： 比較簡單的場景，幾何尺度變換不是很大
較複雜的不規則的草原地形： 相對複雜的，包含不同尺度的細節
僅含有突出一個十字形雕刻面的平面： 對匹配而言，最困難的場景，因爲特徵太少
1. 篩選：點集或曲面的篩選（濾波）
篩選也有以下幾種策略：
-
應用點集內所有可用的點： 無疑是性能最差的方法
-
採用均勻分佈的方式篩選（或是距離等間隔篩選）
-
隨機篩選法
-
根據點的密度，顏色來篩選
-
對一個點集進行篩選，或者兩個點集都進行篩選：對於一般的點集，兩種方法收斂率和結果基本相同，但僅對一個點集篩選的算法，數據運算量相對比較大。
-
另外一個篩選策略是依據向量的分佈情況： 篩選哪些能使點的向量的分佈儘量大的點，這樣的目的是突出 特徵很少的 點集的特徵 （如十字雕刻面）
比較
如下圖 a 隨機採用法， 圖b是向量空間分佈法 的示意圖。 由圖可以明顯看出，空間向量分佈法對點集的較少的特徵有着比較好的提取能力。
Fast ICP暫不考慮一個點的密度顏色信息。
- 對均勻分佈法、隨機法、向量空間分佈法比較。
對水波等簡單場景下：其收斂速度和收斂效果都差別不大。但對於特徵點較少的場景，向量空間分佈法的收斂速度和結果很好，而對於另外兩種方法，基本不能收斂。如下：
2. 匹配：兩個點集之間的點進行配對
匹配的策略有以下幾種：
-
最鄰近點法：此方法還可以應用k-d樹或最鄰近點緩存進行加速
-
Normal shooting：點集P1中取一點，沿其點法向量，到點集P2（曲面的）的交點，形成一個點對
-
投影法：將源點集P1投影到目的點集P2上，沿着P2的相機的視角方向，又稱“reverse calibration”，搜索度量方法包括點對點距離，點對線距離，密度，顏色等。
-
基於向量間角度或顏色的兼容度量方法（Compatibility metric）
這裏先給出投影法（b）與最近鄰法（a）的示意圖：
從圖上看，可以看出最鄰近點法容易受到噪聲的干擾，而投影法不受噪聲的干擾。
比較Normal shooting、最鄰近點法和投影法三種方法：
在雕刻十字面的場景下：
**雖然最鄰近點法容易受到噪聲干擾，但對於特徵比較少的場景下，最鄰近點（兼容）法是唯一能完全收斂的方法。效果最好。**對於投影法，收斂很快，但是效果不是很好。Normal shooting法在這兩方面都表現平平。
對於複雜的草原場景，幾種方法的收斂結果最終都基本相同，但其收斂速度和計算速度卻有很大的差別：
從上圖來看，投影法的計算速度非常快，這是因爲不僅投影法的收斂速度快，而且其算法複雜度爲O(1),而其他的算法複雜度爲O(logN)。
3.權重：給每個匹配的點對分配權重
權重分配的策略有以下幾種：
-
均勻分配，即常數加權。
-
根據點對距離加權，點對間距大，權重就小，反之，權重就大。
-
根據向量的一致性（compability）進行加權，weight = n1 * n2
-
不確定性
這幾種方法的收斂速度和效果都差別大。總體上來講，**向量一致（兼容）**的方法 適應性和速度比其他方法好點。
4.去除：去除不符合條件的點對
這個階段的策略包括
-
固定閾值法：當間距大於一個值時，就去除這個點對
-
固定比例法：每次迭代，去除最差的%n的點對
-
標準差法：將閾值設置爲所有點對間距的標準差*2.5
-
去除曲面邊界的點對，如下圖，曲面邊界點的點對是不合理的，在兩個點集做部分匹配的時候。
這幾種方法的收斂速度差別不大，而且收斂效果也差別不大。但是去除outlier點對這個步驟 對收斂結果 的作用還是明顯的。
5.誤差度量和最小化：基於以上點對，給出每個點對的誤差計算方法
誤差度量的策略：
-
點對的距離平方和
-
點到面的距離平方和
-
點到點和點到面的外推法
![在這裏插入圖片描述]()
比較其收斂速率和效果，點到面和點到面外推法的度量方式效果更好。
Fast ICP分析了不同階段裏，各種因素或策略對算法性能和結果的影響。應可以根據不同的需求，來選取不同的算法對點集或曲面進行配準。
在算法效果差別不大的情況下，儘量選擇簡單的算法，來提高運行速度，如隨機採樣，常數加權，固定閾值等。
Fast ICP在網上有開源的代碼，研究一下對此算法會有更深的理解。
參考
https://zhuanlan.zhihu.com/p/104735380
https://blog.csdn.net/viewcode/article/details/8426846
https://blog.csdn.net/hongbin_xu/article/details/80537100