ICP點雲配準算法

• ICP算法介紹
• ICP算法流程圖
• ICP算法優缺點與改進
• • ICP算法的優點
  • ICP算法的不足
  • ICP算法的改進
• ICP算法PCL庫實作

ICP算法介紹

ICP（Iterative Closest Point），即最近點疊代算法，是最為經典的資料配準算法。其特征在于，通過求取源點雲和目标點雲之間的對應點對，基于對應點對構造旋轉平移矩陣，并利用所求矩陣，将源點雲變換到目标點雲的坐标系下，估計變換後源點雲與目标點雲的誤差函數，若誤差函數值大于閥值，則疊代進行上述運算直到滿足給定的誤差要求.

建立對應點對的點雲集：

最小二乘估計最優變換矩陣：

ICP算法采用最小二乘估計計算最優變換矩陣，目标函數E(R,t)的優化是ICP算法的最後一個階段。在求得目标函數後，采用什麼樣的方法來使其收斂到最小，也是一個比較重要的問題。求解方法有基于奇異值分解的方法、牛頓法等。ICP算法重複進行“确定對應關系的點集→計算最優剛體變換”的過程，直到某個表示正确比對的收斂準則得到滿足。

ICP算法流程圖

ICP算法優缺點與改進

ICP算法的優點

1. 如果初始值合适， 可以獲得不錯的配準效果
2. 不必對處理的點雲集進行分割和特征提取
3. 在較好的變換矩陣初值情況下，可以得到很好的算法收斂性

ICP算法的不足

1. 在搜尋對應點的過程中，計算量非常大，速度慢，這是傳統ICP算法的瓶頸
2. 對待配準點雲的初始位置有一定要求，若所選初始位置不合理，則會導緻算法陷入局部最優
3. 标準ICP算法中尋找對應點時，認為歐氏距離最近的點就是對應點。這種假設有不合理之處，會産生一定數量的錯誤對應點

ICP算法的改進

基于ICP的不足，許多研究者提出了各種改進版本，主要有如下幾個方面：點雲集中點的過濾（采樣）、對應點雲的比對方式、比對點對的權重、比對點對的篩選、點雲轉換後目标函數的選擇和收斂。

• 點雲采樣：

  标準的ICP算法選用所有的點來計算對應點，但通常用于配準的點集元素數量非常大，為了縮減，選用最少的點來表征原始點集的全部特征資訊，點集選取時，可以采用如下方式：1. 均勻采樣（Uniform sub-sampling ）；2.随機采樣（Random sampling）；3.基于特征采樣（Feature based Sampling ）；4.法向空間均勻采樣（Normal-space sampling）

  法向采樣保證了法向上的連續性，如圖：

  

  基于特征的采樣使用一些具有明顯特征的點集來進行配準，大量減少了對應點的數目，提高了效率和精确度，但要求點雲預處理：

  

  • 點集比對方式：

    最近鄰點（Closest Point），穩定，但速度慢；

    

    法方向最近鄰點（Normal Shooting），平滑曲面效果好，但對噪音敏感

    

    投影法（Projection），搜尋對應點速度快

    

  • 加快搜尋效率

    用K-D樹或Voronoi圖加快最近點搜尋效率。

  • 損失（目标）函數

    可以采用不同距離量測方式來定義損失函數，有點到點（point-to-point）、點到線（point-to-line）、點到面(point-to-plane)和面到面(plane-to-plane)的方式。

ICP算法PCL庫實作

PCL點雲庫已經實作了多種點雲配準算法，ICP算法是基于SVD（Singular Value Decomposition）實作的，使用PCL的ICP算法之前要設定的參數：

1. setMaximumIterations，最大疊代次數

2. setEuclideanFitnessEpsilon，設定收斂條件是均方誤差和小于門檻值，停止疊代

3. setTransformtionEpsilon, 設定兩次變化矩陣之間的內插補點（一般設定為1e-10即可）

4. setMaxCorrespondenaceDistance, 設定對應點對之間的最大距離（此值對配準結果影響較大）

PCL庫的ICP實作連結：

http://pointclouds.org/documentation/tutorials/interactive_icp.php#interactive-icp