CubeSLAM：单目 3D 物体检测与没有先验模型的 SLAM
Yang S, Scherer S. CubeSLAM: Monocular 3D Object Detection and SLAM without Prior Models[J]. arXiv preprint arXiv:1806.00557, 2018.
作者： YangShichao：个人主页 Google Scholar Github
卡内基梅隆大学机器人研究所：The Robotics Institute of CUM
演示视频：https://www.youtube.com/watch?v=QnVlexXi9_c

CubeSLAM: Monocular 3D Object Detection and SLAM without Prior Models

注：本文对应的是作者 2018 年 6 月份放在 arixv 上的预印版，正式发表是在 2019 年 4 月（CubeSLAM: Monocular 3D Object SLAM），有些改动，请注意对比。这也是我当时阅读的第一篇物体级 SLAM，现在回头看有些地方翻译和表述的也不太恰当。

注：🌐 Cube SLAM 系列论文，代码注释、总结汇总

0. 摘要

提出一种单张图像的 3D 立方体对象检测方法和没有先验目标模型的多视图物体SLAM，且这两种方法使相互促进的；
对于 3D 检测：从 2D 边界框和消失点采样生成高质量的立方体目标。
- 进一步对物体进行评分，并选择其与图像的边缘对齐。
物体 SLAM：提出具有新测量（novel measurement）功能的多视图 BA 约束，利用单视图的检测结果来联合优化相机位姿、物体和特征点；
- 与特征点相比，物体可以提供更多的集合约束和尺度一致性；
- 在公开数据集中不但得到了较好的位姿估计精度，还提高了 3D 物体的检测精度。

1. 简介

(1) 大多数现有的单目研究可以分别实现对象检测和 SLAM，并且还需要依赖可能不适用于一般环境的先验目标 CAD 模型。本研究在没有先验的情况下进行 3D 对象映射，并同时解决 3D 目标检测和多视图的物体 SLAM；
物体目标检测：
- 利用卷积神经网络（CNN）在大型数据集中检测具有不同大小和视点的不同 2D对象 ^[1]；
- 机器人，无人车中的目标检测 ^[2]；
在 SLAM 中经典的方法是跟踪视觉几何特征，比如点^[ORB-SLAM]、线 ^[4]、跨帧的平面，再通过 BA 约束最小化投影误差或光度误差；
以 3D 目标作为路标不仅可以起到上面几何特征的作用，还可以额外提供语义和几何约束来改善相机位姿。
(2) 本研究提出一个系统来同时实现 3D 目标检测和 SLAM：
- 鉴于2D物体检测，在物体投影后与 2D 边界框紧密贴合的假设下，通过消失点（VP）采样来生成多个有效的立方体方案；
- 利用多视图 BA 中的点和相机进一步优化所选的立方体方案姿态；
- 物体在两个地方被用到：为难以三角化的点提供初始深度；在 BA 中提供几何约束；
- 同时， SLAM 中的相机位姿可以用于改善单视图的目标检测。
(3) 本文贡献
- 提出一种针对单视图的有效且准确的三维立方体拟合方法，无需先前的对象模型或先前的方向和尺寸预测；
- 提出一种物体 SLAM ，在点、相机和物体之间产生新的度量方式。 > 实验证明，两者之间是相互促进的。

2. 相关研究

2.1 单视图 3D 物体检测

有先验 CAD 模型的方法：利用手工制造的特征 ^[5] 或深度网络 ^[6，7，8] 来找到与 RGB 图像对齐的最佳的目标位姿；
没有先验模型的情况：
- 早期方法几何建模，通过 VPs ^[9，10] 的曼哈顿边缘或光线的组合生成对象；
- 在地面上对多个 3D 盒子进行精确的采样，然后通过上下文特征进行选择 ^[11] ；
- 使用投影几何来找到与 2D 边界框紧密贴合的立方体 ^[12] 本研究中将其拓展到没有物体大小和方向预测的情况。

2.2 多视图 object SLAM

基于特征点的 SLAM ；
利用物体来增强建图，大致分为分离(decoupled )和耦合(coupled)方法：
分解方法：首先构建 SLAM 点云图，然后基于点云聚类和图像证据滤波（image evidence ltering ^{[14，15，16]} ）进一步检测和优化 3D 目标姿态；
- 与 2D 目标检测相比有明显的改进，但并不会改变 SLAM 的效果，因此如果SLAM无法构建高质量的地图，则可能无法正常工作;
耦合方法：通常称为对象级 SLAM
- 首个语义 SFM 来联合优化相机位姿、物体、点和平面测量 ^[17]；
- 使用 RGB-D 相机和先验目标 CAD 模型的 SLAM++ ^[18]；
- 将对象表示为球体来校正单目 SLAM 的尺度漂移 ^[19,20]；
- 使用先验数据库的实时单目 SLAM ^[21]；

3. 单视图目标检测

3.1 3D 物体提案生成

3D 立方体可用 9 个自由度表示：3个自由度表示位置，3个自由度表示旋转，3个自由度表示尺寸；立方体坐标系在其中心，与主轴对齐；
基于长方体的投影角应该与2D边界框紧密相关的假设，投影的四条边对应着 4 个约束，还不足以完全约束 9 个自由度，还需要其他信息，比如已提供的或检测到的物体的尺寸和方向；本文利用消失点来改变减少回归参数。
3D 立方体有三个正交轴，根据旋转 R 和相机参数 K，通过透视投影生成三个消失点^[10]；
故可以通过估计物体的三自由度旋转 R 来计算消失点，在立方体的上边缘采样一个角，然后结合三个消失点分析计算其他 7 个角。
> 如图 2 所示, a 为观察到三个面的情况，观察到7个点，并计算出 3 个消失点，假如对点 1 进行采样，可以通过 VP 线与矩形的射线相交确定角 2 和角 3，然后再确定角 4 及其他四个底角；同理吧，b,c 两种情况也一样，先采样一个点，通过消失点计算出其他 7 个点。
总结：首先估计物体三自由度的旋转和上边缘的一个角，然后计算所有其他 2D 点，并进一步进行反投影获得 3D 位置和尺寸。所以现在的问题是：如何获得物体的旋转和顶角？
- 有一种方式是利用深度网络通过大量的数据集直接训练预测得到，但本文为了充分利用 SLAM 优化后的几何约束，选择先进行详尽的采样，再进行评分选择最优的结果；
- 对于平放在地面的物体，用于计算 VP 的旋转可以用物体的偏航角（yaw）和相机的俯仰角（pitch）和滚转角（roll）来表示，并且相机的两个角参数可以通过 SLAM 或其他传感器来获得，以减少采样空间；比如在 KITTL 数据集中，相机几乎与地面平行，具有固定的俯仰和滚转角。

3.2 物体提案评分

对目标物体进行立方体采样有多种方式，如语义分割 ^[11]，边距 ^[5]，HOG 特征 ^[9]；本文提出一种快速有效的成本函数，使长方体与图像的边缘最佳对齐，此方法最适用于带有清晰边缘的四方物体，但由于消失点和 2D 方框也可适用于其他物体，此外 SLAM 为物体提供后期优化。
将图像定义为 I ，立方体方案定义为 x，定义成本函数： > 其中 \(\phi _{dist},\phi_{angle},\phi_{shape}\) 是三项成本项，\(w_1,w_2\)是后两者的权重，在经过小样本训练后得到经验值分别取0.7和2.5 \[ {\color{Blue}E(x|I)=\phi _{dist}(x) + w_1\phi_{angle}(x) + w_2\phi_{shape}(x) \quad (1)} \]
① 距离误差 \(\phi _{dist}\)
2D 长方体边缘应与实际图像的边缘匹配。利用 Canny 边缘检测方法构建距离图，然后再长方体边缘倒角距离（Chamfer distance进行累加求和，再通过 2D 框的大小进行归一化。
② 角度对齐误差 \(\phi _{angle}\)
由于距离误差对假阳性边缘噪声（例如物体表面纹理）非常敏感，故还需要检测线段并测量它们是否与 3.1 节立方体生成期间计算的消失点对齐，定义角度误差为： > 其中 \(p\) 是消失点VP，\(a,b\) 是检测到的线段，\(\theta\) 是两点之间的线角。 \[ \phi_{angle} = \left \| \theta (a,b)-\theta (p,b) \right \| \]
③ 形状误差 \(\phi _{shape}\)
前面两项可以在 2D 图像空间进行评估，但相似的长方体角可能会产生完全不同的 3D 立方体，提出一个代价来惩罚具有较大歪斜比（s=长度/宽度）的立方体，定义形状误差的代价函数为： > 其中 \(\sigma\) 是一个阈值，若歪斜比小于阈值则不会受到惩罚。 \[ \phi_{shape} = max(s - \sigma ,0) \]
如图 3 所示，是物体边缘对齐的评分，右图中左上角是最好的，右下角是最差的。

4. 物体 SLAM

物体 SLAM 基于特征点的 ORB-SLAM，包括相机跟踪的前端和 BA 约束的后端，本文的主要工作是修改了 BA 部分，将特征点、物体和相机位姿约束在一起。

4.1 BA 优化方案

BA 用于联合优化不同地图元素，包括相机位姿，点、线等。考虑一组相机位姿 \(C = \left \{ c_i \right \}\)，和一组 3D 对象路标 \(O = \left \{ o_j \right \}\)，还有一系列的特征点 \(P = \left \{ p_k \right \}\)（因为单独的物体还不足以完全约束相机位姿），可以将 BA 表示成如下的最小二乘问题： > 其中 e 是 C,O,P之间的测量误差，W 是不同误差的权重矩阵，很多现有的库可以解决优化问题，比如g2o和iSAM。

+ 参数：
+ 相机位姿表示为：\(T_{c} \subseteqq SE\left ( 3 \right )\)
特征点位姿表示为：\(P \subseteqq R_{3}\)
物体目标用 9 自由度表示为：\(O = \left \{ T_{o},D \right \}\)，其中 \(T_{o} \subseteqq SE\left ( 3 \right )\) 表示 6 自由度的位姿，\(D \subseteqq R_{3}\) 是 3 自由度的物体的尺寸。

4.2 测量误差

4.2.1 物体-相机测量

根据场景提出两种物体与相机之间的测量误差
第一种情形：3D 物体检测准确时使用的 3D 测量误差
定义来自相机坐标系测量的物体的位姿（本文中下标 m 表示测量量）：\(O_m = \left \{ T_{om},D_m \right \}\)
先将物体真实的位姿旋转至相机坐标系下，再与测量量比较，得到测量误差：
注意：在没有先前的对象模型的情况下，我们基于图像的立方体检测不能区分对象的正面和背面。例如，可以通过将对象坐标系旋转 90 度并交换长宽来表示同一个长方体。因此，需要将目标物体沿高度方向旋转 90 ， 0 和 180 度以得到公式（5）中的最小误差。
第二种情形：2D 测量

将立方体路标投影到图像平面，得到上面图 4 中的 2D 红色边框，然后将其与检测到的 2D 蓝色边框进行比较:
- \((c,d)\) 表示 2D 表框的中心（x1,x2）和尺寸(a,b)；
- 该测量对比公式（5）的 3D 测量具有更小的不确定性，因为 2D 测量更加准确；
- 但同时投影后会丢失信息，因为很多不同的 3D 立方体可能投影到同一个 2D 矩阵，只有一个观察面不足以完全约束相机和物体的姿态。
对象测量的不确定度和权重矩阵 w 要比点方法更复杂；本文简单地给予对象和附近物体更多的测量权重，假设，相机-物体之间的距离为 d ，物体的 2D 检测概率为 p，则权重为（在KITTI数据集中）： \[ {\color{Blue} w = p \times \max ((70 - d),0)/50} \]

4.2.2 物体-点测量

点与物体对象相互约束，如果点所属于物体，则它应位于 3D 长方体内，故可以先将点投影到立方体目标边框上，然后再与立方体尺寸进行比较，此处取 max 是希望点落在立方体内：

4.2.3 点-相机测量

特征点的测量误差部分采用标准的 3D 点重投影误差（参考 ORB-SLAM ^[2]）

4.3 数据关联

多帧之间关联基于目标要比基于特征点的关联更容易，因为包含更多的信息，并可以使用 2D 对象跟踪与匹配的方法，这样使得即使在简单地 2D 目标框重合的情况下也可以运行，但出现如下图 5 所示的严重遮挡情况会出现不稳定；
- 绿点是全局中地图特征点，其他颜色点与具有相同颜色的目标关联；其中正前方青色移动车没有添加为 SLAM 路标，因此没有与之关联的特征点；由于双关，对象重叠区域中的点不与任何对象相关联；
此外，需要从 SLAM 优化结果中检测和剔除动态目标，但是标准目标跟踪方法不能分类它是否是静态的，除非使用特定的运动分割。
因此，本文提出了另一种基于点匹配的目标关联方法；
许多基于点的 SLAM 可以通过描述符匹配和极线搜索来检测动态点，因此若观察到的点足够属于 2D 目标边界框并且接近 3D 空间中的立方体质心，则首先将点关联到对象；（一些最新的实例分割的方法也可用于改善点-对象的关联精度）；然后可以找到可共享地图点最多且超过阈值数（实验中设为10）的对象匹配；
- 此外，在第 4.2 节中 BA 期间计算目标物体与点的测量误差时，也使用折中目标-点的关联方法；
- 通过实验，这种方法适用于像图5中的宽基线匹配，重复对象，遮挡和动态场景。

5. 实验

5.1 实现

5.1.1 目标检测

对于 2D 目标检测，在室内场景使用概率阈值为 0.25 的 YOLO 检测器 ^[26]，在室外 KITTI 数据集中使用概率阈值为 0.5 的 MS-CNN ^[27] ，均可在 GPU 上实时运行；
检测过程：
① 由于数据集中已提供相机的准确位姿（若没有相机位姿，可以通过 SLAM 位姿估计来得到相机的滚转和俯仰角），只需要采样目标的偏航角 yaw 便可计算消失点 VP（对应 3.1 节）；
② 在立方体可以旋转的情况下，生成 15 个偏航角范围为 90 度的物体（对应 4.2 节）；
③ 然后在 2D 边界框的顶部采样 10 个点，注意并非所有的样本都可以生成有效的立方体方案，因为有些立方体的顶角可能在 2D 边界框之外；
本方法的一个优点是不需要有大量的训练数据，只需要调整公式（1）两个权重；

5.1.2 物体 SLAM

SLAM 算法流程如下图所示；
本文建立在 ORB-SLAM2 上，没有更改相机跟踪与关键帧筛选模块，在新创建在关键帧的基础上检测立方体物体，并使之关联，然后使用相机和特征点位姿进行 BA 调整；例如相机旋转较大的具有挑战性场景中的系统鲁棒性
当基线或视差小于阈值时，立方体还用于初始化难以三角测量的特征点的深度，可以在一些具有挑战性的场景中提高稳健性；
由于物体的数量远少于特征点，在实时运行时，物体关联和 BA 优化非常有效；
为了获取单目 SLAM 的绝对地图尺度，提供初始帧的相机高度来确定地图尺度。
本文的物体 SLAM 也可以在没有特征点的情况下独立工作，在一些缺少特征点的环境下，仍然可以通过物体-相机的测量来估计相机位姿。

5.2 单视图物体检测

数据集：具有真实轨迹 3D 边界框注释的 SUN RGBD ^[28] 和 KITTI ^[29] 数据集；
评估方法：采用 3D intersection over union (IoU) 算法（可用于评估两个多维度数据的相似度），而不是仅仅根据旋转和视差进行评估；
- 若 3D IoU 大于 25% ，视为阳性检测 ^[28，11]；
- 由于本方法不依赖先验目标模型，为获得目标位置和尺度的绝对比例，仅评估具有已知相机高度的地面目标；
  对于 KITTI 数据集均知道相机高度，对于 SUN RGBD 数据集，选择了 1670 个图像，可以在视野中完全可见地面对象和地平面

5.2.1 物体提案评估

首先使用参考论文 [30] 的方法训练和验证分析 KITTI 数据集上提案的质量；
- 在参考论文 [11] 中，首先尽量抽取大量的立方体提案（14k），然后通过语义分析等方法选择前 N 个提案召回；
如下图 6a 中红色线（难道不是绿色的线吗？？）所示，在评分之前，本方法可达到 90% 的召回率，每个图像有 800 个原始提案，每个目标约有 200 个原始提案；
在评分之后绿色的线（难道不是红色的线吗？？）所示，仅使用 20 个提案就达到了相同的召回率，比论文 [11] 的方法减少很多；
原因：
① 本文 3D 原始提案的质量很高，因为他们可以保证与 2D 检测到的边界框相匹配；
② 本文高效的评分机制。
注意本文方法有上限，因为在 2D 检测时可能会遗漏物体

5.2.2 最终检测

评估挑选出的最佳提案的最终准确性。
SUN RGBD 数据集
- 暂时未找到训练好的 3D 检测算法，本文在室内环境中比较两种公共方法 SUN primitive ^[9] 和 3D Geometric Phrases (3dgp) ^[31]；
- 在检测和反投影到 3D 空间时，修改了部分代码以使用实际的相机位姿和校准矩阵；
- 为消除 2D 检测器的影响，两种方法均仅评估在 2D 检测框的 IoU 大于 0.7 对应的物体的 3D IoU；
- 如上图 6b 和下图 7 所示，本文方法检测到更多更准确的立方体，所以更加稳健；平均的 3D IoU 与使用先验 CAD 模型的 3dgp 相比更小，但如果仅通过 3dgp 对相同的检测对象进行评估则更高。
KITTI 数据集
- 使用深度网络与其他两种单目算法 ^[12,30] 比较；
- 如图 6b 所示，本文方法与使用先验模型的 SubCNN 表现类似；直接预测车辆方向和尺寸的 [12] 表现更差；
- 由于只有一个具有固定相机视角的对象类“汽车”，因此 CNN 预测比手工设计特征更好；
- 在 6b 中最后一行，选择前 10 个立方体方案进行评估，表明在少数提案中也能有较高的质量。

5.3 物体 SLAM

物体 SLAM 的评估指标：相机位姿估计和 BA 优化之后的 3D 物体 IoU；
- 均方根误差（RMSE）和 KITTI 相对平移误差用于评估相机位姿；

5.3.1 TUM 和 ICL-NUIM RGBD 数据集

TUM 和 ICL-NUIM RGBD 数据集^[32，33] 提供相机 ground truth 轨迹（此处仅使用 RGB 图像）；使用深度图像构造全局点云，并手动标记 3D 立方体作为 ground truth 物体；
首先测试 TUM fr3 cabinet 序列，如图 8a 所示，这个具有挑战性的低纹理数据集，由于特征点少现有单目 SLAM 算法均失败；
利用 4.2 节中的方法，将 3D 物体作为唯一的 SLAM 路标进行物体-相机测量；在图 8a 左侧中使用 SLAM 估计的相机姿态在某些帧下检测到的立方体，可以看出底部边界框存在很大的测量误差，经过多视图优化，得到右侧图的红色立方体基于与真实点云匹配；
实验数据对比如图 8b 所示：
- 在图 1a 中的 ICL 数据的物体建图（其中Mesh网格模型仅用于显示，不用于检测）可以看出，本文方法检测除了不同的物体，展示了没有先前模型的3D检测的优势。

5.3.2 Collected chair 数据集

如图 9a,b 用 Kinect RGBD 相机收集两个椅子的数据集，RGBD ORB-SLAM 测量的相机位姿视为是相机的 ground truth；
图 9a ，在优化之后，立方体框与相关的 3D 特征点紧密联合；
图 9b ，相机旋转剧烈，其定量的误差测量在图 8b 的底部两行中， 3D IoU 也得到了改善；
- DSO 能在第一个数据集中运行，但在第二个数据集中效果糟糕，无法进行定量计算；
- 单目的 ORB-SLAM 在两个数据集中都无法进行初始化，本文方法的立方体检测可以从单个视图提供的深度点来初始化；

5.3.3 KITTI 数据集

测试两种KITTI数据集，一种是提供目标注释 ground truth 的短序列，另一种是没有目标注释的标准测距基准测试的长序列；
由于不确定度低，在 BA 期间的测量误差模型采用 4.2 节中 2D 相机-目标测量；
在于 ORB-SLAM 进行比较时，关闭了其闭环线程，以更好地测量单目尺度漂移；
通过第一帧相机的高度（实验中为 1.7 m）来缩放 ORB-SLAM 的初始地图，就可以直接评估绝对轨迹误差而不需要轨迹尺度对齐；
- 图 10 中，在第一次转弯之前的初始贵极端与 ground truth 匹配良好，说明尺度缩放正确；
在 KITTI 数据集中还使用先验的汽车尺寸（实验中为 w = 3.9 m；l = 1.6 m；h = 1.5 m）初始化目标尺寸，以保持长期的尺度一致性，在参考论文[19,20]中也使用这样的方法；特别是目标在一些序列中观察不到时尤为有用。
对于第一类提供目标注释的短序列，选择了 19 个 KITTI 原始序列，目标注释最多的是 2011_09_26 drive x 序列；
- 如图表 1，3D 目标 IoU 和相机位姿均得到改善；
- 尤其是相机位姿，物体 SLAM 提供的几何约束可以减少 SLAM 的尺度漂移，部分物体建图和位姿估计的可视化效果见图 1b 和图 10 所示。
对于第二类 KITTI 里程计数据集：
单目恢复尺度的方法：
- 文献[34,35]使用恒定的地面高度假设来减少尺度漂移；
- 文献[19,20]基于物体的尺度恢复方法；
没有与 ORB-SLAM 比较，因为没有闭环无法在长序列中恢复尺度，且存在如图 10 所示的漂移；
如表 2 所示，与使用物体恢复尺度的其他 SLAM 比较，它们将车辆表示为球体或仅使用车辆高度信息，没有本文使用的立方体准确；并且与基于地面高度缩放的方法相接近；
在 Seq 02, 06, 10 序列中表现更差是因为长距离可见的物体不多产生漂移；故提出一种地面高度假设与物体 SLAM 结合的方法，若再最近20帧中没有可见物体，会使用点云平面拟合，使用恒定地面高度假设来缩放相机位姿和局部地图；
- 如表 2 中的组合方法，在 KITTI 标准中达到了最精确地水平；
- 注意：基于地面高度的方法也有局限性，比如不适用于飞行器或手持相机，如果地面不可见，比如图 5 中的图像帧，会导致失败；前视车辆遮挡了地面信息，这也是很多方法在 KITTI 07 数据集上失败或表现不佳的原因。

6. 结论

① 研究内容：本文提出一种没有先验物体模型的单目三维物体检测与 SLAM 方法，并首次证明，语义目标检测与几何 SLAM 在一个统一框架中可以相互关联。
- 物体可以为点提供深度约束，为相机提供尺度约束，反过来，SLAM 也为 3D 物体检测提供初始化。
② 实验结果：本文在不同的室内和室外数据集上评估了以上两个功能，在 SUN RGBD 数据集上实现了最准确的目标检测，在 KITTI 里程计数据集上达到了最佳的位姿估计。
③ 未来展望：将来考虑动态目标和使用目标的稠密地图，将更完整的场景理解与 SLAM 优化相结合。

R. 参考文献

2018.11.30
wuyanminmax@gmail.com

📜 论文阅读 | CubeSLAM：单目 3D 物体检测与没有先验模型的 SLAM

文章目录