【PCL】教程 example2 3D点云之间的精确配准（FPFH特征对应关系估计变换矩阵）

这段代码主要实现了点云之间的配准功能，旨在通过估计点云的特征并找到最佳的对应关系来计算一个变换矩阵，从而可以将源点云（src）变换到目标点云（tgt）的坐标系统中。

代码功能和方法总结如下：

1. 估计关键点（estimateKeypoints）：使用pcl::UniformSampling过滤器从原始点云中提取均匀分布的关键点。关键点以1米的半径均匀采样后保存到磁盘上，以便于调试。

2. 估计法线（estimateNormals）：使用pcl::NormalEstimation为每个关键点估计法线。法线提取基于0.5米的搜索半径后，结果保存到磁盘上，以便调试。

3. 估计FPFH特征（estimateFPFH）：使用pcl::FPFHEstimation为每个关键点计算FPFH特征。特征估计在1米的搜索半径内完成，计算结果保存到磁盘上用于调试。

4. 寻找对应关系（findCorrespondences）：使用pcl::CorrespondenceEstimation计算两个点云中关键点的FPFH特征间的对应关系。

5. 拒绝差的对应关系（rejectBadCorrespondences）：使用pcl::CorrespondenceRejectorDistance根据空间距离来拒绝不良的对应关系。设定最大距离为1米。

6. 计算变换矩阵（computeTransformation）：使用pcl::TransformationEstimationSVD 基于剩余的良好对应关系来估计源点云到目标点云的刚性变换。

7. 主程序（main）：作为程序的入口，首先解析命令行参数加载.PCD文件，加载源点云和目标点云数据。然后调用computeTransformation计算最佳变换矩阵。最后，将源点云数据根据计算出的变换矩阵变换后保存到硬盘上。

综上所述，本段代码实现了3D点云之间的精确配准，包括了关键点提取、法线估计、特征计算、对应关系寻找、对应关系过滤和最后的变换矩阵估计等一系列步骤。这种点云配准在3D建模、环境映射、物体检测等领域有着重要的应用。

#include <pcl/console/parse.h> // 包含PCL库中处理命令行参数解析的功能
#include <pcl/point_types.h> // 包含定义了PCL支持的点类型的功能
#include <pcl/point_cloud.h> // 包含点云类的定义
#include <pcl/point_representation.h> // 包含点表示（特征）的定义


#include <pcl/io/pcd_io.h> // 包含PCD文件输入输出的功能
#include <pcl/conversions.h> // 包含点云类型转换的功能
#include <pcl/filters/uniform_sampling.h> // 包含均匀采样的滤波器
#include <pcl/features/normal_3d.h> // 包含计算点云中每个点的法线的功能
#include <pcl/features/fpfh.h> // 包含计算FPFH特征的功能
#include <pcl/registration/correspondence_estimation.h> // 包含估算对应关系的功能
#include <pcl/registration/correspondence_rejection_distance.h> // 包含基于距离的对应关系拒绝功能
#include <pcl/registration/transformation_estimation_svd.h> // 包含使用SVD（单因素分解）方法估算变换矩阵的功能


using namespace pcl; // 使用 PCL 命名空间
using namespace pcl::io; // 使用 PCL 的 IO 命名空间
using namespace pcl::console; // 使用 PCL 的控制台命名空间
using namespace pcl::registration; // 使用 PCL 的注册命名空间
PointCloud<PointXYZ>::Ptr src, tgt; // 定义源点云和目标点云的指针


// 以下为函数定义:



// 估算关键点
void
estimateKeypoints (const PointCloud<PointXYZ>::Ptr &src, 
                   const PointCloud<PointXYZ>::Ptr &tgt,
                   PointCloud<PointXYZ> &keypoints_src,
                   PointCloud<PointXYZ> &keypoints_tgt)
{
  // 获取一个均匀的关键点网格
  UniformSampling<PointXYZ> uniform; // 创建均匀采样的实例
  uniform.setRadiusSearch (1);  // 设置搜索半径为1米


  uniform.setInputCloud (src); // 设置输入的源点云
  uniform.filter (keypoints_src); // 进行滤波，并保留结果到keypoints_src


  uniform.setInputCloud (tgt); // 设置输入的目标点云
  uniform.filter (keypoints_tgt); // 进行滤波，并保留结果到keypoints_tgt


  // 以下为调试目的，可将结果保存到PCD文件并在pcl_viewer中查看
  savePCDFileBinary ("keypoints_src.pcd", keypoints_src); // 保存源关键点到文件
  savePCDFileBinary ("keypoints_tgt.pcd", keypoints_tgt); // 保存目标关键点到文件
}



// 估算法线
void
estimateNormals (const PointCloud<PointXYZ>::Ptr &src, 
                 const PointCloud<PointXYZ>::Ptr &tgt,
                 PointCloud<Normal> &normals_src,
                 PointCloud<Normal> &normals_tgt)
{
  NormalEstimation<PointXYZ, Normal> normal_est; // 创建法线估算实例
  normal_est.setInputCloud (src); // 设置输入的源点云
  normal_est.setRadiusSearch (0.5);  // 设置搜索半径为50厘米
  normal_est.compute (normals_src); // 计算结果保留在normals_src中


  normal_est.setInputCloud (tgt); // 设置输入的目标点云
  normal_est.compute (normals_tgt); // 计算结果保留在normals_tgt中


  // 以下为调试目的，可将结果保存到PCD文件并在pcl_viewer中查看
  PointCloud<PointNormal> s, t;
  copyPointCloud (*src, s); // 拷贝点到s
  copyPointCloud (normals_src, s); // 拷贝法线到s
  copyPointCloud (*tgt, t); // 拷贝点到t
  copyPointCloud (normals_tgt, t); // 拷贝法线到t
  savePCDFileBinary ("normals_src.pcd", s); // 保存源点云的法线到文件
  savePCDFileBinary ("normals_tgt.pcd", t); // 保存目标点云的法线到文件
}



void
computeTransformation (
    const PointCloud<PointXYZ>::Ptr &src,
    const PointCloud<PointXYZ>::Ptr &tgt,
    Eigen::Matrix4f &transform
)
{
    // 获取均匀分布的关键点
    PointCloud<PointXYZ>::Ptr keypoints_src(new PointCloud<PointXYZ>),
                              keypoints_tgt(new PointCloud<PointXYZ>);
    estimateKeypoints(src, tgt, *keypoints_src, *keypoints_tgt); // 调用 estimateKeypoints 方法估计关键点
    print_info("Found %zu and %zu keypoints for the source and target datasets.\n", 
               static_cast<std::size_t>(keypoints_src->size()),
               static_cast<std::size_t>(keypoints_tgt->size())); // 打印信息，输出找到的关键点数量


    // 计算所有关键点的法线
    PointCloud<Normal>::Ptr normals_src (new PointCloud<Normal>),
                            normals_tgt(new PointCloud<Normal>);
    estimateNormals(src, tgt, *normals_src, *normals_tgt); // 调用 estimateNormals 方法计算法线
    print_info("Estimated %zu and %zu normals for the source and target datasets.\n",
               static_cast<std::size_t>(normals_src->size()),
               static_cast<std::size_t>(normals_tgt->size())); // 打印信息，输出计算得到的法线数量


    // 计算每个关键点的 FPFH 特征
    PointCloud<FPFHSignature33>::Ptr fpfhs_src(new PointCloud<FPFHSignature33>), 
                                     fpfhs_tgt(new PointCloud<FPFHSignature33>);
    estimateFPFH(src, tgt, normals_src, normals_tgt, keypoints_src, keypoints_tgt, *fpfhs_src, *fpfhs_tgt); // 调用 estimateFPFH 方法计算 FPFH 特征


    // 查找 FPFH 空间中关键点的对应关系
    CorrespondencesPtr all_correspondences(new Correspondences),
                       good_correspondences(new Correspondences);
    findCorrespondences(fpfhs_src, fpfhs_tgt, *all_correspondences); // 调用 findCorrespondences 方法找到所有对应关系


    // 根据它们的 XYZ 距离拒绝错误的对应关系
    rejectBadCorrespondences(all_correspondences, keypoints_src, keypoints_tgt, *good_correspondences); // 调用 rejectBadCorrespondences 方法拒绝错误的对应关系


    for (const auto& corr : (*good_correspondences))
        std::cerr << corr << std::endl; // 对于每一个剩余的好的对应关系，输出到 cerr


    // 获得给定剩余对应关系后，两组关键点之间的最佳变换
    TransformationEstimationSVD<PointXYZ, PointXYZ> trans_est; // 定义变换估计对象
    trans_est.estimateRigidTransformation(*keypoints_src, *keypoints_tgt, *good_correspondences, transform); // 估算刚性变换矩阵
}


int
main (int argc, char** argv)
{
  // 解析命令行参数以查找 .pcd 文件
  std::vector<int> p_file_indices; // 定义一个整数向量用于存储文件的索引
  p_file_indices = parse_file_extension_argument (argc, argv, ".pcd"); // 解析获得所有后缀为 .pcd 的文件索引
  if (p_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\n", argv[0]); // 提供正确使用的例子
    return (-1); // 返回错误码 -1
  }


  // 加载文件
  print_info ("Loading %s as source and %s as target...\n", argv[p_file_indices[0]], argv[p_file_indices[1]]); // 打印加载信息
  src.reset (new PointCloud<PointXYZ>); // 初始化源点云
  tgt.reset (new PointCloud<PointXYZ>); // 初始化目标点云
  if (loadPCDFile (argv[p_file_indices[0]], *src) == -1 || loadPCDFile (argv[p_file_indices[1]], *tgt) == -1) // 尝试加载文件，如果失败则报错
  {
    print_error ("Error reading the input files!\n"); // 打印错误信息
    return (-1); // 返回错误码 -1
  }


  // 计算最佳变换
  Eigen::Matrix4f transform; // 定义一个 4x4 的变换矩阵
  computeTransformation (src, tgt, transform); // 调用 computeTransformation 函数计算从源点云到目标点云的变换矩阵


  std::cerr << transform << std::endl; // 输出变换矩阵
  // 对数据进行变换并将结果写入磁盘
  PointCloud<PointXYZ> output; // 定义输出点云
  transformPointCloud (*src, output, transform); // 使用计算得到的变换矩阵对源点云进行变换
  savePCDFileBinary ("source_transformed.pcd", output); // 保存变换后的点云到文件
}

此代码是使用点云库（PCL）进行点云注册的示例程序。程序首先加载两个点云文件，然后计算从源点云到目标点云的最佳变换矩阵，并将变换后的源点云保存到新的文件中。这个过程可以应用于多种场景，如 3D 模型重建、环境映射与导航。

 computeTransformation 函数，用于计算从源点云向目标点云变换的最佳矩阵。函数首先估计两组点云的均匀关键点，然后计算关键点的法线，接下来计算关键点的 FPFH 特征。之后，找到两组关键点在 FPFH 特征空间中的对应关系，并拒绝那些距离较远的错误对应关系。最后，根据剩余的正确对应关系，利用 SVD 方法计算得到最佳的变换矩阵。

TransformationEstimationSVD<PointXYZ, PointXYZ> trans_est;
trans_est.estimateRigidTransformation (*keypoints_src, *keypoints_tgt, *good_correspondences, transform);

TransformationEstimationSVD<PointXYZ, PointXYZ>是一个类，用于估计两组点云间的刚体变换。其中，PointXYZ是PCL库中定义的一种点类型，包含了点的XYZ坐标。

刚体变换是指在变换过程中保持物体形状和大小不变，只进行旋转和平移。在点云处理中，刚体变换通常用于将一组点云数据精确地对齐到另一组点云数据中，这个过程就称为点云配准。

TransformationEstimationSVD内部实现了奇异值分解（Singular Value Decomposition, SVD）方法来计算最优化的刚体变换，即寻找一个最佳的旋转矩阵和平移向量，使得在这个变换下，一组点云与另一组点云之间的对应点尽可能地接近。

CorrespondenceRejectorDistance rej;

CorrespondenceEstimation<FPFHSignature33, FPFHSignature33> est;

FPFHEstimation<PointXYZ, Normal, FPFHSignature33> fpfh_est;