3Ｄ点云配准（二多幅点云配准）

在上一篇文章 点云配准（一 两两配准）中我们介绍了两两点云之间的配准原理。本篇文章，我们主要介绍一下PCL中对于多幅点云连续配准的实现过程，重点请关注代码行的注释。
对于多幅点云的配准，它的主要思想是对所有点云进行变换，使得都与第一个点云在统一坐标系中。在每个连贯的、有重叠的点云之间找到最佳的变换，并累积这些变换到全部的点云。能够进行ICP算法的点云需要进行粗略的预匹配，并且一个点云与另一个点云需要有重叠部分。

此处我们以郭浩主编的《点云库PCL从入门到精通》提供的示例demo来介绍一下多幅点云进行配准的过程。

/* ---[ */
int main (int argc, char** argv)
{
    // 加载数据
  std::vector<PCD, Eigen::aligned_allocator<PCD> > data; //存储管理所有打开的点云
  loadData (argc, argv, data); //加载所有点云文件到data
    //检查用户输入
  if (data.empty ())
  {
    PCL_ERROR ("Syntax is: %s <source.pcd> <target.pcd> [*]", argv[0]);
    PCL_ERROR ("[*] - multiple files can be added. The registration results of (i, i+1) will be registered against (i+2), etc");
    PCL_INFO ("Example: %s `rospack find pcl`/test/bun0.pcd `rospack find pcl`/test/bun4.pcd", argv[0]);
    return(-1);
  }
  PCL_INFO ("Loaded %d datasets.", (int)data.size ());
    //创建一个PCL可视化对象
  p = new pcl::visualization::PCLVisualizer (argc, argv, "Pairwise Incremental Registration example"); //创建一个PCL可视化对象
  p->createViewPort (0.0, 0, 0.5, 1.0, vp_1); //用左半窗口创建视口vp_1
  p->createViewPort (0.5, 0, 1.0, 1.0, vp_2); //用右半窗口创建视口vp_2
   PointCloud::Ptr result (new PointCloud), source, target;
  Eigen::Matrix4f GlobalTransform =Eigen::Matrix4f::Identity (), pairTransform;
    for (size_t i = 1; i < data.size (); ++i)  //循环处理所有点云
  {
    source = data[i-1].cloud; //连续配准
    target = data[i].cloud;   //相邻两组点云
      //添加可视化数据
    showCloudsLeft(source, target); //可视化为配准的源和目标点云
    PointCloud::Ptr temp (new PointCloud);
    PCL_INFO ("Aligning %s (%d) with %s (%d).\n", data[i-1].f_name.c_str (), source->points.size (), data[i].f_name.c_str (),target->points.size ());
    //调用子函数完成一组点云的配准，temp返回配准后两组点云在第一组点云坐标下的点云，pairTransform返回从目标点云target到源点云source的变换矩阵。
    //现在我们开始进行实际的匹配，由子函数pairAlign具体实现，
    //其中参数有输入一组需要配准的点云，以及是否进行下采样的设置项，其他参数输出配准后的点云及变换矩阵。
    pairAlign (source, target, temp, pairTransform, true);
      //把当前的两两配对转换到全局变换
      //把当前的两两配对后的点云temp转换到全局坐标系下（第一个输入点云的坐标系）返回result
    pcl::transformPointCloud (*temp, *result, GlobalTransform);
      //update the global transform更新全局变换
      //用当前的两组点云之间的变换更新全局变换
    GlobalTransform = pairTransform * GlobalTransform;
    //保存转换到第一个点云坐标下的当前配准后的两组点云result到文件i.pcd
    std::stringstream ss;
    ss << i << ".pcd";
    pcl::io::savePCDFile (ss.str (),*result, true);
  }
}

对于上述过程中的核心函数pairAlign()，我们重点介绍如下：

/**匹配一对点云数据集并且返还结果

  *参数 cloud_src 是源点云

  *参数 cloud_src 是目标点云

  *参数output输出的配准结果的源点云

  *参数final_transform是在来源和目标之间的转换

  */

void pairAlign (const
PointCloud::Ptr cloud_src, const PointCloud::Ptr cloud_tgt, PointCloud::Ptr output,
Eigen::Matrix4f &final_transform, bool downsample
= false)

{

    //下采样

    //为了一致性和高速的下采样

    //注意：为了大数据集需要允许这项

  PointCloud::Ptr src (new PointCloud);
//存储滤波后的源点云

  PointCloud::Ptr tgt (new PointCloud);
//存储滤波后的目标点云

  pcl::VoxelGrid<PointT> grid; //滤波处理对象

  if (downsample)

  {

    grid.setLeafSize (0.05, 0.05, 0.05);
//设置滤波处理时采用的体素大小

    grid.setInputCloud (cloud_src);

    grid.filter (*src);

    grid.setInputCloud (cloud_tgt);

    grid.filter (*tgt);

  }

  else

  {

    src = cloud_src;

    tgt = cloud_tgt;

  }

    //计算曲面法线和曲率

  PointCloudWithNormals::Ptr points_with_normals_src (new PointCloudWithNormals);

  PointCloudWithNormals::Ptr
points_with_normals_tgt (new PointCloudWithNormals);

  pcl::NormalEstimation<PointT,
PointNormalT> norm_est; //点云法线估计对象

  pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ> ());

  norm_est.setSearchMethod (tree); //设置估计对象采用的搜索对象

  norm_est.setKSearch (30); //设置估计时进行搜索用的k数

  norm_est.setInputCloud (src);

  norm_est.compute
(*points_with_normals_src); //下面分别估计源和目标点云法线

  pcl::copyPointCloud (*src, *points_with_normals_src);

  norm_est.setInputCloud (tgt);

  norm_est.compute
(*points_with_normals_tgt);

  pcl::copyPointCloud (*tgt,
*points_with_normals_tgt);



  //一切准备好之后，可以开始配准了，创建ICP对象，设置它的参数

  //以需要匹配的两个点云作为输入，使用时，参数的设置需要根据自己的数据集进行调整。

    //

    //举例说明我们自定义点的表示（以上定义）

  MyPointRepresentation point_representation;

    //调整'curvature'尺寸权重以便使它和x, y, z平衡

  float alpha[4] = {1.0, 1.0, 1.0, 1.0};

  point_representation.setRescaleValues
(alpha);

    //

    // 配准

  pcl::IterativeClosestPointNonLinear<PointNormalT,
PointNormalT> reg;  //配准对象

  reg.setTransformationEpsilon (1e-6); //设置收敛判断条件，越小精度越大，收敛也越慢

    //将两个对应关系之间的(src<->tgt)最大距离设置为10厘米

    //注意：根据你的数据集大小来调整

  reg.setMaxCorrespondenceDistance (0.1);

    //设置点表示

  reg.setPointRepresentation (boost::make_shared<const MyPointRepresentation> (point_representation));

  reg.setInputCloud
(points_with_normals_src); //设置源点云

  reg.setInputTarget (points_with_normals_tgt); //设置目标点云

    //

    //在一个循环中运行相同的最优化并且使结果可视化

  Eigen::Matrix4f Ti = Eigen::Matrix4f::Identity (), prev,
targetToSource;

  PointCloudWithNormals::Ptr reg_result =
points_with_normals_src;

    //由于这是一个demo，因而希望显示匹配过程的迭代过程，为达到该目的，ICP在内部进行计算时，限制其最大的迭代次数为2，即每迭代两次就认为收敛，停止内部迭代

  reg.setMaximumIterations (2); //设置最大迭代次数



  //手动迭代，此处设置为30次，每手动迭代一次，在配准结果视口对迭代的最新的结果进行刷新显示

  for (int i = 0; i < 30; ++i)

  {

    PCL_INFO ("Iteration Nr. %d.\n", i);

      //为了可视化的目的保存点云

    points_with_normals_src = reg_result;



    reg.setInputCloud
(points_with_normals_src);

    reg.align (*reg_result);

      //在每一个迭代之间累积转换

    Ti = reg.getFinalTransformation () * Ti;

      //如果这次转换和之前转换之间的差异小于阈值

      //则通过减小最大对应距离来改善程序



      //也就是说，如果迭代N次找到的变换和迭代N-1次中找到的变换之间的差异小于传给ICP的变换收敛阈值，我们选择源和目标之间更靠近的对应点距离阈值来改善配准过程



    if (fabs ((reg.getLastIncrementalTransformation () -
prev).sum ()) < reg.getTransformationEpsilon ())

      reg.setMaxCorrespondenceDistance
(reg.getMaxCorrespondenceDistance () - 0.001);

      prev =
reg.getLastIncrementalTransformation ();

      //可视化当前状态

    showCloudsRight(points_with_normals_tgt, points_with_normals_src);

  }

    //



    //一旦找到最优的变换，ICP返回的变换是从源点云到目标点云的变换矩阵，我们求逆变换得到从目标点云到源点云的变换矩阵，并应用到目标点云，变换后的目标点云然后添加到源点云中。



    // 得到目标点云到源点云的变换

    targetToSource = Ti.inverse();

    //

    //把目标点云转换回源框架

  pcl::transformPointCloud (*cloud_tgt, *output,
targetToSource);

  p->removePointCloud ("source");

  p->removePointCloud ("target");

 
PointCloudColorHandlerCustom<PointT> cloud_tgt_h (output, 0, 255, 0);

 
PointCloudColorHandlerCustom<PointT> cloud_src_h (cloud_src, 255, 0, 0);

  p->addPointCloud (output,
cloud_tgt_h, "target", vp_2);

  p->addPointCloud (cloud_src,
cloud_src_h, "source", vp_2);

   PCL_INFO ("Press q to continue the registration.\n");

  p->spin ();

  p->removePointCloud ("source");

  p->removePointCloud ("target");

    //添加源点云到转换目标

  *output += *cloud_src;

    final_transform = targetToSource;

 }

效果图如下，此为动态图，请耐心等待几秒钟，注意迭代过程。

主要参考:郭浩主编的<点云库PCL从入门到精通>。