SLAM: Simultaneous Localization and Mapping

To localise, a robot needs a map. To build a map, it needs to know where it is. This circularity is the SLAM problem — Simultaneous Localization and Mapping. It was considered intractable until Smith, Self, and Cheeseman (1986) showed that joint estimation of all poses and landmarks in a probabilistic framework is consistent.

Formally: given a sequence of control inputs $\mathbf{u}_{1:t}$ and observations $\mathbf{z}_{1:t}$, compute the joint posterior over all robot poses $\mathbf{x}_{1:t}$ and the map $\mathcal{M}$:

$p(\mathbf{x}_{1:t}, \mathcal{M} \mid \mathbf{z}_{1:t}, \mathbf{u}_{1:t})$

This joint posterior is far too large to represent exactly — a 1 km trajectory with a dense 3D map has millions of variables. All practical SLAM systems approximate it, with a key architectural split between the front end (data association) and the back end (optimisation).

The front end ingests raw sensor data and produces relative pose constraints between frames. For visual SLAM: detect features $\rightarrow$ match across frames $\rightarrow$ estimate relative pose via the essential matrix or PnP. For LiDAR SLAM: downsample $\rightarrow$ register scans via ICP or NDT.

The critical output is a keyframe graph: a sparse set of frames (selected when robot motion exceeds a threshold) connected by relative pose edges $\hat{T}_{ij}$ with associated covariance $\Sigma_{ij}$. Front-end quality determines everything downstream — a wrong data association creates a spurious edge that the back end cannot fully correct.

Data association is hardest at revisit: when the robot returns to a previously seen place, it must recognise it (loop closure) without confusing it with a superficially similar location (perceptual aliasing). A corridor of identical doors is the canonical failure scenario.

The back end takes the keyframe graph and finds robot poses $\{T_i\}$ that are maximally consistent with all edge constraints. This is a nonlinear least-squares problem:

$\min_{\{T_i\}} \sum_{(i,j) \in \mathcal{E}} \left\| \log\left(\hat{T}_{ij}^{-1} \cdot T_i^{-1} T_j\right) \right\|_{\Sigma_{ij}^{-1}}^2$

where $\log(\cdot)$ maps a $SE(3)$ transform to its Lie algebra $\mathfrak{se}(3)$ (a 6-vector), and $\|\cdot\|_{\Sigma^{-1}}^2$ is the Mahalanobis norm weighting by edge confidence. Standard solvers: g2o, GTSAM, Ceres. All exploit the sparsity of the Hessian (each pose only connects to a few neighbours) via sparse Cholesky factorisation or incremental QR. A 1000-keyframe graph optimises in milliseconds. The back end is the glue that prevents drift: without it, odometry errors compound linearly; with it, a loop closure can globally correct the entire trajectory.

Loop closure is the event that resets drift. The robot returns to a previously mapped region and recognises it, adding a long-range edge to the pose graph. The back-end re-optimises and the trajectory snaps into global consistency.

For visual SLAM, loop closure uses bag-of-words (BoW) retrieval: a vocabulary tree of visual words (quantised SIFT/ORB descriptors) indexes all keyframes; query the current frame, find candidates above a similarity threshold, verify geometrically with RANSAC. DBoW2 and FBoW are standard libraries. For LiDAR SLAM, histogram-based descriptors (SCAN-CONTEXT, intensity-scan-context) encode the 3D structure compactly for fast retrieval.

False positives are catastrophic: a wrong loop closure introduces a large incorrect edge; the optimiser distorts the entire map. Always gate with a geometric verification step (essential matrix or ICP check) before inserting a loop edge. A conservative policy (miss some loops, accept no false ones) is safer for production robots.

Two historical approaches:

EKF-SLAM was the dominant method in the 1990s-2000s (indoor, small maps). Graph-based SLAM (Grisetti et al., 2010) took over for large-scale autonomy and is now the industry standard. Filter methods survive in tightly coupled IMU-SLAM pipelines where their predict step integrates naturally.

How raw sensor data becomes a consistent global map.

ORB-SLAM3 (Campos et al., 2021) is the most widely-used visual SLAM system in robotics research. Architecture:

• Tracking thread: estimates camera pose frame-by-frame using ORB feature matching against local map points and motion-only BA (bundle adjustment).
• Local mapping thread: adds new keyframes, triangulates new map points, runs local BA over the active window.
• Loop and map merging thread: detects loop closures via DBoW3, verifies with essential matrix, adds loop edges, and triggers Essential Graph optimisation — a sparse subgraph of strong edges for fast global correction.
• Atlas: supports multiple maps that can be merged when previously disconnected sessions are revisited.

ORB-SLAM3 supports monocular, stereo, RGB-D, and IMU modes. Key limitation: it struggles in low-texture environments (white walls, outdoor fields) and fails catastrophically when the number of tracked features drops below ~15. It is not robust to fast motion blur.

Two dominant LiDAR SLAM systems:

Google Cartographer (Hess et al., 2016): 2D/3D LiDAR + IMU. Front end uses scan-to-submap matching via branch-and-bound scan matching. Back end: pose-graph with loop closure using histogram-based scan descriptors. Landmark paper for warehouse robots and building mapping. Very robust in structured indoor environments.

LOAM / LeGO-LOAM (Zhang & Singh, 2014): extracts edge and planar feature points from LiDAR scans based on local curvature, registers edges-to-edges and planes-to-planes. The feature factorisation is the key to robustness in degenerate environments. LeGO-LOAM adds ground segmentation and runs on embedded hardware. LIO-SAM extends this with IMU preintegration and loop closure.

# Pseudo-code: LOAM feature extraction
for ring in scan.rings:
    curvature = compute_curvature(ring)          # second difference
    edge_pts  = ring[curvature > thresh_high]    # sharp features
    plane_pts = ring[curvature < thresh_low]     # smooth features
    register_edges_and_planes(edge_pts, plane_pts, local_map)

Comparison: Cartographer wins indoors; LOAM variants win outdoors and in autonomous driving.

SLAM is not a solved problem. Honest failure modes:

For production deployment: (1) always fuse SLAM with a localisation layer (map matching against a prior HD map); (2) monitor SLAM health with covariance trace and inlier count; (3) degrade gracefully — a robot that knows it is lost and stops is safer than one that confidently navigates to the wrong place. The best SLAM systems fail loudly.