CubeSLAM: Monocular 3D Object SLAM 论文笔记

CubeSLAM: Monocular 3D Object SLAM 论文笔记

摘要

提出了一种静态和动态环境下单幅图像3D长方体目标检测和多视点目标SLAM的方法，并证明了这两个部分是相互促进的。

• 首先，对于单图像目标检测，我们通过2D包围盒和消失点采样生成高质量的长方体轮廓。基于与图像边缘的对齐，进一步对方案进行评分和选择。

• 其次，提出了结合新的物体测量值的多视点捆绑平差（BA）方法，以联合优化摄像机、物体和点的姿态。物体可以提供远距离的几何和比例约束，以改进摄像机的姿态估计，减少单目漂移。

• 我们没有把动态区域当作异常值来处理，而是利用对象表示和运动模型约束来改进摄像机姿态估计。

图1：单目3D对象检测和映射，不需要先前的对象模型。网格模型仅用于可视化，不用于检测。

(A)包含各种对象的ICL NUIM数据，其位置、方向和尺寸通过SLAM进行优化。

(B)Kitti 07。在有物体约束的情况下，我们的单目SLAM可以建立一致的地图并减少比例尺漂移，而不需要回环检测和固定摄像机高度的假设。

单幅图像3D目标检测

A. 3D边界框生成

1）原理：

该算法不是在3D空间中随机采样物体轮廓，而是利用2D包围盒高效地生成3D长方体轮廓。一般的三维长方体可以用9个DOF参数来表示：3个位置自由度t=[tx，ty，tz]，3个旋转自由度R和3个维度自由度d=[dx，dy，dz]。长方体坐标系建立在长方体中心，与主轴对齐。摄像机内参矩阵K也是已知的。基于长方体的投影角应该与2D边界框紧密匹配的假设，2D框的四个边只有四个约束，因此不可能完全约束所有9个参数。还需要其他信息，例如在许多车辆检测算法[12]-[14]中使用的所提供或预测的对象尺寸和方向。我们不依赖于预测的维度，而是利用VP来改变和减少回归参数，以便对一般对象起作用。

VP是投影到透视图像之后的平行线的交点。3D长方体有三个正交轴，根据对象旋转R wrt，投影后可以形成三个VP。摄像机框架和校准矩阵K：

其中Rcol(i)为R的第i列

2）从VP中获取2维角点（corners）：

我们首先展示如何得到基于VP的八个2D长方体角点。由于最多可以同时观测到三个长方体的面，我们可以根据图2所示的可观察面的数量将长方体的构型分为三类。每种构型都可以是左右对称的。这里我们更详细地解释图2(A)。

图2：从2D对象框生成长方体边界框。如果估计了消失点（不知道这个是不是计算机视觉中的多视图几何里面说的消影点vanishing point）和一个角，则其他七个角也可以解析计算。例如，在(A)中，给定角1、角2和角4可以通过直线相交来确定，并且与其他角相同

假设已知或估计了三个VP和顶角p1，且×表示两条直线的交点，则p2=(VP1，p1)×(B，C)，p4类似。P3=(VP1，p4)×(VP2，p2)，p5=(VP3，p3)×(C，D)，其余的角也是如此。

3）从2D角点获取3D长方体姿势：

在获得2D图像空间中的长方体角点后，我们需要估计长方体的3D角点和姿势。我们将对象分为两个场景。

任意姿势对象 ：由于单目尺度的模糊性，我们使用PnP解算器来解算一般长方体的3D位置和尺寸。在数学上，长方体在对象框中的八个3D角是[±dx，±dy，±dz]/2，在相机框中是：R[±dX，±dy，±dz]/2+t。如图2(A)所示，我们可以选择四个相邻的角，如1，2，4，7，它们可以从上面的3D角投影，例如角1：

π是相机投影函数，pi (i = 1，2…8)是2D对象的八个角之一。每个角提供两个约束，因此四个角可以完全约束对象姿态(9 Dof ),除了比例。迭代或非迭代PnP求解器可以在[Multiple view geometry in computer vision]（计算机中的多视图几何）中找到。

同样，在[27]中解释了更详细的预测过程。比例由投影过程中的相机高度确定。

4）示例VP和摘要：

从前两节的分析中，3D边界框估计问题变成了如何获得三个VP和一个顶部2D角，因为在我们获得VP之后，我们可以使用2）计算二维角，然后使用3）计算3D长方体。

根据公式1，Vp由对象旋转矩阵R确定。虽然深度网络可以通过大量的数据训练来直接预测它们，但出于泛化的目的，我们选择手动采样它们，然后对它们进行评分(排名)。

对于一般物体，我们需要采样全旋转矩阵R，而对于地面物体，使用和采样相机的滚动/俯仰和物体的偏航来计算R。更重要的是，在诸如SUN RGB-D或KITTI的数据集中，已经提供了相机的滚动/俯仰。对于多视点视频数据，我们使用SLAM来估计摄像机的姿势。因此，采样空间大大减小，也变得更加准确。在本文的实验中，我们只考虑地面物体。

B. 方案评分

在采样了许多长方体方案后，我们定义了代价函数来对它们进行评分，如图3所示。不同的函数已经被提出，如语义分割[12]、边缘距离[8]、HOG特征[10]。我们提出了一些快速有效的代价函数来使长方体与图像边缘特征对齐。这种方法最适用于“边缘清晰的方形物体”，但也适用于自行车和厕所（toilet）等，由于来自VP和鲁棒边缘滤波的约束，后面的实验显示了这一点。

我们首先将图像表示为 I，将长方体方案（proposal）表示为O={R，t，d}，定义在上一节的1)中，则将成本函数定义为:

图3：长方体提案得分。(左)用于为方案打分的边缘。(右)从相同的2D青色边界框生成的长方体方案。

得分后左上角是最好的，右下角是最差的。

2D长方体边缘应与实际图像边缘匹配。我们首先检测Canny边缘，并在此基础上建立距离变换图。然后，对于每个可见的长方体边缘(图2(a)中的蓝色实线)，我们对其上的10个点进行均匀采样，并汇总所有距离贴图值除以2D长方体的对角线。这类似于倒角距离[10]。

仅在2D图像空间中就可以有效地估计前两种代价。然而，相似的2D长方体拐角可能会生成完全不同的3D长方体。我们增加了一项成本，以惩罚具有较大歪斜(长/宽)比的长方体。可以应用更严格的先验，例如特定类型物体的估计或固定尺寸。

在我们的实验中，s=max(dx/dy，dy/dx)是物体的偏斜率，σ是设置为1的阈值。如果s<σ，则不适用任何处罚。

目标/对象SLAM （OBJECT SLAM）

我们将单图像3D目标检测扩展到多视角目标SLAM，以联合优化目标姿态和摄像机姿态。该系统建立在基于特征点的ORB SLAM2[4]基础上，包括摄像机跟踪前端和BA后端。我们的主要更改是修改了BA，将对象、点和相机姿势一起包括在一起。

静态对象在本节中，动态对象将在下一节中介绍。

A. BA公式

BA是联合优化不同地图组件的过程，如相机姿势和点。在我们的大多数实验中也使用点，因为对象本身通常不能完全约束相机姿势。如果将摄像机姿势、3D长方体和点的集合分别表示为C={Ci}、O={Oj}、P={Pk}，则BA可以表示为一个非线性最小二乘优化问题：

其中e(c，o)，e(c，p)，e(o，p)分别表示摄像机-对象、摄像机-点、对象-点的测量误差。Σ是不同误差测量的协方差矩阵。变量和错误的定义如下所示。然后，可以用G2O[37]和ISAM[38]等许多库中提供的高斯-牛顿或Levenberg MarQuardt算法来解决优化问题

在一些环境中，例如Kitti，我们也可以使用提供的对象尺寸，这样就不需要d来进行优化。下标m表示测量值。坐标系如图4(B)所示。

图4 ：(A) 我们的对象SLAMpipeline。单视图目标检测为SLAM提供了长方体地标和深度初始化，而SLAM则可以估计摄像机的姿态，从而更准确地检测目标。(B)显示BA过程中摄像机、对象和点之间的坐标系和测量误差。

B. 测量误差

1）相机-对象测量：我们提出了物体和摄像机之间的两种测量误差。

​ a）3D测量：第一种是当3D对象检测准确时使用的3D测量，例如，如果使用RGBD传感器。来自单幅图像检测的被检测物体姿势Om=(Tom，dm)用作来自相机帧的物体测量。为了计算其测量误差，我们将地标对象变换到相机帧，然后与测量结果进行比较：


需要注意的是，如果没有先前的对象模型，我们的基于图像的长方体检测无法区分对象的正面或背面。例如，我们可以通过将对象坐标系旋转90◦并将长度与宽度值互换来表示相同的长方体。因此，我们需要沿高度方向旋转0，±90◦，180◦才能找到最小误差在公式8。

​ b）2D测量：对于2D测量，我们将长方体地标投影到图像平面上，以获得如图4(B)中红色矩形所示的2D边界框，然后将其与蓝色检测到的2D边界框进行比较。更详细地说，我们将八个角投影到图像上，并找到投影像素的x、y坐标的最小值和最大值，以形成一个矩形：

其中，[u，v]min，max是所有八个投影角的最小/最大XY坐标，即投影矩形的左上角和右下角。C和S是2D长方体的中心和大小。它们都是二维矢量，因此[c，s]∈R4。

4D矩形误差被定义为：

与公式8中的3D误差相比，该测量误差的不确定度要小得多。因为与3D检测相比，2D对象检测通常更准确。这类似于将地图点投影到图像上以形成重投影误差。然而，投影后也会丢失信息，因为许多不同的3D长方体可以投影到同一个2D矩形上，因此需要更多的观测来完全约束相机的姿势和长方体。

由于复杂的检测过程，与点相比，建模和估计误差协方差Σ或Hessian矩阵W并非易事。

因此，我们简单地给语义可靠和几何接近的对象更多的权重。假设长方体-相机距离为d，物体的2D探测概率为p，那么我们可以在KITTI数据上定义

其中70m为截断距离。参数可能因数据集不同而不同。

2）对象-点测量：

点和对象可以相互提供约束。如果点P属于图4(B)中所示的对象，则它应该位于3D长方体的内部。因此，我们首先将点变换到长方体框架，然后与长方体的维度进行比较，得到三维误差：

其中使用了max函数，因为我们只鼓励点位于长方体内部，而不是精确地位于曲面上。

3）相机-点测量：

我们在基于特征的SLAM中使用标准的三维点重投影误差[4]。

其中，zm为3D点P的观测像素坐标

C. 数据关联

跨帧的数据关联是SLAM的另一个重要部分。

提出了另一种基于特征点匹配的对象关联方法。对于许多基于点的SLAM方法[4]，通过描述符匹配和对极几何检验，可以有效地匹配不同视图中的特征点。

因此，如果在2D对象边界框中观察到点至少两帧，并且它们到长方体中心的3D距离小于1m：

• 我们首先将特征点与其对应的对象相关联。例如，在图5中，特征点与其相关联的对象具有相同的颜色。请注意，当在公式11中计算BA期间的对象-点测量误差时，也使用该对象-点关联。
• 如果两个对象之间的共享特征点数量最多，并且该数量也超过了某个阈值(在我们的实现中为10)，则我们将匹配不同帧中的两个对象。

通过实验，该方法对宽基线匹配、重复目标和遮挡等情况都有很好的效果。属于移动对象的动态特征点被丢弃，因为它们不能满足核线约束。因此，具有较少关联特征点的对象被认为是动态对象，例如图5中的前青色汽车。

kitti07动态和遮挡场景中的对象关联

绿点是非对象点，其他颜色的点与相同颜色的对象相关联。前青色移动的汽车不会被添加为SLAM地标，因为没有与其关联的特征点。由于模糊性，对象重叠区域中的点不与任何对象相关联。

动态SLAM

在这一部分中，我们提出了一种联合估计摄像机姿态和动态对象轨迹的方法。我们的方法对对象做了一些假设，以减少未知数，使问题可解。

两个常用的假设是对象是刚性的，并且遵循某种物理上可行的运动模型。

刚体假设表示点在其关联对象上的位置不会随时间变化。这使得我们可以利用标准的3D地图点重投影误差来优化其位置。

对于运动模型，最简单的形式是匀速恒定运动模型。对于一些特定的对象，如车辆，它还被约束为遵循非完整车轮模型(带有一些侧滑)。

A. 符号

新的地图元素

B. SLAM优化

图6 动态对象SLAM。

• 蓝色节点表示静态SLAM组件
• 红色节点表示新的动态变量(动态对象、点和运动速度)
• 绿色方块是动态地图的新要素，包括运动模型、约束和对物体和点的观测。

动态目标估计的因子图如图6所示。

绿色方块是测量系数，包括等式10中的相机-对象系数。方程式14中的物体速度因子,以及等式15中的点-相机对象因子。

有了这些因素，相机姿势也可以受到动力学元素的约束。

1）对象运动模型：

一般的3D对象运动可以由姿势变换矩阵T∈SE(3)来表示。我们可以对前一个姿势应用T，然后计算当前姿势的姿势误差。

这里，我们采用一种更受限制的非完整车轮模型[39]，该模型也用于其他动态车辆跟踪工作[14]。


其中L是前后轮中心之间的距离。注意，这个模型要求x，y，θ定义在后轮中心，而我们的对象框架定义在车辆中心。两个坐标框架具有需要补偿的L/2偏移。最终的运动模型误差简单地为:

2）动态点观测：

动态点被锚定到其关联的对象，因此它首先被变换到世界帧，然后被投影到相机上。

那么该点的重投影误差为:

C. 动态点关联

跟踪特征点的典型方法是预测其投影位置，搜索附近的特征匹配描述符，然后检查极线几何约束

然而，对于单目动态情况，很难准确预测对象和点的运动，并且当对象运动不准确时，对极几何也不准确。

因此，我们为点和对象的关联设计了不同的方法。

2D KLT稀疏光流算法直接跟踪特征点，不需要三维点的位置。在像素跟踪之后，考虑到对象移动，动态特征的3D位置将被三角测量。数学上，假设两帧的投影矩阵是M1，M2。这两个帧中的3D点位置是P1、P2，对应的像素观察值是z1、z2。两帧之间的物体运动变换矩阵是△T，那么我们可以推断P2 =△T P1。基于投影规则，我们有:

如果我们将M2△T视为补偿物体运动的修正相机姿态，则上述方程是标准的两视图三角测量问题[35],可通过奇异值分解SVD解决。

当像素位移较大时，KLT跟踪可能仍然会失败，例如当另一辆车靠近摄像机时。因此，对于动态目标跟踪，我们不使用Sec中的共享特征点匹配方法。相反，我们直接利用视觉对象跟踪算法[40]。跟踪对象的2D边界框，并根据前一帧预测其位置，然后将其与当前帧中检测到的具有最大重叠率的边界框进行匹配。

Implementation

A. 目标检测

对于2D物体检测，我们使用概率阈值为0.25的YOLO检测器[41]用于室内场景，使用概率阈值为0.5的MS-CNN [42]用于室外KITTI。两者都在GPU上实时运行。

如果一个精确的相机姿态是已知的，例如在SUN RGBD数据集中，我们只需要采样物体偏航来计算VPs

由于长方体可以如第IV -B1（object SLAM中的 measurement误差）节所述进行旋转，因此产生了15个90°范围内的物体偏航样本。然后在2D边界框的顶部边缘采样十个点。请注意，并不是所有的样本都能形成有效的长方体方案，因为一些长方体的角可能位于2D盒之外。在没有提供地面实况摄像机姿态的场景中，我们在最初估计的角度周围20°范围内对摄像机滚动/俯仰进行采样。

对于没有先验信息的单幅图像，我们简单地估计相机与地面平行。对于多视图场景，SLAM用于估计相机姿态。我们的方法的一个优点是，它不需要大量的训练数据，因为我们只需要调整等式4中的两个成本权重。它还可以实时运行，包括2D对象检测和边缘检测。

B. Object SLAM

整个SLAM算法的流水线如图4(a)所示。

系统是基于ORB SLAM2的，我们没有改变相机跟踪和关键帧创建模块。对于新创建的关键帧，我们检测长方体对象，关联它们，然后使用相机姿势和点执行光BA优化。对于动态对象，我们可以根据不同的任务选择重建或忽略它们。当双目基线或视差角小于阈值时，长方体还用于初始化难以三角测量的特征点的深度。

由于对象的数量远小于点，因此对象关联和BA优化实时高效地运行。为了获得单目SLAM的绝对地图比例，提供了初始帧的相机高度来缩放地图。注意，我们的object SLAM也可以不用点独立工作。在一些具有很少特征点的挑战性环境，ORB SLAM无法工作，但我们的算法仍然可以仅使用物体相机测量来估计相机姿态。

C. 动态对象

恒定运动模型假设对于实际数据集可能不成立，因为对象可能加速和减速(例如在图13中)。通过地面真实物体速度分析，我们发现速度大致在5秒左右保持不变。因此，在我们的SLAM中，运动模型约束仅应用于最后5秒的观察。