假如一个点云只是另一个点云的刚性变换的话,可以通过不断缩小两个点云点之间的距离得到两个点云之间的刚性变换矩阵,从而使两个点云可以完全拼接在一起,而这种迭代最近点的算法就是ICP.
ICP主要用在2D重建,机器人定位并实现最佳路径规划以及3D模型的registration等应用场合。
1. 添加必要头文件:
#include
#include
#include
2. 如下代码示例所示:实例化icp并设定参数,setInputSource指定变换前的点云,setInputTarget指定InputSource变换后的点云,新定义的Final点云用来存储应用ICP算法之后所得到的点云结果,如果变换前后点云正确Align的话(即变换点云通过刚性变换之后几乎和变换后点云完全重合),则 icp.hasConverged() = 1 (true),然后输出fitness得分和其他一些相关信息。
pcl::IterativeClosestPoint icp;
icp.setInputSource(cloud_in);
icp.setInputTarget(cloud_out);
pcl::PointCloud Final;
icp.align(Final);
std::cout << "has converged:" << icp.hasConverged() << " score: " <<
icp.getFitnessScore() << std::endl;
std::cout << icp.getFinalTransformation() << std::endl;
此外还可以通过设置setMaximumIterations来确定初次配准时的迭代次数,当初次应用ICP算法时,只可以设置的稍微大一点,以使两个点云的点可以尽可能的接近。
开发环境 Visual 2015&PCL1.8.1
PCL1.8.1下载地址: https://github.com/PointCloudLibrary/pcl/releases/tag/pcl-1.8.1
PCL头文件目录:
$(PCL_ROOT)
$(PCL_ROOT)\include\pcl-1.8
$(PCL_ROOT)\3rdParty\Eigen\eigen3
$(PCL_ROOT)\3rdParty\FLANN\include
$(PCL_ROOT)\3rdParty\VTK\include\vtk-8.0
$(PCL_ROOT)\3rdParty\Boost\include\boost-1_64
PCL库目录:
$(PCL_ROOT)\lib\*.lib
$(PCL_ROOT)\3rdParty\VTK\lib\*.lib
$(PCL_ROOT)\3rdParty\FLANN\lib\*.lib
$(PCL_ROOT)\3rdParty\Boost\lib\*.lib
具体代码可以参考我的github: ICP Example
里面有两个代码,Source.cpp是PCL官方的例子,mian.cpp是我参考3改的测试例子, 运行某个例子时需要把另外一个Exclude掉。编译并运行,每点击一次空格键做一次ICP算法,结果如下:
应用ICP之前,
ICP算法迭代60次之后,
实际变换矩阵:
Applying this rigid transformation to: cloud_in -> cloud_icp
Rotation matrix :
| 0.707 -0.707 0.000 |
R = | 0.707 0.707 0.000 |
| 0.000 0.000 1.000 |
Translation vector :
t = < 0.000, 0.000, 0.400 >
ICP算法得到的转换矩阵
ICP transformation 0 : cloud_icp -> cloud_in
Rotation matrix :
| 0.724 0.690 -0.010 |
R = | -0.690 0.723 -0.022 |
| -0.008 0.023 1.000 |
Translation vector :
t = < 0.010, 0.025, -0.399 >
对比真实转换矩阵和位移,已经非常接近了。所以说ICP算法得到的只是一个近似矩阵,迭代次数决定了与真实矩阵的近似程度。
本博文只是测试了PCL官方的两个ICP的例子,此处使用的点云均为同一个点云,其中点的数目和位置均完全相同,但是在实际应用中,不论是结构光扫描还是激光扫描,同一个3D镜头不同时刻拍出点云的数目是不一样的,而且如果由多个相同镜头不同角度拍摄同一物体,点云的数目也不一样,ICP算法在这些情况下的应用效果还需以后验证。
1. http://pointclouds.org/documentation/tutorials/iterative_closest_point.php
2. http://pointclouds.org/documentation/tutorials/pcl_visualizer.php
3. http://pointclouds.org/documentation/tutorials/interactive_icp.php