[论文]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用单目相机实现了物体级的建图、定位、和动态物体跟踪。
![[论文]CubeSLAM: Monocular 3D Object SLAM_第1张图片](http://img.e-com-net.com/image/info8/ebf87298ded04205af6d426bdd52002c.jpg)
而这位华人作者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数据集做了完善的实验。
![[论文]CubeSLAM: Monocular 3D Object SLAM_第2张图片](http://img.e-com-net.com/image/info8/e9458aaecd0847ca9610d7dc5f51637e.jpg)
我将论文核心理论部分整理如下，欢迎一起讨论，也欢迎私信我指出错误。
核心部分
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个立体面，我们考虑三种可能性。
![[论文]CubeSLAM: Monocular 3D Object SLAM_第3张图片](http://img.e-com-net.com/image/info8/597d5b2c1a1d44d98a408fb388266632.jpg)
图中只要已知VP和其中一个端点，其他端点都能被算出来。
图：比如当已知VP，图a)中给定1点，则2、4点可以通过线相交确定。
3) Get 3D box pose from 2D corners
得到了2D平面内的立方体顶点后，我们要估计其三维位姿。
将物体分成两类。
任意位姿物体:
使用PnP solver来求解通常的立体3D位姿和大小，由于单目尺度不确定性，我们需要确定一个尺度因子。数学上来说，立体的八个3D顶点在物体坐标系下是可描述的： [ ± d x , ± d y , ± d z ] / 2 \left[ \pm d_{x}, \pm d_{y}, \pm d_{z}\right] / 2 [±dx,±dy,±dz]/2， 变换到相机参考系是： R [ ± d x , ± d y , ± d z ] / 2 + t R\left[ \pm d_{x}, \pm d_{y}, \pm d_{z}\right] / 2+t R[±dx,±dy,±dz]/2+t。
我们可以选择4个相连的顶点如1,2,4,7，然后从上述3D交点投影到2D中去，如顶点1有:
p 1 = π ( R [ d x , d y , d z ] / 2 + t ) ) p_{1}=\pi\left(R\left[d_{x}, d_{y}, d_{z}\right] / 2+\mathbf{t}\right) ) p1=π(R[dx,dy,dz]/2+t))
因此每个点可以贡献两个约束，4个点即可估计9DOF的pose，除了scale.
地面物体: 地面物体的roll/pitch都是0. 可以大大简化上述计算。
我们不再需要用复杂的PnP Solver去求解，而是可以直接反向投影地面的顶点到三维地平面上，然后计算其他垂直的顶点。
比如上述在三维地平面上的顶点5，可以表达为 [n,m] ( 相机帧里的向量和距离 ), 相对应的三维顶点 P 5 P_5 P5是反向投影的射线 K − 1 p 5 K^{-1}p_5 K−1p5与地平面的交点:
P 5 = − m n ⊤ ( K − 1 p 5 ) K − 1 p 5 \mathbf{P}_{5}=\frac{-m}{\mathbf{n}^{\top}\left(K^{-1} p_{5}\right)} K^{-1} p_{5} P5=n⊤(K−1p5)−mK−1p5
该公式应该是空间直线与平面交点的表示公式，在 MVG 书里或许有。
更详细地投影过程在论文[27](Pop-up SLAM)中描述。尺度由相机高度决定。
4) Sample VP and Summary
由于VP通过物体旋转矩阵R决定。通过深度网络，可以直接预测VP。我们手动sample它们，并且根据评分排序它们。
( 这部分到底是如何生成的？意思是否是通过神经网络预测物体的旋转矩阵R来预测VP？）
在本文的实验中，我们仅仅考虑了地面物体.!
B. Proposal scoring
示意获取最佳的三维立方体假设。
![[论文]CubeSLAM: Monocular 3D Object SLAM_第4张图片](http://img.e-com-net.com/image/info8/4450637323af40008dacebfaff707146.jpg)
在图中，左图先提取了一些直线，右图分别是根据不同的直线生成的立体矩形假设，左上角是最佳的假设。每个不同假设可以得到不同的代价函数值。由下式计算：
E ( O ∣ I ) = ϕ dist ( O , I ) + w 1 ϕ angle ( O , I ) + w 2 ϕ shape ( O ) E(O | I)=\phi_{\text {dist}}(O, I)+w_{1} \phi_{\text {angle}}(O, I)+w_{2} \phi_{\text {shape}}(O) E(O∣I)=ϕdist(O,I)+w1ϕangle(O,I)+w2ϕshape(O)
其中 O = { R , t , d } O=\{R, \mathbf{t}, \mathbf{d}\} O={R,t,d}， R是旋转，t是平移，d是大小。下面分别介绍三种误差函数：
1) Distance error ϕ d i s t ( O , I ) \phi_{d i s t}(O, I) ϕdist(O,I):
2D立体边界应该与真实图像的边界匹配。通过Canny边缘检测，并生成 DT变换，对于每个可见的立体边，均匀sample 10个点，并综合计算所有距离值，然后除以2d矩形的对角线长度。
2) Angle alignment error ϕ angle ( O , I ) \phi_{\text {angle}}(O, I) ϕangle(O,I):
距离误差与false positive的边很敏感，比如物体表面的纹理。因此，我们也检测长线条段（用LSD检测），然后测量它们的角度是否跟VP点匹配。这些直线首先与3个VP点之一关联，基于点-线支撑关系。
对于每个VP，可以找到最远的两条线段，它们有最小和最大的倾角。
ϕ angle ( O , I ) = ∑ i = 1 : 3 ∥ ⟨ l i − m s , l i − m t ⟩ − ⟨ V P i , l i − m t ⟩ ∥ + ∥ ⟨ l i − n s , l i − n t ⟩ − ⟨ V P i , l i − n t ⟩ ∥ \begin{aligned} \phi_{\text {angle}}(O, I)=& \sum_{i=1 : 3}\left\|\left\langle l_{i_{-} m s}, l_{i_{-} m t}\right\rangle-\left\langle\mathrm{VP}_{i}, l_{i_{-} m t}\right\rangle\right\|+\\ &\left\|\left\langle l_{i_{-} n s}, l_{i_{-} n t}\right\rangle-\left\langle\mathrm{VP}_{i}, l_{i_{-} n t}\right\rangle\right\| \end{aligned} ϕangle(O,I)=i=1:3∑∥⟨li−ms,li−mt⟩−⟨VPi,li−mt⟩∥+∥⟨li−ns,li−nt⟩−⟨VPi,li−nt⟩∥
这里的含义应该是这两条线的交点应该是消影点VP.
那么关键在于为何点-线支撑关系能找到这样的两条最远线段呢. 参考文献给的[11]
3) Shape error ϕ s h a p e ( O ) \phi_{s h a p e}(O) ϕshape(O)
前两个的误差可以在2D图像平面内计算。但是，相似的2D立体端点可能生成完全不一样的3D立方体。我们添加一个误差来惩罚有巨大倾斜( 长宽比 ) 的比率。也可以添加更严格的先验，比如某些特殊种类物体的固定长宽值或比例。
ϕ shape ( O ) = max ( s − σ , 0 ) \phi_{\text {shape}}(O)=\max (s-\sigma, 0) ϕshape(O)=max(s−σ,0)
IV. Object SLAM
我们延伸单图像三维物体检测到多物体SLAM，来联合优化物体位姿和相机位姿。系统基于ORBSLAM2搭建，包含前端相机追踪和后端BA。
我们主要的挑战是完善BA来联合物体、点和相机位姿，本部分将详细介绍。
其他SLAM应用细节可以见VI-B， 静态物体在本部分介绍，动态物体将在下一节强调。
A. Bundle Adjustment Formulation
先放出完整的BA公式：
C ∗ , O ∗ , P ∗ = arg min { C , O , P } ∑ C i , O j , P k ∥ e ( c i , o j ) ∥ Σ i j 2 + ∥ e ( c i , p k ) ∥ Σ i k 2 + ∥ e ( o j , p k ) ∥ Σ j k 2 \begin{aligned} C^{*}, O^{*}, P^{*}=& \arg \min _{\{C, O, P\}} \sum_{C_{i}, O_{j}, P_{k}}\left\|\mathbf{e}\left(c_{i}, o_{j}\right)\right\|_{\Sigma_{i j}}^{2}+\\ &\left\|\mathbf{e}\left(c_{i}, p_{k}\right)\right\|_{\Sigma_{i k}}^{2}+\left\|\mathbf{e}\left(o_{j}, p_{k}\right)\right\|_{\Sigma_{j k}}^{2} \end{aligned} C∗,O∗,P∗=arg{C,O,P}minCi,Oj,Pk∑∥e(ci,oj)∥Σij2+∥e(ci,pk)∥Σik2+∥e(oj,pk)∥Σjk2
分别代表相机-物体，相机-点，物体-点的约束。
B. Measurement Errors
1) 相机-物体测量
物体和相机之间的测量误差有两种:
a ) 3D测量:
第一种是在3D物体检测是准确的时候使用的3D测量，比如使用RGBD相机时。检测的物体位姿，作为物体在当前相机帧的测量。为了计算测量误差，我们转换路标物体到相机帧，然后比较测量误差：
e c o − 3 D = [ log ( ( T c − 1 T o ) T o m − 1 ) s e 3 ∨ d − d m ] e_{c o_{-} 3 D}=\left[\log \left(\left(T_{c}^{-1} T_{o}\right) T_{o m}^{-1}\right)_{\mathfrak{s e}_{3}}^{\vee} \quad \mathbf{d}-\mathbf{d}_{m}\right] eco−3D=[log((Tc−1To)Tom−1)se3∨d−dm]
其中 log 将 SE3的误差映射到 6DOF的切向量空间，因而 e c o − 3 D ∈ R 9 e_{c o_{-} 3 D} \in \mathbb{R}^{9} eco−3D∈R9。为了改善鲁棒性，我们使用 Huber 鲁棒核函数。
注意这里在没有先验时有歧义，无法分清前向和后向。因而在优化时要沿着高度方向旋转0 ±90 180度来获得最小误差。
b) 2D测量:
对于2d测量，我们将立方体路标投影到图像平面来获得2D的bbox，然后与检测的bbox之间比较。简单来说，就是将8个顶点投影到平面内，寻找最小和最大的投影像素x,y坐标来建立一个矩形:
[ u , v ] min = min { π ( R [ ± d x , ± d y , ± d z ] / 2 + t ) ) } [u, v]_{\min }=\min \left\{\pi\left(R\left[ \pm d_{x}, \pm d_{y}, \pm d_{z}\right] / 2+\mathrm{t}\right)\right) \} [u,v]min=min{π(R[±dx,±dy,±dz]/2+t))}
[ u , v ] max = max { π ( R [ ± d x , ± d y , ± d z ] / 2 + t ) ) } [u, v]_{\max }=\max \left\{\pi\left(R\left[ \pm d_{x}, \pm d_{y}, \pm d_{z}\right] / 2+\mathbf{t}\right)\right) \} [u,v]max=max{π(R[±dx,±dy,±dz]/2+t))}
c = ( [ u , v ] min + [ u , v ] max ) / 2 s = [ u , v ] max − [ u , v ] min \begin{array}{l}{\mathbf{c}=\left([u, v]_{\min }+[u, v]_{\max }\right) / 2} \\ {\mathbf{s}=[u, v]_{\max }-[u, v]_{\min }}\end{array} c=([u,v]min+[u,v]max)/2s=[u,v]max−[u,v]min
c和s分别是矩形框的中心和边长。定义两个矩形框之间的误差为:
e c o − 2 D = [ c , s ] − [ c m , s m ] e_{c o_{-} 2 D}=[\mathbf{c}, \mathbf{s}]-\left[\mathbf{c}_{m}, \mathbf{s}_{m}\right] eco−2D=[c,s]−[cm,sm]
即直接让4D参数相减。
这个测量比起上述的3D测量不确定性更小，因为一般2D检测更加准确。但不同的三维实体也可能有相同的2D投影。
2）物体-点测量
若点P属于物体，则应该在3D物体内部。我们首先将点转换到立体坐标系，然后与立方体的大小比较来获得三维误差：
e o p = max ( ∣ T o − 1 P ∣ − d m , 0 ) e_{o p}=\max \left(\left|T_{o}^{-1} P\right|-\mathbf{d}_{m}, \mathbf{0}\right) eop=max(∣∣To−1P∣∣−dm,0)
使用max是鼓励点在立方体内部而非正好在表面。
3） 相机-点测量
我们在基于特征的SLAM中使用标准3D点重投影误差。不多赘述。
C. Data association
使用帧间的点跟踪来跟踪检测框。 考虑哪些检测框内离中心在10像素以内的点。哪些匹配点最多，则关联谁。
同时，对于匹配上的点过少的框( 使用对极几何搜索)，它们可以认为是动态物体而去除掉！！
V. DYNAMIC SLAM
下面将同时估计相机位姿和动态物体的轨迹。
基于几个基本假设：
- 刚体假设: 即物体上的点不会运动。
- 运动假设: 如速度模型; 对于车辆还有非完整轮式模型(nonholonomic wheel model)，即限制侧面滑动.
动态物体跟踪是一个大的话题。在这里简单提到，其核心仍然是追踪动态的点，只是将点聚集一组，彼此之间增加速度约束。
相机位姿也可以被动态元素约束。
1) 物体运动模型
使用 nonholonomic wheel model.
[ t x ′ t y ′ θ ′ ] = [ t x t y θ ] + v Δ t [ cos ( θ ) sin ( θ ) tan ( ϕ ) / L ] \left[\begin{array}{c}{t_{x}^{\prime}} \\ {t_{y}^{\prime}} \\ {\theta^{\prime}}\end{array}\right]=\left[\begin{array}{c}{t_{x}} \\ {t_{y}} \\ {\theta}\end{array}\right]+v \Delta t\left[\begin{array}{c}{\cos (\theta)} \\ {\sin (\theta)} \\ {\tan (\phi) / L}\end{array}\right] ⎣⎡tx′ty′θ′⎦⎤=⎣⎡txtyθ⎦⎤+vΔt⎣⎡cos(θ)sin(θ)tan(ϕ)/L⎦⎤
2) Dynamic point observation
动态点与关联的物体绑定，因而首先被变换到世界系，然后投影到图像平面。
e d p = π ( j T o i i P k , T c j ) − z k j e_{d p}=\pi\left(^{j} T_{o}^{i i} P^{k}, T_{c}^{j}\right)-z_{k j} edp=π(jToiiPk,Tcj)−zkj
C. Dynamic data association
由于匹配动态物体很麻烦，所以静态物体的关联方式不适用。典型方法是预测其投影位置，然后在附近搜索。
但是对于单目动态场景，准确预测物体运动很难，对极几何也不准。
特征点直接用2D KLT 稀疏光流算法来跟踪，不需要3D点位置。
像素跟踪后，动态特征的3D位置通过考虑物体运动完成三角化。
M 1 P 1 = z 1 M 2 Δ T P 1 = z 2 \begin{aligned} M_{1} P_{1} &=z_{1} \\ M_{2} \Delta T P_{1} &=z_{2} \end{aligned} M1P1M2ΔTP1=z1=z2
这是标准的两视图三角化问题.
KLT跟踪在像素位移太大时容易失败。因此我们直接使用视觉物体跟踪算法[40]，上一帧的检测框将被跟踪，并在这一帧预测位置。
VI. IMPLEMENTATIONS
A. Object detetion
使用 YOLO detector在室内，概率阈值0.25；使用MS-CNN在室外KITTI数据集，概率阈值0.5.它们在GPU上实时。
关于误差函数中各部分权重的调节，使用手动调节。因为物体检测的协方差不好测定。这里也是一个futrue work.
详细的实验不赘述，大家可以自行看论文。
一些有价值参考文献
| 标题 | 内容 |
|---|---|
| [12] Xiaozhi Chen, Kaustav Kundu, Ziyu Zhang, Huimin Ma, Sanja Fidler, and Raquel Urtasun. Monocular 3D object detection for autonomous driving. In IEEE Conference on Computer Vision and Pattern Recognition (CVPR), pages 2147–2156, 2016. | 提出了语义分割的cost function |
| [10] Jianxiong Xiao, Bryan Russell, and Antonio Torralba. Localizing 3D cuboids in single-view images. In Advances in neural information processing systems (NIPS), pages 746–754, 2012. | sample大量box时，各距离衡量方式 |
| [11] Varsha Hedau, Derek Hoiem, and David Forsyth. Thinking inside the box: Using appearance models and context based on room geometry. In European Conference on Computer Vision, pages 224–237. Springer,2010. | 点线支撑关系 |
| [14] Peiliang Li, Tong Qin, and andShaojie Shen. Stereo vision-based semantic 3D object and ego-motion tracking for autonomous driving. In The European Conference on Computer Vision (ECCV), September 2018. | 三维物体追踪 |
| [40] Jo˜ao F Henriques, Rui Caseiro, Pedro Martins, and Jorge Batista. Highspeed tracking with kernelized correlation filters. IEEE Transactions on Pattern Analysis and Machine Intelligence, 37(3):583–596, 2015. | 基本视觉物体跟踪算法 |
| [41] Joseph Redmon and Ali Farhadi. YOLO9000: better, faster, stronger. Computer Vision and Pattern Recognition (CVPR), 2017. | 室内物体检测: YOLO! |
| [42] Zhaowei Cai, Quanfu Fan, Rogerio Feris, and Nuno Vasconcelos. A unified multi-scale deep convolutional neural network for fast object detection. In ECCV, 2016. | 室外物体检测 |