【PCL】教程correspondence_grouping对点云模型和点云场景进行特征提取，匹配，以及聚类找到模型与场景的匹配...

基于对应分组的 3D 对象识别

本教程旨在解释如何使用 pcl_recognition 模块执行基于对应分组的 3D 对象识别。具体来说，它解释了如何使用对应分组算法将 3D 描述符匹配阶段后的点对点对应集聚合到当前场景中的模型实例中。对于每个聚类，表示场景中的可能模型实例，对应分组算法还输出该模型在当前场景中的 6DOF 姿态估计的变换矩阵。

[done, 727.952 ms : 13704 points]
Available dimensions: x y z rgba

模型点云 milk_color.pcd

[done, 619.567 ms : 307200 points]
Available dimensions: x y z rgba

场景点云 milk_cartoon_all_small_clorox.pcd

运行测试1

命令行参数：milk_color.pcd milk_cartoon_all_small_clorox.pcd

Model total points: 13704; Selected Keypoints: 739
Scene total points: 307200; Selected Keypoints: 3747
[pcl::SHOTEstimation::computeFeature] The local reference frame is not valid! Aborting description of point with index 627
[pcl::SHOTEstimation::computeFeature] The local reference frame is not valid! Aborting description of point with index 1571
[pcl::SHOTEstimation::computeFeature] The local reference frame is not valid! Aborting description of point with index 951
[pcl::SHOTEstimation::createBinDistanceShape] Point 2274 has 1 (2.500000%) NaN normals in its neighbourhood
[pcl::SHOTEstimation::createBinDistanceShape] Point 170 has 1 (4.347826%) NaN normals in its neighbourhood
[pcl::SHOTEstimation::createBinDistanceShape] Point 794 has 1 (4.166667%) NaN normals in its neighbourhood
[pcl::SHOTEstimation::computeFeature] The local reference frame is not valid! Aborting description of point with index 1803
[pcl::SHOTEstimation::computeFeature] The local reference frame is not valid! Aborting description of point with index 1430
[pcl::SHOTEstimation::computeFeature] The local reference frame is not valid! Aborting description of point with index 1856
Correspondences found: 1823
Model instances found: 1


    Instance 1:
        Correspondences belonging to this instance: 37


            |  1.000  0.000  0.000 |
        R = |  0.000  1.000  0.000 |
            |  0.000 -0.000  1.000 |


        t = < -0.000, -0.000, -0.000 >

运行测试2

参数：milk_color.pcd milk_cartoon_all_small_clorox.pcd -k -c

Model total points: 13704; Selected Keypoints: 739
Scene total points: 307200; Selected Keypoints: 3747
[pcl::SHOTEstimation::computeFeature] The local reference frame is not valid! Aborting description of point with index 627
[pcl::SHOTEstimation::computeFeature] The local reference frame is not valid! Aborting description of point with index 1571
[pcl::SHOTEstimation::computeFeature] The local reference frame is not valid! Aborting description of point with index 951
[pcl::SHOTEstimation::createBinDistanceShape] Point 2274 has 1 (2.500000%) NaN normals in its neighbourhood
[pcl::SHOTEstimation::createBinDistanceShape] Point 170 has 1 (4.347826%) NaN normals in its neighbourhood
[pcl::SHOTEstimation::computeFeature] The local reference frame is not valid! Aborting description of point with index 1430
[pcl::SHOTEstimation::createBinDistanceShape] Point 794 has 1 (4.166667%) NaN normals in its neighbourhood
[pcl::SHOTEstimation::computeFeature] The local reference frame is not valid! Aborting description of point with index 1803
[pcl::SHOTEstimation::computeFeature] The local reference frame is not valid! Aborting description of point with index 1856
Correspondences found: 1823
Model instances found: 1


    Instance 1:
        Correspondences belonging to this instance: 37


            |  1.000  0.000  0.000 |
        R = |  0.000  1.000  0.000 |
            |  0.000 -0.000  1.000 |


        t = < -0.000, -0.000, -0.000 >

源码解析

这段代码是使用PCL (Point Cloud Library) 实现的一个点云识别和匹配的程序。它包括了从文件中加载点云，预处理，关键点提取，描述子计算，寻找模型与场景之间的对应关系，以及使用霍夫变换（Hough Transform）或几何一致性算法（Geometric Consistency）进行聚类寻找点云实例。

主要步骤及其对应功能解析如下：

1. 包含必要的头文件，定义一些别名，例如 PointType, NormalType, RFType (参考框架类型), DescriptorType（描述子类型）等。

2. 声明一些全局变量，用于存储模型和场景的文件名，算法参数，如采样大小，参考帧半径，描述子半径，聚类大小，聚类阈值等。

3. showHelp 函数提供使用程序的帮助信息。

4. parseCommandLine 函数解析命令行参数，并根据这些参数调整程序的行为。

5. computeCloudResolution 函数计算给定点云的分辨率，这是为了使算法能够处理不同分辨率的点云。

6. main 函数中实现了程序的主要流程：

• 解析命令行参数。

• 加载模型和场景的点云数据。

• 如果启用了分辨率不变性，就按照模型点云的分辨率调整各种参数值。

• 通过 pcl::NormalEstimationOMP 为模型和场景点云计算法线。

• 使用 pcl::UniformSampling 对模型和场景点云进行下采样以提取关键点。

• 通过 pcl::SHOTEstimationOMP 为关键点计算描述子。

• 使用 KdTree 找到模型与场景之间的对应关系。

• 根据选择使用的聚类方法（霍夫变换或几何一致性算法）进行实际的聚类操作。

• 输出聚类的结果，这包括找到的模型实例数量以及每个实例的具体变换。

• 最后是可视化，将场景点云、模型点云、关键点、以及匹配对应的线段显示在一个 PCLVisualizer 视图中。

总的来说，这段代码就是对点云模型和点云场景进行特征提取，匹配，以及聚类，找到模型与场景的匹配，并对匹配的部分进行可视化展示。这种类型的点云处理常用于三维模型的识别和定位，在机器人视觉、增强现实等领域有广泛的应用。

// 引入必要的头文件
#include <pcl/io/pcd_io.h> // PCD文件输入输出
#include <pcl/point_cloud.h> // 点云类
#include <pcl/correspondence.h> // 对应关系类
#include <pcl/features/normal_3d_omp.h> // 计算法线的类(带有OMP加速)
#include <pcl/features/shot_omp.h> // 计算SHOT特征描述子的类(带有OMP加速)
#include <pcl/features/board.h> // 计算局部参考框架BOARD的类
#include <pcl/filters/uniform_sampling.h> // 统一采样滤波器
#include <pcl/recognition/cg/hough_3d.h> // 3D霍夫变换
#include <pcl/recognition/cg/geometric_consistency.h> // 几何一致性分组
#include <pcl/visualization/pcl_visualizer.h> // PCL可视化
#include <pcl/kdtree/kdtree_flann.h> // FLANN库的KD树
#include <pcl/kdtree/impl/kdtree_flann.hpp> // KD树的具体实现
#include <pcl/common/transforms.h> // 点云变换
#include <pcl/console/parse.h> // 命令行解析


// 设置一些类型别名，方便引用
typedef pcl::PointXYZRGBA PointType;
typedef pcl::Normal NormalType;
typedef pcl::ReferenceFrame RFType;
typedef pcl::SHOT352 DescriptorType;


// 全局变量，存储输入的模型和场景文件名
std::string model_filename_;
std::string scene_filename_;


// 参数设定
bool show_keypoints_ (false);
bool show_correspondences_ (false);
bool use_cloud_resolution_ (false);
bool use_hough_ (true);
float model_ss_ (0.01f);
float scene_ss_ (0.03f);
float rf_rad_ (0.015f);
float descr_rad_ (0.02f);
float cg_size_ (0.01f);
float cg_thresh_ (5.0f);


// 显示帮助信息函数
void
showHelp (char *filename)
{
  // 打印空行
  std::cout << std::endl;
  // 打印帮助信息的头部边框
  std::cout << "***************************************************************************" << std::endl;
  std::cout << "*                                                                         *" << std::endl;
  // 打印本教程的标题
  std::cout << "*             Correspondence Grouping Tutorial - Usage Guide              *" << std::endl;
  std::cout << "*                                                                         *" << std::endl;
  // 打印帮助信息的底部边框
  std::cout << "***************************************************************************" << std::endl << std::endl;
  // 打印如何使用程序的说明
  std::cout << "Usage: " << filename << " model_filename.pcd scene_filename.pcd [Options]" << std::endl << std::endl;
  // 选项的总起说明
  std::cout << "Options:" << std::endl;
  // 打印-h选项说明，用于显示帮助信息
  std::cout << "     -h:                     Show this help." << std::endl;
  // 打印-k选项说明，用于显示关键点
  std::cout << "     -k:                     Show used keypoints." << std::endl;
  // 打印-c选项说明，用于显示对应关系
  std::cout << "     -c:                     Show used correspondences." << std::endl;
  // 打印-r选项说明，用于处理模型点云的分辨率和改变每个半径的值
  std::cout << "     -r:                     Compute the model cloud resolution and multiply" << std::endl;
  std::cout << "                             each radius given by that value." << std::endl;
  // 打印--algorithm选项说明，用于选择聚类算法
  std::cout << "     --algorithm (Hough|GC): Clustering algorithm used (default Hough)." << std::endl;
  // 打印--model_ss选项说明，设置模型均匀采样的半径
  std::cout << "     --model_ss val:         Model uniform sampling radius (default 0.01)" << std::endl;
  // 打印--scene_ss选项说明，设置场景均匀采样的半径
  std::cout << "     --scene_ss val:         Scene uniform sampling radius (default 0.03)" << std::endl;
  // 打印--rf_rad选项说明，设置参考帧半径
  std::cout << "     --rf_rad val:           Reference frame radius (default 0.015)" << std::endl;
  // 打印--descr_rad选项说明，设置描述子（如SHOT）的搜索半径
  std::cout << "     --descr_rad val:        Descriptor radius (default 0.02)" << std::endl;
  // 打印--cg_size选项说明，设置聚类大小
  std::cout << "     --cg_size val:          Cluster size (default 0.01)" << std::endl;
  // 打印--cg_thresh选项说明，设置聚类的阈值
  std::cout << "     --cg_thresh val:        Clustering threshold (default 5)" << std::endl << std::endl;
}


// 命令行参数解析函数
// 函数：解析命令行参数
void
parseCommandLine (int argc, char *argv[])
{
  // 如果有"-h"参数，则显示帮助信息并退出程序
  if (pcl::console::find_switch (argc, argv, "-h"))
  {
    showHelp (argv[0]);
    exit (0);
  }


  // 解析模型和场景的PCD文件名，文件名应该是命令行参数中".pcd"后缀的文件
  std::vector<int> filenames;
  filenames = pcl::console::parse_file_extension_argument (argc, argv, ".pcd");
  if (filenames.size () != 2)
  {
    std::cout << "Filenames missing.\n"; // 若不是两个文件，则报错
    showHelp (argv[0]);
    exit (-1);
  }


  // 根据文件名在命令行参数中的位置来设置模型和场景文件的全局变量
  model_filename_ = argv[filenames[0]];
  scene_filename_ = argv[filenames[1]];


  // 根据是否有"-k" "-c" "-r"参数来开关关键点显示、对应关系显示和使用点云分辨率的特性
  if (pcl::console::find_switch (argc, argv, "-k"))
  {
    show_keypoints_ = true;
  }
  if (pcl::console::find_switch (argc, argv, "-c"))
  {
    show_correspondences_ = true;
  }
  if (pcl::console::find_switch (argc, argv, "-r"))
  {
    use_cloud_resolution_ = true;
  }


  // 解析使用哪个算法，支持"Hough"或"GC"（几何一致性）两种
  std::string used_algorithm;
  if (pcl::console::parse_argument (argc, argv, "--algorithm", used_algorithm) != -1)
  {
    if (used_algorithm.compare ("Hough") == 0)
    {
      use_hough_ = true; // 如果是"Hough"，设置使用Hough变换
    }else if (used_algorithm.compare ("GC") == 0)
    {
      use_hough_ = false; // 如果是"GC"，设置不使用Hough变换（即使用几何一致性）
    }
    else
    {
      std::cout << "Wrong algorithm name.\n"; // 如果既不是"Hough"也不是"GC"，则报错
      showHelp (argv[0]);
      exit (-1);
    }
  }


  // 解析并设置一般性参数，如模型采样大小、场景采样大小、参考框架半径、描述子半径、聚类大小和聚类阈值
  pcl::console::parse_argument (argc, argv, "--model_ss", model_ss_);
  pcl::console::parse_argument (argc, argv, "--scene_ss", scene_ss_);
  pcl::console::parse_argument (argc, argv, "--rf_rad", rf_rad_);
  pcl::console::parse_argument (argc, argv, "--descr_rad", descr_rad_);
  pcl::console::parse_argument (argc, argv, "--cg_size", cg_size_);
  pcl::console::parse_argument (argc, argv, "--cg_thresh", cg_thresh_);
}


// 计算点云中点的分辨率的函数
// 执行点云的空间分辨率计算，通过平均每个云点与其最近邻居之间的距离。
double
computeCloudResolution (const pcl::PointCloud<PointType>::ConstPtr &cloud)
{
  double res = 0.0;
  int n_points = 0; // 有效点数计数器
  int nres;
  std::vector<int> indices (2); // 存储邻近点索引
  std::vector<float> sqr_distances (2); // 存储到邻近点的平方距离
  pcl::search::KdTree<PointType> tree; // 构建KD树
  tree.setInputCloud (cloud); // 设置输入的点云


  // 遍历点云计算每个点的分辨率
  for (std::size_t i = 0; i < cloud->size (); ++i)
  {
    if (! std::isfinite ((*cloud)[i].x)) // 忽略非法点
    {
      continue;
    }
    // 计算最近的两个邻居（第一个邻居是自身）
    nres = tree.nearestKSearch (i, 2, indices, sqr_distances);
    if (nres == 2)
    {
      res += sqrt (sqr_distances[1]); // 添加第二个邻居的距离（忽略自己）
      ++n_points;
    }
  }
  if (n_points != 0)
  {
    res /= n_points; // 根据有效点数计算平均分辨率
  }
  return res; // 返回点云的平均分辨率
}


// 主函数定义
int
main (int argc, char *argv[])
{
  parseCommandLine (argc, argv); // 解析命令行参数


  // 定义点云对象
  pcl::PointCloud<PointType>::Ptr model (new pcl::PointCloud<PointType> ()); // 模型点云
  pcl::PointCloud<PointType>::Ptr model_keypoints (new pcl::PointCloud<PointType> ()); // 模型的关键点
  pcl::PointCloud<PointType>::Ptr scene (new pcl::PointCloud<PointType> ()); // 场景点云
  pcl::PointCloud<PointType>::Ptr scene_keypoints (new pcl::PointCloud<PointType> ()); // 场景的关键点
  pcl::PointCloud<NormalType>::Ptr model_normals (new pcl::PointCloud<NormalType> ()); // 模型点云的法线
  pcl::PointCloud<NormalType>::Ptr scene_normals (new pcl::PointCloud<NormalType> ()); // 场景点云的法线
  pcl::PointCloud<DescriptorType>::Ptr model_descriptors (new pcl::PointCloud<DescriptorType> ()); // 模型描述子
  pcl::PointCloud<DescriptorType>::Ptr scene_descriptors (new pcl::PointCloud<DescriptorType> ()); // 场景描述子


  // 加载点云数据
  if (pcl::io::loadPCDFile (model_filename_, *model) < 0)
  {
    std::cout << "Error loading model cloud." << std::endl;
    showHelp (argv[0]); // 显示帮助信息并退出
    return (-1);
  }
  if (pcl::io::loadPCDFile (scene_filename_, *scene) < 0)
  {
    std::cout << "Error loading scene cloud." << std::endl;
    showHelp (argv[0]); // 显示帮助信息并退出
    return (-1);
  }


  // 设置分辨率不变性
  if (use_cloud_resolution_) // 如果启用了使用点云分辨率功能
  {
    float resolution = static_cast<float> (computeCloudResolution (model)); // 计算模型点云的分辨率
    if (resolution != 0.0f) // 如果分辨率不为零
    {
      // 使用分辨率缩放各种搜索半径
      model_ss_   *= resolution;
      scene_ss_   *= resolution;
      rf_rad_     *= resolution;
      descr_rad_  *= resolution;
      cg_size_    *= resolution;
    }


    // 打印分辨率和各种半径值
    std::cout << "Model resolution:       " << resolution << std::endl;
    std::cout << "Model sampling size:    " << model_ss_ << std::endl;
    std::cout << "Scene sampling size:    " << scene_ss_ << std::endl;
    std::cout << "LRF support radius:     " << rf_rad_ << std::endl;
    std::cout << "SHOT descriptor radius: " << descr_rad_ << std::endl;
    std::cout << "Clustering bin size:    " << cg_size_ << std::endl << std::endl;
  }
  //使用NormalEstimationOMP估算器计算模型和场景点云的每个点的法向量，使用每个点的10个最近邻居（这个参数似乎对许多数据集都相当不错，不仅仅是提供的那个）。
  // 计算点云法线
  pcl::NormalEstimationOMP<PointType, NormalType> norm_est; // 创建法线估计对象
  norm_est.setKSearch (10); // 设置搜索的邻域点数量为10
  norm_est.setInputCloud (model); // 设置输入的点云为模型点云
  norm_est.compute (*model_normals); // 计算模型点云法线


  norm_est.setInputCloud (scene); // 设置输入的点云为场景点云
  norm_est.compute (*scene_normals); // 计算场景点云法线


  // 下采样点云以提取关键点
  // 对每个云进行降采样，以找到少数几个关键点，这些关键点将与 3D 描述符关联，以执行关键点匹配并确定点对点对应关系。UniformSampling 使用的半径是命令行开关设置的或默认值
  pcl::UniformSampling<PointType> uniform_sampling; // 创建均匀采样滤波对象
  uniform_sampling.setInputCloud (model); // 设置输入的点云为模型点云
  uniform_sampling.setRadiusSearch (model_ss_); // 设置采样半径
  uniform_sampling.filter (*model_keypoints); // 过滤后获得模型关键点
  std::cout << "Model total points: " << model->size () << "; Selected Keypoints: " << model_keypoints->size () << std::endl;
  
  // 对场景点云提取关键点，输出到scene_keypoints，并输出数量
  uniform_sampling.setInputCloud (scene);
  uniform_sampling.setRadiusSearch (scene_ss_);
  uniform_sampling.filter (*scene_keypoints);
  std::cout << "Scene total points: " << scene->size () << "; Selected Keypoints: " << scene_keypoints->size () << std::endl;


  // 计算关键点的描述子
  //在每个模型和场景关键点关联一个 3D 描述符。在我们的教程中，我们使用 SHOTEstimationOMP 计算 SHOT 描述符。
  pcl::SHOTEstimationOMP<PointType, NormalType, DescriptorType> descr_est; // 创建SHOT描述子估计对象
  descr_est.setRadiusSearch (descr_rad_); // 设置描述子计算的搜索半径
  
  // 对模型关键点计算描述子
  descr_est.setInputCloud (model_keypoints); // 设置输入的关键点为模型关键点
  descr_est.setInputNormals (model_normals); // 设置法线
  descr_est.setSearchSurface (model); // 设置在原始模型点云上搜索邻域点
  descr_est.compute (*model_descriptors); // 计算模型点云的描述子


  // 对场景关键点计算描述子
  descr_est.setInputCloud (scene_keypoints);
  descr_est.setInputNormals (scene_normals);
  descr_est.setSearchSurface (scene);
  descr_est.compute (*scene_descriptors); // 计算完成后，描述子存储在scene_descriptors中


  // 寻找模型-场景间的对应关系
  pcl::CorrespondencesPtr model_scene_corrs (new pcl::Correspondences ()); // 用于存储对应关系的智能指针


  pcl::KdTreeFLANN<DescriptorType> match_search; // 创建Kd树搜索对象
  match_search.setInputCloud (model_descriptors); // 将模型描述子设置为搜索空间
  // 我们需要确定模型描述符和场景描述符之间的一对一对应关系。为此，
  //程序使用了一个 KdTreeFLANN，其输入云已经设置为包含模型描述符的云。
  //对于每个关联到场景关键点的描述符，它根据欧几里德距离高效地找到最相
  //似的模型描述符，并将该对添加到对应向量中（仅当两个描述符足够相似时，
  //即它们的平方距离小于阈值 0.25）。
  // 遍历场景的每个关键点描述子，寻找在模型关键点描述子云中的最近邻，并将其添加到对应关系向量中
  for (std::size_t i = 0; i < scene_descriptors->size (); ++i)
  {
    std::vector<int> neigh_indices (1); // 存储最近邻索引的向量
    std::vector<float> neigh_sqr_dists (1); // 存储到最近邻平方距离的向量
    // 如果描述子是NaN值则跳过
    if (!std::isfinite (scene_descriptors->at (i).descriptor[0]))
    {
      continue;
    }
    // 执行最近邻搜索
    int found_neighs = match_search.nearestKSearch (scene_descriptors->at (i), 1, neigh_indices, neigh_sqr_dists);
    // 如果找到一个最近邻，并且其平方距离小于0.25，则认为是匹配，并添加到对应关系中
    if(found_neighs == 1 && neigh_sqr_dists[0] < 0.25f)
    {
      pcl::Correspondence corr (neigh_indices[0], static_cast<int> (i), neigh_sqr_dists[0]);
      model_scene_corrs->push_back (corr);
    }
  }
  std::cout << "Correspondences found: " << model_scene_corrs->size () << std::endl;


  // 实际的聚类过程
  std::vector<Eigen::Matrix4f, Eigen::aligned_allocator<Eigen::Matrix4f> > rototranslations; // 用于存储旋转和平移的集合
  std::vector<pcl::Correspondences> clustered_corrs; // 聚类后的对应关系


  //管道的最后阶段是对之前找到的对应关系的实际聚类。
  //默认算法是Hough3DGrouping，它基于Hough投票过程。请注意，该算法需要
  //为每个关键点关联一个局部参考帧（LRF），这些关键点来自作为参数传递的
  //点云！在这个示例中，我们使用BOARDLocalReferenceFrameEstimation
  //估计器显式计算LRF集，然后再调用聚类算法。
  // 使用Hough投票进行聚类分析
  if (use_hough_) // 如果使用Hough投票方法
  {
    // 计算参考坐标系 (只在使用Hough时计算)
    //
    pcl::PointCloud<RFType>::Ptr model_rf (new pcl::PointCloud<RFType> ()); // 模型点云的参考坐标系
    pcl::PointCloud<RFType>::Ptr scene_rf (new pcl::PointCloud<RFType> ()); // 场景点云的参考坐标系
  
    // 创建BOARDLocalReferenceFrameEstimation对象，用于估计局部参考坐标系
    pcl::BOARDLocalReferenceFrameEstimation<PointType, NormalType, RFType> rf_est;
    rf_est.setFindHoles (true); // 设置为寻找空洞
    rf_est.setRadiusSearch (rf_rad_); // 设置搜索半径
    
    // 计算模型点云的局部参考坐标系
    rf_est.setInputCloud (model_keypoints);
    rf_est.setInputNormals (model_normals);
    rf_est.setSearchSurface (model);
    rf_est.compute (*model_rf);
    
    // 计算场景点云的局部参考坐标系
    rf_est.setInputCloud (scene_keypoints);
    rf_est.setInputNormals (scene_normals);
    rf_est.setSearchSurface (scene);
    rf_est.compute (*scene_rf);
    
    // 设置Hough3DGrouping对象，用于根据参考框架进行聚类
    pcl::Hough3DGrouping<PointType, PointType, RFType, RFType> clusterer;
    clusterer.setHoughBinSize (cg_size_); // 设置Hough空间每个bin的尺寸
    clusterer.setHoughThreshold (cg_thresh_); // 设置Hough空间的阈值
    clusterer.setUseInterpolation (true); // 设置是否使用插值
    clusterer.setUseDistanceWeight (false); // 设置是否使用距离权重
    
    // 输入模型关键点、参考坐标系、场景关键点、场景参考坐标系和模型场景对应关系
    clusterer.setInputCloud (model_keypoints);
    clusterer.setInputRf (model_rf);
    clusterer.setSceneCloud (scene_keypoints);
    clusterer.setSceneRf (scene_rf);
    clusterer.setModelSceneCorrespondences (model_scene_corrs);
    
    // 执行聚类操作，结果保存在rototranslations和clustered_corrs中
    //clusterer.cluster (clustered_corrs); // 聚类结果
    clusterer.recognize (rototranslations, clustered_corrs); // 识别结果
  }
  else // 使用GeometricConsistency进行聚类分析
  {//可以使用 GeometricConsistencyGrouping 算法，而不是 
  //Hough3DGrouping，方法是使用之前描述的适当命令行开关。
  //在这种情况下，不需要 LRF 计算，因此我们只是创建算法类的实例，
  //传递正确的参数并调用 recognize 方法。
    // 设置GeometricConsistencyGrouping对象，用于几何一致性聚类
    pcl::GeometricConsistencyGrouping<PointType, PointType> gc_clusterer;
    gc_clusterer.setGCSize (cg_size_); // 设置几何一致性聚类的尺寸
    gc_clusterer.setGCThreshold (cg_thresh_); // 设置几何一致性聚类的阈值
  
    // 输入模型关键点、场景关键点、模型场景对应关系
    gc_clusterer.setInputCloud (model_keypoints);
    gc_clusterer.setSceneCloud (scene_keypoints);
    gc_clusterer.setModelSceneCorrespondences (model_scene_corrs);
    
    // 执行聚类操作，结果同样保存在rototranslations和clustered_corrs中
    //gc_clusterer.cluster (clustered_corrs); // 聚类结果
    //recognize 方法返回一个向量rototranslations，表示场景中每个模型实例的变换（旋转 + 平移），
    //通过绝对定向获得；以及一个对应项向量（对应项向量的向量），表示聚类的输出，即该向量的每个元素都是一个对应项集，表示场景中特定模型实例的对应项。
    gc_clusterer.recognize (rototranslations, clustered_corrs); // 识别结果
  }
  //
  // 输出结果
  //
  std::cout << "Model instances found: " << rototranslations.size () << std::endl; // 输出发现的模型实例数量
  for (std::size_t i = 0; i < rototranslations.size (); ++i)
  {
    std::cout << "\n    Instance " << i + 1 << ":" << std::endl; // 输出实例编号
    std::cout << "        Correspondences belonging to this instance: " << clustered_corrs[i].size () << std::endl; // 输出属于此实例的对应关系数量
  
    // 打印旋转矩阵和平移向量
    Eigen::Matrix3f rotation = rototranslations[i].block<3,3>(0, 0); // 获取旋转矩阵
    Eigen::Vector3f translation = rototranslations[i].block<3,1>(0, 3); // 获取平移向量
  
    printf ("\n");
    printf ("            | %6.3f %6.3f %6.3f | \n", rotation (0,0), rotation (0,1), rotation (0,2));
    printf ("        R = | %6.3f %6.3f %6.3f | \n", rotation (1,0), rotation (1,1), rotation (1,2));
    printf ("            | %6.3f %6.3f %6.3f | \n", rotation (2,0), rotation (2,1), rotation (2,2));
    printf ("\n");
    printf ("        t = < %0.3f, %0.3f, %0.3f >\n", translation (0), translation (1), translation (2));
  }
  
  //
  // 可视化
  //程序在 PCLVisualizer 窗口中显示带有红色叠加层的场景云，其中找到
  //模型的实例。如果使用了命令行开关 -k 和 -c ，程序还将显示模型云的
  //“独立”渲染。如果启用了关键点可视化，关键点将显示为蓝色点，如果启用
  //了对应关系可视化，它们将显示为每个对应关系的绿色线，该对应关系经历
  //了聚类过程。
  pcl::visualization::PCLVisualizer viewer ("Correspondence Grouping"); // 创建可视化窗口
  viewer.addPointCloud (scene, "scene_cloud"); // 将场景点云添加到可视化窗口中
  
  pcl::PointCloud<PointType>::Ptr off_scene_model (new pcl::PointCloud<PointType> ()); // 创建模型点云
  pcl::PointCloud<PointType>::Ptr off_scene_model_keypoints (new pcl::PointCloud<PointType> ()); // 创建模型关键点点云
  
  if (show_correspondences_ || show_keypoints_) // 如果需要显示对应关系或关键点
  {
    // 将模型和模型关键点平移，以免与场景点云重合
    pcl::transformPointCloud (*model, *off_scene_model, Eigen::Vector3f (-1,0,0), Eigen::Quaternionf (1, 0, 0, 0));
    pcl::transformPointCloud (*model_keypoints, *off_scene_model_keypoints, Eigen::Vector3f (-1,0,0), Eigen::Quaternionf (1, 0, 0, 0));
  
    // 给平移后的模型上色并添加到可视化窗口中
    pcl::visualization::PointCloudColorHandlerCustom<PointType> off_scene_model_color_handler (off_scene_model, 255, 255, 128);
    viewer.addPointCloud (off_scene_model, off_scene_model_color_handler, "off_scene_model");
  }
  
  if (show_keypoints_) // 如果需要显示关键点
  {
    // 给场景中的关键点设置颜色处理器（蓝色）并将其添加到可视化视图中
    pcl::visualization::PointCloudColorHandlerCustom<PointType> scene_keypoints_color_handler (scene_keypoints, 0, 0, 255);
    viewer.addPointCloud (scene_keypoints, scene_keypoints_color_handler, "scene_keypoints"); // 添加场景关键点到可视化视图中
    // 设置场景关键点的显示属性，例如点的大小
    viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 5, "scene_keypoints"); // 设置点的大小为5
    
    // 给模型中的平移后关键点设置颜色处理器（蓝色）并将其添加到可视化视图中
    pcl::visualization::PointCloudColorHandlerCustom<PointType> off_scene_model_keypoints_color_handler (off_scene_model_keypoints, 0, 0, 255);
    viewer.addPointCloud (off_scene_model_keypoints, off_scene_model_keypoints_color_handler, "off_scene_model_keypoints"); // 添加模型关键点到可视化视图中
    // 设置模型关键点的显示属性，例如点的大小
    viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 5, "off_scene_model_keypoints"); // 设置点的大小为5
  }
    
  for (std::size_t i = 0; i < rototranslations.size (); ++i)
  {
    pcl::PointCloud<PointType>::Ptr rotated_model (new pcl::PointCloud<PointType> ()); // 创建旋转后的模型
    pcl::transformPointCloud (*model, *rotated_model, rototranslations[i]); // 将模型点云按照旋转变换变换位置
  
    std::stringstream ss_cloud;
    ss_cloud << "instance" << i; // 为每个实例创建一个唯一标识
  
    // 给旋转后的模型上色并添加到可视化窗口中
    pcl::visualization::PointCloudColorHandlerCustom<PointType> rotated_model_color_handler (rotated_model, 255, 0, 0);
    viewer.addPointCloud (rotated_model, rotated_model_color_handler, ss_cloud.str ());
  
    if (show_correspondences_) // 如果需要显示对应关系
    {
      for (std::size_t j = 0; j < clustered_corrs[i].size (); ++j)
      {
        std::stringstream ss_line;
        ss_line << "correspondence_line" << i << "_" << j; // 为每条对应关系线创建一个唯一标识
        PointType& model_point = off_scene_model_keypoints->at (clustered_corrs[i][j].index_query); // 获取模型关键点
        PointType& scene_point = scene_keypoints->at (clustered_corrs[i][j].index_match); // 获取场景关键点
  
        // 在模型关键点和场景关键点之间画线表示对应关系
        viewer.addLine<PointType, PointType> (model_point, scene_point, 0, 255, 0, ss_line.str ());
      }
    }
  }


        
  while (!viewer.wasStopped ()) // 等待用户关闭可视化窗口
  {
    viewer.spinOnce (); // 不断更新可视化界面
  }


  return (0); // 结束程序
}
// 如果您更喜欢以云分辨率单位指定半径：
// $ ./correspondence_grouping milk.pcd milk_cartoon_all_small_clorox.pcd -r --model_ss 7.5 --scene_ss 20 --rf_rad 10 --descr_rad 15 --cg_size 1

笔记

filenames = pcl::console::parse_file_extension_argument (argc, argv, ".pcd");

nres = tree.nearestKSearch (i, 2, indices, sqr_distances);

pcl::UniformSampling<PointType>

pcl::SHOTEstimationOMP<PointType, NormalType, DescriptorType>

pcl::BOARDLocalReferenceFrameEstimation<PointType, NormalType, RFType>

// 初始化点云和法线的指针
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::Normal>::Ptr normals (new pcl::PointCloud<pcl::Normal>);
pcl::PointCloud<pcl::ReferenceFrame>::Ptr frames (new pcl::PointCloud<pcl::ReferenceFrame>);


// 创建BOARDLocalReferenceFrameEstimation对象
pcl::BOARDLocalReferenceFrameEstimation<pcl::PointXYZ, pcl::Normal, pcl::ReferenceFrame> lrf_estimator;
lrf_estimator.setInputCloud(cloud);             // 设置输入点云
lrf_estimator.setInputNormals(normals);         // 设置点云的法线
lrf_estimator.setRadiusSearch(0.03);            // 设置局部区域的搜索半径


// 计算局部参考框架
lrf_estimator.compute(*frames);                 // 存储输出的局部参考框架

在这个示例中，frames 包含了输入点云 cloud 中每个点的局部参考框架。通过 setRadiusSearch 设定了局部邻域的搜索半径。这些局部参考框架可以被后续的算法用于例如特征描述子的匹配等。

pcl::Hough3DGrouping<PointType, PointType, RFType, RFType>

在这个示例中，通过设置模型和场景的点云、局部参考框架以及模型到场景的对应关系后，调用recognize方法，实现对象识别与姿态估计。models_detected保存识别到的模型的副本，transformations_detected保存相应模型在场景中的姿态变换

transformations_detected 是识别出的模型坐标系相对于场景坐标系下位姿变换（位姿坐标）的表示。这里的每一个 Eigen::Matrix4f 对象都是一个变换矩阵，包含了平移和旋转信息，能够将模型从其本身的坐标系变换到匹配的场景中的相应位置和方向。

pcl::GeometricConsistencyGrouping<PointType, PointType>

// 初始化点云指针
pcl::PointCloud<pcl::PointXYZ>::Ptr model(new pcl::PointCloud<pcl::PointXYZ>());
pcl::PointCloud<pcl::PointXYZ>::Ptr scene(new pcl::PointCloud<pcl::PointXYZ>());


pcl::Correspondences correspondences;


// 准备几何一致性分组对象
pcl::GeometricConsistencyGrouping<pcl::PointXYZ, pcl::PointXYZ> gc_grouping;
gc_grouping.setInputCloud(model);                          // 设置输入模型点云
gc_grouping.setSceneCloud(scene);                          // 设置场景点云
gc_grouping.setModelSceneCorrespondences(&correspondences);// 设置模型与场景之间的对应关系


// 设置几何一致性群组的参数
gc_grouping.setGCSize(0.01);                              // 设置聚类尺寸
gc_grouping.setGCThreshold(10);                           // 设置算法的最小对应关系集大小


// 进行几何一致性分组来识别模型
std::vector<Eigen::Matrix4f, Eigen::aligned_allocator<Eigen::Matrix4f>> transformations;
std::vector<pcl::Correspondences> clustered_corrs;
gc_grouping.recognize(transformations, clustered_corrs); //执行模型识别，输出变换矩阵

在这段代码中，transformations 是一个包含了各个匹配到模型到场景中实例的变换矩阵的向量，这些变换矩阵描述了如何将模型数据集中的点云移动和旋转以匹配场景中的相应位置。clustered_corrs 则包含了对应于每一个变换矩阵的模型点到场景点的对应关系集合。

基于对应分组的三维对象识别 —— 点云库 0.0 文档 --- 3D Object Recognition based on Correspondence Grouping — Point Cloud Library 0.0 documentation (pcl.readthedocs.io)

https://pcl.readthedocs.io/projects/tutorials/en/latest/correspondence_grouping.html

The end