【毕业课题学习】6DOF物体识别及抓取-PCL点云处理（二）_点云6d抓取

 

【毕业课题学习】6DOF物体识别及抓取-PCL点云处理

直接进入正题，上接讲到PCL官网的有物体识别相关的教程，那就直接剖析这个recognition识别教程，看看是不是我们所需要的内容。
进入Recognition栏中的3D Object Recognition based on Correspondence Grouping，因为效果图确实是我们想要的。

3D Object Recognition based on Correspondence Grouping教程分析

进入教程后，看作文都知道先看首尾，首页便是关于教程的介绍，写到：该教程旨在解释如何使用pcl_recognition模块进行3d物体识别。解释了该算法一系列流程（pipeline），接下来就是分布拆解开给你们看。
教程整个pipeline比较清晰，对于初学者可能会存在几个问题：

1. C++的传参，以及编译过程。初学在ubuntu系统下编译C++代码，以及传入参数，着实懵逼，不知道有这种骚操作，和在一些codeblock或者vc++直接按运行键的傻瓜式操作，确实很多不一样的地方，不过实质都是一样的。在windows上visual stdio上去运行该套代码，还需要做一些传参方面的修改，如，直接在程序内给好参数。 （这部分不清楚的建议从PCL小例程（如读入pcd文件，输出）先尝试下。
2. PCL基础的熟悉，特别visual可视化，第一次用可视化的时候是显示那只兔子，滚轮可以调大小，左键调方向（简单来说），但是里面的点，这代表着什么？可以从头推起，我们在可视化里显示的是点云图，点云图是传感器照射目标所采集到的点，点与点之间的距离是实际现实生活中的距离，而传感器照射，从哪个角度位置拍的，这就是视点（viewpoint）。我们一开始打开并可视化的点云图，云点之间的距离，当前的视点又是怎样的呢？可以先尝试用些熟悉的pcd文件实验一下 ~ v ~
3. 算法原理，该算法不只是用到recognition模板中的类，还涉及到下采样、描述符等内容，接下来会细讲。

编译运行

如果是在linux系统下直接编译运行，按照官网的说明基本不会出错。而如果是在vs下去做，会有一些报错（我就碰到好多问题）
最后根据下面这两篇博客解决了问题：
PCL-基于对应分组的三维物体识别
PCL特征点与配准（1）代码解释
tips：我在做实验过程中发现，运行结果始终为单位矩阵，且没有偏移量，而图上显示是有偏移。那时因为在可视化步骤里，人为添加了偏移，而奶盒的模型和场景中实际没有偏移。我怀疑奶盒的模型实际就是场景点云中抠出来的。。。

顺便把模型文件也发出来，有些时候官网下不下来文件：
链接：https://pan.baidu.com/s/1BLsKOi-D1hoU355pJASSlA
提取码：kaiq
复制这段内容后打开百度网盘手机App，操作更方便哦
——————————————

算法流程 (主函数从上至下)

① 加载读入模型modle和场景scene点云文件
② 计算空间分辨率（resolution）（可选）
③ 计算两片点云每个点的表面法线
④ 对两片点云进行下采样获得关键点
⑤ 对所有关键点计算描述符（discriptor)
⑥ 对两片点云中的描述符进行匹配对应起来
⑦ 用Hough或GH算法 将上述匹配好的描述符进行聚类，并求出实例（clustered_corrs）（聚类出来的每个类称为一个实例）和旋转位移矩阵
⑧ 以实例、旋转矩阵输出显示结果
⑨ 可视化

每个步骤实现过程（与上面流程对应

建议跟着代码从main函数往下看
or 哪里不会看哪里

tips：下面的半径参数都是以米为单位（包括pcd中的点坐标也都是米为单位。）

1. Load clouds 加载点云。

用的pcl::io::loadPCDFile加载场景点云和模型点云。

2. Set up resolution invariance 设置分辨率

也可以说是点云密度，这里是计算点云中每个点与最近邻的点的距离，最后求平均。主要是为了应变点密不同的模型文件，而带来的参数变化问题。（这里默认不用，自己手动调参更易学习

3. Compute Normals 计算表面法线

具体实现方法就不说了，这里用的参数 setKSearch，设置Kd_Tree的搜索近邻的数量，计算点云中的每个点法线是利用的该点的n个近邻，可以理解为将这n个点看作是一个曲面，将这个曲面的法线作为该点的法线，而这个n也就是这里setKSearch的参数。

4. Downsample Clouds to Extract keypoints 下采样提取关键点

下采样的目的主要还是为了降低算法复杂度，你想一个点云几十万个点，都拿来计算会造成很多不便，用下采样提取一部分点拿来做运行，就足够用于识别了。该算法中用的下采样算法是uniform_sampling，平均采样，可以说是最简单最随意的采样法了，简单来说就是：在点云中用很多的球形结构（结构可以叫做一个体素voxel），每个球里面所包含到的所有点云中的点，用这些点的质心（一个点）来替代这些点（*个点），也就做到了下采样的目的。（ps:这里用其他的保存特征更完整的下采样算法效果应该更好。

这里用到的参数有model_ss_，scene_ss_，这个就代表这个球的半径，球越大，球包含的点就更多，下采样后得到的关键点就更少，反之… （这里需要根据你的点云的密度，你肯定要保证具有足够多的关键点能够拿来后续做识别，还要让关键点足够少，来降低运算时间等消耗。）

5. Comput e Descriptor for keypoints 对关键点计算描述符

这里用到的是SHOT描述符。很多初入PCL经常会接触到descriptor这个单词，那这是什么呢：从字面来看，就是描述，描述的一个点的特征，几何特征等。比如一个点的表面法线就可以说是对特征的描述。

思考一下：法线是一个向量，能代表多少多少这个点的几何特征，如果将模型旋转了，这个向量是不是也就旋转了，“特征描述”是不是就变了。所以就有人为了这个invariance不变性，将这个点的法线作为参考，与“周围一定空间”中的近邻点的法线夹角分别求cos，再进行直方图统计这些夹角信息（邻近点的位置），那么得到的这个直方图是不是就比单纯一个法线代表的多呢？而且以法线作为参考线，就不用担心物体旋转后特征变化（这里涉及到参照物的概念，比如你坐在电梯上，相对于地板，你动了，而相对于电梯，你没动），这就是SHOT。

这里需要设置的参数有descr_rad_，这个值就是加粗的周围一定空间，其实还是用的一个球体区域，这个球体的半径也就是descr_rad_，半径越大，球内的点就越多，计算该点描述符的复杂度也就越大，因为参与描述的点变多了，描述的也就越多。为了后续做匹配，应当有足够多的点参与描述（这个参数的设置也是要考虑点密的大小。
SHOT的大致解释

6. Find Model-Scene Correspondences with KdTree 描述符匹配

在步骤5中已经求出来场景和模型点云关键点的描述符，这一步骤就是将这两片点云的描述符进行匹配。匹配的原理就是先生成模型点云描述符的kd_tree，分别将场景点云的每个描述符带到这个kd_tree中求取该描述符最近邻，若距离小于一个阈值则认为，该描述符与它的最近邻是匹配的，所代表的点就是匹配点对，最后将 匹配点对 存入向量中。

可以这样理解：把每个描述符都当作是多维空间中的一个“点”，如果场景中的某个“点”与模型中的某个“点”足够近（实际是特征足够相似），则认为是匹配的。

这里用到的参数有一个0.25f，后面有解释 add match only if the squared descriptor distance is less than 0.25 (SHOT descriptor distances are between 0 and 1 by design)，就是说描述符之间的平方距离小于0.25认为是匹配的（这里的值是经过归一化了的，0到1之间），所以基本不用改这里。

7. Actual Clustering 聚类（重难点）

前面已经讲到了匹配好的点对，那么场景中这些匹配的点对都是目标奶盒（目标物体）所产生的吗？很明显不是，比如奶盒的一个角上的点，结果和另一个角上的点描述符匹配（特征相似），那么就是错误的匹配，更或者是奶盒上的点和其他东西上的点匹配上了，最后形成了错误匹配。

首先应当知道，两个有方向的向量可以求出旋转量，加上位置的变化可以求出位移量，加在一起就是6DOF（位姿）。而每对匹配点对其实都可以生成一个这样的变换矩阵（坐标代表位置，法线代表向量方向），那我们需要的求解的就是目标物体所产生的位姿变换（一个变换矩阵），正确的匹配点对求出来的才是正确的变换，由于一些小的误差，正确的那些匹配点对所求出来的矩阵也会存在小的误差。这一步骤所做的工作就是把匹配点对给聚类（分配）成多个实例（目标），最后计算整个的变换。

算法中提供了两个实现方法，一个是Hough，一个是GC。先把参数解释了，原理稍微麻烦一点，放这篇的后面具体讲解。直接介绍Hough，GC参数比Hough还少一个：

Hough算法，前面几排是和GC不一样的，做了一个求参考坐标系（Reference Frames）的步骤，求的是模型每个关键点的参考坐标系，原理实现简单来说利用每个点周围范围（这里还是个球体结构，rf_rad_代表球体的半径）的那些点来生成一个起参考作用的坐标系，（前面SHOT就是用法线做参考，这里更粗暴，直接生成个参考系拿来做参照，意义一致，都是为了旋转不变性）。

而后是聚类，有两个参数： cg_size_还是可以理解为一个球体结构的半径大小，之前讲了：有些许误差的 正确的 匹配点 所产生结果有些许误差，而这个球体的半径就是容差范围，把邻近的匹配也认作是同一个实例对象的匹配。从结果来说 cg_size_越大，一个聚类实例中的匹配点对数就会越大。 另一个参数cg_thresh_，阈值，匹配点对数大于这个阈值，认为是一个目标实例，最后将这个实例所求出作为变换矩阵存起来。换言之，如果场景中有多个奶盒目标，最后可能检测出多个目标实例，算出其结果。
至此，目标姿态变换结果已经算出来了，存在了rototranslations和clustered_corrs里。

8. Output results 输出结果

printf出实例的个数和矩阵

9. Visualization 可视化

  //
  //  可视化
  //
  pcl::visualization::PCLVisualizer viewer ("Correspondence Grouping");
  //显示界面中添加场景点云
  viewer.addPointCloud (scene, "scene_cloud");

  pcl::PointCloud<PointType>::Ptr off_scene_model (new pcl::PointCloud<PointType> ());
  pcl::PointCloud<PointType>::Ptr off_scene_model_keypoints (new pcl::PointCloud<PointType> ());
  
//如果设定correspondences_或show_keypoints_为真
  if (show_correspondences_ || show_keypoints_)
  {
    //  将模型及其关键点向x轴负方向偏移1m，Quaternion（1，0，0，0）（四元数的值代表姿态旋转，这个值代表姿态不变）
    //  将模型偏移的原因是：为了方便观察
    pcl::transformPointCloud (*model, *off_scene_model, Eigen::Vector3f (-1,0,0), Eigen::Quaternionf (1, 0, 0, 0));
    pcl::transformPointCloud (*model_keypoints, *off_scene_model_keypoints, Eigen::Vector3f (-1,0,0), Eigen::Quaternionf (1, 0, 0, 0));
    //  将偏离后的模型点云显示在界面中，颜色为黄色
    pcl::visualization::PointCloudColorHandlerCustom<PointType> off_scene_model_color_handler (off_scene_model, 255, 255, 128);
    viewer.addPointCloud (off_scene_model, off_scene_model_color_handler, "off_scene_model");
  }
  
  //若show_keypoints_为真，则
  if (show_keypoints_)
  {
    //  将场景中的关键点和模型中的关键点设置为蓝色，且点的大小设置为5
    pcl::visualization::PointCloudColorHandlerCustom<PointType> scene_keypoints_color_handler (scene_keypoints, 0, 0, 255);
    viewer.addPointCloud (scene_keypoints, scene_keypoints_color_handler, "scene_keypoints");
    viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 5, "scene_keypoints");

    pcl::visualization::PointCloudColorHandlerCustom<PointType> off_scene_model_keypoints_color_handler (off_scene_model_keypoints, 0, 0, 255);
    viewer.addPointCloud (off_scene_model_keypoints, off_scene_model_keypoints_color_handler, "off_scene_model_keypoints");
    viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 5, "off_scene_model_keypoints");
  }
  //  将模型的点云图 利用得到的旋转偏移矩阵 映射到场景中去，并以红色显示
  for (std::size_t i = 0; i < rototranslations.size (); ++i)
  {
    pcl::PointCloud<PointType>::Ptr rotated_model (new pcl::PointCloud<PointType> ());
    pcl::transformPointCloud (*model, *rotated_model, rototranslations[i]);

    std::stringstream ss_cloud;
    ss_cloud << "instance" << i;

    pcl::visualization::PointCloudColorHandlerCustom<PointType> rotated_model_color_handler (rotated_model, 255, 0, 0);
    viewer.addPointCloud (rotated_model, rotated_model_color_handler, ss_cloud.str ());
 
   //若show_correspondences_设置为真，则用绿色的线显示匹配点对的配对关系
    if (show_correspondences_)
    {
      for (std::size_t j = 0; j < clustered_corrs[i].size (); ++j)
      {
        std::stringstream ss_line;
        ss_line << "correspondence_line" << i << "_" << j;
        PointType& model_point = off_scene_model_keypoints->at (clustered_corrs[i][j].index_query);
        PointType& scene_point = scene_keypoints->at (clustered_corrs[i][j].index_match);

        //  We are drawing a line for each pair of clustered correspondences found between the model and the scene
        viewer.addLine<PointType, PointType> (model_point, scene_point, 0, 255, 0, ss_line.str ());
      }
    }
  }
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61

---

至此，整个代码就搞定了，建议再找些点云文件，试试做匹配实验，自己手动调下参，收益会更多。下一篇打算详细解释一下Hough投票聚类算法。