PCL系列——如何使用迭代最近点法（ICP）配准

PCL系列

说明

通过本教程，我们将会学会：

如何使用迭代最近点法（Iterative Closest Point）判断一个点云是否是另一个点云的刚体变换
使用的方法是：最小化两个点云中对应点之间的距离，并刚性变换他们。
代码说明：先生成输入点云（待变换的点云）和目标点云（变换参照），然后创建ICP对象，设置该对象的输入点云和目标点云，然后进行配准，并显示ICP配准信息和变换矩阵。

操作

在VS2010 中新建一个文件 iterative_closest_point.cpp，然后将下面的代码复制到文件中。
参照之前的文章，配置项目的属性。设置包含目录和库目录和附加依赖项。

#include <iostream> //标准输入/输出
#include <pcl/io/pcd_io.h> //pcd文件输入/输出
#include <pcl/point_types.h> //各种点类型
#include <pcl/registration/icp.h> //ICP(iterative closest point)配准

int main (int argc, char** argv)
{
  //创建点云指针
  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_in (new pcl::PointCloud<pcl::PointXYZ>); //创建输入点云（指针）
  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_out (new pcl::PointCloud<pcl::PointXYZ>); //创建输出/目标点云（指针）

  //生成并填充点云cloud_in
  cloud_in->width    = 5;
  cloud_in->height   = 1;
  cloud_in->is_dense = false;
  cloud_in->points.resize (cloud_in->width * cloud_in->height); //变形，无序
  for (size_t i = 0; i < cloud_in->points.size (); ++i) //随机数初始化点的坐标
  {
    cloud_in->points[i].x = 1024 * rand () / (RAND_MAX + 1.0f);
    cloud_in->points[i].y = 1024 * rand () / (RAND_MAX + 1.0f);
    cloud_in->points[i].z = 1024 * rand () / (RAND_MAX + 1.0f);
  }
  std::cout << "Saved " << cloud_in->points.size () << " data points to input:"
      << std::endl;
  //打印点云cloud_in中所有点的坐标信息
  for (size_t i = 0; i < cloud_in->points.size (); ++i) std::cout << "    " <<
      cloud_in->points[i].x << " " << cloud_in->points[i].y << " " <<
      cloud_in->points[i].z << std::endl;

  // 填充点云cloud_out
  *cloud_out = *cloud_in; //初始化cloud_out
  std::cout << "size:" << cloud_out->points.size() << std::endl;
  for (size_t i = 0; i < cloud_in->points.size (); ++i)
    cloud_out->points[i].x = cloud_in->points[i].x + 0.7f; //平移cloud_in得到cloud_out
  std::cout << "Transformed " << cloud_in->points.size () << " data points:"
      << std::endl;
  //打印点云cloud_out中所有点的坐标信息（每一行对应一个点的xyz坐标）
  for (size_t i = 0; i < cloud_out->points.size (); ++i)
    std::cout << "    " << cloud_out->points[i].x << " " <<
      cloud_out->points[i].y << " " << cloud_out->points[i].z << std::endl;
  //*********************************
  // ICP配准
  //*********************************
  pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp; //创建ICP对象，用于ICP配准
  icp.setInputCloud(cloud_in); //设置输入点云
  icp.setInputTarget(cloud_out); //设置目标点云（输入点云进行仿射变换，得到目标点云）
  pcl::PointCloud<pcl::PointXYZ> Final; //存储结果
  //进行配准，结果存储在Final中
  icp.align(Final); 
  //输出ICP配准的信息（是否收敛，拟合度）
  std::cout << "has converged:" << icp.hasConverged() << " score: " <<
  icp.getFitnessScore() << std::endl;
  //输出最终的变换矩阵（4x4）
  std::cout << icp.getFinalTransformation() << std::endl;

 return (0);
}

重新生成项目。
到改项目的Debug目录下，按住Shift，同时点击鼠标右键，在当前窗口打开CMD窗口。
在命令行中输入iterative_closest_point.exe，可以得到下图所示结果。

参考

How to use iterative closest point