Week 12 — Simultaneous Localization and Mapping (SLAM) Day 5 of 7
Why This Matters
A SLAM frontend drifts. Always. Whether you run EKF-SLAM, ORB-SLAM3, or LeGO-LOAM, the pose estimate accumulates error every frame. A typical visual-inertial odometry system drifts at 0.5–1% of distance traveled. Over a 200 m warehouse loop, that is 1–2 m of position error. Over a 2 km outdoor survey, it is 10–20 m. The map warps with the trajectory, so the same wall gets mapped twice, 1.5 m apart.
This is not a cosmetic problem. If a 1.2 m wide AGV (like a MiR250) drives a loop through a 1.8 m aisle and the map has two copies of the same rack, the planner either refuses to plan or routes through a phantom wall. Localization against a doubled map fails because the scan matcher finds two equally good answers. The robot stops. The shift supervisor calls you at 02:00.
Loop closure detection is the fix. It recognizes that the robot is looking at a place it has seen before. That recognition produces a constraint between the current pose and an old pose. A pose-graph optimizer then distributes the accumulated error across the whole trajectory. Without it, SLAM is just odometry with extra steps. With it, drift is bounded by the loop closure accuracy, not by distance traveled.
The stakes are real. The 2010 KITTI benchmark showed that pure visual odometry on sequence 00 (3.7 km) accumulates 47 m of translational error before any loop closure. Adding loop closures brings that down to under 1 m. That is the difference between a working delivery robot and a paperweight.
Core Concepts
Place Recognition: Turning Sensors into Candidate Matches
Loop closure has two stages. First, detection: does the current observation match any past observation? Second, verification: is that match geometrically correct? Detection is cheap and noisy. Verification is expensive and precise.
For LiDAR, the standard method is Scan Context (Kim & Kim, 2018). It projects a 3D scan into a 2D polar grid: 20 radial bins × 60 azimuth bins. Each cell stores the maximum z-value. The descriptor is a 20×60 matrix. Matching uses a column-wise distance, with circular shift to handle yaw. Scan Context achieves ~90% recall at 100% precision on the KITTI benchmark with a threshold of 0.15–0.2. It runs at 20+ Hz on a single CPU core.
For cameras, the workhorse is DBoW2 (Gálvez-López & Tardós, 2012), used in ORB-SLAM. It builds a vocabulary tree from ~1M ORB descriptors, quantized into 1M visual words via k-means with k=10 and depth=6. Each image becomes a bag of words vector. Matching uses an inverted index. On a 1M-image database, retrieval takes ~10 ms. ORB-SLAM2 requires a minimum score of 0.015 between candidate frames, plus a covisibility check, before attempting verification.
The key insight: place recognition must be viewpoint-tolerant. A robot approaching a corner from the north and later from the east sees different pixels but the same place. Bag-of-words and Scan Context both handle this by discarding spatial layout at the detection stage.
Verification: Geometric Consistency via RANSAC
A candidate match is not a loop closure until you prove it. Detection gives you “image 412 looks like image 1873.” Verification asks: “do the features agree on a rigid transform?”
For visual SLAM, you take the ORB matches between the two frames and run RANSAC to fit the essential matrix $E$ (5-point algorithm) or a PnP solution (3-point, if you have 3D points). With 500 matches and a 50% outlier rate, RANSAC needs about 20 iterations to find a model with 99% confidence (for a 5-point minimal set, $N = \log(0.01)/\log(1 - 0.5^5) \approx 20$). ORB-SLAM2 requires at least 20 inliers after RANSAC before accepting a loop.
For LiDAR, verification uses ICP or GICP between the current scan and the matched historical submap. A good loop closure registers with a fitness score (mean squared point-to-plane distance) below 0.3 m² and a final transform within 2 m and 10° of the odometry prediction.
Here is a worked example. Suppose the odometry says the robot is at $(x, y) = (50.0, 12.0)$ m. The candidate loop closure from Scan Context is frame 1873, whose pose was $(49.2, 11.5)$ m. The relative transform from ICP comes back as $\Delta x = 0.3$ m, $\Delta y = 0.2$ m, $\Delta\theta = 1.5°$. This is consistent with the odometry prediction (0.8 m and 0.5 m apart, well within the 2 m gate). Accept. If ICP returned $\Delta x = 4.1$ m, it would fail the gate and be rejected as a false positive.
The Pose Graph and Drift Correction
Once you have a verified loop closure, you build a pose graph. Nodes are robot poses. Edges are relative transforms: odometry edges from consecutive frames, loop closure edges from detected matches. Each edge has an information matrix (inverse covariance).
The optimizer minimizes:
$$\sum_{(i,j) \in \mathcal{E}} \mathbf{e}{ij}^\top \Omega{ij} \mathbf{e}_{ij}$$
where $\mathbf{e}{ij} = \text{Log}(\mathbf{T}{ij}^{-1} \mathbf{T}_i^{-1} \mathbf{T}j)$ is the error in the Lie algebra, and $\Omega{ij}$ is the information matrix. For a 200 m loop with 2000 poses and 5 loop closures, g2o solves this in about 50 ms with Levenberg-Marquardt.
The correction is dramatic. A 2 m accumulated drift at the loop closure point gets distributed across all 2000 poses, so each pose shifts by roughly 1 mm. The map snaps into alignment. The doubled wall disappears.
Mathematical Foundation
The loop closure constraint error, in SE(3), is:
$$\mathbf{e}{ij} = \log\left( \mathbf{T}{ij}^{-1} \cdot \mathbf{T}_i^{-1} \cdot \mathbf{T}_j \right)^\vee$$
where:
- $\mathbf{T}_i \in SE(3)$ is the estimated world-to-robot pose at node $i$ (4×4 homogeneous matrix).
- $\mathbf{T}_j$ is the pose at node $j$.
- $\mathbf{T}_{ij}$ is the measured relative transform from $i$ to $j$ (from odometry or loop closure).
- $\log(\cdot)^\vee$ maps the SE(3) matrix to a 6-vector $(\rho, \phi)$ — translation and rotation error.
- $\Omega_{ij} \in \mathbb{R}^{6\times 6}$ is the information matrix, typically $\Omega = \text{diag}(1/\sigma_x^2, 1/\sigma_y^2, 1/\sigma_z^2, 1/\sigma_{rx}^2, 1/\sigma_{ry}^2, 1/\sigma_{rz}^2)$.
The optimizer solves $\min_{{\mathbf{T}}} \sum \mathbf{e}{ij}^\top \Omega{ij} \mathbf{e}_{ij}$ using Gauss-Newton. The Jacobian of the error w.r.t. $\mathbf{T}_i$ is approximately $-\mathbf{J}r^{-1}(\mathbf{e}{ij}) \cdot \text{Ad}(\mathbf{T}_j^{-1})$, where $\text{Ad}$ is the SE(3) adjoint.
Worked numeric example. Consider a 2D simplification for clarity. The robot is at node $i$ with pose $\mathbf{T}_i = (x=10.0, y=5.0, \theta=0.0)$. The odometry chain predicts node $j$ at $\mathbf{T}j = (x=10.5, y=5.0, \theta=0.02)$. A loop closure measurement says the relative transform is $\mathbf{T}{ij} = (0.4, 0.0, 0.0)$.
The predicted relative transform is $\mathbf{T}_i^{-1} \mathbf{T}_j = (0.5, 0.0, 0.02)$. The error is:
$$\mathbf{e} = (0.4, 0.0, 0.0) - (0.5, 0.0, 0.02) = (-0.1, 0.0, -0.02)$$
With information matrix $\Omega = \text{diag}(100, 100, 400)$ (i.e., $\sigma_x = \sigma_y = 0.1$ m, $\sigma_\theta = 0.05$ rad), the cost contribution is:
$$(-0.1)^2 \cdot 100 + 0^2 \cdot 100 + (-0.02)^2 \cdot 400 = 1.0 + 0 + 0.16 = 1.16$$
The optimizer will adjust $\mathbf{T}_j$ (and all intermediate poses) to reduce this. After convergence, the loop closure edge has near-zero error, and the 0.1 m drift is spread across the trajectory.
Practical Example
Scenario: A Clearpath Jackal UGV with a 16-beam Velodyne VLP-16 and a payload of 3 kg survey equipment. It runs a 400 m loop around a university building at 1.0 m/s. LeGO-LOAM is the frontend. The map must be accurate to 0.15 m RMS for the survey team’s GIS overlay.
Step 1 — Odometry drift budget. LeGO-LOAM drifts at roughly 0.8% of distance on this kind of loop (mixed indoor/outdoor, some vegetation). Over 400 m, that is 3.2 m of drift at the loop closure point. The 0.15 m tolerance is 21× tighter than the raw drift. Loop closure is mandatory.
Step 2 — Detection with Scan Context. The system stores Scan Context descriptors every 2 m (200 descriptors for the loop). When the robot returns to the start, the descriptor matches the first one with a distance of 0.12 (below the 0.15 threshold). The candidate is frame 0.
Step 3 — Verification with ICP. The current scan is registered against the submap around frame 0. ICP runs 30 iterations. Fitness score is 0.08 m² (below the 0.3 m² threshold). The transform is $\Delta x = 2.9$ m, $\Delta y = -1.1$ m, $\Delta\theta = 3.2°$. This is within the 2 m translational gate? No — 2.9 m exceeds it. The gate is widened to 4 m for this loop because the drift budget was 3.2 m. Accept.
Step 4 — Pose graph optimization. 200 nodes, 199 odometry edges, 1 loop closure edge. g2o with Levenberg-Marquardt, 10 iterations. Final RMS error across all edges: 0.04 m. The trajectory closes to within 0.08 m. The map is rebuilt from the corrected poses. The doubled wall is gone.
Step 5 — Validation. The survey team checks 10 ground-control points (RTK GPS surveyed). Max horizontal error: 0.11 m. Within the 0.15 m tolerance. Ship it.
| Parameter | Value | Rationale |
|---|---|---|
| Scan Context threshold | 0.15 | 90% recall, 100% precision on KITTI |
| ICP fitness threshold | 0.3 m² | Rejects vegetation and dynamic objects |
| Translational gate | 4.0 m | 1.25× the drift budget |
| Rotational gate | 10° | Handles yaw drift over 400 m |
| Min inliers (visual) | 20 | ORB-SLAM2 default |
| Optimizer | g2o LM | 50 ms for 200 nodes |
Common Pitfalls
1. Perceptual aliasing in repetitive environments. What goes wrong: the robot matches a corridor in building A to an identical corridor in building B, 80 m away. The pose graph gets a false edge and the map folds in half. Why: bag-of-words and Scan Context discard spatial layout, so identical geometry produces identical descriptors. Detect it by checking that the loop closure transform is consistent with odometry (within the drift gate). Fix it by requiring a sequence of consecutive matches (temporal consistency over 3–5 frames) before accepting, and by using a covisibility check like ORB-SLAM2’s.
2. Dynamic objects creating false matches. What goes wrong: a forklift parked in the same spot for two hours appears in both the early and late scans. ICP registers to the forklift, not the wall, and the loop closure is off by 0.5 m. Why: ICP minimizes distance to all points, and a large planar object (the forklift side) dominates. Detect it by checking the inlier ratio — if fewer than 60% of points are inliers, reject. Fix it by filtering dynamic points (LeGO-LOAM’s ground segmentation, or a simple range-image change detector) before ICP.
3. Overconfidence in loop closure information matrices. What goes wrong: the optimizer distributes the correction too aggressively because the loop closure edge is assigned $\sigma = 0.01$ m when the real accuracy is 0.2 m. The trajectory bends sharply near the loop closure, creating a kink that breaks the map elsewhere. Why: the information matrix is set by hand and does not reflect the actual registration uncertainty. Detect it by inspecting the residual distribution — a few large residuals indicate over-tight edges. Fix it by computing the covariance from the ICP Hessian (the inverse of $J^\top J$ at convergence) and scaling by the fitness score.
4. Missing loop closures due to viewpoint change. What goes wrong: the robot returns to the start but facing the opposite direction. The Scan Context descriptor is yaw-invariant (circular shift), but the ORB bag-of-words vector is not. The visual system fails to detect the loop, and drift stays at 3.2 m. Why: visual place recognition depends on seeing the same features, and a 180° view change loses most of them. Detect it by monitoring the drift estimate — if it exceeds 2× the expected budget with no loop closures, flag it. Fix it by fusing LiDAR and visual detection, or by using a rotation-invariant descriptor like NetVLAD.
Key Takeaways
- Loop closure is the only thing that bounds drift. Odometry drifts at 0.5–1% of distance; without loop closure, a 400 m loop accumulates 2–4 m of error. The pose graph distributes that error across all poses.
- Detection and verification are separate stages. Use a cheap, viewpoint-tolerant descriptor (Scan Context, DBoW2) for detection, then a geometric check (RANSAC, ICP) with strict gates for verification. Never skip verification.
- Gate loop closures against odometry. A valid loop closure must be within the drift budget of the odometry prediction. A 2.9 m correction on a 3.2 m drift budget is fine; a 40 m correction is a false positive.
- Set information matrices from real covariance. Compute the covariance from the registration Hessian, not by hand. Over-tight loop closure edges create trajectory kinks and map artifacts.
- Require temporal consistency. A single frame match is not enough. Require 3–5 consecutive matches before accepting a loop closure. This kills most perceptual aliasing failures.
Tomorrow
Tomorrow we put it all together: Python Practice: SLAM with g2o, where you will build a pose graph from scratch, add loop closure edges, and watch the optimizer correct a drifted trajectory in real time.