Pose Graphs & Loop Closure
← Part 3 recap: jointly estimating pose and unknown landmarks
Every part so far optimized a single moment. Real robots move continuously, and each small step of motion — odometry — is itself a noisy measurement. String enough of those together and small errors compound into serious drift. This part builds a chain of poses, watches that drift happen, and then shows the one trick — revisiting a known landmark — that lets a single joint optimization correct the entire trajectory at once.
What's new: a chain of poses
Recap
Before: one pose (and maybe some landmarks), estimated once. Now: a whole sequence of poses p₀, p₁, …, pᴸ — one per moment in time — linked together by odometry: a noisy measurement of how far and which way the robot moved between consecutive poses, taken from the robot's own wheels, IMU, or visual motion estimate.
This structure — small nodes (poses) connected by measured edges (odometry, plus the occasional landmark sighting) — is exactly what people mean by a pose graph. Optimizing it is the same least-squares machinery as every part before this; the only new ingredient is the shape of one more residual: relative motion instead of a bearing.
Composing motion: the same rotation, the other way
Foundations
The robot always reports odometry in its own frame — "I moved 2.5 forward and drifted 0.4 to my left" — because that's all its wheels or IMU actually know. Drag the heading slider and watch the same reported motion produce a completely different push across the world, depending only on which way the robot happened to be facing when it made that move.
Dead reckoning: drift with no correction
The problem
The robot drives a six-step loop. Its true path is the faint dashed line. Step through the noisy odometry one link at a time, integrating it forward from the known start (p₀ is exact — only the motion in between is noisy) — no fitting, no optimizer, just addition. Watch the estimate quietly wander away from the truth, worst at the far end where the fewest corrections have had any chance to help. This is dead reckoning, and it's what you get with odometry alone.
Loop closure: one sighting corrects everything
The fix
Same six-step trajectory. With the box unchecked, the robot only spots the landmark once, right at the end — Gauss-Newton can straighten out the last pose, but everything before it is still running on odometry alone. Check the box to also give it a sighting of that same landmark back at pose 1 — a genuine loop closure — and re-run: the error at every single pose drops, not just the ones directly touching the landmark, because the optimizer now has to satisfy the odometry chain and both sightings of the same landmark at once, all in one joint solve. The dashed rays from the robot to the landmark are exactly the two constraints being satisfied.
| Pose | Dead reck. | Estimate |
|---|
JᵀJ block structure (18×18: pose 1 … pose 6) — banded, since each pose only shares an edge with its neighbor; a landmark sighting shows up as extra weight on its own pose's diagonal block, not a new off-diagonal link. Hover a cell for its real value.
Pose 1's own 3×3 diagonal block (x, y, θ) — real JᵀJ values, not just color:
Levenberg-Marquardt on the graph
Same damping, a chain-shaped problem
λI just grows to match, again.Same loop-closed graph, but with noisier odometry this time — enough that plain Gauss-Newton's first couple of steps can be genuinely bad guesses. Adaptive λ keeps early steps small and cautious, then gets out of the way once the estimate is close.
Playground: drive, drift, and close the loop
Put it all together
Same loop-closed six-pose trajectory. Race gradient descent, Gauss-Newton, and Levenberg-Marquardt (full Newton skipped again, for the same reason as part 3 — the curvature terms for a whole chain of odometry edges get unwieldy fast). The table tracks average pose position error across all six poses.
| Method | Iters | Avg. pos err | Status |
|---|
Cheat sheet
Recap
| Concept | What changed from part 3 |
|---|---|
| Unknown | One pose (+ optional landmarks) → a whole trajectory of poses, one per time step |
| New edge type | Odometry — a noisy relative-motion measurement between two consecutive poses |
| New failure mode | Drift — errors compound step after step with nothing to correct them |
| The fix | Loop closure — re-observing something already seen, and re-optimizing every pose together instead of chaining forward |
| Sparsity pattern | Banded / chain-shaped — each pose only shares a direct edge with its neighbor, even across a loop closure |
| Update rules | Identical in form to every part before this — just a (much) bigger state vector |
This is, essentially, a pocket-sized pose-graph SLAM back-end — the same shape of problem that runs continuously on real robots and self-driving cars, just with thousands of poses and landmarks instead of six and one. From here, the last remaining piece is dimensionality: everything on this page happened in a flat 2D plane, where a heading is a single angle. Real-world robots, drones, and cameras rotate in full 3D — where "angle" stops being one number.