[论文]CubeSLAM: Monocular 3D Object SLAM

前言

上一篇文章调研了RGBD相机的动态物体追踪的工作 MaskFusion:
https://zhuanlan.zhihu.com/p/62059382
文章的尺度主要还是限制在桌面上，拿起一些物体测试跟踪效果。离我所理解的物体级的SLAM还有一些差距。
近期开始调研物体级的SLAM，这篇文章还不错: CubeSLAM: Monocular 3D Object SLAM， 它发表在 2019年 最近一期的 IEEE Transactions on Robotics上（当然之前出过会议文章）。CubeSLAM用单目相机实现了物体级的建图、定位、和动态物体跟踪。

而这位华人作者Shichao Yang在CMU Robotics Institute实验室的导师指导下做工作。列一下他的其他工作：

• Pop-up SLAM： 一个很优秀的室内面slam work， IROS2016

• Monocular Object and Plane SLAM in Structured Environments: 将CubeSLAM和 Pop-up-SLAM结合起来出的一篇 ，有时间再做它的分析。

我觉得他的探索思路是非常合理的： 传统基于特征点的SLAM基本已没啥可做的了（有做的赶紧告诉我），点之上的高维表达包括线、面、物体，这些先验暂时无法做到通用，但在室内的结构环境下有不少可以尝试的点。必然需要解决这些物体的观测模型、地图中的表达、在BA中的融合数学模型等等。
对于语义SLAM来说，我们的前端有检测或分割图像的输入，而地图中物体的表达方式是什么呢？ICRA2019一篇： Structure Aware SLAM using Quadrics and Planes 探讨了用双曲面表达物体的方式。室内先验下 面、物体 如何在地图中表达，并以此形成一套观测、融合的体系，是至少目前还在研究的东西。
我们先回过头看CubeSLAM，CubeSLAM里则提出用立方体来建模物体，这是我们最直观的物体理解方式。它提出了整套观测误差函数，并放入ORBSLAM2的BA过程中统一优化。这里面提出几点很有价值的想法是:

• 如何从单目利用2D bounding box， 辅助Vanish Points(消影点) 来恢复三维立方体结构
• 如何在传统SLAM的静态假设上，将动态物体（车辆等）也追踪起来。实现相机位姿估计和运动物体位姿估计。

论文在室内SUN RGBD数据集和室外KITTI数据集做了完善的实验。

我将论文核心理论部分整理如下，欢迎一起讨论，也欢迎私信我指出错误。

核心部分

III. Single Image 3D Object Detection

A. 3D box proposal generation

1) Principles
3Dbox用9个自由度描述，除了6维刚体变换外，还增加了3个dimension: 即长宽高.

由于一个前端矩形检测的四个端点只能提供4个约束，因而需要其他的信息。如：提供或预测的物体大小，以及朝向。(在许多车辆检测算法中用到)

我们使用VP来改变和减少回归参数，而不去预测依赖的维度。
VP是平行线的相交点。一个3D cuboid有三个正交轴，因此可以形成3个VPs，而且只与物体旋转有关!

V P i = K R c o l ( i ) , i ∈ { 1 , 2 , 3 } \mathrm{VP}_{i}=K R_{c o l(i)}, \quad i \in\{1,2,3\} VPi​=KRcol(i)​,i∈{ 1,2,3}

2) Get 2D corners from the VP
由于最多能看到3个立体面，我们考虑三种可能性。

图中只要已知VP和其中一个端点，其他端点都能被算出来。
图：比如当已知VP，图a)中给定1点，则2、4点可以通过线相交确定。

3) Get 3D box pose from 2D corners
得到了2D平面内的立方体顶点后，我们要估计其三维位姿。
将物体分成两类。
任意位姿物体:
使用PnP solver来求解通常的立体3D位姿和大小，由于单目尺度不确定性，我们需要确定一个尺度因子。数学上来说，立体的八个3D顶点在物体坐标系下是可描述的：$\left[\pm d_x,\pm d_y,\pm d_z\right]/2$， 变换到相机参考系是：$R\left[\pm d_x,\pm d_y,\pm d_z\right]/2+t$。
我们可以选择4个相连的顶点如1,2,4,7，然后从上述3D交点投影到2D中去，如顶点1有:
$$p_1=\pi \left(R\left[d_x,d_y,d_z\right]/2+\mathbf{t}\right))$$

因此每个点可以贡献两个约束，4个点即可估计9DOF的pose，除了scale.

地面物体: 地面物体的roll/pitch都是0. 可以大大简化上述计算。
我们不再需要用复杂的PnP Solver去求解，而是可以直接反向投影地面的顶点到三维地平面上，然后计算其他垂直的顶点。
比如上述在三维地平面上的顶点5，可以表达为 [n,m] ( 相机帧里的向量和距离 ), 相对应的三维顶点 $P_5$是反向投影的射线$K^{-1}{p}_5$与地平面的交点:
$${\mathbf{P}}_5=\frac{-m}{{\mathbf{n}}^{\top }\left(K^{-1}{p}_5\right)}{K}^{-1}{p}_5$$

该公式应该是空间直线与平面交点的表示公式，在 MVG 书里或许有。

更详细地投影过程在论文[27](Pop-up SLAM)中描述。尺度由相机高度决定。

4) Sample VP and Summary
由于VP通过物体旋转矩阵R决定。通过深度网络，可以直接预测VP。我们手动sample它们，并且根据评分排序它们。
( 这部分到底是如何生成的？意思是否是通过神经网络预测物体的旋转矩阵R来预测VP？）
在本文的实验中，我们仅仅考虑了地面物体.!

B. Proposal scoring

示意获取最佳的三维立方体假设。

在图中，左图先提取了一些直线，右图分别是根据不同的直线生成的立体矩形假设，左上角是最佳的假设。每个不同假设可以得到不同的代价函数值。由下式计算：
$$E(O|I)={\varphi }_{\text{dist}}(O,I)+w_1$$