物体6D位姿的含义

本文同步于微信公众号：3D视觉前沿，欢迎大家关注。

在一些有关物体6D位姿估计或者机器人抓取的论文中，我们常会听到一个词：物体6D位姿估计 (6D object pose estimation)，那什么是物体的6D位姿呢？它和SLAM中的相机6D位姿一样吗？

6D是指6个自由度，代表了3个自由度的位移 (也叫平移 (Translation))，以及3个自由度的空间旋转 (Rotation)，合起来就叫位姿 (Pose)。位姿是一个相对的概念，指的是两个坐标系之间的位移和旋转变换。物体6D位姿和相机6D位姿是相似的，区别在于从哪个坐标系变换到相机坐标系。

相机6D位姿是指拍摄当前图像时刻，相机坐标系相对于世界坐标系发生的平移和旋转变换。世界坐标系可以定义在任意位置，也可以和当前相机坐标系重合。相机6D位姿通常用世界系到相机系的RT变换来表示，也即： T c = R c w ∗ T w + t c w T_c = R_{cw} * T_w + t_{cw} Tc​=Rcw​∗Tw​+tcw​，其中 R c w R_{cw} Rcw​代表由世界系到相机系的旋转， t c w t_{cw} tcw​ 代表由世界系到相机系的平移， T c T_c Tc​代表相机系下的3D点， T w T_w Tw​代表世界系下的3D点。

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

具体我们看一个物体6D位姿的例子，数据来自preprocessed LineMod dataset数据集，原始物体3D模型本身的重心和朝向与世界系是对齐的，其坐标值为 T m T_m Tm​：

使用相机拍摄带有目标物体的场景后，恢复相机坐标系下的点云如下，此时目标物体的3D坐标为 T c T_c Tc​：

基于给定的Ground Truth的物体6D位姿，camRm2c代表物体所在世界系到相机系的旋转矩阵R，camtm2c代表物体所在世界系到相机系的平移矩阵t：

cam_R_m2c: [0.09630630, 0.99404401, 0.05100790, 0.57332098, -0.01350810, -0.81922001, -0.81365103, 0.10814000, -0.57120699]，
cam_t_m2c: [-105.35775150, -117.52119142, 1014.87701320]，

使用 T c = R c m ∗ T m + t c m T_c = R_{cm} * T_m + t_{cm} Tc​=Rcm​∗Tm​+tcm​，将物体变换到相机系下如下图：

此时可以看到，根据物体的6D位姿 R c m R_{cm} Rcm​ 和 t c m t_{cm} tcm​ 将物体由本身的世界系变换到了相机系下，将物体3D点与场景3D点叠加在一起的效果如下：

此时，也可以结合相机的内参K，

cam_K: [572.4114, 0.0, 325.2611, 0.0, 573.57043, 242.04899, 0.0, 0.0, 1.0]

将物体投影到2d，如下图，可以看到也是正确的。

因此，物体的6D位姿是和物体3D模型本身的世界坐标系相关的，同一个相机系下，不同物体的6D位姿是不一样的；如果保持物体和相机的相对位置不变，则物体的位姿是不变的；如果物体不动，相机系发生了移动，则新的物体位姿需要在原来的基础上再叠加相机系的相对移动位姿 △ R △R △R和 △ t △t △t，这也是使用 LabelFusion 构建6D真值数据集的原理。

p.s. 上文中使用的是preprocessed的LineMOD数据集，物体的3D模型中心在 ( 0 , 0 , 0 ) (0,0,0) (0,0,0)位置，这里把原始的LineMOD数据集也测试一下。同样是ape类别，其3D模型如下图。

可以看到其与上文的3D模型差别了绕y轴旋转180°，并且旋转后的重心需要向坐标原点平移。首先计算旋转矩阵 R m m o R_{mm_o} Rmmo​​，再计算 t m m o t_{mm_o} tmmo​​。

绕y轴旋转180°，旋转矩阵 R m m o = [ − 1 , 0 , 0 , 0 , 1 , 0 , 0 , 0 , − 1 ] R_{mm_o}=[-1,0,0,0,1,0,0,0,-1] Rmmo​​=[−1,0,0,0,1,0,0,0,−1]；原始物体的中心为 c m o = ( 4.5445 , 4.6495 , − 44.5433 ) c_{m_o}=(4.5445,4.6495,-44.5433) cmo​​=(4.5445,4.6495,−44.5433)，由于 T m = R m m o ∗ T m o + t m m o T_m=R_{mm_o}*T_{m_o}+t_{mm_o} Tm​=Rmmo​​∗Tmo​​+tmmo​​，其中 t m m o = c m − R m m o ∗ c m o t_{mm_o}=c_m-R_{mm_o}*c_{m_o} tmmo​​=cm​−Rmmo​​∗cmo​​，而 c m = ( 0 , 0 , 0 ) c_m=(0,0,0) cm​=(0,0,0)为preprocessed后的物体的中心坐标，由此可得 t m m o = ( 4.5445 , − 4.6495 , − 44.5433 ) t_{mm_o}=(4.5445,-4.6495,-44.5433) tmmo​​=(4.5445,−4.6495,−44.5433)。

按照原始LineMOD提供的真值，有 T c = R c m o ∗ T m o + t c m o T_c = R_{cm_o} * T_{m_o} + t_{cm_o} Tc​=Rcmo​​∗Tmo​​+tcmo​​，其中 R c m o R_{cm_o} Rcmo​​代表了由原始物体向相机系物体的旋转， T m o T_{m_o} Tmo​​代表原始物体的坐标点， t c m o t_{cm_o} tcmo​​代表原始物体向相机系物体的平移。可以进行变换：

T c = [ R c m o t c m o ] ∗ T m o T_c = [R_{cm_o} t_{cm_o}] * T_{m_o} Tc​=[Rcmo​​tcmo​​]∗Tmo​​
= [ R c m o t c m o ] ∗ [ R m m o T − R m m o T t m m o ] ∗ [ R m m o t m m o ] ∗ T m o = [R_{cm_o}t_{cm_o}]*[R_{mm_o}^T -R_{mm_o}^Tt_{mm_o}]*[R_{mm_o} t_{mm_o}]*T_{m_o} =[Rcmo​​tcmo​​]∗[Rmmo​T​−Rmmo​T​tmmo​​]∗[Rmmo​​tmmo​​]∗Tmo​​
= [ R c m o t c m o ] ∗ [ R m m o T − R m m o T t m m o ] ∗ T m =[R_{cm_o}t_{cm_o}]*[R_{mm_o}^T -R_{mm_o}^Tt_{mm_o}]*T_m =[Rcmo​​tcmo​​]∗[Rmmo​T​−Rmmo​T​tmmo​​]∗Tm​
= [ R c m t c m ] ∗ T m =[R_{cm} t_{cm}] * T_m =[Rcm​tcm​]∗Tm​

其中第二行第二项是第三项的逆。根据原始LineMOD提供的真值：

R c m o R_{cm_o} Rcmo​​为：
-0.0963063 0.994044 -0.0510079
-0.573321 -0.0135081 0.81922
0.813651 0.10814 0.571207 r

t c m o t_{cm_o} tcmo​​为：
-111.814 -78.3622 1036.12

代入上式第三行，得到第四行最终的preprocessed后的物体的6D位姿 [ R c m t c m ] [R_{cm} t_{cm}] [Rcm​tcm​]，和preprocessed LineMOD提供的真值是一致的。