【PCL】教程 example1实现PointNormal类型点云间的迭代最近点(ICP)配准

PCL 注册 API 

将各种 3D 点云数据视图一致地对齐到完整模型的过程称为注册。其目标是找到在全局坐标框架中单独获取的视图的相对位置和方向，以便它们之间的交叉区域完美重叠。因此，对于从不同视图获取的每个点云数据集，我们需要一个能够将它们对齐到单个点云模型的系统，以便可以应用后续处理步骤，如分割和对象重建。

代码首先包含了一系列pcl库的头文件，用于工作流中的各种点云处理步骤：输入输出、常用计算、点类型定义、特征计算、配准等。使用了pcl命名空间下的多个模块。定义了几种类型别名，例如PointNormal被定义为PointT。

在findCorrespondences函数中，代码创建了一个CorrespondenceEstimationBackProjection的对象，用来估计两个点云之间的对应关系。该函数依赖于点云的表面法线信息，使用带法线的点云来估计源点云src和目标点云tgt之间的对应关系。

rejectBadCorrespondences函数负责拒绝/过滤掉坏的对应关系，它首先使用CorrespondenceRejectorMedianDistance根据中位数因子来拒绝离群值，然后根据点法线被CorrespondenceRejectorSurfaceNormal作为标准进一步拒绝不满足阈值的对应关系。

findTransformation函数利用点到平面的最小二乘法估计（TransformationEstimationPointToPlaneLLS）来找到最佳变换，帮助将src点云对齐到tgt点云。

view函数是用于可视化结果的辅助函数。如果设置了可视化标志，它会显示源点云、目标点云以及它们之间的对应关系。

在icp函数中，执行了迭代最近点（ICP）算法，基于以上定义的函数完成对两个点云的配准。这个过程会循环查找对应关系，拒绝坏的对应关系，估计变换，并不断更新源点云的位置直到收敛。最终，将变换矩阵返回。

saveTransform函数负责将最终的变换矩阵写入一个外部文件中，以便于其他应用或者分析使用。

main函数是程序的入口点，解析命令行参数来获取输入输出文件，设置调试和可视化选项，加载点云文件，调用ICP算法配准点云，保存变换矩阵，并在标准输出中打印变换矩阵以供查看。

该程序的总体目标是，加载两个PCD文件，利用ICP算法寻找一个变换矩阵，从而可以最好地使两个点云对齐。在这一过程中，它还提供了错误拒绝策略，以提高配准的准确性，并且可选地支持结果的可视化。最后，将最佳变换矩阵保存到文件，并打印到控制台。

#include <fstream> // 包含C++文件操作库
#include <pcl/common/angles.h> // 包含PCL库中的角度转换相关功能
#include <pcl/console/parse.h> // 包含PCL命令行解析功能
#include <pcl/point_types.h> // 包含PCL点类型定义
#include <pcl/point_cloud.h> // 包含PCL点云数据结构
#include <pcl/point_representation.h> // 包含PCL点表示方法


#include <pcl/io/pcd_io.h> // 包含PCL的PCD文件输入输出操作
#include <pcl/console/time.h> // 包含PCL计时功能
#include <pcl/features/normal_3d.h> // 包含PCL表面法线估计相关功能
#include <pcl/features/fpfh.h> // 包含PCL的FPFH特征描述子计算功能
#include <pcl/registration/correspondence_estimation.h> // 包含PCL点云配准中的对应点估计功能
#include <pcl/registration/correspondence_estimation_normal_shooting.h> // 包含PCL取法线方向作为对应点搜索方向的对应点估计方法
#include <pcl/registration/correspondence_estimation_backprojection.h> // 包含PCL基于反投影的对应点估计方法
#include <pcl/registration/correspondence_rejection_median_distance.h> // 包含PCL基于中值距离的对应点剔除方法
#include <pcl/registration/correspondence_rejection_surface_normal.h> // 包含PCL基于表面法线一致性的对应点剔除方法
#include <pcl/registration/transformation_estimation_point_to_plane_lls.h> // 包含PCL的点到平面最小二乘法变换估计方法
#include <pcl/registration/default_convergence_criteria.h> // 包含PCL的默认收敛判断条件定义


#include <pcl/visualization/pcl_visualizer.h> // 包含PCL的点云可视化工具


using namespace pcl;  // 使用PCL命名空间
using namespace pcl::io; // 使用PCL输入输出命名空间
using namespace pcl::console;  // 使用PCL控制台命名空间
using namespace pcl::registration; // 使用PCL注册命名空间
using namespace pcl::visualization; // 使用PCL可视化命名空间


typedef PointNormal PointT;  // 定义点类型别名，这里使用包含法线的点类型
typedef PointCloud<PointT> Cloud; // 定义点云类型别名
typedef Cloud::Ptr CloudPtr;  // 定义点云智能指针别名
typedef Cloud::ConstPtr CloudConstPtr; // 定义点云常量智能指针别名


CloudPtr src, tgt; // 声明源点云和目标点云的智能指针


bool rejection = true; // 是否执行对应点剔除
bool visualize = false; // 是否执行可视化


PCLVisualizer::Ptr vis; // PCL可视化实例的智能指针



// 以下是一系列函数的定义

// 对应估计（correspondence estimation）
void
findCorrespondences (const CloudPtr &src,
                     const CloudPtr &tgt,
                     Correspondences &all_correspondences)
{
  // CorrespondenceEstimationNormalShooting<PointT, PointT, PointT> est; // 创建基于法线射击的对应点估计实例
  // CorrespondenceEstimation<PointT, PointT> est; // 创建基于最近点的对应点估计实例
  CorrespondenceEstimationBackProjection<PointT, PointT, PointT> est; // 创建基于反投影的对应点估计实例
  est.setInputSource (src); // 设置源点云
  est.setInputTarget (tgt); // 设置目标点云
  
  est.setSourceNormals (src); // 设置源点云的法线
  est.setTargetNormals (tgt); // 设置目标点云的法线
  est.setKSearch (10); // 设置在目标点云中搜索最近点的数量上限
  est.determineCorrespondences (all_correspondences); // 计算对应点
  // est.determineReciprocalCorrespondences (all_correspondences); // 计算双向最近邻对应点（注释掉了）
}
//拒绝/过滤掉坏的对应关系
void
rejectBadCorrespondences (const CorrespondencesPtr &all_correspondences,
                          const CloudPtr &src,
                          const CloudPtr &tgt,
                          Correspondences &remaining_correspondences)
{
  CorrespondenceRejectorMedianDistance rej; // 创建基于中值距离的对应点剔除实例
  rej.setMedianFactor (8.79241104); // 设置中值因子
  rej.setInputCorrespondences (all_correspondences); // 设置输入的对应点
  
  rej.getCorrespondences (remaining_correspondences); // 执行剔除操作
  return; // 返回，以下代码被注释
  
  CorrespondencesPtr remaining_correspondences_temp (new Correspondences); // 创建剩余对应点的智能指针
  rej.getCorrespondences (*remaining_correspondences_temp); // 执行剔除并保存结果
  PCL_DEBUG ("[rejectBadCorrespondences] Number of correspondences remaining after rejection: %d\n", remaining_correspondences_temp->size ());


  // Reject if the angle between the normals is really off
  CorrespondenceRejectorSurfaceNormal rej_normals; // 创建基于表面法线一致性的对应点剔除实例
  rej_normals.setThreshold (std::acos (deg2rad (45.0))); // 设定法线夹角的阈值
  rej_normals.initializeDataContainer<PointT, PointT> (); // 初始化数据容器
  rej_normals.setInputCloud<PointT> (src); // 设置源点云
  rej_normals.setInputNormals<PointT, PointT> (src); // 设置源点云的法线
  rej_normals.setInputTarget<PointT> (tgt); // 设置目标点云
  rej_normals.setTargetNormals<PointT, PointT> (tgt); // 设置目标点云的法线
  rej_normals.setInputCorrespondences (remaining_correspondences_temp); // 设置输入的对应点
  rej_normals.getCorrespondences (remaining_correspondences); // 执行剔除操作
}


void
findTransformation (const CloudPtr &src,
                    const CloudPtr &tgt,
                    const CorrespondencesPtr &correspondences,
                    Eigen::Matrix4d &transform)
{
  TransformationEstimationPointToPlaneLLS<PointT, PointT, double> trans_est; // 创建点到平面最小二乘变换估计实例
  trans_est.estimateRigidTransformation (*src, *tgt, *correspondences, transform); // 估计刚性变换
}


void
view (const CloudConstPtr &src, const CloudConstPtr &tgt, const CorrespondencesPtr &correspondences)
{
  if (!visualize || !vis) return; // 如果不需要可视化或可视化实例未创建，直接返回
  PointCloudColorHandlerCustom<PointT> green (tgt, 0, 255, 0);  // 创建绿色的点云颜色处理器
  if (!vis->updatePointCloud<PointT> (src, "source"))
  {
    vis->addPointCloud<PointT> (src, "source"); // 添加或更新源点云到可视化中
    vis->resetCameraViewpoint ("source"); // 重置相机视角到源点云
  }
  if (!vis->updatePointCloud<PointT> (tgt, green, "target")) vis->addPointCloud<PointT> (tgt, green, "target"); // 添加或更新目标点云到可视化中
  vis->setPointCloudRenderingProperties (PCL_VISUALIZER_OPACITY, 0.5, "source"); // 设置源点云的透明度
  vis->setPointCloudRenderingProperties (PCL_VISUALIZER_OPACITY, 0.7, "target"); // 设置目标点云的透明度
  vis->setPointCloudRenderingProperties (PCL_VISUALIZER_POINT_SIZE, 6, "source"); // 设置源点云的点大小
  pcl::console::TicToc tt; // 创建计时器
  tt.tic (); // 开始计时
  if (!vis->updateCorrespondences<PointT> (src, tgt, *correspondences, 1)) 
    vis->addCorrespondences<PointT> (src, tgt, *correspondences, 1, "correspondences"); // 添加或更新对应点到可视化中
  tt.toc_print (); // 打印计时结果
  vis->setShapeRenderingProperties (PCL_VISUALIZER_LINE_WIDTH, 5, "correspondences"); // 设置对应点连接线宽度
  // vis->setShapeRenderingProperties (PCL_VISUALIZER_COLOR, 1.0, 0.0, 0.0, "correspondences"); // 设置对应点连接线颜色（注释掉了）
  vis->spin (); // 开始交互式可视化
}


void
icp (const PointCloud<PointT>::Ptr &src,  // ICP迭代过程的函数定义
     const PointCloud<PointT>::Ptr &tgt,
     Eigen::Matrix4d &transform)
{
  CorrespondencesPtr all_correspondences (new Correspondences), 
                     good_correspondences (new Correspondences); // 创建所有对应点和好的对应点的智能指针


  PointCloud<PointT>::Ptr output (new PointCloud<PointT>); // 创建输出的点云智能指针
  *output = *src; // 将源点云内容拷贝到输出点云
  //初始化从源点云到目标点云的变换矩阵
  Eigen::Matrix4d final_transform (Eigen::Matrix4d::Identity ()); // 初始化最终变换矩阵为单位矩阵


  int iterations = 0; // 初始化迭代次数
  DefaultConvergenceCriteria<double> converged (iterations, transform, *good_correspondences); // 创建默认的收敛判断准则实例


  // ICP循环
  do
  {
    // Find correspondences
    findCorrespondences (output, tgt, *all_correspondences); // 查找对应点
    PCL_DEBUG ("Number of correspondences found: %d\n", all_correspondences->size ()); // 打印找到的对应点数量


    if (rejection)
    {
      // Reject correspondences
      rejectBadCorrespondences (all_correspondences, output, tgt, *good_correspondences); // 剔除对应点
      PCL_DEBUG ("Number of correspondences remaining after rejection: %d\n", good_correspondences->size ()); // 打印剔除后剩余的对应点数量
    }
    else
      *good_correspondences = *all_correspondences; // 如果不执行剔除，则所有找到的对应点都是好的


    // Find transformation
    findTransformation (output, tgt, good_correspondences, transform); // 查找变换矩阵
 
    // Obtain the final transformation    
    final_transform = transform * final_transform; // 更新最终变换矩阵


    // Transform the data
    transformPointCloudWithNormals (*src, *output, final_transform.cast<float> ()); // 将源点云根据最终变换矩阵进行变换


    // Check if convergence has been reached
    ++iterations; // 迭代次数加一
  
    // Visualize the results
    view (output, tgt, good_correspondences); // 可视化结果
  }
  while (!converged); // 检查是否收敛
  transform = final_transform; // 结束时更新传出变换矩阵
}


void
saveTransform (const std::string &file, const Eigen::Matrix4d &transform)
{
  ofstream ofs;  // 定义一个文件输出流对象
  ofs.open (file.c_str (), std::ios::trunc | std::ios::binary);  // 以截断和二进制模式打开文件
  for (int i = 0; i < 4; ++i)  // 遍历变换矩阵的行
    for (int j = 0; j < 4; ++j)  // 遍历变换矩阵的列
      ofs.write (reinterpret_cast<const char*>(&transform (i, j)), sizeof (double));  // 将矩阵的每个元素写入文件
  ofs.close ();  // 关闭文件
}


/* ---[ */
int
main (int argc, char** argv)
{
  // Check whether we want to enable debug mode
  bool debug = false;  // 定义一个标志，指示是否启用调试模式
  parse_argument (argc, argv, "-debug", debug);  // 解析命令行参数，检查是否启用调试模式
  if (debug)
    setVerbosityLevel (L_DEBUG);  // 如果启用调试模式，则设置日志级别为调试


  parse_argument (argc, argv, "-rejection", rejection);  // 解析命令行参数，检查是否启用拒绝策略
  parse_argument (argc, argv, "-visualization", visualize);  // 解析命令行参数，检查是否启用可视化
  if (visualize)
    vis.reset (new PCLVisualizer ("Registration example"));  // 如果启用可视化，则初始化可视化窗口


  // Parse the command line arguments for .pcd and .transform files
  std::vector<int> p_pcd_file_indices, p_tr_file_indices;  // 定义两个向量，用于存储.pcd和.transform文件的索引
  p_pcd_file_indices = parse_file_extension_argument (argc, argv, ".pcd");  // 解析命令行参数，获取.pcd文件的索引
  if (p_pcd_file_indices.size () != 2) // 如果不是两个.pcd文件，打印错误并退出
  {
    print_error ("Need one input source PCD file and one input target PCD file to continue.\n");
    print_error ("Example: %s source.pcd target.pcd output.transform\n", argv[0]);
    return (-1);
  }
  p_tr_file_indices = parse_file_extension_argument (argc, argv, ".transform");  // 解析命令行参数，获取.transform文件的索引
  if (p_tr_file_indices.size () != 1) // 如果没有一个.transform文件，打印错误并退出
  {
    print_error ("Need one output transform file to continue.\n");
    print_error ("Example: %s source.pcd target.pcd output.transform\n", argv[0]);
    return (-1);
  }
  
  // Load the files
  print_info ("Loading %s as source and %s as target...\n", argv[p_pcd_file_indices[0]], argv[p_pcd_file_indices[1]]);  // 打印加载文件的信息
  src.reset (new PointCloud<PointT>);  // 初始化源点云
  tgt.reset (new PointCloud<PointT>);  // 初始化目标点云
  if (loadPCDFile (argv[p_pcd_file_indices[0]], *src) == -1 || loadPCDFile (argv[p_pcd_file_indices[1]], *tgt) == -1) // 如果读取文件失败，打印错误并退出
  {
    print_error ("Error reading the input files!\n");
    return (-1);
  }


  // Compute the best transformtion
  Eigen::Matrix4d transform;  // 定义变换矩阵
  icp (src, tgt, transform);  // 调用ICP算法计算最佳变换


  saveTransform (argv[p_tr_file_indices[0]], transform);  // 保存变换到文件


  std::cerr.precision (15);  // 设置输出精度
  std::cerr << transform << std::endl;  // 打印变换矩阵
}
/* ]--- */

实现点云间的迭代最近点(ICP)配准，并保存计算得到的变换矩阵到文件中。程序首先解析命令行参数，包括是否启用调试模式、是否进行拒绝策略检查和是否启用可视化。接着，它加载源点云和目标点云文件，并执行ICP算法来计算两个点云间的最佳变换。计算完成后，将变换矩阵保存到文件，并打印出变换矩阵。

点云智能指针 与 点云常量智能指针区别

CorrespondenceEstimationBackProjection<PointT, PointT, PointT>

CorrespondenceRejectorMedianDistance 与
 CorrespondenceRejectorSurfaceNormal

TransformationEstimationPointToPlaneLLS<PointT, PointT, double>

resetCameraViewpoint

vis->updateCorrespondences<PointT>(src, tgt, *correspondences, 1)

ofs.open(file.c_str(),
         std::ios::trunc | std::ios::binary);

ofs.write(reinterpret_cast<const char*>(&transform(i, j)),
          sizeof(double));

DefaultConvergenceCriteria<double> converged (iterations, transform, *good_correspondences);