Visual SLAM

The estimation problem#

SLAM is maximum a posteriori inference over a trajectory $\mathcal{X} = {\mathbf{x}_0 \dots \mathbf{x}_T}$ and a map $\mathcal{M} = {\mathbf{m}_1 \dots \mathbf{m}_K}$ given measurements $\mathcal{Z}$:

$$ \mathcal{X}^, \mathcal{M}^ = \arg\max \; p(\mathcal{X}, \mathcal{M} \mid \mathcal{Z}) $$

With Gaussian noise this becomes nonlinear least squares:

$$ \min_{\mathcal{X}, \mathcal{M}} \sum_{k} \bigl\lVert h_k(\mathcal{X}, \mathcal{M}) - \mathbf{z}k \bigr\rVert^2{\Sigma_k} $$

where $\lVert \mathbf{e} \rVert^2_{\Sigma} = \mathbf{e}^\top \Sigma^{-1} \mathbf{e}$ is the Mahalanobis norm. The weights in the code are $\Sigma^{-1/2}$, folded into the residual so an ordinary least-squares solver produces the correct answer.

Pose-graph SLAM marginalises out the landmarks, leaving only relative-pose constraints between poses. It is cheaper and less accurate than full bundle adjustment, and it is the right structure for large-scale loop closure.

Manifolds#

Poses live on $SE(2)$ or $SE(3)$, which are Lie groups, not vector spaces. Two consequences.

Composition is not addition. The relative pose is $\mathbf{z}_{ij} = \mathbf{x}_i^{-1} \circ \mathbf{x}j$, which is exactly what between computes. The residual is $\log\bigl(\hat{\mathbf{z}}{ij}^{-1} \circ \mathbf{x}_i^{-1} \circ \mathbf{x}_j\bigr)$, using the logarithm map into the Lie algebra $\mathfrak{se}(3)$.

Increments must be applied on the manifold. The optimiser works in a local tangent space and retracts via the exponential map, $\mathbf{x} \leftarrow \mathbf{x} \circ \exp(\delta)$. Adding a Euclidean increment to a rotation matrix leaves the manifold.

Our 2D code hides this: the wrap call is exactly the $SO(2)$ logarithm map, and the rest happens to work because $SO(2)$ is one-dimensional and commutative. In 3D it must be done properly, which is why GTSAM and Sophus exist.

Sparsity#

The information matrix $\Lambda = J^\top \Sigma^{-1} J$ has non-zeros only where two poses share a constraint. For a trajectory with a few loop closures it is banded plus a few off-diagonal entries, so a sparse Cholesky factorisation under a good variable ordering is close to linear in the number of poses.

Kaess et al. (2008, 2012), iSAM and iSAM2, exploit this incrementally: new measurements update the factorisation rather than rebuilding it, and only the affected variables are relinearised — using a data structure called the Bayes tree. This is what makes real-time back ends possible.

Historically, EKF-SLAM maintained a dense covariance over poses and landmarks at $O(K^2)$ cost per update, and became intractable beyond a few hundred landmarks. The move to sparse smoothing around 2006-2010 is the single biggest step change in the field's history.

Loop closure and its failure#

The front end proposes closures by appearance. Two lineages:

• Bag of visual words. DBoW2 (Gálvez-López and Tardós, T-RO 2012) quantises ORB descriptors into a vocabulary tree and retrieves similar images by inverted index. Fast, CPU-only, and what ORB-SLAM uses.
• Learned place recognition. NetVLAD and its successors produce a global descriptor per image, more robust to lighting and season change, at GPU cost.

FAB-MAP (Cummins and Newman, IJRR 2008) is the important theoretical contribution: a probabilistic appearance model that accounts for the fact that visual words co-occur, so that seeing a generic word carries little evidence. Without such a correction, perceptual aliasing dominates.

A false closure is catastrophic, so the back end needs protection. Switchable constraints (Sünderhauf and Protzel, 2012) attach a learned switch variable $s_{ij} \in [0,1]$ to each loop edge, scaling its information matrix and penalising deviation from 1. The optimiser can then turn a bad closure off. Dynamic covariance scaling and max-mixture models achieve similar robustness. Any deployed system needs one of these.

Visual-inertial fusion#

An IMU changes the problem qualitatively rather than incrementally.

• It makes scale observable. A monocular camera alone cannot recover metric scale, exactly as in structure from motion. An accelerometer measures metres per second squared, which supplies it.
• It makes roll and pitch observable through the gravity direction.
• It bridges short tracking failures — motion blur, a covered lens, a textureless wall.

The technical key is IMU pre-integration (Forster et al., T-RO 2017), which integrates high-rate IMU measurements between two keyframes into a single relative-motion factor whose value does not need recomputing when the linearisation point changes. Without it, every optimiser iteration would re-integrate thousands of samples.

ORB-SLAM3 (Campos et al., T-RO 2021) combines this with multi-map handling: when tracking is lost, it starts a new map and merges it later if a place is recognised, rather than discarding everything.

Learned SLAM#

DROID-SLAM (Teed and Deng, NeurIPS 2021) replaces the feature front end with dense optical flow from a RAFT-style recurrent network, and iteratively updates poses and per-pixel depth through a differentiable dense bundle-adjustment layer. It is markedly more robust than feature-based systems on hard sequences, and it requires a substantial GPU.

The broader 2025-2026 trend is toward feed-forward geometry models used as SLAM front ends, giving dense pose and depth without correspondence search. The back end — sparse nonlinear least squares over a graph — has not been displaced, and the reason is worth stating: the graph is where the consistency constraint lives, and consistency is what a learned per-frame predictor cannot supply.

Benchmarks#

EuRoC MAV (drone, visual-inertial), TUM RGB-D (handheld indoor), KITTI Odometry (driving), TUM-VI and 4Seasons (long-term, changing conditions).

The standard metrics come from Sturm et al. (IROS 2012): ATE, absolute trajectory error after aligning the estimate to ground truth by a rigid or similarity transform, and RPE, relative pose error over fixed intervals. Report both. ATE is dominated by drift and loop closure; RPE measures local accuracy and is the number that matters for control.

What to learn next#

• Structure from motion — the offline sibling, with all the time in the world.
• Neural radiance fields — turning the recovered poses into a photorealistic scene.
• Feature matching with SIFT and ORB — the front end this back end depends on.