官网：
https://pcl.readthedocs.io/projects/tutorials/en/latest/#recognition

https://pointclouds.org/documentation/group__recognition.html

博文：
https://github.com/Ewenwan/MVision/tree/master/PCL_APP/Recognition
PCL行人检测

此模块个人代码：https://github.com/HuangCongQing/pcl-learning/tree/master/13recognition

理论基础

代码实践

1基于对应分组的三维物体识别3D Object Recognition based on Correspondence Grouping

PCL-基于对应分组的三维物体识别_lizhengze1117的博客

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

该算法输入一个具体的物体的点云，从场景中找出与该物体点云相匹配的，这种方法可以用来抓取指定的物体等等.

算法思路

  1. 【1】计算法线向量 pcl::NormalEstimationOMP 
  2.      近邻邻域内 协方差矩阵PCA 降维到二维平面 计算法线向量
  3.    [PCA降维原理](http://blog.codinglabs.org/articles/pca-tutorial.html)
  4.    前面说的是二维降到一维时的情况，假如我们有一堆散乱的三维点云,则可以这样计算法线：
  5.    1）对每一个点，取临近点，比如取最临近的50个点，当然会用到K-D树
  6.    2）对临近点做PCA降维，把它降到二维平面上,可以想象得到这个平面一定是它的切平面(在切平面上才可以尽可能分散）
  7.    3）切平面的法线就是该点的法线了，而这样的法线有两个，取哪个还需要考虑临近点的凸包方向
  9. 【2】下采样滤波使用均匀采样 pcl::UniformSampling
  10.         （可以试试体素格子下采样）得到关键点
  11.     一般下采样是通过构造一个三维体素栅格（正方体格子内保留一个点），
  12.     然后在每个体素内用体素内的所有点的重心近似显示体素中的其他点，
  13.     这样体素内所有点就用一个重心点来表示，进行下采样的来达到滤波的效果，
  14.     这样就大大的减少了数据量，
  15.     特别是在配准，曲面重建等工作之前作为预处理，
  16.     可以很好的提高程序的运行速度，
  17.     均匀采样 (半径球体内保留一个点)
  19. 【3】为keypoints关键点计算SHOT(法线方向直方图特征)描述子 pcl::SHOTEstimationOMP
  20.     1. 半径r内划分32个空间区域 
  21.     2. 计算区域内每个点与中心点p法线np之间的夹角余弦cosθ=nv·np
  22.     3. 对夹角余弦值进行11个格子直方图统计 
  23.     4. 计算结果进行归一化得到一个352（32*11=352）维特征
  25. 【4】按存储方法KDTree匹配两个点云（描述子向量匹配）点云分组得到匹配的组 描述 点对匹配关系
  26.      1. pcl::KdTreeFLANN<DescriptorType> 描述最距离最近临近
  27. 　2. 为每一个场景点云　在　模型点云中匹配一个最近点　（<0.25f 为匹配）
  29. 【5】参考帧 霍夫聚类 / 集合一致性 聚类得到 匹配点云cluster  变换矩阵和 匹配点对关系
  30. 估计模型参考帧 pcl::BOARDLocalReferenceFrameEstimation
  31. 霍夫聚类 pcl::Hough3DGrouping 几何一致性性质 pcl::GeometricConsistencyGrouping
  33. 【6】分组显示 平移矩阵 T 将模型点云按T变换后显示 以及显示 点对之间的连线

代码实现

Before you begin, you should download the PCD dataset used in this tutorial from GitHub (milk.pcd and milk_cartoon_all_small_clorox.pcd)

创建文件：correspondence_grouping.cpp
准备资源：../milk.pcd ../milk_cartoon_all_small_clorox.pcd
编译执行：
./correspondence_grouping ../milk.pcd ../milk_cartoon_all_small_clorox.pcd -c
./correspondence_grouping ../milk.pcd ../milk_cartoon_all_small_clorox.pcd -c -k
命令行输入指令说明：

  1. -k:                在源点云和场景点云中    显示关键点 
  2. -c:                显示对应点连线

个人代码：13recognition识别

  1. /*
  2.  * @Description: 基于对应分组的三维物体识别
  3.  * https://github.com/Ewenwan/MVision/tree/master/PCL_APP/Recognition
  4.  * https://blog.csdn.net/lizhengze1117/article/details/103230261
  5.  * @Author: HCQ
  6.  * @Company(School): UCAS
  7.  * @Email: 1756260160@qq.com
  8.  * @Date: 2020-10-23 11:11:31
  9.  * @LastEditTime: 2020-10-23 11:25:17
  10.  * @FilePath: /pcl-learning/13recognition识别/1基于对应分组的三维物体识别/correspondence_grouping.cpp
  11.  */
  13. /*
  14. 基于对应分组的三维物体识别
  15. 基于pcl_recognition模块的三维物体识别。
  16. 具体来说，它解释了如何使用对应分组算法，
  17. 以便将3D描述符匹配阶段之后获得的一组点到点对应集集合到当前场景中的模型实例中。
  18. 每个集群，代表一个可能的场景中的模型实例，
  19. 对应的分组算法输出的
  20. 变换矩阵识别当前的场景中，
  21. 模型的六自由度位姿估计 6DOF pose estimation 。
  22. 执行命令
  23. ./correspondence_grouping ../milk.pcd ../milk_cartoon_all_small_clorox.pcd -c -k
  24. 【1】计算法线向量  近邻邻域内 协方差矩阵PCA 降维到二维平面 计算法线向量
  25.    PCA降维原理 http://blog.codinglabs.org/articles/pca-tutorial.html
  26.    前面说的是二维降到一维时的情况，假如我们有一堆散乱的三维点云,则可以这样计算法线：
  27.    1）对每一个点，取临近点，比如取最临近的50个点，当然会用到K-D树
  28.    2）对临近点做PCA降维，把它降到二维平面上,可以想象得到这个平面一定是它的切平面(在切平面上才可以尽可能分散）
  29.    3）切平面的法线就是该点的法线了，而这样的法线有两个，取哪个还需要考虑临近点的凸包方向
  30. 【2】下采样滤波使用均匀采样（可以试试体素格子下采样）得到关键点
  31. 【3】为keypoints关键点计算SHOT描述子
  32. 【4】按存储方法KDTree匹配两个点云（描述子向量匹配）点云分组得到匹配的组 描述 点对匹配关系
  33. 【5】参考帧霍夫聚类/集合一致性聚类得到 匹配点云cluster  平移矩阵和 匹配点对关系
  34. 【6】分组显示 平移矩阵 T 将模型点云按T变换后显示 以及显示 点对之间的连线
  35. */
  36. #include <pcl/io/pcd_io.h>// 文件、设备读写
  37. #include <pcl/point_cloud.h>//基础pcl点云类型
  38. #include <pcl/correspondence.h>//分组算法 对应表示两个实体之间的匹配（例如，点，描述符等）。
  39. // 特征
  40. #include <pcl/features/normal_3d_omp.h>//法向量特征
  41. #include <pcl/features/shot_omp.h> //描述子 shot描述子 0～1
  42. // https://blog.csdn.net/bengyanluo1542/article/details/76061928?locationNum=9&fps=1
  43. // (Signature of Histograms of OrienTations)方向直方图特征
  44. #include <pcl/features/board.h>
  45. // 滤波
  46. #include <pcl/filters/uniform_sampling.h>//均匀采样 滤波
  47. // 识别
  48. #include <pcl/recognition/cg/hough_3d.h>//hough算子
  49. #include <pcl/recognition/cg/geometric_consistency.h> //几何一致性
  50. // 可视化
  51. #include <pcl/visualization/pcl_visualizer.h>//可视化
  52. // kdtree
  53. #include <pcl/kdtree/kdtree_flann.h>// kdtree 快速近邻搜索
  54. #include <pcl/kdtree/impl/kdtree_flann.hpp>
  55. // 转换
  56. #include <pcl/common/transforms.h>//点云转换 转换矩阵
  57. // 命令行参数
  58. #include <pcl/console/parse.h>//命令行参数解析
  59. // 别名
  60. /*
  61. shot 特征描述
  62. 构造方法：以查询点p为中心构造半径为r 的球形区域，沿径向、方位、俯仰3个方向划分网格，
  63. 其中径向2次，方位8次（为简便图中径向只划分了4个），俯仰2次划分网格，
  64. 将球形区域划分成32个空间区域。
  65. 在每个空间区域计算计算落入该区域点的法线nv和中心点p法线np之间的夹角余弦cosθ=nv·np，
  66. 再根据计算的余弦值对落入每一个空间区域的点数进行直方图统计（划分11个），
  67. 对计算结果进行归一化，使得对点云密度具有鲁棒性，得到一个352维特征（32*11=352）。
  68. （原论文：Unique Signatures of Histograms for Local Surface）
  69. */
  70. typedef pcl::PointXYZRGBA PointType;//PointXYZRGBA数据结构 点类型 位置和颜色
  71. typedef pcl::Normal NormalType;//法线类型
  72. typedef pcl::ReferenceFrame RFType;//参考帧
  73. typedef pcl::SHOT352 DescriptorType;//SHOT特征的数据结构（32*11=352）
  75. std::string model_filename_;//模型的文件名
  76. std::string scene_filename_;//场景文件名
  78. //算法参数 Algorithm params
  79. bool show_keypoints_ (false);
  80. bool show_correspondences_ (false);
  81. bool use_cloud_resolution_ (false);
  82. bool use_hough_ (true);
  83. float model_ss_ (0.01f);//模型采样率
  84. float scene_ss_ (0.03f);//场景采样率
  85. float rf_rad_ (0.015f);
  86. float descr_rad_ (0.02f);
  87. float cg_size_ (0.01f);//聚类 霍夫空间设置每个bin的大小
  88. float cg_thresh_ (5.0f);//聚类阈值
  90. // 打印帮组信息 程序用法
  91. void
  92. showHelp (char *filename)
  93. {
  94.   std::cout << std::endl;
  95.   std::cout << "***************************************************************************" << std::endl;
  96.   std::cout << "*                                                                         *" << std::endl;
  97.   std::cout << "*             Correspondence Grouping Tutorial - Usage Guide              *" << std::endl;
  98.   std::cout << "*                                                                         *" << std::endl;
  99.   std::cout << "***************************************************************************" << std::endl << std::endl;
  100.   std::cout << "Usage: " << filename << " model_filename.pcd scene_filename.pcd [Options]" << std::endl << std::endl;
  101.   std::cout << "Options:" << std::endl;
  102.   std::cout << "     -h:                     Show this help." << std::endl;
  103.   std::cout << "     -k:                     Show used keypoints." << std::endl;//关键点
  104.   std::cout << "     -c:                     Show used correspondences." << std::endl;//分组算法
  105.   std::cout << "     -r:                     Compute the model cloud resolution and multiply" << std::endl;
  106.   std::cout << "                             each radius given by that value." << std::endl;
  107.   std::cout << "     --algorithm (Hough|GC): Clustering algorithm used (default Hough)." << std::endl;//聚类算法
  108.   std::cout << "     --model_ss val:         Model uniform sampling radius (default 0.01)" << std::endl;//模型采样率
  109.   std::cout << "     --scene_ss val:         Scene uniform sampling radius (default 0.03)" << std::endl;//场景采样率
  110.   std::cout << "     --rf_rad val:           Reference frame radius (default 0.015)" << std::endl;//参考帧 半径
  111.   std::cout << "     --descr_rad val:        Descriptor radius (default 0.02)" << std::endl;//描述子计算半径
  112.   std::cout << "     --cg_size val:          Cluster size (default 0.01)" << std::endl;//聚类
  113.   std::cout << "     --cg_thresh val:        Clustering threshold (default 5)" << std::endl << std::endl;//聚类数量阈值
  114. }
  116. // 命令行参数解析
  117. void
  118. parseCommandLine (int argc, char *argv[])
  119. {
  120.   // -h 打印帮组信息Show help
  121.   if (pcl::console::find_switch (argc, argv, "-h"))
  122.   {
  123.     showHelp (argv[0]);
  124.     exit (0);
  125.   }
  127.   //  模型和场景文件 Model & scene filenames
  128.   std::vector<int> filenames;
  129.   filenames = pcl::console::parse_file_extension_argument (argc, argv, ".pcd");
  130.   if (filenames.size () != 2)
  131.   {
  132.     std::cout << "Filenames missing.\n";
  133.     showHelp (argv[0]);
  134.     exit (-1);
  135.   }
  137.   model_filename_ = argv[filenames[0]];//模型文件
  138.   scene_filename_ = argv[filenames[1]];//场景文件
  140.   //程序 行为定义 Program behavior
  141.   if (pcl::console::find_switch (argc, argv, "-k"))
  142.   {
  143.     show_keypoints_ = true;//显示关键点
  144.   }
  145.   if (pcl::console::find_switch (argc, argv, "-c"))
  146.   {
  147.     show_correspondences_ = true;//显示对应分组
  148.   }
  149.   if (pcl::console::find_switch (argc, argv, "-r"))
  150.   {
  151.     use_cloud_resolution_ = true;//点云分辨率
  152.   }
  153.   // 聚类算法 
  154.   std::string used_algorithm;
  155.   if (pcl::console::parse_argument (argc, argv, "--algorithm", used_algorithm) != -1)
  156.   {
  157.     if (used_algorithm.compare ("Hough") == 0)
  158.     {
  159.       use_hough_ = true;
  160.     }else if (used_algorithm.compare ("GC") == 0)
  161.     {
  162.       use_hough_ = false;
  163.     }
  164.     else
  165.     {
  166.       std::cout << "Wrong algorithm name.\n";
  167.       showHelp (argv[0]);
  168.       exit (-1);
  169.     }
  170.   }
  172.   //一般参数变量General parameters
  173.   pcl::console::parse_argument (argc, argv, "--model_ss", model_ss_);
  174.   pcl::console::parse_argument (argc, argv, "--scene_ss", scene_ss_);
  175.   pcl::console::parse_argument (argc, argv, "--rf_rad", rf_rad_);
  176.   pcl::console::parse_argument (argc, argv, "--descr_rad", descr_rad_);
  177.   pcl::console::parse_argument (argc, argv, "--cg_size", cg_size_);
  178.   pcl::console::parse_argument (argc, argv, "--cg_thresh", cg_thresh_);
  179. }
  181. // 计算点云分辨率 点云 每个点距离最近点之间的距离和 的平均值
  182. double
  183. computeCloudResolution (const pcl::PointCloud<PointType>::ConstPtr &cloud)
  184. {
  185.   double res = 0.0;
  186.   int n_points = 0;
  187.   int nres;//临近点的索引
  188.   std::vector<int> indices (2);// 索引
  189.   std::vector<float> sqr_distances (2);//距离平方 
  190.   pcl::search::KdTree<PointType> tree;//搜索方法 kdtree
  191.   tree.setInputCloud (cloud);//输入点云
  193.   for (size_t i = 0; i < cloud->size (); ++i)//遍历每一个点
  194.   {
  195.     if (! pcl_isfinite ((*cloud)[i].x))//剔除 NAN点
  196.     {
  197.       continue;
  198.     }
  199.     //Considering the second neighbor since the first is the point itself.
  200.     nres = tree.nearestKSearch (i, 2, indices, sqr_distances);//得到的是距离平方
  201.     if (nres == 2)//最近点第一个为自身第二个为除了自己离自己最近的一个点
  202.     {
  203.       res += sqrt (sqr_distances[1]);//开根号后想家
  204.       ++n_points;
  205.     }
  206.   }
  207.   if (n_points != 0)
  208.   {
  209.     res /= n_points;//最近点之间的距离 的平均值
  210.   }
  211.   return res;
  212. }
  214. // 主函数
  215. int
  216. main (int argc, char *argv[])
  217. {
  218. //======== 【1】命令行参数解析========================
  219.   parseCommandLine (argc, argv);
  221. //======== 【2】新建必要的 指针变量===================
  222.   pcl::PointCloud<PointType>::Ptr model (new pcl::PointCloud<PointType> ());//模型点云
  223.   pcl::PointCloud<PointType>::Ptr model_keypoints (new pcl::PointCloud<PointType> ());//模型点云的关键点 点云
  224.   pcl::PointCloud<PointType>::Ptr scene (new pcl::PointCloud<PointType> ());//场景点云
  225.   pcl::PointCloud<PointType>::Ptr scene_keypoints (new pcl::PointCloud<PointType> ());//场景点云的 关键点 点云
  226.   pcl::PointCloud<NormalType>::Ptr model_normals (new pcl::PointCloud<NormalType> ());//模型点云的 法线向量
  227.   pcl::PointCloud<NormalType>::Ptr scene_normals (new pcl::PointCloud<NormalType> ());//场景点云的 法线向量
  228.   pcl::PointCloud<DescriptorType>::Ptr model_descriptors (new pcl::PointCloud<DescriptorType> ());//模型点云 特征点的 特征描述子
  229.   pcl::PointCloud<DescriptorType>::Ptr scene_descriptors (new pcl::PointCloud<DescriptorType> ());//场景点云 特征点的 特征描述子
  231.   //
  232.   //=======【3】载入点云==========================
  233.   //
  234.   if (pcl::io::loadPCDFile (model_filename_, *model) < 0)//模型点云
  235.   {
  236.     std::cout << "Error loading model cloud." << std::endl;
  237.     showHelp (argv[0]);
  238.     return (-1);
  239.   }
  240.   if (pcl::io::loadPCDFile (scene_filename_, *scene) < 0)//场景点云
  241.   {
  242.     std::cout << "Error loading scene cloud." << std::endl;
  243.     showHelp (argv[0]);
  244.     return (-1);
  245.   }
  247.   //
  248.   //======【4】设置分辨率 变量==========================
  249.   //
  250.   if (use_cloud_resolution_)//使用分辨率
  251.   {
  252.     float resolution = static_cast<float> (computeCloudResolution (model));//计算分辨率
  253.     if (resolution != 0.0f)
  254.     {
  255.       model_ss_   *= resolution;//更新参数
  256.       scene_ss_   *= resolution;
  257.       rf_rad_     *= resolution;
  258.       descr_rad_  *= resolution;
  259.       cg_size_    *= resolution;
  260.     }
  262.     std::cout << "Model resolution:       " << resolution << std::endl;
  263.     std::cout << "Model sampling size:    " << model_ss_  << std::endl;
  264.     std::cout << "Scene sampling size:    " << scene_ss_  << std::endl;
  265.     std::cout << "LRF support radius:     " << rf_rad_    << std::endl;
  266.     std::cout << "SHOT descriptor radius: " << descr_rad_ << std::endl;
  267.     std::cout << "Clustering bin size:    " << cg_size_   << std::endl << std::endl;
  268.   }
  270.   //
  271.   //========【5】计算法线向量============== 
  272.   //
  273.   pcl::NormalEstimationOMP<PointType, NormalType> norm_est;//多核 计算法线模型 OpenMP
  274. //  pcl::NormalEstimation<PointType, NormalType> norm_est;//多核 计算法线模型 OpenMP
  275.   norm_est.setKSearch (10);//最近10个点 协方差矩阵PCA分解 计算 法线向量
  276.   pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ> ());
  277.   //norm_est.setSearchMethod (tree);// 多核模式 不需要设置 搜索算法
  278.   norm_est.setInputCloud (model);//模型点云
  279.   norm_est.compute (*model_normals);//模型点云的法线向量
  281.   norm_est.setInputCloud (scene);//场景点云
  282.   norm_est.compute (*scene_normals);//场景点云的法线向量
  284.   //
  285.   //=======【6】下采样滤波使用均匀采样（可以试试体素格子下采样）得到关键点=========
  286.   //
  288.   pcl::UniformSampling<PointType> uniform_sampling;//下采样滤波模型
  289.   uniform_sampling.setInputCloud (model);//模型点云
  290.   uniform_sampling.setRadiusSearch (model_ss_);//模型点云搜索半径
  291.   uniform_sampling.filter (*model_keypoints);//下采样得到的关键点
  292.   std::cout << "Model total points: " << model->size () << "; Selected Keypoints: " << model_keypoints->size () << std::endl;
  294.   uniform_sampling.setInputCloud (scene);//场景点云
  295.   uniform_sampling.setRadiusSearch (scene_ss_);//点云搜索半径
  296.   uniform_sampling.filter (*scene_keypoints);//下采样得到的关键点
  297.   std::cout << "Scene total points: " << scene->size () << "; Selected Keypoints: " << scene_keypoints->size () << std::endl;
  300.   //
  301.   //========【7】为keypoints关键点计算SHOT描述子Descriptor===========
  302.   //
  303.   pcl::SHOTEstimationOMP<PointType, NormalType, DescriptorType> descr_est;//shot描述子
  304.   descr_est.setRadiusSearch (descr_rad_);
  306.   descr_est.setInputCloud (model_keypoints);
  307.   descr_est.setInputNormals (model_normals);
  308.   descr_est.setSearchSurface (model);
  309.   descr_est.compute (*model_descriptors);//模型点云描述子
  311.   descr_est.setInputCloud (scene_keypoints);
  312.   descr_est.setInputNormals (scene_normals);
  313.   descr_est.setSearchSurface (scene);
  314.   descr_est.compute (*scene_descriptors);//场景点云描述子
  316.   //
  317.   //========【8】按存储方法KDTree匹配两个点云（描述子向量匹配）点云分组得到匹配的组====== 
  318.   //
  319.   pcl::CorrespondencesPtr model_scene_corrs (new pcl::Correspondences ());//最佳匹配点对组
  321.   pcl::KdTreeFLANN<DescriptorType> match_search;//匹配搜索
  322.   match_search.setInputCloud (model_descriptors);//模型点云描述子
  323.   // 在 场景点云中 为 模型点云的每一个关键点 匹配一个 描述子最相似的 点
  324.   for (size_t i = 0; i < scene_descriptors->size (); ++i)//遍历场景点云
  325.   {
  326.     std::vector<int> neigh_indices (1);//索引
  327.     std::vector<float> neigh_sqr_dists (1);//描述子距离
  328.     if (!pcl_isfinite (scene_descriptors->at(i).descriptor[0])) //跳过NAN点
  329.     {
  330.       continue;
  331.     }
  332.     int found_neighs = match_search.nearestKSearch (scene_descriptors->at (i), 1, neigh_indices, neigh_sqr_dists);
  333.     if(found_neighs == 1 && neigh_sqr_dists[0] < 0.25f) //在模型点云中 找 距离 场景点云点i shot描述子距离 <0.25 的点  
  334.     {
  335.       pcl::Correspondence corr (neigh_indices[0], static_cast<int> (i), neigh_sqr_dists[0]);
  336.       //   neigh_indices[0] 为模型点云中 和 场景点云 点   scene_descriptors->at (i) 最佳的匹配 距离为 neigh_sqr_dists[0]  
  337.       model_scene_corrs->push_back (corr);
  338.     }
  339.   }
  340.   std::cout << "Correspondences found: " << model_scene_corrs->size () << std::endl;//匹配点云对 数量
  342.   //
  343.   //===========【9】执行聚类================
  344.   //
  345.   std::vector<Eigen::Matrix4f, Eigen::aligned_allocator<Eigen::Matrix4f> > rototranslations;//变换矩阵 旋转矩阵与平移矩阵
  346. // 对eigen中的固定大小的类使用STL容器的时候，如果直接使用就会出错 需要使用 Eigen::aligned_allocator 对齐技术
  347.   std::vector<pcl::Correspondences> clustered_corrs;//匹配点 相互连线的索引
  348. // clustered_corrs[i][j].index_query 模型点 索引
  349. // clustered_corrs[i][j].index_match 场景点 索引
  351.   //  使用 Hough3D 3D霍夫 算法寻找匹配点
  352.   if (use_hough_)
  353.   {
  354.     //
  355.     //=========计算参考帧的Hough（也就是关键点）=========
  356.     //
  357.     pcl::PointCloud<RFType>::Ptr model_rf (new pcl::PointCloud<RFType> ());//模型参考帧
  358.     pcl::PointCloud<RFType>::Ptr scene_rf (new pcl::PointCloud<RFType> ());//场景参考帧
  359.     //======估计模型参考帧（特征估计的方法（点云，法线，参考帧）
  360.     pcl::BOARDLocalReferenceFrameEstimation<PointType, NormalType, RFType> rf_est;
  361.     rf_est.setFindHoles (true);
  362.     rf_est.setRadiusSearch (rf_rad_); //设置搜索半径
  364.     rf_est.setInputCloud (model_keypoints);//模型关键点
  365.     rf_est.setInputNormals (model_normals);//法线向量
  366.     rf_est.setSearchSurface (model);//模型点云
  367.     rf_est.compute (*model_rf);//计算模型参考帧
  369.     rf_est.setInputCloud (scene_keypoints);//场景关键点
  370.     rf_est.setInputNormals (scene_normals);//法线向量
  371.     rf_est.setSearchSurface (scene);//场景点云
  372.     rf_est.compute (*scene_rf);//场景参考帧
  374.     //  聚类 聚类的方法 Clustering
  375.    //对输入点与的聚类，以区分不同的实例的场景中的模型
  376.     pcl::Hough3DGrouping<PointType, PointType, RFType, RFType> clusterer;
  377.     clusterer.setHoughBinSize (cg_size_);//霍夫空间设置每个bin的大小
  378.     clusterer.setHoughThreshold (cg_thresh_);//阈值
  379.     clusterer.setUseInterpolation (true);
  380.     clusterer.setUseDistanceWeight (false);
  382.     clusterer.setInputCloud (model_keypoints);//模型点云 关键点
  383.     clusterer.setInputRf (model_rf);//模型点云参考帧
  384.     clusterer.setSceneCloud (scene_keypoints);//场景点云关键点
  385.     clusterer.setSceneRf (scene_rf);//场景点云参考帧
  386.     clusterer.setModelSceneCorrespondences (model_scene_corrs);//对于组关系
  388.     //clusterer.cluster (clustered_corrs);//辨认出聚类的对象
  389.     clusterer.recognize (rototranslations, clustered_corrs);
  390.   }
  391.   else // 或者使用几何一致性性质 Using GeometricConsistency
  392.   {
  393.     pcl::GeometricConsistencyGrouping<PointType, PointType> gc_clusterer;
  394.     gc_clusterer.setGCSize (cg_size_);//设置几何一致性的大小
  395.     gc_clusterer.setGCThreshold (cg_thresh_);//阀值
  397.     gc_clusterer.setInputCloud (model_keypoints);
  398.     gc_clusterer.setSceneCloud (scene_keypoints);
  399.     gc_clusterer.setModelSceneCorrespondences (model_scene_corrs);
  401.     //gc_clusterer.cluster (clustered_corrs);//辨认出聚类的对象
  402.     gc_clusterer.recognize (rototranslations, clustered_corrs);
  403.   }
  405.   //
  406.   //========【10】输出识别结果=====Output results=====
  407.   // 
  408.   // 找出输入模型是否在场景中出现
  409.   std::cout << "Model instances found: " << rototranslations.size () << std::endl;
  410.   for (size_t i = 0; i < rototranslations.size (); ++i)
  411.   {
  412.     std::cout << "\n    Instance " << i + 1 << ":" << std::endl;
  413.     std::cout << "        Correspondences belonging to this instance: " << clustered_corrs[i].size () << std::endl;
  415.     // 打印 相对于输入模型的旋转矩阵与平移矩阵  rotation matrix and translation vector
  416.     // [R t]
  417.     Eigen::Matrix3f rotation = rototranslations[i].block<3,3>(0, 0);//旋转矩阵
  418.     Eigen::Vector3f translation = rototranslations[i].block<3,1>(0, 3);//平移向量
  420.     printf ("\n");
  421.     printf ("            | %6.3f %6.3f %6.3f | \n", rotation (0,0), rotation (0,1), rotation (0,2));
  422.     printf ("        R = | %6.3f %6.3f %6.3f | \n", rotation (1,0), rotation (1,1), rotation (1,2));
  423.     printf ("            | %6.3f %6.3f %6.3f | \n", rotation (2,0), rotation (2,1), rotation (2,2));
  424.     printf ("\n");
  425.     printf ("        t = < %0.3f, %0.3f, %0.3f >\n", translation (0), translation (1), translation (2));
  426.   }
  428.   //
  429.   //======可视化 Visualization===============================
  430.   //
  431.   pcl::visualization::PCLVisualizer viewer ("Correspondence Grouping");//对应组
  432.   viewer.addPointCloud (scene, "scene_cloud");//添加场景点云
  434.   pcl::PointCloud<PointType>::Ptr off_scene_model (new pcl::PointCloud<PointType> ());// 模型点云 变换后的点云
  435.   pcl::PointCloud<PointType>::Ptr off_scene_model_keypoints (new pcl::PointCloud<PointType> ());//关键点
  437.   if (show_correspondences_ || show_keypoints_) //可视化 平移后的模型点云
  438.   {
  439.     //  We are translating the model so that it doesn't end in the middle of the scene representation
  440.      //就是要对输入的模型进行旋转与平移，使其在可视化界面的中间位置 x轴负方向平移1个单位 
  441.     pcl::transformPointCloud (*model, *off_scene_model, Eigen::Vector3f (-1,0,0), Eigen::Quaternionf (1, 0, 0, 0));
  442.     pcl::transformPointCloud (*model_keypoints, *off_scene_model_keypoints, Eigen::Vector3f (-1,0,0), Eigen::Quaternionf (1, 0, 0, 0));
  444.     pcl::visualization::PointCloudColorHandlerCustom<PointType> off_scene_model_color_handler (off_scene_model, 255, 255, 128);
  445.     viewer.addPointCloud (off_scene_model, off_scene_model_color_handler, "off_scene_model");//显示平移后的模型点云
  446.   }
  448.   if (show_keypoints_)//可视化 场景关键点 和 模型关键点
  449.   {
  450.     pcl::visualization::PointCloudColorHandlerCustom<PointType> scene_keypoints_color_handler (scene_keypoints, 0, 0, 255);
  451.     viewer.addPointCloud (scene_keypoints, scene_keypoints_color_handler, "scene_keypoints");//可视化场景关键点
  452.     viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 5, "scene_keypoints");
  453.     // 可视化点参数 大小
  455.     pcl::visualization::PointCloudColorHandlerCustom<PointType> off_scene_model_keypoints_color_handler (off_scene_model_keypoints, 0, 0, 255);//可视化模型关键点
  456.     viewer.addPointCloud (off_scene_model_keypoints, off_scene_model_keypoints_color_handler, "off_scene_model_keypoints");
  457.     viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 5, "off_scene_model_keypoints");
  458.   }
  460.   for (size_t i = 0; i < rototranslations.size (); ++i)//对于 模型在场景中 匹配的 点云
  461.   {
  462.     pcl::PointCloud<PointType>::Ptr rotated_model (new pcl::PointCloud<PointType> ());//按匹配变换矩阵 模型点云
  463.     pcl::transformPointCloud (*model, *rotated_model, rototranslations[i]);//将模型点云按匹配的变换矩阵旋转
  465.     std::stringstream ss_cloud;//字符串输出流
  466.     ss_cloud << "instance" << i;//识别出的实例
  468.     pcl::visualization::PointCloudColorHandlerCustom<PointType> rotated_model_color_handler (rotated_model, 255, 0, 0);
  469.     viewer.addPointCloud (rotated_model, rotated_model_color_handler, ss_cloud.str ());//添加模型按识别 变换矩阵变换后 显示
  471.     if (show_correspondences_)//显示匹配点 连线
  472.     {
  473.       for (size_t j = 0; j < clustered_corrs[i].size (); ++j)
  474.       {
  475.         std::stringstream ss_line;//匹配点 连线 字符串
  476.         ss_line << "correspondence_line" << i << "_" << j;
  477.         PointType& model_point = off_scene_model_keypoints->at (clustered_corrs[i][j].index_query);//模型点
  478.         PointType& scene_point = scene_keypoints->at (clustered_corrs[i][j].index_match);//场景点
  480.         //  显示点云匹配对中每一对匹配点对之间的连线
  481.         viewer.addLine<PointType, PointType> (model_point, scene_point, 0, 255, 0, ss_line.str());
  482.       }
  483.     }
  484.   }
  486.   while (!viewer.wasStopped ())
  487.   {
  488.     viewer.spinOnce ();
  489.   }
  491.   return (0);
  492. }

输出结果

image.png

实现效果

结果显示：关键点（蓝色），识别出的模型实例（红色），对应点对连接线（绿色） 源点云（黄色）

命令行输入指令说明：

-k:                在源点云和场景点云中    显示关键点 
-c:                显示对应点连线

./correspondence_grouping ../milk.pcd ../milk_cartoon_all_small_clorox.pcd -c

image.png

./correspondence_grouping ../milk.pcd ../milk_cartoon_all_small_clorox.pcd -c -k

image.png

error

[pcl::SHOTEstimation::computeFeature] The local reference frame is not valid! Aborting description of point with index 3045
这个错误是由于正常半径较小的事实，一些法线是NaN（在设置半径的球体内它们的邻域中没有点），导致SHOT出现问题。
只需要把代码中的，13recognition识别 - 图4搜索半径改大点。

2隐式形状模型 ISM (Implicit Shape Model)

隐式形状模型_海贼王-CSDN博客
PCLImplicit Shape Model隐式形状模型ISM - 代码天地

本教程让我们学会implicit shape model算法，通过pcl::ism::ImplicitShapeModel 类来实现，这个算法是hough变换和Bag of Feature方法的结合，目的是实现：拥有一些已知类别的不同对象的训练集点云；该算法计算某个模型，该模型随后将用于预测给定云中不属于训练集的对象中心。
该算法分为两步：一是训练，二是识别点云中不在训练集里的对象。
训练主要包括六步：

  1. 关键点检测。相当于训练集点云的简化。在这步中，利用体素华网格方法简化所有点云，留下来的点被作为keypoints。
  2. 对每个关键点进行特征估计。使用了FPFH估计，笔记https://blog.csdn.net/yamgyutou/article/details/105407902。
  3. 使用k-means对特征进行聚类构造visual（或几何）words字典。获得的聚类代表visual words。集群中的每个特征都是这个visual words的实例（instance）。
  4. 计算每个单个的实例到中心的方向，从关键点到给定的点云质心的方向。
  5. 对于每个visual words，计算统计权重。
  6. 对于每个估计了特征的关键点，计算权重。

当进行完训练并且获得训练模型后，进行对象搜索（或识别），主要分为：

  1. 关键点检测。
  2. 特征估计。
  3. 在字典中搜索每个特征最近的visual word。
  4. 对于训练好的每个visual word的实例，添加具有相应方向和投票权的投票。
  5. 上一步给了我们一组指向预期中心的方向和每张选票的权重。为了得到对应于中心的单点，需要对这些投票进行分析。为此，算法使用非最大值抑制方法。用户只需要考虑感兴趣对象的半径，其余的将由ISMVoteList::findStrongestPeaks () 方法实现。

代码实现

创建文件：implicit_shape_model.cpp
准备资源：../ism_train_cat等pcd文件
编译执行：
./implicit_shape_model ../ism_train_cat.pcd 0 ../ism_train_horse.pcd 1 ../ism_train_lioness.pcd 2 ../ism_train_michael.pcd 3 ../ism_train_wolf.pcd 4 ../ism_test_cat.pcd 0 

./implicit_shape_model      ../ism_train_cat.pcd      0      ../ism_train_horse.pcd    1      ../ism_train_lioness.pcd  2      ../ism_train_michael.pcd  3      ../ism_train_wolf.pcd     4  ../ism_test_cat.pcd       0

最后的参数是你要检测的点云和你感兴趣的类别。(比如猫)

个人代码：13recognition识别

/*
 * @Description: 隐式形状模型 ISM （隐形状模型 （Implicit Shape Model））
https://github.com/Ewenwan/MVision/tree/master/PCL_APP/Recognition#2--%E9%9A%90%E5%BC%8F%E5%BD%A2%E7%8A%B6%E6%A8%A1%E5%9E%8B-ism-implicit-shape-model-%E8%AE%AD%E7%BB%83%E6%A8%A1%E5%9E%8B%E8%AF%86%E5%88%AB%E6%A8%A1%E5%9E%8B%E7%82%B9%E4%BA%91
 https://blog.csdn.net/qq_25491201/article/details/51153812
 * @Author: HCQ
 * @Company(School): UCAS
 * @Email: 1756260160@qq.com
 * @Date: 2020-10-23 12:29:56
 * @LastEditTime: 2020-10-23 12:33:39
 * @FilePath: /pcl-learning/13recognition识别/2隐式形状模型 ISM (Implicit Shape Model)/implicit_shape_model.cpp
 */

/*
隐式形状模型 ISM （隐形状模型 （Implicit Shape Model））
原理类似视觉词袋模型
计算所有　训练数据　点云的　特征点和特征描述子　ｋ均值聚类　得到视觉词典

    这个算法是把Hough转换和特征近似包进行结合。
    有训练集，这个算法将计算一个确定的模型用来预测一个物体的中心。

ISM算法是 《Robust Object Detection with Interleaved Categorization and Segmentation》正式提出的。
大牛作者叫Bastian Leibe，他还写过其它几篇关于ISM算法的文章。
该算法用于行人和车的识别效果良好。
[主页](http://www.vision.rwth-aachen.de/software/ism)

Full-Resolution Residual Networks for Semantic Segmentation in Street Scenes
用于街景语义分割的全分辨率残差网络（CVPR-12）


这个算法由两部分组成，第一部分是训练，第二部分是物体识别。
训练，它有以下6步:
    1.检测关键点，keypoint detection。这只是一个训练点云的简化。
        在这个步骤里面所有的点云都将被简化，通过体素格下采样　voxel grid　这个途径。
        余下来的点就是特征点；
    2.对特征点，计算快速点特征直方图特征　FPFH，需要计算法线特征；
    3.通过k-means聚类算法对特征进行聚类得到视觉（几何）单词词典；
    4.计算每一个实例（聚类簇，一个视觉单词）里面的特征关键点　到　聚类中心关键点　的　方向向量；
    5.对每一个视觉单词，依据每个关键点和中心的方向向量，计算其统计学权重；

    6.对每一个关键点计算学习权重，与关键点到聚类中心距离有关。

我们在训练的过程结束以后，接下来就是对象搜索的进程。
    1.特征点检测。
    2.对每个特征点计算　特征描述子。
    3.对于每个特征点对应的特征描述子搜索最近的　训练阶段得到的视觉单词。
    4.对于每一个特征点计算　类别投票权重（视觉单词统计学权重　关键点学习权重）。
    5.前面的步骤给了我们一个方向集用来预测中心与能量。

上面的步骤很多涉及机器学习之类的，大致明白那个过程即可.

./implicit_shape_model
      ism_train_cat.pcd      0
      ism_train_horse.pcd    1
      ism_train_lioness.pcd  2
      ism_train_michael.pcd  3
      ism_train_wolf.pcd     4
      ism_test_cat.pcd       0
[数据](http://pointclouds.org/documentation/tutorials/implicit_shape_model.php#implicit-shape-model)


*/
#include <iostream>
#include <pcl/io/pcd_io.h>
#include <pcl/features/normal_3d.h>//计算法线特征
#include <pcl/features/feature.h>
#include <pcl/visualization/cloud_viewer.h>//可视化
#include <pcl/features/fpfh.h>//　快速点特征直方图特征
#include <pcl/features/impl/fpfh.hpp>// 
#include <pcl/recognition/implicit_shape_model.h>
#include <pcl/recognition/impl/implicit_shape_model.hpp>

int
main (int argc, char** argv)
{
  if (argc == 0 || argc % 2 == 0)
    return (-1);

  unsigned int number_of_training_clouds = (argc - 3) / 2;
// 法线向量　特征　估计
  pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> normal_estimator;
  normal_estimator.setRadiusSearch (25.0);

  std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> training_clouds;//训练点云　所有的
  std::vector<pcl::PointCloud<pcl::Normal>::Ptr> training_normals;//训练点云　法线特征
  std::vector<unsigned int> training_classes;// 类别

// 对训练数据点云　提取　法线特征
  for (unsigned int i_cloud = 0; i_cloud < number_of_training_clouds - 1; i_cloud++)
  {
    pcl::PointCloud<pcl::PointXYZ>::Ptr tr_cloud(new pcl::PointCloud<pcl::PointXYZ> ());//单个点云
    if ( pcl::io::loadPCDFile <pcl::PointXYZ> (argv[i_cloud * 2 + 1], *tr_cloud) == -1 )
      return (-1);

    pcl::PointCloud<pcl::Normal>::Ptr tr_normals = (new pcl::PointCloud<pcl::Normal>)->makeShared ();//共享指针
    normal_estimator.setInputCloud (tr_cloud);
    normal_estimator.compute (*tr_normals);//估计这个　点云的　法线特征

    unsigned int tr_class = static_cast<unsigned int> (strtol (argv[i_cloud * 2 + 2], 0, 10));//类别

    training_clouds.push_back (tr_cloud);//训练点云
    training_normals.push_back (tr_normals);//点云法线
    training_classes.push_back (tr_class);//类别
  }
// 在法线特征上　提取　fphf快速点特征直方图　计算法线夹角　直方图统计
  pcl::FPFHEstimation<pcl::PointXYZ, pcl::Normal, pcl::Histogram<153> >::Ptr fpfh
    (new pcl::FPFHEstimation<pcl::PointXYZ, pcl::Normal, pcl::Histogram<153> >);
  fpfh->setRadiusSearch (30.0);//搜索半价
  pcl::Feature< pcl::PointXYZ, pcl::Histogram<153> >::Ptr feature_estimator(fpfh);

  pcl::ism::ImplicitShapeModelEstimation<153, pcl::PointXYZ, pcl::Normal> ism;
  ism.setFeatureEstimator(feature_estimator);//特征估计器
  ism.setTrainingClouds (training_clouds);//训练点云
  ism.setTrainingNormals (training_normals);//训练法线
  ism.setTrainingClasses (training_classes);//类别
  ism.setSamplingSize (2.0f);//采样

  pcl::ism::ImplicitShapeModelEstimation<153, pcl::PointXYZ, pcl::Normal>::ISMModelPtr model = boost::shared_ptr<pcl::features::ISMModel>
    (new pcl::features::ISMModel);//模型
  ism.trainISM (model);//训练模型

  std::string file ("trained_ism_model.txt");
  model->saveModelToFile (file);//保存模型

  model->loadModelFromfile (file);//载入模型


// 载入测试点云　
  unsigned int testing_class = static_cast<unsigned int> (strtol (argv[argc - 1], 0, 10));
  pcl::PointCloud<pcl::PointXYZ>::Ptr testing_cloud (new pcl::PointCloud<pcl::PointXYZ> ());
  if ( pcl::io::loadPCDFile <pcl::PointXYZ> (argv[argc - 2], *testing_cloud) == -1 )
    return (-1);
//　估计测试点云　的　法线
  pcl::PointCloud<pcl::Normal>::Ptr testing_normals = (new pcl::PointCloud<pcl::Normal>)->makeShared ();
  normal_estimator.setInputCloud (testing_cloud);
  normal_estimator.compute (*testing_normals);
// 寻找点云　
  boost::shared_ptr<pcl::features::ISMVoteList<pcl::PointXYZ> > vote_list = ism.findObjects (
    model,
    testing_cloud,
    testing_normals,
    testing_class);

// 启动分类的进程。代码将会告诉算法去找testing_class类型的物体，
//在给定的testing_cloud这个点云里面。注意算法将会使用任何你放进去进行训练的模型。
//在分类操作以后，一列的决策将会以pcl::ism::ISMVoteList这个形式返回。
  double radius = model->sigmas_[testing_class] * 10.0;
  double sigma = model->sigmas_[testing_class];
  std::vector<pcl::ISMPeak, Eigen::aligned_allocator<pcl::ISMPeak> > strongest_peaks;
  vote_list->findStrongestPeaks (strongest_peaks, testing_class, radius, sigma);

  pcl::PointCloud <pcl::PointXYZRGB>::Ptr colored_cloud = (new pcl::PointCloud<pcl::PointXYZRGB>)->makeShared ();
  colored_cloud->height = 0;
  colored_cloud->width = 1;

  pcl::PointXYZRGB point;
  point.r = 255;
  point.g = 255;
  point.b = 255;

  for (size_t i_point = 0; i_point < testing_cloud->points.size (); i_point++)
  {
    point.x = testing_cloud->points[i_point].x;
    point.y = testing_cloud->points[i_point].y;
    point.z = testing_cloud->points[i_point].z;
    colored_cloud->points.push_back (point);
  }
  colored_cloud->height += testing_cloud->points.size ();

  point.r = 255;
  point.g = 0;
  point.b = 0;
  for (size_t i_vote = 0; i_vote < strongest_peaks.size (); i_vote++)
  {
    point.x = strongest_peaks[i_vote].x;
    point.y = strongest_peaks[i_vote].y;
    point.z = strongest_peaks[i_vote].z;
    colored_cloud->points.push_back (point);
  }
  colored_cloud->height += strongest_peaks.size ();

  pcl::visualization::CloudViewer viewer ("Result viewer");
  viewer.showCloud (colored_cloud);
  while (!viewer.wasStopped ())
  {
  }

  return (0);
}

输出结果

实现效果

红点表示物体的中心。
如果你想要可视化决策的过程，你就会看到如下的东西。蓝色是决策点

33D物体识别的假设检验 Tutorial: Hypothesis Verification for 3D Object Recognition

https://github.com/Ewenwan/MVision/tree/master/PCL_APP/Recognition#3-3d
https://blog.csdn.net/qq_25491201/article/details/51156128

3D物体识别的假设检验 
    如何做3D物体识别通过验证模型假设在聚类里面。在描述器匹配后，
    这次我们将运行某个相关组算法在PCL里面为了聚类点对点相关性的集合，
    决定假设物体在场景里面的实例。
    在这个假定里面，全局假设验证算法将被用来减少错误的数量。

代码实现

创建文件：global_hypothesis_verification.cpp
准备资源：../milk.pcd ../milk_cartoon_all_small_clorox.pcd
编译执行：./global_hypothesis_verification ../milk.pcd ../milk_cartoon_all_small_clorox.pcd

个人代码：13recognition识别

/*
 * @Description: 3D物体识别的假设检验
 * https://github.com/Ewenwan/MVision/tree/master/PCL_APP/Recognition#3-3d%E7%89%A9%E4%BD%93%E8%AF%86%E5%88%AB%E7%9A%84%E5%81%87%E8%AE%BE%E6%A3%80%E9%AA%8C-%E5%AF%B9%E5%BA%94%E5%88%86%E7%BB%84%E7%9A%84%E4%B8%89%E7%BB%B4%E7%89%A9%E4%BD%93%E8%AF%86%E5%88%AB-icp%E7%82%B9%E4%BA%91%E9%85%8D%E5%87%86%E9%AA%8C%E8%AF%81%E7%BB%93%E6%9E%9C
 * https://blog.csdn.net/qq_25491201/article/details/51156128
 * @Author: HCQ
 * @Company(School): UCAS
 * @Email: 1756260160@qq.com
 * @Date: 2020-10-23 12:47:05
 * @LastEditTime: 2020-10-23 13:07:53
 * @FilePath: /pcl-learning/13recognition识别/33D物体识别的假设检验/global_hypothesis_verification.cpp
 */

/*3D物体识别的假设检验 
如何做3D物体识别通过验证模型假设在聚类里面。在描述器匹配后，
这次我们将运行某个相关组算法在PCL里面为了聚类点对点相关性的集合，
决定假设物体在场景里面的实例。
在这个假定里面，全局假设验证算法将被用来减少错误的数量。


*/
#include <pcl/io/pcd_io.h>// 文件、设备读写
#include <pcl/point_cloud.h>//基础pcl点云类型
#include <pcl/correspondence.h>//分组算法 对应表示两个实体之间的匹配（例如，点，描述符等）。
#include <pcl/features/normal_3d_omp.h>//法向量特征 + 曲率特征
#include <pcl/features/shot_omp.h>//描述子 shot描述子 0～1
#include <pcl/features/board.h>
#include <pcl/filters/uniform_sampling.h>//均匀采样 滤波
#include <pcl/recognition/cg/hough_3d.h>//hough算子
#include <pcl/recognition/cg/geometric_consistency.h>//几何一致性
#include <pcl/recognition/hv/hv_go.h>//模型假设验证 Hypothesis Verification 
#include <pcl/registration/icp.h>//icp点云配准
#include <pcl/visualization/pcl_visualizer.h>//可视化
#include <pcl/kdtree/kdtree_flann.h>// kdtree 快速近邻搜索
#include <pcl/kdtree/impl/kdtree_flann.hpp>
#include <pcl/common/transforms.h> //点云转换 转换矩阵
#include <pcl/console/parse.h>//命令行参数解析
// 别名
typedef pcl::PointXYZRGBA PointType;//PointXYZRGBA数据结构 点类型 位置和颜色
typedef pcl::Normal NormalType;     //法线类型　　法线＋曲率
typedef pcl::ReferenceFrame RFType; //参考帧
typedef pcl::SHOT352 DescriptorType;//SHOT特征的数据结构（32*11=352）
//点云风格　颜色　和大小　结构体
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_;

//算法参数 Algorithm params
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);//聚类 霍夫空间设置每个bin的大小
float cg_thresh_ (5.0f);//聚类阈值
int icp_max_iter_ (5);//icp最大迭代次数
float icp_corr_distance_ (0.005f);//icp相关性距离
float hv_clutter_reg_ (5.0f);//模型假设验证 Hypothesis Verification 
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);

// 打印帮组信息 程序用法=========================================================
void
showHelp (char *filename)
{
  std::cout << std::endl;
  std::cout << "***************************************************************************" << std::endl;
  std::cout << "*                                                                         *" << std::endl;
  std::cout << "*          Global Hypothese Verification 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;
  std::cout << "     -h:                          Show this help." << std::endl;
  std::cout << "     -k:                          Show keypoints." << std::endl;
  std::cout << "     --algorithm (Hough|GC):      Clustering algorithm used (default Hough)." << std::endl;
  std::cout << "     --model_ss val:              Model uniform sampling radius (default " << model_ss_ << ")" << std::endl;
  std::cout << "     --scene_ss val:              Scene uniform sampling radius (default " << scene_ss_ << ")" << std::endl;
  std::cout << "     --rf_rad val:                Reference frame radius (default " << rf_rad_ << ")" << std::endl;
  std::cout << "     --descr_rad val:             Descriptor radius (default " << descr_rad_ << ")" << std::endl;
  std::cout << "     --cg_size val:               Cluster size (default " << cg_size_ << ")" << std::endl;
  std::cout << "     --cg_thresh val:             Clustering threshold (default " << cg_thresh_ << ")" << std::endl << std::endl;
  std::cout << "     --icp_max_iter val:          ICP max iterations number (default " << icp_max_iter_ << ")" << std::endl;
  std::cout << "     --icp_corr_distance val:     ICP correspondence distance (default " << icp_corr_distance_ << ")" << std::endl << std::endl;
  std::cout << "     --hv_clutter_reg val:        Clutter Regularizer (default " << hv_clutter_reg_ << ")" << std::endl;
  std::cout << "     --hv_inlier_th val:          Inlier threshold (default " << hv_inlier_th_ << ")" << std::endl;
  std::cout << "     --hv_occlusion_th val:       Occlusion threshold (default " << hv_occlusion_th_ << ")" << std::endl;
  std::cout << "     --hv_rad_clutter val:        Clutter radius (default " << hv_rad_clutter_ << ")" << std::endl;
  std::cout << "     --hv_regularizer val:        Regularizer value (default " << hv_regularizer_ << ")" << std::endl;
  std::cout << "     --hv_rad_normals val:        Normals radius (default " << hv_rad_normals_ << ")" << std::endl;
  std::cout << "     --hv_detect_clutter val:     TRUE if clutter detect enabled (default " << hv_detect_clutter_ << ")" << std::endl << std::endl;
}

// 命令行参数解析==============================================================
void
parseCommandLine (int argc,
                  char *argv[])
{
  // 打印帮组信息Show help
  if (pcl::console::find_switch (argc, argv, "-h"))
  {
    showHelp (argv[0]);
    exit (0);
  }

  // 模型和场景文件  Model & scene filenames
  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]];

  // 程序 行为定义  Program behavior
  if (pcl::console::find_switch (argc, argv, "-k"))
  {
    show_keypoints_ = true;//显示关键点
  }
// 聚类算法 
  std::string used_algorithm;
  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 << "Wrong algorithm name.\n";
      showHelp (argv[0]);
      exit (-1);
    }
  }

  //一般参数变量General parameters
  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[])
{
//======== 【1】命令行参数解析=================================
  parseCommandLine (argc, argv);
//======== 【2】新建必要的 指针变量============================
  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> ());//场景点云 特征点的 特征描述子

//=======【3】载入点云=========================================
  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);
  }

//========【5】计算 法向量和曲率 ===========================================
  pcl::NormalEstimationOMP<PointType, NormalType> norm_est;//多核 计算法线模型 
  norm_est.setKSearch (10);//最近10个点 协方差矩阵PCA分解 计算 法线向量
  norm_est.setInputCloud (model);//模型点云
  norm_est.compute (*model_normals);//模型点云的法线向量

  norm_est.setInputCloud (scene);//场景点云
  norm_est.compute (*scene_normals);//场景点云的法线向量

//=======【6】下采样滤波使用均匀采样（可以试试体素格子下采样）得到关键点==========
  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;

  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;

//========【7】为keypoints关键点计算SHOT描述子Descriptor=================
  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);//场景点云描述子

//========【8】按存储方法KDTree匹配两个点云（描述子向量匹配）点云分组得到匹配的组========
  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;
  // 为  场景点云 的每一个关键点 匹配模型点云一个 描述子最相似的 点 < 0.25f
  for (size_t i = 0; i < scene_descriptors->size (); ++i)
  {
    std::vector<int> neigh_indices (1);//设置最近邻点的索引
    std::vector<float> neigh_sqr_dists (1);//申明最近邻平方距离值
    if (!pcl_isfinite (scene_descriptors->at (i).descriptor[0]))//跳过NAN点
    {
      continue;
    }
// sscene_descriptors->at (i)是给定点云描述子, 1是临近点个数 ，neigh_indices临近点的索引  neigh_sqr_dists是与临近点的平方距离值
    int found_neighs = match_search.nearestKSearch (scene_descriptors->at (i), 1, neigh_indices, neigh_sqr_dists);
    if (found_neighs == 1 && neigh_sqr_dists[0] < 0.25f)//（描述子与临近的距离在一般在0到1之间）才添加匹配
 //在模型点云中 找 距离 场景点云点i shot描述子距离 <0.25 的点  
    {
      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;

//===========【9】实际的配准方法的实现 执行聚类================
  //变换矩阵 旋转矩阵与平移矩阵 
// 对eigen中的固定大小的类使用STL容器的时候，如果直接使用就会出错 需要使用 Eigen::aligned_allocator 对齐技术
  std::vector<Eigen::Matrix4f, Eigen::aligned_allocator<Eigen::Matrix4f> > rototranslations;
  std::vector<pcl::Correspondences> clustered_corrs;//匹配点 相互连线的索引
// clustered_corrs[i][j].index_query 模型点 索引
// clustered_corrs[i][j].index_match 场景点 索引

  if (use_hough_)//  使用 Hough3D 3D霍夫 算法寻找匹配点
  {
    //=========计算参考帧的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);

//  聚类 聚类的方法 Clustering
//对输入点与的聚类，以区分不同的实例的场景中的模型
    pcl::Hough3DGrouping<PointType, PointType, RFType, RFType> clusterer;
    clusterer.setHoughBinSize (cg_size_);
    clusterer.setHoughThreshold (cg_thresh_);
    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);

    clusterer.recognize (rototranslations, clustered_corrs);
  }
  else// 或者使用几何一致性性质 Using GeometricConsistency
  {
    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);
  }

  /**
   * Stop if no instances
   */
  if (rototranslations.size () <= 0)
  {
    cout << "*** No instances found! ***" << endl;
    return (0);
  }
  else
  {
    cout << "Recognized Instances: " << rototranslations.size () << endl << endl;
  }

  /**
   * Generates clouds for each instances found 
   */
  std::vector<pcl::PointCloud<PointType>::ConstPtr> instances;

  for (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]);
    instances.push_back (rotated_model);
  }

//ICP 点云配准==========================================================
  std::vector<pcl::PointCloud<PointType>::ConstPtr> registered_instances;
  if (true)
  {
    cout << "--- ICP ---------" << endl;

    for (size_t i = 0; i < rototranslations.size (); ++i)
    {
      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);//匹配到的点云
      registered_instances.push_back (registered);
      cout << "Instance " << i << " ";
      if (icp.hasConverged ())
      {
        cout << "Aligned!" << endl;
      }
      else
      {
        cout << "Not Aligned!" << endl;
      }
    }

    cout << "-----------------" << endl << endl;
  }

// 模型假设验证  Hypothesis Verification=====================================

  cout << "--- Hypotheses Verification ---" << endl;
  std::vector<bool> hypotheses_mask;  // Mask Vector to identify positive hypotheses

  pcl::GlobalHypothesesVerification<PointType, PointType> GoHv;

  GoHv.setSceneCloud (scene);  // Scene Cloud
  GoHv.addModels (registered_instances, true);  //Models to verify

  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);  // i-element TRUE if hvModels[i] verifies hypotheses

  for (int i = 0; i < hypotheses_mask.size (); i++)
  {
    if (hypotheses_mask[i])
    {
      cout << "Instance " << i << " is GOOD! <---" << endl;
    }
    else
    {
      cout << "Instance " << i << " is bad!" << endl;
    }
  }
  cout << "-------------------------------" << endl;

//======可视化 Visualization====================================================
  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 (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" << endl;
    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);
}

输出结果

image.png

实现效果

./global_hypothesis_verification milk.pcd milk_cartoon_all_small_clorox.pcd
image.pngimage.png
有效的假设是图中绿色的部分
你可以模拟更多错误通过使用一个尺寸参数对于hough相关组决策算法。
./global_hypothesis_verification milk.pcd milk_cartoon_all_small_clorox.pcd —cg_size 0.035
image.png