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

/* ---[ */
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);
  }
}

/**匹配一对点云数据集并且返还结果
  *参数 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;
 }
效果图如下，此为动态图，请耐心等待几秒钟，注意迭代过程。