测试程序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物体识别与位姿估计程序。它主要实现了以下几个步骤：

1. 载入模型和场景的点云数据文件（.pcd）。

2. 使用法线估计来计算点云中每个点的法线。

3. 对点云进行均匀采样以提取关键点。

4. 为关键点计算SHOT描述子。

5. 建立模型与场景之间的对应关系。

6. 使用Hough Transform或Geometric Consistency进行模型与场景关键点的分组。

7. 对分组后的结果进行结构化的对齐（使用ICP算法迭代最近点对齐）。

8. 应用假设验证算法来确定对象实例的存在，并验证这些假设。

9. 可视化最后的结果。

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

• 包含库：引入PCL库中处理点云、特征提取、滤波、对齐、可视化等功能的头文件。

• 类型定义：定义了点类型PointXYZRGBA和其他一些PCL使用的基本结构类型。

• CloudStyle结构体：定义了点云的可视化风格，包括颜色和尺寸。

• 参数定义：声明了一系列的参数变量，如关键点的显示、使用的聚类算法、采样半径等，供之后的过程中使用。

• 帮助信息函数showHelp()：如果用户需要帮助，显示程序的使用方法和选项。

• 解析命令行参数函数parseCommandLine()：处理输入的命令行参数，设定程序运行时的选项。

• main函数：程序的主入口点，调用上述的函数和处理数据的功能模块。

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

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

// 包含处理点云数据所需要的PCL库的头文件
#include <pcl/io/pcd_io.h> // 包含了读写PCD(Point Cloud Data)文件的功能
#include <pcl/point_cloud.h> // 定义了pcl::PointCloud<T>，用于存储点云
#include <pcl/correspondence.h> // 提供了寻找和管理点对应关系的方法
#include <pcl/features/normal_3d_omp.h> // 包含了计算点云法线的OMP(OpenMP)并行版本的函数
#include <pcl/features/shot_omp.h> // 包含了计算SHOT特征的OMP(OpenMP)并行版本的函数
#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/recognition/hv/hv_go.h> // 提供了一个全局假设验证的方法，用于模型识别和配准验证
#include <pcl/registration/icp.h> // 包含了迭代最近点（Iterative Closest Point，ICP）算法的实现
#include <pcl/visualization/pcl_visualizer.h> // 提供了点云可视化的类和方法
#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;// 定义点云风格的结构体，包括颜色和大小
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<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); // 退出程序}// 计算法线估算pcl::NormalEstimationOMP<PointType, NormalType> 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<PointType> uniform_sampling;uniform_sampling.setInputCloud (model); // 设置输入点云（模型）uniform_sampling.setRadiusSearch (model_ss_); // 设置搜索半径uniform_sampling.filter (*model_keypoints); // 过滤操作，结果保存在model_keypointsstd::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_keypointsstd::cout << "Scene total points: " << scene->size () << "; Selected Keypoints: " << scene_keypoints->size () << std::endl;// 计算关键点的描述子pcl::SHOTEstimationOMP<PointType, NormalType, DescriptorType> 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<DescriptorType> 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<PointType>::Ptr model_good_kp (new pcl::PointCloud<PointType> ());pcl::PointCloud<PointType>::Ptr scene_good_kp (new pcl::PointCloud<PointType> ());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<Eigen::Matrix4f, Eigen::aligned_allocator<Eigen::Matrix4f> > rototranslations; // 存储旋转和平移变换std::vector < pcl::Correspondences > clustered_corrs; // 存储聚类后的对应关系// 判断是否使用Hough变换方法if (use_hough_){// 初始化模型和场景的参考帧点云pcl::PointCloud<RFType>::Ptr model_rf (new pcl::PointCloud<RFType>());pcl::PointCloud<RFType>::Ptr scene_rf (new pcl::PointCloud<RFType>());// 设置参考帧估计的参数和对象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);// 设置Hough聚类的参数和对象pcl::Hough3DGrouping<PointType, PointType, RFType, RFType> 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<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);// 执行识别，获取旋转和平移矩阵，及聚类后的对应关系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<pcl::PointCloud<PointType>::ConstPtr> instances;// 遍历所有的旋转平移矩阵(rototranslations)for (std::size_t i = 0; i < rototranslations.size(); ++i){// 根据当前的rototranslations对模型点云进行变换pcl::PointCloud<PointType>::Ptr rotated_model(new pcl::PointCloud<PointType>());pcl::transformPointCloud(*model, *rotated_model, rototranslations[i]);// 把变换后的模型加入到instances集合中instances.push_back(rotated_model);}/*** ICP(迭代最近点)算法*/std::vector<pcl::PointCloud<PointType>::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<PointType, PointType> icp;icp.setMaximumIterations(icp_max_iter_);icp.setMaxCorrespondenceDistance(icp_corr_distance_);icp.setInputTarget(scene);         // 设置目标点云(场景)icp.setInputSource(instances[i]);  // 设置源点云(模型实例)pcl::PointCloud<PointType>::Ptr registered(new pcl::PointCloud<PointType>()); // 创建用于存储对齐后点云的对象icp.align(*registered); // 执行ICP算法registered_instances.push_back(registered); // 把对齐后的点云存入registered_instancesstd::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<PointType, PointType> 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<PointType>::Ptr off_scene_model(new pcl::PointCloud<PointType>());pcl::PointCloud<PointType>::Ptr off_scene_model_keypoints(new pcl::PointCloud<PointType>());pcl::PointCloud<PointType>::Ptr off_model_good_kp(new pcl::PointCloud<PointType>());// 对模型点云进行平移变换以便在视窗中清晰显示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<PointType> 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<PointType> 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<PointType> 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<PointType> 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<PointType> 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<PointType, PointType> GoHv;

pcl::IterativeClosestPoint<PointType, PointType> icp;