【毕业课题学习】6DOF物体识别及抓取-PCL点云处理（四）---Hough voting详解

最近一直在搞UR5在Moveit和Gazebo中的联调仿真，搞了一周多，我发现好像我的重点好像不应该放在这。。。能用就行了，在过程中，我给UR在模型文件中添加了夹爪和realsense相机以便能够在家就有实际操控的感觉。
现在回到重点，6Dof物体识别~~
这次主要文献是 Object recognition in 3D scenes with occlusions and clutter by Hough voting 图也基本是从文献里来的
以PCL官网中的示例代码为例（前面几节有讲

这个方法大多是将其划分为基于点云配准的识别方法

---

整体识别流程pipeline

从图中可以看到，Hough voting算法就只是整个pipeline中的一部分（橙色两块）。

分析以下这个算法的实现：

• 已知条件
  • 目标的模型点云
  • 场景的模型点云
• 求解
  • 识别目标模型
  • 计算模型点云到场景点云的变换矩阵

图我就不解释了，重点还是在橙色部分，离线那部分，也就是说我有了模型文件，我可以先把离线部分完成了，数据存下来，后续做识别，只需要输入场景点云，将离线部分数据拿出来用即可，就不需要每次都把模型的部分计算一遍（节省时间）。
流程前半部分描述符匹配，一般用的是局部描述符，例如SHOT等，这里主要考量还是得看模型是什么，针对模型来挑选 对模型特征描述的最清楚的 描述符。计算描述符后，利用Kd-Tree找近邻的方式来做匹配，之前的博客也有讲。

---

这里我提前说一下该算法的效果：

1. 可实现场景中多目标物体识别; 官方的代码可完成单目标多实例检测（如场景中若存在多个奶盒），推广后自然可以完成多目标多实例检测（多用几次该代码，对不同的模型进行匹配…
2. 适用于识别多无纹理，多形状的的刚体。 比如一个水壶、鸭子玩具，识别效果就比较好；而对于方盒这种多平面的效果就不太好，因为你的特征不明显，在做匹配的时候就容易出错。

---

直奔主题——Hough voting实现

假设我们需要去识别场景中一个目标物体，且场景点云、模型点云 都是3d网格的形式

预处理

对模型点云和场景点云做如下两步：

1. 拿到点云后，先做什么——滤波、采样，提取关键点（懒一点，平均下采样，得到的点它也叫关键点key-point，(lll￢ω￢)）
2. 计算局部描述符

对模型点云的处理中多了一步：

• 计算模型点云每个关键点的局部参考坐标系 。以下统称LRF（local reference frame）
  很关键，这也是为什么该算法能实现旋转不变性的原因所在（SHOT描述子中也用到了LRF）。计算LRF不多做解释，简单理解就是：对于每个关键点，结合该点的领域的特征来生成一个坐标系，这个坐标系起参考的作用

之后，将两片点云的局部描述符做点对匹配，（常见的就是基于欧几里得距离做匹配）

基于LRF的方法思路

解释一下图二（照着解释梳理下去）：
左边是模型(M)点云，右边是场景点云（S）
蓝色的点代表提取出的关键点；蓝色点上的三轴代表计算出的局部参考坐标系；
虚线箭头代表 模型中的关键点 与 场景中的关键点匹配上（利用其描述符），绿色的代表正确匹配，红色的代表错误匹配。
论文中C^M取的是模型的中心点（pcl有计算中心点的实现）

解释完参数，讲解：
将模型的重心点C^M分别与3个关键点做连线，分别得到一个向量（方向+距离），即图中蓝色的实线。有了向量，有了局部参考系，在场景点云中，反推场景中该目标的重心点-C^S。图二中可以看到，两个正确匹配的关键点反推出来的C^M是同一个点！
将其推广到有很多个关键点的点云，那么可想而知，在场景点云中的正确匹配的关键点反推出来的C^M点 在三维空间中 一定会聚集在一起。

投票机制

这里用图三的例子来介绍投票的机制。
先看模型，是个圆柱体：中间的点代表中心点，其余的代表几个关键点，蓝色箭头是关键点指向中心点的向量。红色虚线是错误匹配的点对，绿色虚线是正确匹配的点对。
场景：有一个圆柱实例，还有一个方体、圆锥体。
接下来来做识别：
按照前面LRF部分讲的，在场景中匹配上的关键对，利用对应向量来反推物体的中心点。可以看到图三中，错误匹配的三个点反推出的中心点则各自落入了其他的方框，而两个正确匹配的点反推出的中心点落入了红色方框，那么明眼人都看得出，正确的目标（圆柱体）中心点就是在这个红色方框内。
我识别出正确的目标中心（图三中的红色方框），是不是就可以理解为 我识别出来了目标模型呢？
差不多就是这个意思。

为什么叫投票
你看图三右边场景点云部分，是将其划分成了一个个的小方块（体素votex)，由关键点和向量反推出来的箭头终点，会落入到一个体素中。你把体素看作是投票箱（bin），箭头终点所指的地方看作是票（vote），那么，投票箱中的票数越多，置信度也就更高，人为设定个阈值，也就可以用来求取正确的目标中心，也就是识别目标，哪怕场景中存在多个实例，在这里也就是多个投满票的投票箱而已。
这里我们就解决了识别物体！

求解6Dof

之前是在场景中求出了中心点，点与点之间，可以计算出位置偏移 t，也就是目标模型与场景中目标物体的位移变化。
而旋转矩阵R怎么求呢？

在图四中，可以清楚看到坐标系的一个变化。模型和场景的全局坐标系（Global RF）认作是视点坐标系。F^M_i和F^S_j是利用领域关系求出来的LRF。Global RF到LRF的转换是已知的（学过变换矩阵应该就了解）。中间那个R^MS_L旋转矩阵，代表模型中的关键点的LRF —> 场景中对应关键点的LRF 的旋转矩阵。

为方便理解，我们将一个相机（视点或者说原点处）坐标系当作是场景和模型的全局参考坐标系Global RF，相机坐标固定不动。想象相机拍摄到了两块点云，一个是model 一个是scene中的model对象，现在要求这两个之间的姿态变换，那么用匹配点对的LRF之间的旋转矩阵，即可代表这个姿态变化。点云中有多对匹配点，将每对求出来的姿态变化求平均，即是提到的 R^MS_L

结果

在上述算法中，我们求取得到了：

• 模型点云 -> 场景点云中的实例 = 变换矩阵T

可能现在就比较迷，求出这个有什么用，我要做抓取的话，需要的是目标物体在当前世界坐标系中的6DoF位姿。
在模型点云（比如是一个深度相机提前拍好拟合后的），求得点云中心后，人为设定目标物体的坐标系（比如水壶，正对方向是x轴，竖直是z轴），确定好后，用求得的变换矩阵T，对模型中的目标物体坐标系做转换，即可得到目标物体在当前相机坐标系中的6DoF位姿，利用标定外参矩阵，就是最后目标物体在当前世界坐标系中的6Dof位姿

抑或是实际抓取应用中，对于二指夹爪，人为提前设定模型点云里的抓取点p^m，（比如抓可乐瓶的瓶身，让机械臂工具坐标系到达瓶身上方一点，并选择合适位姿）

p^s=Rp^m+T

p^s即，在当前相机坐标系下的抓取点，让机械臂进行抓取就行了。

当然这过程还涉及什么路径规划、碰撞检测等内容。Moveit功能包了解一下……

---

总结

基于Hough voting的pipeline梳理到此，论文中还有很多如降低误差等细节部分未作过多解释，有时间可以对照着原文来看。博客只是把大致原理介绍清楚。在该代码实际测试过程中，我发现基于点云配准，对于已知物体，比如一个球，其实并不需要完完全全的6Dof位姿，抓哪都一样，扩展一下，抓圆柱体，只要抓到身子就行了。还是得看抓取的目标是什么，如果是规则物体，可能只需要目标的中心点+长轴，就可以完成二指抓取了。所以对于各例，可以进一步简化pipeline。
在我的linux系统笔记本上
调用pcl官网程序，参数调整合适的情况下，跑一次3s左右（没用 icp 精度上有待考量）
参数不合适的情况下，多则十多秒
下图是拿现有的不同姿态扳手（且人为做了偏移）做的匹配效果：

---

下一步，准备再找些开源代码试一试效果，欢迎留言评论好用的算法。