PCL如何使用ICP(iterative closest point)算法

ICP介绍

假如一个点云只是另一个点云的刚性变换的话，可以通过不断缩小两个点云点之间的距离得到两个点云之间的刚性变换矩阵，从而使两个点云可以完全拼接在一起，而这种迭代最近点的算法就是ICP.

Why ICP 算法？

ICP主要用在2D重建，机器人定位并实现最佳路径规划以及3D模型的registration等应用场合。

如何使用PCL里的ICP算法？

 1. 添加必要头文件：

#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/registration/icp.h>  

 2. 如下代码示例所示：实例化icp并设定参数，setInputSource指定变换前的点云，setInputTarget指定InputSource变换后的点云,新定义的Final点云用来存储应用ICP算法之后所得到的点云结果，如果变换前后点云正确Align的话（即变换点云通过刚性变换之后几乎和变换后点云完全重合），则 icp.hasConverged() = 1 (true)，然后输出fitness得分和其他一些相关信息。

  pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp;
  icp.setInputSource(cloud_in);
  icp.setInputTarget(cloud_out);
  pcl::PointCloud<pcl::PointXYZ> Final;