物体6D姿态含义与估计方法总结

一、什么是物体6D姿态？

6D位姿，即6个自由度的位姿，包括3个自由度的位移（Translation）和3个自由度的空间旋转（Rotation），合起来就叫位姿 (Pose)。位姿是一个相对的概念，指的是两个坐标系之间的位移和旋转变换。它描述了物体在三维空间中的位置和朝向，是计算机视觉、机器人学等领域中的一个重要概念。

位移（Translation）：表示物体在三维空间中的位置，通常用一个三维向量（X, Y, Z）来表示物体在世界坐标系或相机坐标系中的坐标。
旋转（Rotation）：表示物体在三维空间中的朝向，通常用一个旋转矩阵（Rotation Matrix）或四元数（Quaternion）来表示物体相对于某个参考坐标系的旋转。

物体6D位姿和相机6D位姿是相似的，区别在于从哪个坐标系变换到相机坐标系。相机6D位姿是指拍摄当前图像时刻，相机坐标系相对于世界坐标系发生的平移和旋转变换。世界坐标系可以定义在任意位置，也可以和当前相机坐标系重合。相机6D位姿通常用世界系到相机系的RT变换来表示，也即：T_c = R_cw * T_w + t_cw，其中R_cw代表由世界系到相机系的旋转，t_cw代表由世界系到相机系的平移，T_c代表相机系下的3D点，T_w代表世界系下的3D点。

而物体6D位姿是指拍摄当前图像时刻，相机坐标系相对于原始物体所在的世界系，发生的平移和旋转变换。原始物体可以放在世界系的任何位置，而且通常将物体本身的重心和朝向与世界系对齐。物体6D位姿通常用原始物体所在世界系到相机系的RT变换来表示，也即：T_c = R_cm * T_m + t_cm，其中R_cm代表由原始物体 (model) 所在的世界系到相机系的旋转，t_cm代表由物体所在的世界系到相机系的平移，T_c代表相机系下物体的3D点，T_m代表物体所在世界系下物体的3D点。因此，当世界系和物体本身对齐时，相机的6D位姿等价于物体的6D位姿。

二、3D物体6D姿态的估计方法

1、Correspondence-based method**

这类方法针对纹理丰富的物体，是最常用的方法；该方法又可以分为两类：寻找已知模型3D点和观测RGB图像2D像素点之间的对应；寻找已知模型3D点和观测Depth图像3D点之间的对应；

3D-2D对应
3D模型首先投影到N个角度，得到N张模板RGB图像，记录3D点和2D像素之间的对应；采集单个视角下RGB图像后，提取特征点如 SIFT，SURF，ORB等，寻找和模板图像之间的对应 (2D-2D)；这样我们得到了3D点和当前观测RGB图像2D像素点的对应，使用Perspective-n-Point(PnP)算法即可恢复当前视角图像的位姿；（类似基于特征点vSLAM中的重定位过程）

3D-3D对应
观测的Depth图像可以结合内参转换为3D点云，需要和已有完整3D点云进行配准；当单视角观测点云和完整3D点云位姿偏差较大时，需要进行全局配准（粗配准）；当单视角观测点云和完整3D点云位姿很接近时，可以使用局部配准（细配准），如ICP等方法；全局配准方法主要分为2类，一类基于RANSAC类方法如Super 4PCS [2014-Super 4PCS]，随机选择3对或者4对点进行投票，选择最优的对应最为最终变换；一类是基于特征描述符的方法，在点云上分别提取显著性特征点，根据描述符如SpinImages，FPFH，SHOT等，寻找3D点之间的对应，确定变换；全局配准的结果可以用局部配准方法进行优化；

2、Template-based method

这类方法针对的是弱纹理或者无纹理图像，也即Correspondence-based method不能处理的情况；这类方法中，很难提取特征点，能利用的可以是RGB图像的梯度信息；完整3D模型也投影到N个角度，得到N张模板RGB图像，记录每张投影时对应的位姿；通过比较观测RGB图像和模板RGB图像的梯度信息，寻找到最相似模板图像，以该模板对应的位姿作为观测图像对应的位姿；该类方法的代表方法是LineMode方法 [2012-Model based]；

3、Voting-based method

这类方法主要针对的是具有遮挡的情况；依据是图像中的每一个局部都能够对整体的输出产生投票；代表性方法是基于Object coordinate的方法 [2014-Learning 6d object pose]，每一个Object coordinate都能预测一对3D-3D之间的变换，通过产生大量假设再refine得到最终结果；此外还有基于Hough forest的方法 [2014-Latent-class]，基于Point Pair Features (PPF) [2012-3d object detection] 的方法；此外，还有DenseFusion方法，分别对RGB图像和Depth对应3D点云使用3DCNN网络得到融合的pixel-wise dense feature，每一个feature都能预测一个姿态，最后通过投票得到最后的6D姿态；该方法是当前精度最高的算法；

4、Regression-based method

该类方法主要学习图像特征和6D位姿之间的联系；常用的方法为在基于深度学习进行目标检测的基础上，再回归出物体的6D位姿；常见的方法可以分为两类，一类直接回归出物体的6D姿态，包括PoseCNN [2017-Posecnn], Deep-6DPose [2018-Deep-6dpose]等；另一类方法预测关键3D点在2D图像上的投影位置，然后使用pnp方法恢复姿态，包括BB8 [2017-Bb8]，Tekin’18 [2018-Real-time], SSD6D [2017-Ssd-6d]等；

三、总结

以上方法中，Correspondence-based method精度高，是主流采用的方法，在进行机器人抓取时较为常用；弊端是需要3D物体精确的3D模型；Template-based method方法精度较为受限；Voting-based方法精度较高，然而运算速度慢；Regression-based method方法精度高，扩展性好，但需要大量训练数据。

相关链接
从物体定位、物体姿态估计到平行抓取器抓取估计
物体6D位姿的含义