ICP迭代最近點最佳化__PCL

點雲之間的局部配准問題採樣迭代最近點演算法（IteratedClosest Points, ICP），ICP演算法因為其思想簡單，精度高等特點成為了局部配準的主流演算法。

迭代最近點演算法（ICP）顧名思義，就是採用迭代最佳化的思想以空間距離作為匹配點的選擇依據，通過不斷調整點雲的位姿使得匹配點之間距離累計最小。假設有兩組點雲，其中一個目標點雲A另一個為參考點雲B，ICP演算法的目的是為了算出一個最優的旋轉矩陣R和平移向量t使得變換後的點雲A能與B達到最精確的匹配。如公式（1）所示，求得匹配點誤差平方和達到極小的R，t：

ICP需要反覆執行以下兩步直至收斂：第一步，計算A點集與B點集之間的匹配點對；第二步，根據前一步得到的匹配點對計算出A點集與B點集之間的轉換矩陣，並對A點集進行相應的轉換。在取得了點集A與B的匹配點基礎上，點集之間的轉換矩陣求解也分為兩種方式：利用線性代數的求解（主要是SVD），以及利用非線性最佳化方式的求解（類似於Bundle Adjustment）。其中奇異值分解法（SVD）因為準確、穩定以及高效等特點被廣泛運用，接下來介紹利用SVD方法的轉換矩陣計算方法。

首先，定義兩組點的質心：

隨後，在誤差函數中做如下處理：

仔細觀察左右兩項，可以發現左邊只和旋轉矩陣R相關，而右邊既有R也有t，但只和質心相關。只要獲得了R，令第二項為零就能得到t。那麼最下化問題就轉化為如下公式：

    解得R後，即可求解t。