UcoSLAM

Muñoz-Salinas 2019 · Paper

One-line summary — Fuses natural keypoints with squared planar fiducial markers (ArUco) in one SLAM framework, combining the reliability and metric scale of markers with the coverage of natural features.

Problem

Most SLAM systems use natural landmarks such as keypoints, but these are “unstable over time, repetitive in many cases or insufficient for a robust tracking (e.g. in indoor buildings)”, and BoW relocalization has limited performance under viewpoint changes and repetitive patterns. Marker-only SLAM (the authors’ earlier SPM-SLAM) gives unambiguous data association and correct metric scale, but requires at least two markers visible per image, so it cannot scale to large environments. UcoSLAM integrates both landmark types in a single map: scale is fixed as soon as one marker is seen, markers can be placed only at strategic locations, and keypoints provide coverage everywhere else.

Method & architecture

The map is $\mathcal{W}=\{\mathcal{K},\mathcal{P},\mathcal{M},\mathcal{G},\mathcal{D}\}$: keyframes $\mathcal{K}$, triangulated map points $\mathcal{P}$, markers $\mathcal{M}$ (each with side length $s$, pose $\mathrm{M}\in\mathbb{SE}(3)$, and four known corners), a covisibility graph $\mathcal{G}$ (a shared point adds weight 1 to an edge, a shared marker adds 4), and an FBoW keyframe database $\mathcal{D}$. The pipeline is the classic ORB-SLAM-style loop — initialization, tracking, keyframe insertion, local/global optimization, loop closing, relocalization — with a parallel Map Manager thread, plus map save/load for deployment.

• Initialization runs keypoint-based (homography vs essential matrix, as in ORB-SLAM2) and marker-based (SPM-SLAM) initialization in parallel; markers win ties, giving instant metric scale, and marker pose ambiguity (two near-identical reprojections for a distant planar marker) is resolved using multiple views.
• Tracking jointly minimizes point and marker-corner reprojection errors. With reprojection error $e(\mathrm{T},\mathbf{x},\delta,\mathbf{u})=\Psi(\mathrm{T},\mathbf{x},\delta)-\mathbf{u}$ (projection function $\Psi$, camera intrinsics $\delta$), the frame pose is

$$\mathbf{f}_{\mathrm{T}}=\arg\min_{\mathrm{T}}\left(\mathbf{w_p^f}\,H(\Upsilon_p^{\mathbf{f}},\mathrm{T})+\mathbf{w_m^f}\,H(\Upsilon_m^{\mathbf{f}},\mathrm{T})\right),$$

where $H(\Upsilon_p^{\mathbf{f}},\mathrm{T})$ is the Huber-robustified sum of point reprojection errors (weighted by $\Omega_{\mathbf{g}}=\eta^{-\mathbf{g}_l}\mathrm{I}$, down-weighting keypoints from coarse pyramid levels) and $H(\Upsilon_m^{\mathbf{f}},\mathrm{T})$ sums the squared reprojection errors of the four corners of each valid marker. Because points vastly outnumber markers, the balance is set by

$$\mathbf{w_m^f}=\frac{1}{2}\min\left(1,\frac{\mathbf{n_f}}{\tau_m}\right),\qquad \mathbf{w_p^f}=1-\mathbf{w_m^f},$$

with $\mathbf{n_f}$ the number of valid markers in the frame and $\tau_m=5$ by default.

• Map optimization is sparse Levenberg-Marquardt bundle adjustment over keyframe poses, point positions, and marker poses: $\arg\min_{\mathbf{k}_{\mathrm{T}},\mathbf{p}_{\mathbf{x}},\mathbf{m}_{\mathrm{M}}} E(\Upsilon_p)+E(\Upsilon_m)$, run locally (covisible keyframes) after keyframe insertion and globally after loop closure.
• Loop closure and relocalization are marker-first: a re-observed marker not seen near the reference keyframe triggers an immediate, unambiguous loop closure — corrected via $\mathrm{Sim}(3)$ pose-graph optimization before the marker is allowed into tracking — while keypoint loop closure runs asynchronously via FBoW. Relocalization tries known markers first and falls back to BoW + RANSAC PnP.
• Culling keeps for every marker the three most distant keyframes observing it, then removes keyframes whose matched points are seen in at least three other keyframes.

Results

Compared against ORB-SLAM2 and LDSO on KITTI (20 monocular sequences), EuRoC-MAV (20), TUM RGB-D (10), and SPM (8), using a proposed pairwise score $\mathbf{S}_{\rho}(\mathbf{a},\mathbf{b})\in[-1,1]$ that combines ATE (computed on commonly tracked frames) with the percentage of tracked frames. ORB-SLAM2 scores $-0.10$ to $-0.14$ against UcoSLAM across confidence levels $\rho$ (UcoSLAM slightly better); LDSO scores $-0.37$ to $-0.40$ against UcoSLAM (clearly worse). On the marker-equipped SPM dataset, keypoints+markers beats both markers-only ($\mathbf{S}$ from $-0.187$ to $-0.625$ against it) and keypoints-only (e.g. video1: ATE 0.057 m at 100% tracked vs 0.601 m at 64.5% for keypoints-only). Average speed (fps over tracked frames): UcoSLAM 2.6 (SLAM) / 19.8 (tracking) vs ORB-SLAM2 1.6 / 12.5 and LDSO 3.0 / 2.4, with Wilcoxon tests confirming significance vs ORB-SLAM2. In a repetitive office building (50 ceiling markers, four ~20 m corridors, a 12,000-frame phone sequence), ORB-SLAM2, LDSO, and both single-landmark UcoSLAM modes failed; only the combined keypoints+markers mode built a coherent map, with BoW relocalization returning up to six false candidates per frame that marker IDs disambiguate.

Why it matters for SLAM

UcoSLAM shows that artificial and natural landmarks are complementary: markers contribute metric scale, drift-free anchors, and reliable relocalization/loop closure, while features provide continuous coverage between them. This makes it highly practical in environments you control — warehouses, labs, industrial AR setups — where placing a few markers is cheap insurance, and its map save/load design made it unusually deployment-friendly among research systems of its era.

Related

• ORB-SLAM2 — the keypoint-based SLAM design UcoSLAM builds upon
• Scale ambiguity — the monocular problem that markers solve for free
• Visual Place Recognition (VPR) — the appearance-based loop closure that markers replace
• Landmark — general notion of map landmarks
• LDSO — the direct-method baseline it was compared against