[论文笔记|DOS]CubeSLAM: Monocular 3D Object SLAM

摘要

大多数现有的SLAM方法都假设环境是静态的或基本上是静态的。动态区域的特征常被当作异常值来处理，而不用于相机姿态估计，但这一假设在许多实际环境中可能不成立。在这项工作中，在动态情况下不再将运动的物体视为异常点，而是基于动态点观测和运动模型约束，共同优化相机和运动物体的轨迹----->将2D、3D物体检测和SLAM姿态估计融合在一起以适用于静态和动态环境。

本文提出了一种用于静态和动态环境下的单目图像3D长方体物体检测和多视点对象SLAM，并证明了这两部分可以相互改进。具体步骤为:

1. 首先，对于单目图像物体检测，我们从2D边界框和消失点采样中生成高质量的长方体。
2. 然后,基于与图像边缘的对齐，进一步对这个生成的3D长方体进行评分和选择。
3. 其次，提出了一种多视点BA，以实现对相机、目标和点的姿态优化。对象可以提供远程几何(long-range geometric)和尺度约束，以提高相机姿态估计和减少单目漂移。

没有将动态区域视为异常值，而是利用目标表示和运动模型约束来改进相机姿态估计。在SUN RGBD和KITTI上进行的三维检测实验表明，与现有方法相比，该方法具有更好的准确性和鲁棒性。在公共空间、KITTI和我们自己收集的数据集上，我们的SLAM方法实现了最先进的单目相机姿态估计，同时提高了三维物体检测的精度。

贡献如下：

• 提出了一种高效、准确、鲁棒的单目图像3D长方体检测方法。
• object SLAM方法具有相机、目标和点之间的测量，在许多数据集上实现了更好的姿态估计。
• 结果表明，物体检测与SLAM是相适应的。
• 提出了一种利用运动物体改进动态场景姿态估计的方法。

Ⅰ. 单图像3D目标检测

A. 3D box proposal generation

1) Principles:

本文利用2D边界框有效地生成3D长方体proposals，而不是在三维空间中随机采样目标proposals。

一般的3D长方体可以用9个自由度参数表示:3自由度的位置$t=[t_x,t_y,t_z]$、3自由度的旋转$R$、以及三自由度的长宽高dimensions：$d=[d_x,d_y,d_z]$。长方体坐标系建在长方体中心，与主轴对齐。相机内参矩阵K也是已知的。由于一个前端矩形检测的四个端点只能提供4个约束，因而需要其他的信息。如：提供或预测的物体大小，以及朝向。(在许多车辆检测算法中用到)

不依赖于预测的dimensions，我们利用消失点VP来改变和减少回归参数，以适应一般对象。

VP是平行线在透视图像上投影后的交点。3D长方体有三个正交的轴线，根据物体相对于相机坐标系下的旋转R和标定内参矩阵K，进行投影后可以形成三个VP：

其中，$R_{col(i)}$是第$i$个R的列。这部分的解析可以看我的另一篇博客——利用消失点进行相机标定

2) Get 2D corners from the VP

如何基于VP得到8个2D长方体顶点?由于最多可以同时观察到三个长方体面，我们可以根据图2所示的可观察面的数量将长方体结构分为三种常见的类别。每个结构都可以是左右对称的。

图2:2D目标框生成长方体proposals。如果估计了三个消失点和一个顶点，其他七个顶点也可以通过解析计算得到。如(a)中给定顶点1，则顶点2、顶点4可以通过直线交点确定，其他顶点也一样。

假设已知或估计了三个VP和顶点$p_1$, x表示两条直线的交点，则$p_2=\overline{(V{P}_1,p_1)}\times \overline{(B,C)}$…以此类推…

3) Get 3D box pose from 2D corners:

得到了2D平面内的长方体顶点后，我们要估计其三维位姿。我们将物体分为两种情况。

• 任意位姿物体：由于单目尺度的不确定性，使用PnP来求解一般长方体的3D位置和尺寸。
  在数学上，长方体在物体坐标系下的的八个3D顶点为$[\pm d_x,\pm d_y,\pm d_z]/2$，转换到相机坐标系为:$R[\pm d_x,\pm d_y,\pm d_z]/2+t$。如图2(a)所示，我们可以选择四个相邻的顶点，例如1、2、4、7，然后从上述3D顶点投影到2D中去，如顶点1有:
  

其中，$\pi$是相机映射函数，$p_i$是八个2D物体顶点之一。每个顶点提供了两个约束，这样四个顶点可以完全约束对象的姿态(9自由度)，除了尺度。找到迭代或非迭代PnP求解器进行求解。

• 地面物体：在地平面上建立世界坐标系，然后物体的滚/俯仰角度为0。与上一节类似，我们可以从VP中得到8个2D角。然后，不利用公式(2)中复杂的PnP求解器，直接将地面的顶点像素反投影到三维地平面，然后再计算其他垂直角，形成3D长方体。例如,上述在三维地平面上的顶点5可以表示成[n, m](相机帧里的法线和距离), 相对应的三维顶点$P_5$是反向投影的射线$K^{-1}{p}_5$与地平面的交点：
  

该公式应该是空间直线与平面交点的表示公式。比例是由投影过程中的相机高度决定的。

4) Sample VP and Summary

从前两节的分析来看，边界估计的问题变成了如何得到三个消失点VP和一个2D顶点。因为在得到VP之后，我们可以用Ⅰ- A2来计算2D顶点，然后用Ⅰ- A3来计算3D边界框。由公式(1)可知，VP由物体旋转矩阵R确定，为了便于推广，我们选择手动对它们进行采样，然后对它们进行评分(排序)。

在本文的实验中，作者仅仅考虑了那些位于地面的物体

B. Proposal scoring

通过上述获得很多长方体候选后，设置损失函数对其评分。

图3.对长方体方案评分。左图先提取了一些直线，用于给方案打分。右图分别是根据不同的直线生成的立体矩形假设(2D青色边框)。左上方是最好的，右下方是最差的。每个不同假设可以得到不同的代价函数值。

把图像表示为$I$，依照$I-A(1)$长方体方案表示为 O = { R , t , d } O = \{R, t, d\} O={R,t,d}，则损失函数可以定义为：

其中，${\phi }_{dist},{\phi }_{angle},{\phi }_{shape}$是三种不同类型的损失函数；$w_1,w_2$作为损失函数的权重，在小样本数据集上手动搜索后分别设置为0.8，1.5。

1）距离误差${\phi }_{dist}(O,I)$

2D长方体边应与实际图像边匹配。首先检测Canny边缘并基于其建立距离变换映射。然后，对于每条可见的长方体边(图2(a)中的实心蓝线)，我们均匀地在其上采样10个点，并综合计算所有与对应的Canny边缘的距离值，然后除以2d矩形的对角线长度。

2）角度对准误差${\phi }_{angle}(O,I)$

检测长线段（用LSD检测）(如图3中的绿线所示)，测量其角度是否与消失点对齐。这些线首先与基于点-线支持关系的三个VPs之一关联。然后对于每个$V{P}_i$，我们可以找到斜率最小和最大的两条最外侧线段，分别表示为$<l_{i\_ms}，l_{i\_mt}>$和$<l_{i\_ns}，l_{i\_nt}>$。$<a,b>$表示端点为(a, b)的直线的倾斜角。最后角度对准误差为：

3) 形状误差${\phi }_{shape}(O)$

相似的2D长方体顶点会产生不同的3D长方体。我们增加了一个成本来惩罚长方体与一个大的倾斜(长/宽)比率。

其中，$s=max(d_x/d_y,d_y/d_x)$是物体的倾斜比率；$\sigma$是阈值且在实验中设置为1。如果s <$\sigma$，则不惩罚。

Ⅱ. object SLAM

我们将单目图像的三维物体检测扩展到多视图物体级别的SLAM，共同优化目标姿态和相机姿态。系统构建在基于特征点的ORB SLAM2，其中包括相机跟踪前端和BA后端。这篇论文完善了BA来联合物体、点和相机位姿。本节讨论静态物体，下一节讨论动态物体。

A. Bundle Adjustment Formulation

将一组相机姿态，3D长方体和点分别表示为 C = { C i } , O = { O j } , P = { P k } C = \{C_i\}, O = \{O_j\}, P = \{P_k\} C={Ci​},O={Oj​},P={Pk​}，BA可以表述为一个非线性最小二乘优化问题：

其中$e(c,o)、e(c,p)、e(o,p)$分别表示相机-物体、相机-点、物体-点的测量误差。$\Sigma$是不同的误差测量的协方差矩阵。然后用g2o等库中的Gauss-newton或LM算法求解优化问题。

Notations:

• $T_c\in SE(3)$：相机位姿
• $P\in R^3$： 点
• O = { T o , d } O = \{T_o, d\} O={To​,d}：长方体对象建模为9自由度的参数。其中，$T_o=[R|t]\in SE(3)$是6自由度的相机位姿，$d\in R^3$是长方体维数。
• 下标m表示测量值。坐标系如图4(b)所示。
  

图4:(a)我们的object SLAM的流程。单视图目标检测为SLAM提供了长方体路标和深度初始化，SLAM则可以估计相机姿态，从而实现更精确的目标检测。(b)显示BA期间相机、物体和点之间的坐标关系和测量误差。

B. Measurement Errors

1）相机-物体

测量误差有两种。

（a）3D测量

第一种是在3D物体检测是准确的时候使用的3D测量，比如使用RGBD相机时。Section I-A中单幅图像检测到的物体位姿$O_m=(T_{om},d_m)$，作为来自相机帧的物体测量。为了计算其测量误差，将路标物体变换到相机坐标系中，然后与测量结果进行比较：

将Huber鲁棒成本函数应用于所有测量误差，以提高鲁棒性。

需要注意的是，在没有先验时有歧义，无法区分物体的正面或背面。例如，我们可以通过旋转物体坐标系90◦和交换长度和宽度值来表示同一个长方体。因此，我们需要沿着高度方向旋转0，±90,180，在Eq. 8中找到最小的误差。

（b）2D测量

在二维测量中，我们将长方体路标点投影到图像平面上，得到如图4(b)中红色矩形所示的二维边界框，然后与检测到的蓝色的二维边界框进行比较。更详细地说，我们将8个顶点投影到图像上，找到投影像素的最大和最小x、 y坐标，形成一个矩形：

其中，$[u,v{]}_{min,max}$为八个投影顶点的最小/最大xy坐标，即投影矩形的左上角和右下角。c和s是2D方框的中心和大小。它们都是二维向量，因此$[c,s]\in R^4$。直接让4D参数相减，将4D矩形误差定义为:

与公式(8)中的3D误差相比，这种测量误差的不确定性要小得多，因为2D物体检测通常比3D检测更精确。这类似于将地图点投影到图像上以表示重投影误差。但是由于许多不同的三维长方体可以投射到同一个二维矩形上，因此需要更多的观测来充分约束相机的姿态和长方体，因此在投影后会丢失信息。

2）物体-点

如果点P属于图4(b)中所示的物体，它应该位于3D长方体的内部。首先将点变换到长方体坐标系，然后与长方体维度(长宽高)进行比较，得到三维误差:

使用max是鼓励点在立方体内部而非正好在表面。

3) 相机-点

在基于特征的SLAM中，我们使用了标准的三维点重投影误差：

其中，$z_m$为3D点$P$观测到的像素坐标。

C. Data association

提出了一种基于特征点匹配的目标关联方法。对于许多基于点的SLAM方法，通过描述子匹配和对极几何搜索，可以有效地匹配不同视图中的特征点。

1. 如果在至少两帧的2D物体边界框中观察到的点，且其与长方体中心的3D距离小于1m，则首先将特征点与对应的物体关联起来。例如在图5中，特征点与其关联物体的颜色相同。注意，在计算公式(11)中BA期间的物体-点测量误差时，也使用了这种物体-点关联。
2. 匹配不同帧中的两个物体，如果它们之间共享特征点的数量最多，并且数量超过了一定的阈值(在我们的实现中是10个像素)。

哪些匹配点最多，则关联谁。

同时，运动物体的动态特征点由于不能满足外极约束而被丢弃。对于匹配上的点过少的框( 使用对极几何搜索)，它们可以认为是动态物体而去除掉！！比如图5中的前青色汽车。

图5:KITTI 07动态和遮挡场景下的物体关联。绿色点为非物体点，其他颜色的点与相同颜色的物体相关联。前青色移动的汽车不作为SLAM路标点，因为没有特征点是与它相关。物体重叠区域内的点由于模糊性不与任何物体相关联。

Ⅲ. DYNAMIC SLAM

上一节处理静态物体SLAM。在本节中，我们提出一种联合估计相机姿态和动态物体轨迹的方法。

首先对物体做一些假设，以减少未知的数量，使问题可解：

• 物体是刚性的，并且遵循某种物理上可行的运动模型。刚体假设表明，点在其相关物体上的位置不随时间变化。这允许我们利用标准的3D地图点重投影误差来优化它的位置。
• 运动假设: 匀速恒运动模型; 对于车辆还有非完整轮式模型(nonholonomic wheel model)，即限制侧面滑动.

A. Notations

在上一节A中，除了静态SLAM之外，还定义了一些新的地图元素。对于动态物体$O^i$，我们需要估计其在每一观察帧$j$中的姿态${}^j{O}^i$。我们使用动态点来表示与运动物体相关的特征点。对于运动物体$O_i$上的动态点$P_k$，将其固定在物体上的位置表示为${}^i{P}^k$(根据刚性假设其是固定的)。它的世界位姿将随时间而改变，不适合SLAM优化。

图6:动态对象SLAM。蓝色节点表示静态SLAM部分，红色节点表示新的动态变量。绿色方块是动态地图的新要素，包括运动模型约束和物体与点的观测。

B. SLAM optimization

动态物体估计的因素图如图6所示。蓝色节点代表静态SLAM部分，红色节点代表动态物体、点和运动速度。绿色方块是测量因子，包括Eq. 10中的相机-物体因子，Eq. 14中的物体-速度因子，以及Eq. 15中的点-相机-物体因子。有了这些因素，相机的姿态也可以受到动态元素的约束。

1) Object motion model:

采用非完整车轮模型(non-holonomic wheel model)，汽车运动用线性速度$v$和转向角$\phi$表示。假设飞行器在局部平面上近似运动，则物体的roll/pitch=0,z轴平移量$t_z=0$。只需要$t_x,t_y,\theta$(heading yaw)来表示其完整状态$T_o=[R(\theta )|[t_x,t_y,0{]}^{\prime }]$。由速度预测的物体运动状态为：

其中$L$为前后轮中心之间的距离。注意，这个模型要求$x,y,\theta$ 在后轮中心被定义，而物体坐标系在车辆中心被定义。两个坐标系有需要补偿的$L/2$偏移量。最终运动模型误差为：

2) Dynamic point observation:

如前所述，动态点被固定在与它相关联的物体上，因此它首先被转换为世界坐标系，然后投影到相机上。假设第$k$个点在第$i$个物体上的局部位置为${}^i{P}^k$，第$j$个图像中物体的位姿为${}^j{T}_o^i$，则该点的重投影误差为：

其中$T_c^j$为第$j$个相机位姿，$z_{kj}$为该点观测像素。

C. Dynamic data association

点和物体关联方法:特征点由二维KLT稀疏光流算法直接跟踪，不需要三维点位置。像素跟踪后，考虑到物体的运动，将动态特征点的3D位置三角化。数学上，假设两个坐标系的投影矩阵为$M_1,M_2$，在两帧的3D点位置分别为$P_1、P_2$，对应的像素观测值分别为$z_1、z_2$。两帧之间的物体运动变换矩阵为$∆T$，则可以推断$P_2=∆T{P}_1$。根据投影规则，可以得到：

如果我们把$M_2∆T$看作一个补偿了物体运动的修正相机位姿，上面的方程就是标准的两视图三角化问题，可以用奇异值分解SVD来求解。

当像素位移较大时，KLT跟踪仍然可能失败。因此，对于动态物体跟踪，我们没有使用第Ⅱ-C节中的共享特征点匹配方法。相反，我们直接使用视觉目标跟踪算法，上一帧的检测框将被跟踪，并在这一帧预测位置。