测试程序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 <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_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<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_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<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;
