（6）物体6自由度位姿估计主流方法----待完善

参考链接：目前物体6自由度位姿估计（6-Dof pose estimation）有哪些主流方法？

主流的方法有下面几种：

1. 基于模板匹配的方法

代表论文：

1.Gradient Response Maps for Real-Time Detection of Textureless Objects

2.Model Based Training, Detection and Pose Estimation of Texture-Less 3D Objects in Heavily Cluttered Scenes

这个以Stefan Hinterstoisser的LINEMOD为代表。方法是在可能的SE3空间通过渲染对要检测的物体作充分的采样，提取足够鲁棒的模板，再对模板进行匹配就可以大致的估计位姿，最后ICP精化结果。作者在优化模板匹配速度上做了很多工作，效果也确实不错。

1. 基于点的方法

Going Further with Point Pair Features这篇论文，Hinterstoisser对PPF做了很多优化工作，让PPF的估计准确率基本可以达到2016年的 state of the art。对于PPF（2010）这样一个简单的算法来说，可以达到这样的性能是非常了不起的。

1.Model Globally, Match Locally: Efficient and Robust 3D Object Recognition （PPF）

2.Going Further with Point Pair Features（Hinterstoisser）

3.An Efficient RANSAC for 3D Object Recognition in Noisy and Occluded Scenes（object ransac）

4.A performance evaluation of point pair features

这类方法基本上是通过点云上面少量的点对构成描述子来做的。最经典的文章就是Bertram Drost的Model Globally, Match Locally: Efficient and Robust 3D Object Recognition。这篇文章，作者的命名为“全局建模，局部匹配”，我觉得非常精确，它非常高度的概括了这个算法的思想。先说全局建模，就是对模型的点云中的所有任意的两个点法都计算PPF描述子，构建模型hash表，以描述子为key，以这两个点法为value。在从scene点晕中匹配的时候，同样对scene中的所有的任意两个点法计算PPF描述子，在模型hash表中查询，这样可以得到所有可能匹配点法对。由于，两个点法对如果匹配，可以计算出其变换的刚体变换矩阵，也就是我们要求的pose。这样可以在SE3的位姿空间进行投票，以消除一些误匹配。这就是局部匹配。当然作者还是做了很多工作来处理SE3空间的投票问题（这不好做）。

An Efficient RANSAC for 3D Object Recognition in Noisy and Occluded Scenes是Chavda Papazov的，他好像和Sami Haddadin（ franka机器人）是一个组的。这篇文章前面步骤与Drost那篇相同，都是计算PPF描述子构建模型的Hash表，然后对scene点云采样匹配。这里的采样是基于RANSAC的思路，随机取两个点法，得到描述子后丢Hash表匹配，可以计算出一个可能的pose，那么如何知道这个pose是不是正确的呢，Papazov采用的是对于计算出来的pose用一个目标函数作快速的假设检验，留下得分最高的。由于引入了随机采样这个方法每次得出的pose都不一样，速度也是时快时慢，但是贵在有时可以对付及其严重的点云遮盖。

1. 基于描述子的方法

1.Point Cloud Library - Three-Dimensional Object Recognition and 6 DoF Pose Estimation

2.A Global Hypotheses Verification Method for 3D Object Recognition (假设检验)

3.A Comprehensive Performance Evaluation of 3D Local Feature Descriptors

由于，我们知道三个对应点对就可以解析的解出pose，所以如何让匹配点对更精确更鲁棒是描述子方法的研究重点，也涌现了很多的方法，比如PFH,FPFH,SHOT等。在得到匹配的点对之后，我们可以按照Point Cloud Library - Three-Dimensional Object Recognition and 6 DoF Pose Estimation文章中给出的local pipline 和global pipline，对点云进行处理来完成pose estimation。A Comprehensive Performance Evaluation of 3D Local Feature Descriptors是国防科技大学郭裕兰教授的一篇关于各种描述子性能的比较综述文章。

1. 霍夫森林（vote based）

1.Recovering 6D Object Pose and Predicting Next-Best-View in the Crowd（有源码）

2.Latent-Class Hough Forests for 6 DoF Object Pose Estimation

这两篇文章都使用了一种霍夫森林的方法，其思想是建立图像patch与SE3中的pose的对应关系，就是训练一个随机森林。然后检测的时候从图像中提取patch，在SE3空间投票以推算最终的pose。

1. Object Coordiantes 回归法

1.Learning 6D Object Pose Estimation using 3D Object Coordinates

2.Uncertainty-Driven 6D Pose Estimation of Objects and Scenes from a Single RGB Image

这一类方法的特点有两个，一是不使用patch来训练随机森林，这是因为patch大小不好确定，二是不直接建立图像中元素到SE3空间的映射，而是建立图像中元素到Object Coordinates也就是模型自身坐标的映射。第一篇文章中作者构建了一个随机森林，建立了图像中每一个像素与模型坐标的映射。输入一张图像，随机森林可以判断这张图片的每一个像素它属于那个物体，并且告知这个像素在物体的那个部位。有了这个对应关系（当然存在很多误匹配）作者采用了一个基于采样的方法抽取了物体最终的pose。

第二篇文章与第一篇类似，特点是不使用RGBD图像，而使用RGB图像，也可以很好的估计pose。

1. end to end

1.BB8: A Scalable, Accurate, Robust to Partial Occlusion Method for Predicting the 3D Poses of Challenging Objects without Using Depth

2.SSD-6D: Making RGB-Based 3D Detection and 6D Pose Estimation Great Again

1. 概率法

1.Probabilistic Approaches for Pose Estimation

2.Bingham Distribution-Based Linear Filter for Online Pose Estimation（有源码）

这个叫概率法，不知道恰不恰当。主要提一个概率模型Bingham Distribution，这个模型是对高斯分布的一个拓展，可以对四元数的随机分布建模。有了这个模型，作者构建了一个Bingham distribution-based filtering (BF)来在线的估计pose的旋转部分。 这样的迭代下去就可以对两个点云配准。作者表示这个方法对噪声和点云密度鲁棒性很好，而且比之ICP又快又准。

目前姿态估计的一些问题：

(1)遮挡问题
(2)速度过慢
(3)仅仅有二维的姿态是不够的，目前也有这一类的研究，关于直接从2d到3d的姿态进行直接估计。这一点才是未来的趋势。