【PCL】教程global_hypothesis_verification 通过验证模型假设来实现 3D 对象识别与位姿估计...

测试程序1

milk.pcd milk_cartoon_all_small_clorox.pcd

终端输出1：


Model total points: 12575; Selected Keypoints: 193
Scene total points: 307200; Selected Keypoints: 7739
[pcl::SHOTEstimation::computeFeature] The local reference frame is not valid! Aborting description of point with index 2034
[pcl::SHOTEstimation::createBinDistanceShape] Point 3952 has 1 (7.692307%) NaN normals in its neighbourhood
[pcl::SHOTEstimation::createBinDistanceShape] Point 4625 has 1 (5.263158%) NaN normals in its neighbourhood
[pcl::SHOTEstimation::computeFeature] The local reference frame is not valid! Aborting description of point with index 797
[pcl::SHOTEstimation::computeFeature] The local reference frame is not valid! Aborting description of point with index 238
[pcl::SHOTEstimation::createBinDistanceShape] Point 806 has 1 (4.761905%) NaN normals in its neighbourhood
[pcl::SHOTEstimation::computeFeature] The local reference frame is not valid! Aborting description of point with index 3509
[pcl::SHOTEstimation::createBinDistanceShape] Point 4685 has 1 (2.857143%) NaN normals in its neighbourhood
[pcl::SHOTEstimation::computeFeature] The local reference frame is not valid! Aborting description of point with index 1593
[pcl::SHOTEstimation::createBinDistanceShape] Point 4686 has 1 (2.941176%) NaN normals in its neighbourhood
[pcl::SHOTEstimation::computeFeature] The local reference frame is not valid! Aborting description of point with index 1605
[pcl::SHOTEstimation::createBinDistanceShape] Point 3099 has 1 (2.500000%) NaN normals in its neighbourhood
[pcl::SHOTEstimation::computeFeature] The local reference frame is not valid! Aborting description of point with index 3116
[pcl::SHOTEstimation::computeFeature] The local reference frame is not valid! Aborting description of point with index 2097
[pcl::SHOTEstimation::computeFeature] The local reference frame is not valid! Aborting description of point with index 3577
[pcl::SHOTEstimation::computeFeature] The local reference frame is not valid! Aborting description of point with index 3629
[pcl::SHOTEstimation::computeFeature] The local reference frame is not valid! Aborting description of point with index 2463
Correspondences found: 3394
Recognized Instances: 1
 
 
--- ICP ---------
Instance 0 Aligned!
-----------------
 
 
--- Hypotheses Verification ---
Occlusion cloud not set, using scene_cloud instead...
Computing cues took 1.4674ms.
Computing clutter cues took 2.948ms.
SA search... took 11.7504ms.
Instance 0 is bad!
-------------------------------

测试程序2

milk.pcd milk_cartoon_all_small_clorox.pcd --cg_size 0.035

终端输出2：


Model total points: 12575; Selected Keypoints: 193
Scene total points: 307200; Selected Keypoints: 7739
[pcl::SHOTEstimation::computeFeature] The local reference frame is not valid! Aborting description of point with index 2034
[pcl::SHOTEstimation::createBinDistanceShape] Point 3952 has 1 (7.692307%) NaN normals in its neighbourhood
[pcl::SHOTEstimation::createBinDistanceShape] Point 4625 has 1 (5.263158%) NaN normals in its neighbourhood
[pcl::SHOTEstimation::computeFeature] The local reference frame is not valid! Aborting description of point with index 797
[pcl::SHOTEstimation::computeFeature] The local reference frame is not valid! Aborting description of point with index 238
[pcl::SHOTEstimation::computeFeature] The local reference frame is not valid! Aborting description of point with index 2097
[pcl::SHOTEstimation::createBinDistanceShape] Point 3099 has 1 (2.500000%) NaN normals in its neighbourhood
[pcl::SHOTEstimation::createBinDistanceShape] Point 806 has 1 (4.761905%) NaN normals in its neighbourhood
[pcl::SHOTEstimation::computeFeature] The local reference frame is not valid! Aborting description of point with index 1593
[pcl::SHOTEstimation::computeFeature] The local reference frame is not valid! Aborting description of point with index 3509
[pcl::SHOTEstimation::computeFeature] The local reference frame is not valid! Aborting description of point with index 3116
[pcl::SHOTEstimation::computeFeature] The local reference frame is not valid! Aborting description of point with index 3577
[pcl::SHOTEstimation::computeFeature] The local reference frame is not valid! Aborting description of point with index 1605
[pcl::SHOTEstimation::createBinDistanceShape] Point 4685 has 1 (2.857143%) NaN normals in its neighbourhood
[pcl::SHOTEstimation::computeFeature] The local reference frame is not valid! Aborting description of point with index 3629
[pcl::SHOTEstimation::createBinDistanceShape] Point 4686 has 1 (2.941176%) NaN normals in its neighbourhood
[pcl::SHOTEstimation::computeFeature] The local reference frame is not valid! Aborting description of point with index 2463
Correspondences found: 3394
Recognized Instances: 12
 
 
--- ICP ---------
Instance 0 Aligned!
Instance 1 Aligned!
Instance 2 Aligned!
Instance 3 Aligned!
Instance 4 Aligned!
Instance 5 Aligned!
Instance 6 Aligned!
Instance 7 Aligned!
Instance 8 Aligned!
Instance 9 Aligned!
Instance 10 Aligned!
Instance 11 Aligned!
-----------------
 
 
--- Hypotheses Verification ---
Occlusion cloud not set, using scene_cloud instead...
Computing cues took 53.9605ms.
Computing clutter cues took 3.4176ms.
SA search... took 36.8681ms.
Instance 0 is bad!
Instance 1 is bad!
Instance 2 is bad!
Instance 3 is bad!
Instance 4 is bad!
Instance 5 is bad!
Instance 6 is bad!
Instance 7 is bad!
Instance 8 is bad!
Instance 9 is bad!
Instance 10 is bad!
Instance 11 is bad!
-------------------------------

源码解析

这段代码是一个使用PCL（Point Cloud Library，点云库）的3D物体识别与位姿估计程序。它主要实现了以下几个步骤：

载入模型和场景的点云数据文件（.pcd）。
使用法线估计来计算点云中每个点的法线。
对点云进行均匀采样以提取关键点。
为关键点计算SHOT描述子。
建立模型与场景之间的对应关系。
使用Hough Transform或Geometric Consistency进行模型与场景关键点的分组。
对分组后的结果进行结构化的对齐（使用ICP算法迭代最近点对齐）。
应用假设验证算法来确定对象实例的存在，并验证这些假设。
可视化最后的结果。

各部分段落详细说明如下：

包含库：引入PCL库中处理点云、特征提取、滤波、对齐、可视化等功能的头文件。
类型定义：定义了点类型PointXYZRGBA和其他一些PCL使用的基本结构类型。
CloudStyle结构体：定义了点云的可视化风格，包括颜色和尺寸。
参数定义：声明了一系列的参数变量，如关键点的显示、使用的聚类算法、采样半径等，供之后的过程中使用。
帮助信息函数showHelp()：如果用户需要帮助，显示程序的使用方法和选项。
解析命令行参数函数parseCommandLine()：处理输入的命令行参数，设定程序运行时的选项。
main函数：程序的主入口点，调用上述的函数和处理数据的功能模块。

整个代码一开始通过解析命令行参数确定运行配置，然后依次加载模型和场景点云文件，对其进行处理并最后对齐和验证假设，如果检测到对象实例则进行可视化展示。

程序的方法是首先读取模型和现场的点云数据，并对这些数据进行预处理，比如通过均匀采样来减少计算量，然后使用法线估计为关键点计算出法线。利用关键点的描述子匹配模型和现场点云的关键点，匹配成功的关键点对用于后续的鲁棒匹配过程。之后，使用Hough变换或几何一致性（根据参数设定）来聚集对应关系及其估计对象的位姿。估计出的位姿经过ICP细化后，应用全局假设验证方法来确认哪些位姿是真实存在的。最后通过PCLVisualizer显示最终点云及其识别和验证的结果。


// 包含处理点云数据所需要的PCL库的头文件
#include  // 包含了读写PCD(Point Cloud Data)文件的功能
#include  // 定义了pcl::PointCloud，用于存储点云
#include  // 提供了寻找和管理点对应关系的方法
#include  // 包含了计算点云法线的OMP(OpenMP)并行版本的函数
#include  // 包含了计算SHOT特征的OMP(OpenMP)并行版本的函数
#include  // 包含了计算BOARD特征的函数，常用于关键点描述
#include  // 提供了一种均匀下采样的方法
#include  // 包含了使用3D霍夫变换进行粗略配准的方法
#include  // 提供了基于几何一致性的模型识别方法
#include  // 提供了一个全局假设验证的方法，用于模型识别和配准验证
#include  // 包含了迭代最近点（Iterative Closest Point，ICP）算法的实现
#include  // 提供了点云可视化的类和方法
#include  // 包含了基于FLANN的Kd树搜索的实现
#include  // 包含了Kd树搜索的实现代码，通常是模板类的实现部分
#include  // 提供了点云变换的方法，如旋转和平移
#include  // 包含了解析命令行参数的函数
 
 
// 定义点云库中的几种重要数据类型
typedef pcl::PointXYZRGBA PointType;
typedef pcl::Normal NormalType;
typedef pcl::ReferenceFrame RFType;
typedef pcl::SHOT352 DescriptorType;
 
 
// 定义点云风格的结构体，包括颜色和大小
struct CloudStyle
{
    double r; // 红色分量
    double g; // 绿色分量
    double b; // 蓝色分量
    double size; // 点的大小
 
 
    // 构造函数，用于初始化点云的显示风格
    CloudStyle (double r,
                double g,
                double b,
                double size) :
        r (r),
        g (g),
        b (b),
        size (size)
    {
    }
};
 
 
// 定义几种不同的点云显示风格
CloudStyle style_white (255.0, 255.0, 255.0, 4.0);
CloudStyle style_red (255.0, 0.0, 0.0, 3.0);
CloudStyle style_green (0.0, 255.0, 0.0, 5.0);
CloudStyle style_cyan (93.0, 200.0, 217.0, 4.0);
CloudStyle style_violet (255.0, 0.0, 255.0, 8.0);
 
 
// 定义用来存储文件名的全局变量
std::string model_filename_;
std::string scene_filename_;
 
 
// 定义算法参数变量，这些参数可以通过命令行改变
bool show_keypoints_ (false); // 是否展示关键点
bool use_hough_ (true); // 是否使用霍夫聚类
float model_ss_ (0.02f); // 模型点云的采样大小
float scene_ss_ (0.02f); // 场景点云的采样大小
float rf_rad_ (0.015f); // 参考帧半径
float descr_rad_ (0.02f); // 描述子半径
float cg_size_ (0.01f); // 聚类大小
float cg_thresh_ (5.0f); // 聚类阈值
int icp_max_iter_ (5); // ICP 最大迭代次数
float icp_corr_distance_ (0.005f); // ICP 对应点对最大距离
float hv_resolution_ (0.005f); // 假设验证的分辨率
float hv_occupancy_grid_resolution_ (0.01f); // 占据网格分辨率
float hv_clutter_reg_ (5.0f); // 杂物正则化
float hv_inlier_th_ (0.005f); // 内点阈值
float hv_occlusion_th_ (0.01f); // 遮挡阈值
float hv_rad_clutter_ (0.03f); // 杂物半径
float hv_regularizer_ (3.0f); // 正则化器
float hv_rad_normals_ (0.05); // 法线半径
bool hv_detect_clutter_ (true); // 是否检测杂物
 
 
/**
 * 打印帮助信息
 * @param filename 可执行程序的名称
 */
void
showHelp (char *filename)
{
  // 打印帮助信息的头部
  std::cout << std::endl;
  std::cout << "***************************************************************************" << std::endl;
  std::cout << "*                                                                         *" << std::endl;
  std::cout << "*          全局假设验证教程 - 使用指南                                   *" << std::endl;
  std::cout << "*                                                                         *" << std::endl;
  std::cout << "***************************************************************************" << std::endl << std::endl;
  // 打印如何使用程序的指令格式
  std::cout << "用法: " << filename << " 模型文件名.pcd 场景文件名.pcd [选项]" << std::endl << std::endl;
  // 打印可以使用的选项和默认设置
  std::cout << "选项:" << std::endl;
  std::cout << "     -h:                          展示此帮助信息。" << std::endl;
  std::cout << "     -k:                          展示关键点。" << std::endl;
  std::cout << "     --algorithm (Hough|GC):      使用的聚类算法（默认为Hough）。" << std::endl;
  std::cout << "     --model_ss val:              模型均匀采样半径（默认 " << model_ss_ << "）" << std::endl;
  std::cout << "     --scene_ss val:              场景均匀采样半径（默认 " << scene_ss_ << "）" << std::endl;
  std::cout << "     --rf_rad val:                参考帧半径（默认 " << rf_rad_ << "）" << std::endl;
  std::cout << "     --descr_rad val:             描述符半径（默认 " << descr_rad_ << "）" << std::endl;
  std::cout << "     --cg_size val:               聚类大小（默认 " << cg_size_ << "）" << std::endl;
  std::cout << "     --cg_thresh val:             聚类阈值（默认 " << cg_thresh_ << "）" << std::endl << std::endl;
  std::cout << "     --icp_max_iter val:          ICP最大迭代次数（默认 " << icp_max_iter_ << "）" << std::endl;
  std::cout << "     --icp_corr_distance val:     ICP对应点距离（默认 " << icp_corr_distance_ << "）" << std::endl << std::endl;
  std::cout << "     --hv_clutter_reg val:        杂物调整器（默认 " << hv_clutter_reg_ << "）" << std::endl;
  std::cout << "     --hv_inlier_th val:          内点阈值（默认 " << hv_inlier_th_ << "）" << std::endl;
  std::cout << "     --hv_occlusion_th val:       遮挡阈值（默认 " << hv_occlusion_th_ << "）" << std::endl;
  std::cout << "     --hv_rad_clutter val:        杂物半径（默认 " << hv_rad_clutter_ << "）" << std::endl;
  std::cout << "     --hv_regularizer val:        正则化值（默认 " << hv_regularizer_ << "）" << std::endl;
  std::cout << "     --hv_rad_normals val:        法向量半径（默认 " << hv_rad_normals_ << "）" << std::endl;
  std::cout << "     --hv_detect_clutter val:     如果启用杂物检测为TRUE（默认 " << hv_detect_clutter_ << "）" << std::endl << std::endl;
}
 
 
/**
 * 解析命令行参数
 * @param argc 参数数量
 * @param argv 参数数组
 */
void
parseCommandLine (int argc,
                  char *argv[])
{
  // 显示帮助信息
  if (pcl::console::find_switch (argc, argv, "-h"))
  {
    showHelp (argv[0]); // 如果参数中有-h，调用showHelp函数显示帮助信息
    exit (0); // 然后退出程序
  }
 
 
  // 解析模型和场景文件名
  std::vector<int> filenames; // 存储文件名参数的向量
  filenames = pcl::console::parse_file_extension_argument (argc, argv, ".pcd"); // 获取pcd文件的参数位置
  if (filenames.size () != 2) // 如果不是两个文件名，说明参数有误
  {
    std::cout << "文件名缺失。\n"; // 打印错误信息
    showHelp (argv[0]); // 显示帮助信息
    exit (-1); // 退出程序
  }
 
 
  model_filename_ = argv[filenames[0]]; // 设置模型文件名
  scene_filename_ = argv[filenames[1]]; // 设置场景文件名
 
 
  // 解析程序行为参数
  if (pcl::console::find_switch (argc, argv, "-k"))
  {
    show_keypoints_ = true; // 如果有-k参数，设置展示关键点为true
  }
 
 
  std::string used_algorithm; // 存储使用的算法
  // 解析--algorithm参数，如果指定了就更新use_hough_的值
  if (pcl::console::parse_argument (argc, argv, "--algorithm", used_algorithm) != -1)
  {
    if (used_algorithm.compare ("Hough") == 0) // 比较算法名称
    {
      use_hough_ = true;
    }
    else if (used_algorithm.compare ("GC") == 0)
    {
      use_hough_ = false;
    }
    else
    {
      std::cout << "算法名称错误。\n"; // 如果不是上述两种算法，打印错误信息
      showHelp (argv[0]); // 显示帮助信息
      exit (-1); // 退出程序
    }
  }
 
 
  // 解析通用参数
  // 使用pcl::console::parse_argument来解析命令行参数并更新对应的全局变量
  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_);
  pcl::console::parse_argument (argc, argv, "--icp_max_iter", icp_max_iter_);
  pcl::console::parse_argument (argc, argv, "--icp_corr_distance", icp_corr_distance_);
  pcl::console::parse_argument (argc, argv, "--hv_clutter_reg", hv_clutter_reg_);
  pcl::console::parse_argument (argc, argv, "--hv_inlier_th", hv_inlier_th_);
  pcl::console::parse_argument (argc, argv, "--hv_occlusion_th", hv_occlusion_th_);
  pcl::console::parse_argument (argc, argv, "--hv_rad_clutter", hv_rad_clutter_);
  pcl::console::parse_argument (argc, argv, "--hv_regularizer", hv_regularizer_);
  pcl::console::parse_argument (argc, argv, "--hv_rad_normals", hv_rad_normals_);
  pcl::console::parse_argument (argc, argv, "--hv_detect_clutter", hv_detect_clutter_);
}
 
 
// 主函数
int
main (int argc,
      char *argv[])
{
  // 解析命令行参数
  parseCommandLine (argc, argv);
 
 
  // 初始化不同类型点云的智能指针
  pcl::PointCloud::Ptr model (new pcl::PointCloud ());
  pcl::PointCloud::Ptr model_keypoints (new pcl::PointCloud ());
  pcl::PointCloud::Ptr scene (new pcl::PointCloud ());
  pcl::PointCloud::Ptr scene_keypoints (new pcl::PointCloud ());
  pcl::PointCloud::Ptr model_normals (new pcl::PointCloud ());
  pcl::PointCloud::Ptr scene_normals (new pcl::PointCloud ());
  pcl::PointCloud::Ptr model_descriptors (new pcl::PointCloud ());
  pcl::PointCloud::Ptr scene_descriptors (new pcl::PointCloud ());
 
 
  // 读取点云文件
  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); // 退出程序
  }
 
 
  // 计算法线估算
  pcl::NormalEstimationOMP norm_est;
  norm_est.setKSearch (10); // 设置在估算一个点法线时考虑多少个最近点
  norm_est.setInputCloud (model); // 设置输入点云（模型）
  norm_est.compute (*model_normals); // 计算点云法线
 
 
  norm_est.setInputCloud (scene); // 设置输入点云（场景）
  norm_est.compute (*scene_normals); // 计算点云法线
 
 
  // 对点云进行下采样，以提取关键点
  pcl::UniformSampling uniform_sampling;
  uniform_sampling.setInputCloud (model); // 设置输入点云（模型）
  uniform_sampling.setRadiusSearch (model_ss_); // 设置搜索半径
  uniform_sampling.filter (*model_keypoints); // 过滤操作，结果保存在model_keypoints
  std::cout << "Model total points: " << model->size () << "; Selected Keypoints: " << model_keypoints->size () << std::endl;
 
 
  uniform_sampling.setInputCloud (scene); // 设置输入点云（场景）
  uniform_sampling.setRadiusSearch (scene_ss_); // 设置搜索半径
  uniform_sampling.filter (*scene_keypoints); // 过滤操作，结果保存在scene_keypoints
  std::cout << "Scene total points: " << scene->size () << "; Selected Keypoints: " << scene_keypoints->size () << std::endl;
 
 
  // 计算关键点的描述子
  pcl::SHOTEstimationOMP descr_est;
  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); // 计算描述子
 
 
  // 使用Kd树查找模型与场景之间的对应关系
  pcl::CorrespondencesPtr model_scene_corrs (new pcl::Correspondences ());
  pcl::KdTreeFLANN match_search;
  match_search.setInputCloud (model_descriptors);
  std::vector<int> model_good_keypoints_indices;
  std::vector<int> scene_good_keypoints_indices;
 
 
  // 在点云描述子中找出最相似的点对应关系
  for (std::size_t i = 0; i < scene_descriptors->size (); ++i)
  {
    // 如果描述子是非有限数（比如NaN等），则跳过
    if (!std::isfinite (scene_descriptors->at (i).descriptor[0])) 
    {
      continue;
    }
    // 在模型描述子中找到与当前场景描述子最接近的一点
    std::vector<int> neigh_indices (1);
    std::vector<float> neigh_sqr_dists (1);
    int found_neighs = match_search.nearestKSearch (scene_descriptors->at (i), 1, neigh_indices, neigh_sqr_dists);
    // 如果这一点确实存在，并且距离小于一个阈值（这里设为0.25f），则认为这是一对匹配点
    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); // 将对应关系添加到对应关系集
      model_good_keypoints_indices.push_back (corr.index_query); // 储存好的模型关键点索引
      scene_good_keypoints_indices.push_back (corr.index_match); // 存储好的场景关键点索引
    }
  }
  pcl::PointCloud::Ptr model_good_kp (new pcl::PointCloud ());
  pcl::PointCloud::Ptr scene_good_kp (new pcl::PointCloud ());
  pcl::copyPointCloud (*model_keypoints, model_good_keypoints_indices, *model_good_kp); // 复制好的模型关键点为新的点云数据
  pcl::copyPointCloud (*scene_keypoints, scene_good_keypoints_indices, *scene_good_kp); // 复制好的场景关键点为新的点云数据
 
 
  std::cout << "Correspondences found: " << model_scene_corrs->size () << std::endl; // 输出找到的对应关系个数
 
 
  // 聚类
  std::vector > rototranslations; // 存储旋转和平移变换
  std::vector < pcl::Correspondences > clustered_corrs; // 存储聚类后的对应关系
 
 
  // 判断是否使用Hough变换方法
  if (use_hough_)
  {
    // 初始化模型和场景的参考帧点云
    pcl::PointCloud::Ptr model_rf (new pcl::PointCloud());
    pcl::PointCloud::Ptr scene_rf (new pcl::PointCloud());
 
 
    // 设置参考帧估计的参数和对象
    pcl::BOARDLocalReferenceFrameEstimation 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);
 
 
    // 设置Hough聚类的参数和对象
    pcl::Hough3DGrouping clusterer;
    clusterer.setHoughBinSize(cg_size_); // 设置Hough空间的bin大小
    clusterer.setHoughThreshold(cg_thresh_); // 设置Hough空间的阈值
    clusterer.setUseInterpolation(true); // 设置是否使用插值
    clusterer.setUseDistanceWeight(false); // 设置是否使用距离加权
 
 
    // 设置Hough聚类的输入
    clusterer.setInputCloud(model_keypoints);
    clusterer.setInputRf(model_rf);
    clusterer.setSceneCloud(scene_keypoints);
    clusterer.setSceneRf(scene_rf);
    clusterer.setModelSceneCorrespondences(model_scene_corrs);
 
 
    // 执行识别，获取旋转和平移矩阵，及聚类后的对应关系
    clusterer.recognize(rototranslations, clustered_corrs);
  }
  else
  {
    // 若不使用Hough方法，则使用几何一致性聚类方法
    pcl::GeometricConsistencyGrouping 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);
 
 
    // 执行识别，获取旋转和平移矩阵，及聚类后的对应关系
    gc_clusterer.recognize(rototranslations, clustered_corrs);
  }
 
 
  // 如果没有找到任何实例，则停止
  if (rototranslations.size() <= 0)
  {
    std::cout << "*** No instances found! ***" << std::endl;
    return (0);
  }
  else
  {
    std::cout << "Recognized Instances: " << rototranslations.size() << std::endl << std::endl;
  }
 
 
  /**
   * 为每个发现的实例生成点云
   */
  std::vector::ConstPtr> instances;
  
  // 遍历所有的旋转平移矩阵(rototranslations)
  for (std::size_t i = 0; i < rototranslations.size(); ++i)
  {
    // 根据当前的rototranslations对模型点云进行变换
    pcl::PointCloud::Ptr rotated_model(new pcl::PointCloud());
    pcl::transformPointCloud(*model, *rotated_model, rototranslations[i]);
    // 把变换后的模型加入到instances集合中
    instances.push_back(rotated_model);
  }
  
  /**
   * ICP(迭代最近点)算法
   */
  std::vector::ConstPtr> registered_instances; // 用来存放ICP算法对齐后的实例
  // 如果需要执行ICP算法
  if (true)
  {
    std::cout << "--- ICP ---------" << std::endl;
  
    // 遍历所有的实例(instances)
    for (std::size_t i = 0; i < rototranslations.size(); ++i)
    {
      // 创建ICP对象并设置参数
      pcl::IterativeClosestPoint icp;
      icp.setMaximumIterations(icp_max_iter_);
      icp.setMaxCorrespondenceDistance(icp_corr_distance_);
      icp.setInputTarget(scene);         // 设置目标点云(场景)
      icp.setInputSource(instances[i]);  // 设置源点云(模型实例)
      pcl::PointCloud::Ptr registered(new pcl::PointCloud()); // 创建用于存储对齐后点云的对象
      icp.align(*registered); // 执行ICP算法
      registered_instances.push_back(registered); // 把对齐后的点云存入registered_instances
      std::cout << "Instance " << i << " ";
      // 输出ICP算法是否收敛以及对齐质量
      if (icp.hasConverged())
      {
        std::cout << "Aligned!" << std::endl;
      }
      else
      {
        std::cout << "Not Aligned!" << std::endl;
      }
    }
  
    std::cout << "-----------------" << std::endl << std::endl;
  }
  
  /**
   * 假设验证
   */
  std::cout << "--- Hypotheses Verification ---" << std::endl;
  std::vector<bool> hypotheses_mask; // 创建一个用于存放验证结果的布尔向量
  
  // 创建全局假设验证对象
  pcl::GlobalHypothesesVerification GoHv;
  
  // 设置该对象的场景点云
  GoHv.setSceneCloud(scene);
  // 将ICP算法后注册的点云模型添加到全局假设验证中，设置为真实模型
  GoHv.addModels(registered_instances, true);
  // 设置全局假设验证的分辨率
  GoHv.setResolution(hv_resolution_);
  // 设置占用栅格的分辨率
  GoHv.setResolutionOccupancyGrid(hv_occupancy_grid_resolution_);
  // 设置内点阈值
  GoHv.setInlierThreshold(hv_inlier_th_);
  // 设置遮挡阈值
  GoHv.setOcclusionThreshold(hv_occlusion_th_);
  // 设置正则化器的值
  GoHv.setRegularizer(hv_regularizer_);
  // 设置杂波的半径
  GoHv.setRadiusClutter(hv_rad_clutter_);
  // 设置杂波正则化器的值
  GoHv.setClutterRegularizer(hv_clutter_reg_);
  // 设置是否检测杂波
  GoHv.setDetectClutter(hv_detect_clutter_);
  // 设置法线的半径
  GoHv.setRadiusNormals(hv_rad_normals_);
  
  // 运行假设验证
  GoHv.verify();
  // 获取假设验证的结果，对于数组中每个元素如果是真则表明对应的模型满足假设验证
  GoHv.getMask(hypotheses_mask);
  
  // 对于每个模型实例，打印它是否通过了假设验证
  for (std::size_t i = 0; i < hypotheses_mask.size(); i++)
  {
    if (hypotheses_mask[i])
    {
      std::cout << "Instance " << i << " is GOOD! <---" << std::endl;
    }
    else
    {
      std::cout << "Instance " << i << " is bad!" << std::endl;
    }
  }
  std::cout << "-------------------------------" << std::endl;
  
  /**
   * 可视化
   */
  // 创建一个PCLVisualizer视窗并添加场景点云
  pcl::visualization::PCLVisualizer viewer("Hypotheses Verification");
  viewer.addPointCloud(scene, "scene_cloud");
  
  // 创建和变换用于可视化的点云
  pcl::PointCloud::Ptr off_scene_model(new pcl::PointCloud());
  pcl::PointCloud::Ptr off_scene_model_keypoints(new pcl::PointCloud());
  
  pcl::PointCloud::Ptr off_model_good_kp(new pcl::PointCloud());
  // 对模型点云进行平移变换以便在视窗中清晰显示
  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::transformPointCloud(*model_good_kp, *off_model_good_kp, Eigen::Vector3f(-1, 0, 0), Eigen::Quaternionf(1, 0, 0, 0));
  
  // 如果设置为显示关键点
  if (show_keypoints_)
  {
    // 定义好点云样式为白色
    CloudStyle modelStyle = style_white;
    // 用自定义颜色处理器设置点云颜色
    pcl::visualization::PointCloudColorHandlerCustom off_scene_model_color_handler(off_scene_model, modelStyle.r, modelStyle.g, modelStyle.b);
    // 把模型点云添加到可视化对象中
    viewer.addPointCloud(off_scene_model, off_scene_model_color_handler, "off_scene_model");
    // 设置点云的渲染属性（如点大小）
    viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, modelStyle.size, "off_scene_model");
  }
  
  // 如果设置为显示关键点
  if (show_keypoints_)
  {
    // 定义好点云样式为紫色
    CloudStyle goodKeypointStyle = style_violet;
    // 用自定义颜色处理器设置好的模型关键点点云颜色
    pcl::visualization::PointCloudColorHandlerCustom model_good_keypoints_color_handler(off_model_good_kp, goodKeypointStyle.r, goodKeypointStyle.g, goodKeypointStyle.b);
    // 把好的模型关键点点云添加到可视化对象中
    viewer.addPointCloud(off_model_good_kp, model_good_keypoints_color_handler, "model_good_keypoints");
    // 设置点云的渲染属性（如点大小）
    viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, goodKeypointStyle.size, "model_good_keypoints");
  
    // 用自定义颜色处理器设置好的场景关键点点云颜色
    pcl::visualization::PointCloudColorHandlerCustom scene_good_keypoints_color_handler(scene_good_kp, goodKeypointStyle.r, goodKeypointStyle.g, goodKeypointStyle.b);
    // 把好的场景关键点点云添加到可视化对象中
    viewer.addPointCloud(scene_good_kp, scene_good_keypoints_color_handler, "scene_good_keypoints");
    // 设置点云的渲染属性（如点大小）
    viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, goodKeypointStyle.size, "scene_good_keypoints");
  }
  
  // 遍历所有实例并将它们添加到可视化对象中
  for (std::size_t i = 0; i < instances.size(); ++i)
  {
    // 创建字符串流以构造实例的标签
    std::stringstream ss_instance;
    ss_instance << "instance_" << i;
  
    // 定义实例点云样式为红色
    CloudStyle clusterStyle = style_red;
    // 用自定义颜色处理器设置实例点云颜色
    pcl::visualization::PointCloudColorHandlerCustom instance_color_handler(instances[i], clusterStyle.r, clusterStyle.g, clusterStyle.b);
    // 把实例点云添加到可视化对象中
    viewer.addPointCloud(instances[i], instance_color_handler, ss_instance.str ());
    // 设置点云的渲染属性（如点大小）
    viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, clusterStyle.size, ss_instance.str ());
  
    // 根据假设验证的结果设置对齐后实例点云的颜色为绿色或青色
    CloudStyle registeredStyles = hypotheses_mask[i] ? style_green : style_cyan;
    ss_instance << "_registered";
    // 用自定义颜色处理器设置对齐后实例点云颜色
    pcl::visualization::PointCloudColorHandlerCustom registered_instance_color_handler(registered_instances[i], registeredStyles.r, registeredStyles.g, registeredStyles.b);
    // 把对齐后实例点云添加到可视化对象中
    viewer.addPointCloud(registered_instances[i], registered_instance_color_handler, ss_instance.str ());
    // 设置点云的渲染属性（如点大小）
    viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, registeredStyles.size, ss_instance.str ());
  }
  // 开始渲染循环直到用户关闭窗口
  while (!viewer.wasStopped())
  {
    viewer.spinOnce();
  }
 
 
  return (0); // 程序正常退出
}

pcl::GlobalHypothesesVerification GoHv;

pcl::IterativeClosestPoint icp;

相关阅读:
Java计算机网络篇-HTTP
【map的实际应用，学习map，不用=白学】3302. 表达式求值【如何得到运算符优先级？自己过一遍示例即可明白】【怎么比较运算符的优先级？map】
微信公众号消息推送，自动回复
 二叉树的存储
 【原创】Ubuntu Pro RealTime linux(Ubuntu22.04 安装PREEMPT-RT实时内核/PREEMPT-RT/ubuntu官方PREEMPT-RT)
win10无hyper-v设置
 SUSE zypper 添加源和离线下载rpm包
 Vue项目实战之人力资源平台系统（一）框架介绍及项目环境搭建
 Leetcode刷题详解——将x减到0的最小操作数
 Linux学习资源Index
原文地址：https://blog.csdn.net/cxyhjl/article/details/138056189