# CubeSLAM

> Yang 2019 · [Paper](https://arxiv.org/abs/1806.00557)

**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](orb-slam.md)
- [Pop-up SLAM](pop-up-slam.md)
- [SLAM++](../level-04-rgbd-slam/slampp.md)
- [DynaSLAM II](dynaslam-ii.md)
- [VDO-SLAM](vdo-slam.md)