PCL: Registration模块之IterativeClosestPoint点云配准

参考文献：

关于此类的详细信息和学术上的理论推导大家可以查看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，红色为匹配后点云）:





4个参数分别为：1.5，1e-10，0.1，100, 匹配的相当不好。





4个参数分别为：1.5，1e-10，0.01，100，完美重合。

下面试一下我自己扫的小盒子。（单位mm，注意参数变化）。



4个参数分别为：1500，1e-10，1，300, 匹配的相当不好。



4个参数分别为：1500，1e-10，0.1，300, 完美重合。

强烈建议大家使用CMake进行项目管理和使用最新版的PCL1.7.1。

对了我的邮箱为littletinygo@sina.com，我也是刚刚接触这个领域，希望与大家多多交流共同学习、共同进步。