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

【毕业课题学习】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 ());
      }
    }
  }

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

©️2020 CSDN 皮肤主题: 大白 设计师:CSDN官方博客 返回首页