PCL: Registration模組之IterativeClosestPoint點雲配準

查志強發表於2016-08-30

【原文：http://blog.csdn.net/u012337034/article/details/38307219】

參考文獻：

關於此類的詳細資訊和學術上的理論推導大家可以檢視PaulJ. Besl的文章“A Method forRegistration of 3-D Shapes”。

小結：

ICP匹配，中文應該叫臨近點迭代吧，是計算機圖形學中的一個非常有用的演算法，尤其是在三維重建點雲配準中有著極其重要的地位，此外在SLAM等移動機器人導航等領域也有著很大的用武之地。

經過了十多年的發展ICP也有著很多的變種，今天我們首先熟悉下最基本的ICP匹配演算法，PCL中的實現與參考文獻中的一致，最終的變換矩陣都是基於SVD（奇異值分解）的。

在具體的實踐中，一共有3個約束來終止迭代：

迭代次數，預設值為10；
上次轉換與當前轉換的差值；
前後兩次迭代方差的差值。

當然我們還可以使用kdtree加速演算法。

還有一點需要重點提示下，輸入的點雲需要經過預處理，過於複雜和過多噪聲的點與將會出現“Invalid (NaN, Inf) point coordinates given to nearestKSearch!”的錯誤，比如我使用了官網教程中“樓梯的點雲“。

不同版本的PCL函式命名可能不同，函式個數可能也不一樣。

詳細介紹：

引數輸入輸出：

此類由基類Registration派生，生成物件方式也很簡單，如下：

Pcl:: IterativeClosestPoint<SourcePointTargetPoint> icp

成員函式：

這裡我就不一一介紹所有的成員函式了，只是把幾個非常重要的成員函式給列出來，並給出其的使用方法：

inline void inline void setSearchMethodTarget(const KdTreePtr &tree) kdtree加速搜尋，還有一個Target的函式，用法與之一致。
inline void setInputSource (constPointCloudSourceConstPtr &cloud) 需要匹配的點雲。
inline void setInputTarget (constPointCloudTargetConstPtr &cloud) 基準點雲，也就是從Source到Target的匹配。
inline void setMaxCorrespondenceDistance (doubledistance_threshold) 忽略在此距離之外的點，如果兩個點雲距離較大，這個值要設的大一些（PCL預設距離單位是m）。
inline void setTransformationEpsilon (doubleepsilon) 第2個約束，這個值一般設為1e-6或者更小。
inline void setEuclideanFitnessEpsilon (doubleepsilon) 第3個約束，前後兩次迭代誤差的差值。
inline void setMaximumIterations (intnr_iterations) 第1個約束，迭代次數，幾十上百都可能出現。
inline void align (PointCloudSource &output)輸出配準後點雲。
inline Matrix4 getFinalTransformation () 獲取最終的轉換矩陣。

效果展示：

最近終於把PCL1.7.1搞定了，大家可以參考我的另一篇部落格來安裝，IDE為vs2010。我使用的是經典的史丹佛小兔子，原始碼和源點雲檔案我已上傳到CSDN（點此下載）。所需要的核心程式碼如下（我只是列出了部分需要設定引數的程式碼）：

[cpp]view
 plain copy
 print?

icp.setInputSource(cloud_source);  

icp.setInputTarget(cloud_target);  

icp.setMaxCorrespondenceDistance(0.1);  

icp.setTransformationEpsilon(1e-6);  

icp.setEuclideanFitnessEpsilon(1);  

icp.setMaximumIterations(50);  

icp.align(*cloud_source_registration);  

Eigen::Matrix4f transformation = icp.getFinalTransformation();

由上述程式碼片可知：一共有4個資料需要設定，這裡我們修改不同的資料看下效果圖（綠色為Target，藍色為Source，紅色為匹配後點雲）: