CubeSLAM

 

 

相关工作

单幅图像3D目标检测：与2D图像相比，从单张图像中检测3D物体要困难得多，因为需要考虑更多的物体姿态变量和相机投影几何。现有的三维检测方法可以分为两类:有或没有形状先验，如CAD模型。在有先验的模型中，可以通过关键点透视n点(PnP)匹配[7]、手工制作的纹理特征[8]或更近期的深度网络来找到与RGB图像对齐的最佳物体姿态[3,9]。

如果没有先验模型，对象通常用长方体来表示。典型的方法是将几何建模与学习相结合。例如，对象可以通过曼哈顿边缘或通过VPs的光线组合生成[10,11]。Chen等人提出对地面上的许多3D盒子进行详尽的采样，然后根据各种上下文特征[12]进行选择。与我们相似的两个工作是[13,14]，它使用射影几何来寻找长方体，以紧密匹配2D边框。我们将它扩展到不需要预测物体大小和方向的情况下。

多视角物体SLAM：耦合方法通常称为对象级SLAM。Bao等人提出了第一个语义SfM，用于联合优化相机姿态、对象、点和面[19]。Salas等人[20]提出了一个实用的SLAM系统，称为SLAM++，使用RGB-D相机和先前的物体模型。Frost等人将物体表示为球体，以纠正单目SLAM[21]的尺度漂移，[22]也是如此。最近，在[23]中提出了一种利用已有目标模型的实时单目目标SLAM方法。Rubino等人[24]解析求解了多视点三维椭球目标定位，QuadriSLAM[25]将其扩展为在线SLAM，无需事先建立模型。在[26]中解决了对象SLAM的不确定数据关联问题。Yang等人[27]也提出了类似的想法，将场景理解与SLAM相结合，但只适用于平面。最近，也有一些基于端到端深度学习的无对象表示的SLAM，如DVSO[28]、DeepVO[29]。他们已经在KITTI数据集上取得了出色的性能，但是，他们是否能推广到新的环境还不清楚。

动态环境SLAM：动态环境下的SLAM一直是一个具有挑战性的问题。现有的方法大多将动态区域特征视为异常值，仅利用静态背景进行姿态估计[4,30,31]。在解决静态SLAM问题后，还有一些工作是对动态物体的轨迹进行检测、跟踪和优化，以构建完整的三维地图[14,32,33]。该优化算法基于目标重投影误差、目标运动模型以及对目标的点特征观测。然而，由于缺乏可靠的静态背景特征，这些方法在高度动态的环境中可能会失败。

最近有一个利用动态点BA来改进相机姿态估计的工作，基于刚性形状和恒定运动假设[34]，然而，论文显示了有限的真实数据集结果，并没有明确地表示在地图上的对象

单幅图像3D目标检测

3D建议框生成

原理：不同于在三维空间中随机采样对象建议，我们利用二维包围盒来高效地生成三维长方体建议。一个3D包围盒可以用9DOF参数表示：3DOF位置，3DOF旋转R，3DOF维度。在长方体中心建立长方体坐标系，并与主轴对齐。相机内在标定矩阵K也是已知的。基于长方体的投影角与二维边框紧密贴合的假设，二维边框的四个边只有四个约束，因此不可能完全约束所有9个参数。其他信息是需要的，例如在许多车辆检测算法[12][14]中所提供或预测的对象尺寸和方向。我们不是依赖于预测的维数，而是利用VP来改变和减少回归参数，以便适用于一般物体。

注：VP点是指消影点，三维立体空间中，一组平行的直线不会相交，或者可以说相交于同一无穷远点，该无穷远点在相机的透视投影作用下会在图像平面中成像，成像点成为消影点。

VP是投影到透视图像[35]后的平行线交点。一个三维长方体有三个正交轴，可以在投影后形成三个VP，其依赖于相对相机坐标系的物体旋转矩阵R和标定矩阵K。

从VP得到2D角点：我们首先展示了如何根据VP得到8个二维长方体角点。由于最多可以同时观察到三个长方体面，我们可以根据图2所示的可观察面数量将长方体构型分为三种常见的类型。每种构型都可以是左右对称的。在这里，我们对图2(a)进行更详细的解释。

根据2D角点得到3D物体姿态：在得到二维图像空间中的长方体角点后，还需要对长方体的三维角点和姿态进行估计。我们将对象分为两种场景。

任意姿态物体：由于单目尺度模糊的问题，我们利用PnP求解器求解了一般长方体的三维位置和尺寸。数学上来说，立体的八个3D顶点在物体坐标系下是可描述的： ， 变换到相机参考系是：  。 我们可以选择4个相连的顶点如1,2,4,7，然后从上述3D交点投影到2D中去，如顶点1有:

π 为相机投影函数， 为八个2D物体角点的其中一个。每个点可以贡献两个约束，4个点即可估计9DOF的pose，除了scale. 在[35]中可以找到迭代或非迭代的PnP求解器。

地面物体：因为地面物体的roll/picth都是0，可以大大简化上述过程。与前一节类似，我们可以从VP获得8个2D角。那么，我们可以不使用公式2中复杂的PnP求解器，直接将地面角的像素反向投影到三维地平面上，然后计算其他垂直角，形成三维长方体。该方法是非常有效的，并具有解析表达式。比如上述在三维地平面上的顶点5，可以表达为 [n,m] ( 相机帧里的向量和距离 ), 相对应的三维顶点 是反向投影的射线  与地平面的交点: 类似地，[27]中解释了更详细的投影过程。尺度是由投影过程中的相机高度决定的。

采样VP点和总结：通过前两节的分析，框估计问题变成了如何得到三个VPs和一个顶部的2D角，因为我们得到VP后，可以使用Sec. III-A2来计算2D角，然后使用Sec. III-A3来计算3D框。

由式1可知，VP是由物体旋转矩阵r确定的。虽然深度网络可以通过大量的数据训练直接对其进行预测，但为了泛化，我们选择对其进行人工抽样，然后对其进行评分(排序)。

对于一般的物体，我们需要对全旋转矩阵R进行采样，而对于地物，我们使用相机的滚转/俯仰和物体的偏航来计算R。更重要的是，在SUN RGBD或KITTI等数据集中，已经提供了相机的滚转/俯仰。对于多视角视频数据，我们使用SLAM来估计摄像机的姿态。从而大大减小了采样空间，提高了采样精度。在本文的实验中，我们只考虑了地物。

建议框评分

在对多个长方体建议进行抽样后，我们定义损失函数来对其进行评分，如图3所示。不同的功能被提出，如语义分割[12]，边缘距离[8]，HOG特征[10]。我们提出了一些快速有效的损失函数来实现长方体与图像边缘特征的对齐。这种方法最适用于边缘清晰的四方形物体，但由于VP和鲁棒边缘滤波的约束，也可以很好地用于自行车和厕所等。

距离误差：二维长方体的边缘应该与实际图像的边缘相匹配。我们首先检测出Canny边缘，并在此基础上构建距离变换图。然后，对于每一个可见的长方体边(图2(a)中的蓝色实线)，我们在其上均匀采样10个点，并将所有的距离地图值除以2D方框的对角线。这类似于倒角距离[10]。

角度误差：距离误差对物体表面纹理等有噪声的假阳性边缘非常敏感。因此，我们也检测长线段[36](如图3中的绿线所示)，并测量其角度是否与消失点对齐。这些线首先与基于点线支持关系[11]的三个VPs中的一个相关联。对于每个VP，我们可以找到最远的两条线段，其倾角分别为最大和最小。

形状误差：前两种代价仅在二维图像空间中就能得到有效的评价。然而，相似的二维长方体角可能会产生截然不同的三维长方体。我们增加了一个成本来惩罚具有较大斜率(长/宽)的长方体。更严格的先验可以应用，例如特定类型的物体的估计或固定尺寸。

物体级SLAM

将单图像三维目标检测扩展到多视图目标SLAM，实现目标姿态和相机姿态的联合优化。该系统基于特征点ORB SLAM2[4]构建，包括相机跟踪前端和BA后端。我们的主要变化是修改的BA，包括物体，点和相机位姿一起，这将在本节详细解释。其他SLAM实现细节在第VI-B部分。本节将使用静态对象，下一节将讨论动态对象。

 

BA公式

BA可以表示为非线性最小二乘优化。∑ 为不同测量误差的协方差矩阵。

 是不同误差测量的协方差矩阵。变量和误差的定义在后续给出。然后优化问题将会通过许多库的高斯-牛顿或者LM算法解决，如g2o和iSAM。

然后用g2o[37]和iSAM[38]等库中可用的Gauss-newton或Levenberg- Marquardt算法求解优化问题。

 

测量误差

 

数据关联

跨帧的数据关联是SLAM的另一个重要部分。与点匹配相比，由于包含了更多的纹理，并且可以使用多种2D对象跟踪或模板匹配方法，因此对象关联似乎更容易。甚至2D框重叠也可以在一些简单的场景中工作。但是，如果存在严重的重复物体遮挡，如图5所示，这些方法就不具有鲁棒性。另外，目前的SLAM优化需要检测和移除动态对象，但标准的目标跟踪方法无法对其是否是静态的进行分类，除非使用特定的运动分割。

因此，我们提出了另一种基于特征点匹配的对象关联方法。对于许多基于点的SLAM方法[4]，通过描述子匹配和极几何检查，可以有效地匹配不同视点的特征点。因此，如果在至少两帧的2D对象边界框中观察到点，并且它们到长方体中心的三维距离小于1m，我们首先将特征点与它们对应的对象关联起来。例如在图5中，特征点与它们关联的对象具有相同的颜色。需要注意的是，在计算Eq. 11中BA的物点测量误差时，也使用了这种物点关联。最后，我们在不同帧中匹配两个对象，如果它们之间共享特征点的数量最多，并且数量也超过一定的阈值(在我们的实现中是10个)。通过我们的实验，该方法适用于宽基线匹配、重复对象和遮挡。针对运动对象的动态特征点不能满足极线约束的问题，将其剔除。因此，关联特征点较少的对象被认为是动态对象，如图5中前面的青色汽车。

动态SLAM

前一节讨论静态对象SLAM。在本节中，我们提出了一种联合估计相机姿态和动态目标轨迹的方法。我们的方法对对象做了一些假设，以减少未知的数量，使问题可以解决。两种常用的假设是物体是刚性的，并遵循某种物理上可行的运动模型。刚体假设表明一个点在其相关物体上的位置不会随时间改变。这使得我们可以利用标准的3D地图点重投影误差来优化其位置。对于运动模型，最简单的形式是匀速恒速运动模型。对于一些特定的物体，如车辆，它被额外地约束跟随非完整车轮模型(有一些侧滑)。

动态目标估计的因子图如图6所示。蓝色节点是静态SLAM组件，红色节点代表动态对象、点和运动速度。绿色方块是测量因子，包括Eq. 10中的相机-物体因子，Eq. 14中的物体-速度因子，Eq. 15中的点-相机-物体因子，如下所述。有了这些因素，相机的姿态也可以受到动态元素的限制。

 

SLAM优化

 

 

 

动态数据关联

通过实验，我们发现IV-C节中针对静态环境的关联方法由于难以匹配动态点特征而不适用于动态情况。跟踪特征点的典型方法是预测其投影位置，搜索临近的特征匹配描述符，然后检查极线几何约束[4]。然而，对于单目动态情况，很难准确预测物体和点的运动，当物体运动不准确时，极线约束也不一定准确。

因此，我们为点和对象的关联设计了不同的方法。二维KLT稀疏光流算法直接跟踪特征点，不需要三维点位置。像素跟踪后，根据目标的运动情况，对动态特征的三维位置进行三角化。

当像素位移很大时，KLT跟踪仍然可能失败，例如当另一辆车接近和靠近摄像头时。因此，对于动态目标跟踪，我们不采用第IV-C节中的共享特征点匹配方法。相反，我们直接利用视觉目标跟踪算法[40]。跟踪物体的2D边框，根据前一帧预测其位置，然后与当前帧中重叠率最大的检测到的边框进行匹配。

 

实现

物体检测

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

如果一个精确的相机姿态是已知的，例如在SUN RGBD数据集，我们只需要采样物体的偏转来计算VPs，如第III-A4节所解释的。如IV-B1节所述，长方体可以旋转，生成了物体偏航角在90°范围内的15个样本。然后在2D边界盒的上边缘采样10个点。注意，并不是所有的样本都可以形成有效的长方体建议，因为一些长方体角可能位于2D盒子的外面。在没有地面真实相机姿势提供的情况下，我们采样相机滚动/俯仰在20°范围内围绕最初估计的角度。对于没有先验信息的单一图像，我们简单地估计相机与地面平行。对于多视角场景，SLAM用于估计相机姿态。我们的方法的一个优点是它不需要大量的训练数据，因为我们只需要调整Eq. 4中的两个代价权重。它还可以实时运行，包括二维物体检测和边缘检测。

物体SLAM

整个SLAM算法的流程如图4(a)所示。正如在第四节中提到的，我们的系统是基于ORB SLAM2的，我们没有改变相机跟踪和关键帧创建模块。对于新创建的关键帧，我们检测长方体对象，关联它们，然后执行捆绑调整相机的姿态和点。对于动态对象，我们可以根据不同的任务选择重构或忽略它们。当立体基线或视差角小于阈值时，长方体还可用于初始化难以三角化的特征点的深度。这可以提高在一些具有挑战性的场景下的鲁棒性，如在实验中被控制的大型摄像机旋转。由于对象的数量远远少于点数，对象关联和BA优化实时高效运行。为了获得单目SLAM的绝对地图比例尺，提供了初始帧的相机高度来比例尺地图。注意，我们的对象SLAM也可以在没有点数的情况下独立工作。在一些具有挑战性的环境中，很少的特征点，ORB SLAM不能工作，但我们的算法仍然可以估计相机的姿态仅使用物体相机测量。

在优化过程中有不同的代价(见第四节)，有些代价在像素空间，如Eq. 10，有些代价在欧氏空间，如Eq. 8和11，因此需要对它们之间的权值进行调整。由于长方体检测不确定度分析比较困难，我们主要通过检查测量的数量和大小来手动调整目标成本权重，使不同类型的测量贡献大致相同。例如，与点相比，只有很少的物体，但它们在Eq. 10中的重投影误差要比点大得多。从我们的实验来看，物体相机和点相机的测量具有相似的权重。

 

动态物体

动态对象的实现主要遵循前一节的内容，但有一些不同。恒定运动模型假设可能不适用于实际数据集，因为物体可能加速或减速(如图13所示)。通过地面真实物体的速度分析，我们发现速度在大约5秒内大致保持不变。因此，在我们的SLAM中，运动模型约束只适用于最近5秒的观测。

实验

检测：在KITTI数据集上，我们使用深度网络与其他单目算法[2,12,13]进行了比较。SubCNN附加地使用先前的模型。预测结果由作者提供。AP是在中等级别的汽车上进行评估的。从表I可以看出，我们的方法与SubCNN和Mono3d的性能相似。由于SubCNN产生了很多假阳性检测，因此它们的AP值很低。最好的方法是Deep3D[13]，它使用深度网络直接预测车辆的方向和尺寸。因为只有一个对象类car具有固定的相机姿势和对象形状，CNN预测比我们手工设计的特征更好。最后一行是我们选择的前10个长方体提案的评估，以显示我们的提案生成部分仍然可以生成高质量的提案。

物体SLAM：然后，我们对目标SLAM的性能进行了评估，包括相机姿态估计和BA优化后的三维目标IoU。我们证明了在不同的数据集中，SLAM和目标检测可以相互受益。均方根误差(RMSE)[47]和KITTI平移误差[44]用于评估相机姿态。注意，虽然我们的算法是单目SLAM，但我们可以从第一帧s相机高度得到地图比例尺，因此，我们直接评估绝对轨迹误差，而不按比例尺对其进行对齐。为了更好地评估单眼姿态漂移，我们在使用和比较ORB SLAM时关闭loop closure模块。

结论

在这项工作中，我们传达了一个一般的方法，单目三维目标检测和SLAM映射，没有先验的目标模型，在静态和动态环境。更重要的是，我们首次证明了语义对象检测和几何SLAM可以在一个统一的框架中相互受益。

针对单幅图像三维目标检测，提出了一种基于消失点的二维边界盒快速生成高质量长方体目标的新方法。然后，通过图像提示对提案进行有效的评分。在SLAM部分，我们提出了一种基于摄像机、物体和点之间的测量功能的物体级SLAM，并提出了新的目标关联方法，以健壮地处理遮挡和动态运动。目标可以为相机姿态估计提供远距离的几何和尺度约束。反过来，SLAM也为检测和优化3D对象提供了相机姿态初始化。对于动态场景，我们也证明了在新的测量约束下，通过紧耦合优化，运动目标和点也可以改善相机的姿态估计。

我们在不同的室内和室外数据集上对这两部分进行了评价，在SUN RGBD子集数据和KITTI测程数据集上实现了最佳的三维目标检测精度和相机姿态估计。在未来，我们也对使用对象的密集映射感兴趣。更完整的场景理解也可以与SLAM优化相结合。

思考

框架为ORB-SLAM2，包括相机跟踪前端和BA后端。我们的主要变化是修改BA。先利用神经网络检测物体，然后根据2D框恢复3D结构。然后利用物体-点，物体-相机，点-相机的测量误差做BA。然后做数据关联。对于动态物体，假设物体是刚性的，并且服从特定的运动模型，如匀速运动模型。然后观测动态点，进行动态数据关联。

问题

1、2D物体检测只用于恢复三维结构,没有场景地图。

2、只考虑了在地面上的物体，对于这种吊灯，树叶的一部分场景不能处理。

3、没有回环检测

4、测试都是在数据集上进行的，没有实际场景运行。

5、是否可以考虑融合语义信息