CubeSLAM

Yang 2019 · Paper

One-line summary — Introduced 3D cuboid object landmarks into monocular SLAM, providing long-range geometric and scale constraints that reduce drift, while modelling dynamic objects rather than rejecting them.

Problem

Monocular SLAM suffers from scale ambiguity and accumulating drift over long trajectories, and most systems assume static scenes, treating dynamic-region features as outliers. Meanwhile monocular 3D object detection is hard because a single view under-constrains object pose, and most methods need prior CAD/shape models. CubeSLAM’s observation is that the two problems solve each other: objects carry scale and long-range constraints that points do not, SLAM’s multi-view consistency improves single-view detection, and moving objects can be modelled instead of discarded — the paper demonstrates “that the two parts can improve each other.”

Method & architecture

Built on ORB-SLAM2 (tracking and keyframe creation unchanged); each new keyframe gets cuboid detection, object association, then a BA over cameras, objects, and points.

Single-image cuboid detection. A cuboid has 9 DoF: position $\mathbf{t}$, rotation $R$, dimensions $\mathbf{d}=[d_x,d_y,d_z]$. Since a 2D bounding box gives only four constraints, proposals are generated by sampling vanishing points — $\text{VP}_i=KR_{col(i)},\ i\in\{1,2,3\}$ — plus one top corner; the remaining seven corners follow by line intersections. For ground objects, a ground corner pixel $p_5$ back-projects onto the ground plane $[\mathbf{n},m]$ analytically:

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

with scale fixed by camera height. Proposals are scored by $E(O|I)=\phi_{dist}(O,I)+w_{1}\phi_{angle}(O,I)+w_{2}\phi_{shape}(O)$ — Chamfer distance of cuboid edges to Canny edges, alignment of long line segments with VPs, and a skew-ratio penalty ($w_1=0.8$, $w_2=1.5$).

Object BA. With cameras $C$, cuboids $O$, points $P$, BA minimises

$$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}$$

• Camera–object, 3D form: $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$ (tested over $0,\pm90°,180°$ yaw flips since fronts/backs are indistinguishable); 2D form: project the eight corners, take the bounding rectangle’s center/size $[\mathbf{c},\mathbf{s}]\in\mathbb{R}^4$ and compare, $e_{co\_2D}=[\mathbf{c},\mathbf{s}]-[\mathbf{c}_{m},\mathbf{s}_{m}]$ — less informative but far less uncertain.
• Object–point: points on an object must lie inside it, $e_{op}=\max(|T_{o}^{-1}P|-\mathbf{d}_{m},\mathbf{0})$.
• Camera–point: standard reprojection $e_{cp}=\pi(T_{c}^{-1}P)-z_{m}$.

Association matches objects across frames via shared feature points (≥10 shared points, points within 1 m of the cuboid center) — robust to occlusion and repeated objects. Cuboids also initialise depth for hard-to-triangulate points.

Dynamic objects. Rigid moving objects keep their points anchored in the object frame; a nonholonomic wheel model predicts $[t_x^{\prime},t_y^{\prime},\theta^{\prime}]=[t_x,t_y,\theta]+v\Delta t\,[\cos\theta,\ \sin\theta,\ \tan\phi/L]$, giving a motion-model error $e_{mo}$, and dynamic points add reprojection factors $e_{dp}=\pi({}^{j}T_{o}^{i}\,{}^{i}P^{k},T^{j}_{c})-z_{kj}$, so moving cars constrain the camera. Dynamic points are tracked by KLT and triangulated with motion-compensated projection matrices.

Results

• Single-view detection: on SUN RGB-D subset, 3D IoU 0.39 vs 0.36 for SUN Primitive (0.45 vs 0.42 for 3dgp on its detected images); ~90% 3D proposal recall with about 50 proposals at 2D IoU 0.6. On KITTI, recall of 90% with just 20 scored proposals (Mono3D samples ~14k); top-10 proposals reach 0.38 IoU / 0.75 AP.
• Object SLAM as mutual benefit: on TUM fr3_cabinet (low texture — existing monocular SLAM all fail), object-only SLAM reaches 0.17 m pose error and lifts object IoU from 0.46 to 0.64 after BA. On 10 KITTI raw sequences, mean object IoU rises 0.42 → 0.47 and translation error drops from 4.95% (ORB, no loop closure) to 1.62%.
• KITTI odometry benchmark: object-based CubeSLAM achieves 2.74% mean translation error vs 9.75% for prior object-based scale correction (Frost et al.); combined with ground-plane scaling it reaches 1.78% — state-of-the-art monocular accuracy at the time, without loop closure or a constant camera-height assumption.
• Dynamic KITTI sequences: camera translation error 3.42 vs 4.96 (ORB no-LC) mean, dynamic-object 3D IoU 0.32 → 0.39, and moving-car depth estimation error 4.9% mean vs 6.8%/7.9% for prior stereo-based trackers; velocity estimates track ground truth under a piecewise-constant motion assumption.

Why it matters for SLAM

CubeSLAM pioneered object-level monocular SLAM without prior object models, showing that semantic object detection and geometric SLAM are mutually beneficial: SLAM’s multi-view consistency improves 3D object detection, and objects reduce SLAM drift and scale error. Its treatment of dynamic objects as first-class citizens foreshadowed later dynamic-SLAM systems such as DynaSLAM II and VDO-SLAM, and its cuboid representation, together with QuadricSLAM’s ellipsoids, defined the object-as-landmark research direction.

Related

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