Graph SLAM

The trick is almost insulting in its simplicity. Pin every place the robot stood. Tie a spring between every pair of places you have a measurement for. Then let go and watch the whole map relax into the shape that makes the springs happiest.

Try this:

1. Build a path. Tap Build path and watch the blue estimate peel away from the gray dashed truth; that growing gap is accumulated odometry drift. The status line and the drift counter track it as you go.
2. Feel the tension. The drifted path is a graph of stretched springs. Nothing has snapped it back yet.
3. Close the loop. Tap Add loop closure. A coiled spring flashes between the current pose and the earlier pose it just recognised, with both ends ringed: "I've been here before."
4. Optimise. Tap it and watch the springs relax: the drifted path fades to a ghost while the estimate snaps onto the truth and the drift counter collapses toward zero.
5. Repeat the lap. Tap Repeat lap to drive the loop again, then add more closures. Each high-tension spring you add pulls the map tighter; watch the drift shrink with every solve.

Why filtering doesn't scale

EKF SLAM (last lesson) re-touches the entire state every single step. As the map grows, that covariance update balloons toward $O(n^2)$, and worse, once it commits to a pose, that pose is frozen. A measurement you take an hour from now can never reach back and correct a mistake you made an hour ago.

Graph SLAM refuses to commit. It treats localization not as a frantic real-time guess but as a giant offline jigsaw: collect all the constraints, then solve the whole puzzle in one shot. The poses are nodes; the measurements are edges between them. That picture has a name.

The rubber-band graph

Forget the matrices for a moment. Imagine every place the robot stood is a pin, and every measurement is a rubber band between two pins.

• Odometry: "I rolled one metre forward" is a short band between consecutive pins. Reliable over a step, but a thousand of them in a row carry a thousand small lies.
• Loop closure: "Pose 200 looks exactly like pose 1" is a long, taut band yanked across the whole loop.

When the robot has drifted, those bands are stretched and quivering with stored energy. Press Optimise and you are simply letting the structure go slack: the graph slides into the single configuration where total tension is lowest. Every pose nudges its neighbours, the error spreads out evenly, and the map clicks into place.

One closure changes everything

Here is the part that feels like magic the first time you see it. A robot drives a long loop through a building, drifting a little with every metre, so by the time it returns the map shows the corridor missing itself by half a room. Then it recognises a doorway: I started here. That one recognition adds one edge, and when you optimise, the correction doesn't just fix the last pose. It propagates backward through every pose on the loop, because they're all springs now, all coupled. Hours of drift vanish in one solve.

How does a robot know it has come back to a place it has seen before? That question, recognising a previously visited spot and trusting it enough to add the edge, is its own hard problem with its own name: loop closure detection.

Sebastian Thrun and colleagues turned this graph view of SLAM into the methods self-driving cars rely on.

↘ The math: minimizing tension with Gauss-Newton

The state is the whole trajectory stacked into one vector:

$$\mathbf{x} = [x_1, y_1, \theta_1, \ldots, x_T, y_T, \theta_T]$$

Each edge $(i,j)$ contributes a residual, a measure of how badly poses $i$ and $j$ disagree with their measurement $\mathbf{z}_{ij}$:

$$\mathbf{e}_{ij} = \mathbf{z}_{ij} - h(x_i, x_j)$$

The total "tension" is the sum of those residuals, each weighted by its information matrix $\Omega_{ij}$ (a stiff spring gets a big $\Omega$):

$$F(\mathbf{x}) = \sum_{(i,j) \in \mathcal{E}} \mathbf{e}_{ij}^{T} \, \Omega_{ij} \, \mathbf{e}_{ij}$$

We want the poses that minimise $F$. Linearize the residuals, build the system, and take a Gauss-Newton step:

$$H \, \Delta\mathbf{x} = -\mathbf{b}$$$$\mathbf{x}^{k+1} = \mathbf{x}^{k} + \Delta\mathbf{x}$$

The win hides in $H$. Because each pose is only tied to its neighbours and a handful of loop closures, $H$ is sparse: overwhelmingly zeros. Sparse linear algebra exploits that, which is how libraries like g2o, GTSAM, and Ceres solve graphs of millions of variables in milliseconds instead of choking on a dense $O(n^2)$ filter. Loop closures are exactly the off-diagonal entries that pull distant poses into agreement.

↘ Where g2o, GTSAM, and Ceres came from

The three back-ends most people reach for were each born in a different corner of the field.

• g2o ("general graph optimization") came out of robotics labs in Germany around 2011 and was built specifically for pose graphs and SLAM factor graphs. If you read a SLAM paper from the 2010s, the experiments very likely ran on g2o.
• GTSAM (Georgia Tech Smoothing and Mapping) leans on a clever data structure called the Bayes tree to re-solve only the part of the graph a new measurement actually disturbs, which is what lets it run incrementally as the robot drives rather than re-solving everything from scratch.
• Ceres Solver is Google's general nonlinear least-squares library. It was not written for robots at all. It grew out of the bundle adjustment behind Google Street View and Maps, then turned out to be exactly the hammer SLAM needed.

All three do the same core job from the math above: build $H$, exploit its sparsity, take Gauss-Newton (or Levenberg-Marquardt) steps. They differ mostly in how they bookkeep the graph and how cleanly they let you bolt on a new kind of edge.

Key takeaways

• Don't filter step-by-step; optimise the whole path at once. Past poses stay editable until you solve.
• Every constraint is a spring; optimisation is the relaxation state. Minimise the total tension and the map falls into place.
• Loop closures are the high-tension springs that snap accumulated drift away in a single solve.
• Sparsity is the secret. Most poses only touch their neighbours, so the solve stays fast even at city scale.