物体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_{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位姿。

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

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

基于给定的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_{cm} * T_m + t_{cm}$，将物体变换到相机系下如下图：

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

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

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

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

扫描二维码关注公众号，回复： 11525324 查看本文章

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

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

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

绕y轴旋转180°，旋转矩阵 $R_{mm_o}=[-1,0,0,0,1,0,0,0,-1]$；原始物体的中心为 $c_{m_o}=(4.5445,4.6495,-44.5433)$，由于 $T_m=R_{mm_o}*T_{m_o}+t_{mm_o}$，其中 $t_{mm_o}=c_m-R_{mm_o}*c_{m_o}$，而 $c_m=(0,0,0)$为preprocessed后的物体的中心坐标，由此可得 $t_{mm_o}=(4.5445,-4.6495,-44.5433)$。

按照原始LineMOD提供的真值，有 $T_c = R_{cm_o} * T_{m_o} + t_{cm_o}$，其中 $R_{cm_o}$代表了由原始物体向相机系物体的旋转， $T_{m_o}$代表原始物体的坐标点， $t_{cm_o}$代表原始物体向相机系物体的平移。可以进行变换：

$T_c = [R_{cm_o} t_{cm_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}$
$=[R_{cm_o}t_{cm_o}]*[R_{mm_o}^T -R_{mm_o}^Tt_{mm_o}]*T_m$
$=[R_{cm} t_{cm}] * T_m$

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

$R_{cm_o}$为：
-0.0963063 0.994044 -0.0510079
-0.573321 -0.0135081 0.81922
0.813651 0.10814 0.571207 r

$t_{cm_o}$为：
-111.814 -78.3622 1036.12

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