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

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, 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, and relocalization. The optimizer minimizes residuals weighted by measurement covariance, so a bad association can pull the whole graph into a convincing but false solution.

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. The estimate becomes trustworthy only when large residuals and suspect closures remain inspectable rather than hidden by a polished trajectory.

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

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.