CubeSLAM

Yang 2019 · 论文

一句话总结 — 将3D立方体物体地标引入单目SLAM，提供了点无法给出的长距离几何与尺度约束，同时对动态物体进行建模而非将其剔除。

问题

单目SLAM存在尺度模糊性并会在长轨迹上累积漂移，而且大多数系统假设场景是静态的，将动态区域的特征当作外点处理。同时，单目3D物体检测本身也很困难，因为单一视角对物体位姿的约束不足，大多数方法还需要先验的CAD/形状模型。CubeSLAM的洞见在于，这两个问题可以互相解决：物体能提供点所不具备的尺度和长距离约束，SLAM的多视角一致性可以改善单视角检测，而运动物体可以被建模而非丢弃——论文证明了”这两部分可以相互提升”。

方法与架构

该系统基于ORB-SLAM2构建（跟踪和关键帧创建部分保持不变）；每个新关键帧都会进行立方体检测、物体关联，然后对相机、物体和点进行联合BA。

单图立方体检测。 一个立方体具有9个自由度：位置 $\mathbf{t}$、旋转 $R$、尺寸 $\mathbf{d}=[d_x,d_y,d_z]$。由于一个2D边界框只提供四个约束，候选方案是通过采样消失点生成的——$\text{VP}_i=KR_{col(i)},\ i\in\{1,2,3\}$——再加上一个顶部角点；其余七个角点通过直线相交求得。对于地面上的物体，一个落地角点像素 $p_5$ 可以解析地反投影到地平面 $[\mathbf{n},m]$ 上：

$$\mathbf{P}_{5}=\frac{-m}{\mathbf{n}^{\top}(K^{-1}p_{5})}K^{-1}p_{5}$$

其尺度由相机高度确定。候选方案通过 $E(O|I)=\phi_{dist}(O,I)+w_{1}\phi_{angle}(O,I)+w_{2}\phi_{shape}(O)$ 打分——即立方体边到Canny边缘的Chamfer距离、长线段与消失点的对齐程度，以及一个偏斜比惩罚项（$w_1=0.8$，$w_2=1.5$）。

物体BA。 给定相机 $C$、立方体 $O$、点 $P$，BA最小化

$$C^{*},O^{*},P^{*}=\underset{\{C,O,P\}}{\arg\min}\sum\parallel\mathbf{e}(c_{i},o_{j})\parallel_{\Sigma_{ij}}^{2}+\parallel\mathbf{e}(c_{i},p_{k})\parallel_{\Sigma_{ik}}^{2}+\parallel\mathbf{e}(o_{j},p_{k})\parallel_{\Sigma_{jk}}^{2}$$

• 相机-物体，3D形式：$e_{co\_3D}=[\log\big((T_{c}^{-1}T_{o})T_{om}^{-1}\big)^{\vee}_{\mathfrak{se}_{3}}\quad\mathbf{d}-\mathbf{d}_{m}]\in\mathbb{R}^9$（由于正面/背面难以区分，还测试了 $0,\pm90°,180°$ 的偏航角翻转）；2D形式：投影八个角点，取其外接矩形的中心/尺寸 $[\mathbf{c},\mathbf{s}]\in\mathbb{R}^4$ 进行比较，$e_{co\_2D}=[\mathbf{c},\mathbf{s}]-[\mathbf{c}_{m},\mathbf{s}_{m}]$——信息量较小，但不确定性也小得多。
• 物体-点：物体上的点必须位于物体内部，$e_{op}=\max(|T_{o}^{-1}P|-\mathbf{d}_{m},\mathbf{0})$。
• 相机-点：标准重投影 $e_{cp}=\pi(T_{c}^{-1}P)-z_{m}$。

关联通过跨帧共享特征点进行物体匹配（至少10个共享点，且这些点位于立方体中心1米以内）——对遮挡和重复物体具有鲁棒性。立方体还能为难以三角化的点初始化深度。

动态物体。 刚性运动物体的点保持锚定在物体坐标系中；一个非完整轮式模型预测 $[t_x^{\prime},t_y^{\prime},\theta^{\prime}]=[t_x,t_y,\theta]+v\Delta t\,[\cos\theta,\ \sin\theta,\ \tan\phi/L]$，给出一个运动模型误差 $e_{mo}$，而动态点则加入重投影因子 $e_{dp}=\pi({}^{j}T_{o}^{i}\,{}^{i}P^{k},T^{j}_{c})-z_{kj}$，因此运动中的汽车反而能约束相机。动态点通过KLT跟踪，并用运动补偿后的投影矩阵进行三角化。

实验结果

• 单视图检测：在SUN RGB-D子集上，3D IoU为0.39，而SUN Primitive为0.36（在3dgp检测到的图像上，二者分别为0.45和0.42）；在2D IoU为0.6的条件下，约50个候选方案即可达到约90%的3D候选召回率。在KITTI上，仅用20个打分候选方案即可达到90%的召回率（Mono3D需要约14000个候选方案）；前10个候选方案可达到0.38 IoU / 0.75 AP。
• 物体SLAM作为相互增益：在TUM fr3_cabinet（低纹理——现有单目SLAM均失败）上，仅物体SLAM达到0.17 m位姿误差，且经BA后物体IoU从0.46提升到0.64。在10个KITTI原始序列上，平均物体IoU从0.42提升到0.47，平移误差从4.95%（ORB，无回环检测）降至1.62%。
• KITTI里程计基准：基于物体的CubeSLAM达到平均平移误差2.74%，而此前基于物体的尺度校正方法（Frost等）为9.75%；结合地平面尺度校正后达到1.78%——在当时是无回环检测、且不假设恒定相机高度条件下最先进的单目精度。
• 动态KITTI序列：相机平移误差平均为3.42，而ORB（无回环检测）为4.96，动态物体3D IoU从0.32提升到0.39，运动车辆深度估计误差平均为4.9%，而此前基于立体的跟踪方法为6.8%/7.9%；在分段恒定运动假设下，速度估计能够跟踪真值。

对SLAM的意义

CubeSLAM开创了不依赖先验物体模型的物体级单目SLAM，证明了语义物体检测与几何SLAM能够相互增益：SLAM的多视角一致性能提升3D物体检测，而物体则能减小SLAM的漂移和尺度误差。它将动态物体作为一等公民进行处理的思路，预示了后来的动态SLAM系统，如DynaSLAM II和VDO-SLAM，其立方体表示与QuadricSLAM的椭球体表示一起，共同定义了”物体即地标”的研究方向。

相关条目

• ORB-SLAM
• Pop-up SLAM
• SLAM++
• DynaSLAM II
• VDO-SLAM