Section 29.5: SLAM: graph-based and visual SLAM

"SLAM is memory under cross-examination by geometry."

A Loop Closure That Came Back With Receipts

Problem First

Figure 29.5.1 sketches this reasoning problem in miniature: measurements, state estimates, decisions, and replayable evidence all feeding one loop-closure moment.

Picture a robot whose map of a single hallway has quietly split into two parallel ghost corridors, three meters apart, because nobody told it that the wall it sees now is the same wall it saw a minute ago: frame-by-frame correction is too local when the robot revisits a place after a long loop. A loop closure says that two far-apart timestamps are actually nearby in space, and this is called global trajectory snapping, which can bend the entire trajectory into consistency.

Graph SLAM represents poses and landmarks as variables and measurements as factors. Visual SLAM adds feature tracking, keyframes, bundle adjustment (jointly refining camera poses and 3D landmark positions to minimize reprojection error), and relocalization (re-finding the robot's pose from scratch against the stored map after tracking is lost; the mechanics and cost of this step are covered later in this section). The optimizer minimizes residuals weighted by measurement covariance, so a bad association can pull the whole graph into a convincing but false solution.

Concretely, a visual SLAM front end runs four repeating steps: extract features (typically corner-like keypoints such as ORB) from each incoming frame, match them against the previous keyframe and against nearby map landmarks, triangulate new 3D landmark positions from matched feature pairs, and decide whether the frame becomes a new keyframe. Bundle adjustment then periodically re-optimizes the most recent window of poses and landmarks jointly, while the pose-graph optimizer described below handles the longer-range loop-closure corrections. Keeping these two optimization scopes straight, local bundle adjustment over a sliding window versus global pose-graph optimization over the whole trajectory, is what lets a visual SLAM system stay real-time while still correcting decade-old drift on a loop closure.

That sensitivity to bad associations is exactly why the graph cannot be allowed to grow without bound: the more nodes it carries, the more chances a wrong factor has to corrupt the solution and the less time the optimizer has to react. Keyframes matter physically. A robot running at 30 Hz accumulates 1,800 pose nodes in a single minute, roughly 108,000 in a one-hour session. Even a sparse Cholesky solver (a numerical method that factors the graph's sparse information matrix instead of inverting it directly, the standard way pose-graph optimizers solve for corrections) on a desktop CPU typically cannot keep such a dense graph current with incoming data in real time. Selecting only a sparse subset keeps the graph tractable. It also forces the system to commit to which observations are worth retaining for loop closure, which decides whether the robot corrects drift before it reaches a doorway or a shelf edge.

When a frame becomes a keyframe

A frame is promoted to a keyframe when it differs enough from the last keyframe, measured by the number of tracked feature matches that survive between the two frames or by the distance and rotation the camera has moved. Once selected, the keyframe stores its descriptors, 3D landmark positions, and pose estimate; subsequent optimization factors connect keyframes rather than every raw frame, so the graph stays sparse while preserving the geometric observations needed for loop closure.

Before the math: why does the optimizer not just correct the most recent poses and leave earlier ones alone? What forces a single loop closure to move nodes from twenty minutes ago?

Formal Model

For graph-based and visual SLAM, the posterior is a factor graph over poses, landmarks, feature tracks, and loop closures. Its deployable form is a graph with residuals, covariances, robust costs, and rejected associations.

$$ x^*=\arg\min_x \sum_k \|r_k(x)\|_{\Omega_k}^{2},\quad \|r\|_{\Omega}^{2}=r^\top\Omega r $$

The evidence terms are odometry factors, visual or scan factors, landmark observations, loop closures, and priors. Each residual \(r_k(x)\) is the mismatch between what a sensor actually measured and what the current pose estimate predicts it should have measured; the optimizer's job is to shrink these mismatches as much as the graph structure allows. The estimate earns trust only when large residuals and suspect closures stay inspectable instead of hidden behind a polished trajectory. A clean-looking map that conceals a false closure is not a map; it is a confident mistake waiting for the robot to act on it.

To see why loop closure matters globally, consider a robot that drifts 0.5 m per 10 m traveled over a 200 m loop. By the time it returns to the start, accumulated odometry error places its estimated position roughly 10 m from the true start. A correct loop closure adds a factor constraining the current pose to match the initial pose. The optimizer then redistributes that 10 m correction across all 200 m of trajectory nodes, each shifting slightly, as illustrated in Figure 29.5.2. Without the closure, every downstream planning query over that map inherits the uncorrected drift. With a false closure of similar apparent confidence, the same redistribution mechanism applies, bending the entire trajectory in the wrong direction.

Worked Diagnostic

Code Fragment 1 is the graph-SLAM sanity check: add a small set of factors, inspect residuals before and after optimization, and verify that a false closure would be visible.

# Compare two residuals with different information weights.
# High-confidence loop closures should matter more only when association is correct.
import numpy as np

residuals = np.array([0.20, 1.00])
information = np.array([25.0, 4.0])
weighted_cost = np.sum(information * residuals ** 2)
print(f"weighted_cost={weighted_cost:.2f}")
print(f"largest_term={np.argmax(information * residuals ** 2)}")

Expected output interpretation. The second factor dominates the objective even though its information weight is lower, because its residual is much larger. The output should be read as a loop-closure sanity check: a single bad association can outweigh several small nominal residuals and bend the optimized trajectory unless robust loss or outlier rejection intervenes.

Tool Workflow

Once you trust the residual intuition from the hand-traced example, you rarely build the optimizer yourself: production pipelines hand the same factor-graph machinery to battle-tested libraries.

Use the hand factor example to expose residuals and Jacobian intuition, then use GTSAM, Ceres, ORB-SLAM-style systems, or Kimera for real logs. The hand calculation remains the guard against blind optimizer trust.