GraphSLAM & pose graphs

EKF-SLAM fuses each measurement into a covariance and then throws the measurement away. FastSLAM resamples and throws away trajectory hypotheses. GraphSLAM throws nothing away: it keeps every constraint as an edge in a graph and solves for the configuration of nodes that best satisfies all of them at once. That one decision — keep the raw measurements — is why the graph formulation can close large loops, reject a bad match after the fact, and re-linearise everything, and it is why essentially every modern SLAM system is built this way.

Symbols used on this page are defined in the Nomenclature → optimisation & robust fitting.

This is the same nonlinear least-squares machinery as bundle adjustment, specialised to pose-to-pose constraints.

1 · The formulation

1.1 Nodes and edges

A pose graph has nodes $\mathbf x_i$ (robot poses, optionally landmarks) and edges carrying relative measurements. Each edge $(i,j)$ contributes a measured relative transform $\mathbf z_{ij}$ with an information matrix $\boldsymbol\Omega_{ij}$.

1.2 The objective

Each edge's residual is the mismatch between the measured relative transform and the one implied by the current node estimates:

$$ \mathbf e_{ij}\!\left(\mathbf x_i, \mathbf x_j\right) = \log\!\left( \mathbf Z_{ij}^{-1}\,\mathbf X_i^{-1}\mathbf X_j \right)^\vee , $$

where $\mathbf X$ are the poses as transformation matrices and $\log(\cdot)^\vee$ maps the residual transform into a vector in the Lie algebra. The MAP estimate minimises

$$ \boxed{\;\mathbf X^\star = \arg\min_{\mathbf X} \sum_{(i,j) \in \mathcal{E}} \mathbf e_{ij}^\top \boldsymbol\Omega_{ij}\,\mathbf e_{ij}\;} $$

If the residual is zero the estimated relative pose exactly matches the measurement. Odometry edges will be nearly satisfied; a loop-closure edge in a drifted graph will have a large residual, and the optimiser's job is to distribute that error over the loop.

2 · Solving it

2.1 Gauss-Newton on a graph

Linearise each residual about the current estimate. Because $\mathbf e_{ij}$ depends on exactly two nodes, its Jacobian is nonzero in only two block columns:

$$ \mathbf A_{ij} = \frac{\partial \mathbf e_{ij}}{\partial \mathbf x_i}, \qquad \mathbf B_{ij} = \frac{\partial \mathbf e_{ij}}{\partial \mathbf x_j}, $$

and each edge contributes additively to just four blocks of the system matrix and two of the right-hand side:

$$ \mathbf H_{ii} \mathrel{+}= \mathbf A^\top\boldsymbol\Omega\mathbf A, \quad \mathbf H_{ij} \mathrel{+}= \mathbf A^\top\boldsymbol\Omega\mathbf B, \quad \mathbf H_{ji} \mathrel{+}= \mathbf B^\top\boldsymbol\Omega\mathbf A, \quad \mathbf H_{jj} \mathrel{+}= \mathbf B^\top\boldsymbol\Omega\mathbf B, $$ $$ \mathbf b_i \mathrel{+}= \mathbf A^\top\boldsymbol\Omega\,\mathbf e, \qquad \mathbf b_j \mathrel{+}= \mathbf B^\top\boldsymbol\Omega\,\mathbf e, $$

then solve $\mathbf H\,\delta = -\mathbf b$ and update. The construction is a simple loop over edges — which is why pose-graph optimisers are much easier to implement than their reputation suggests.

2.2 The sparsity is the point

$\mathbf H$ has exactly the sparsity pattern of the graph. A trajectory with sequential odometry gives a block-tridiagonal matrix; each loop closure adds one off-diagonal block pair. Since a real trajectory has $O(n)$ odometry edges and far fewer loop closures, $\mathbf H$ has $O(n)$ nonzeros out of $n^2$ entries.

Sparse Cholesky with a good fill-reducing ordering (AMD, nested dissection) solves these in milliseconds to seconds. As with bundle adjustment, the ordering matters enormously — the wrong permutation produces catastrophic fill-in and destroys the advantage.

2.3 The information-form view

There is a clean duality worth naming. The EKF maintains a covariance, which becomes dense as everything correlates. The graph maintains an information matrix, which stays sparse because information is additive over measurements and each measurement touches only two nodes. The same posterior, two representations, opposite sparsity structure — and the information form is the one that scales. This is the central insight of the "Square Root SAM" line of work, and marginalising a node out of the graph is exactly the Schur complement operation from BA §3.2; it creates a dense clique among that node's neighbours, which is the price of forgetting.

3 · Working on the manifold

Poses are not vectors: $SE(3)$ is a Lie group, and naive addition of pose parameters is wrong. The standard approach parameterises the increment in the Lie algebra and applies it multiplicatively:

$$ \mathbf X_i \leftarrow \mathbf X_i \, \exp\!\left(\delta\boldsymbol\xi_i^\wedge\right), \qquad \delta\boldsymbol\xi \in \mathbb{R}^6 . $$

The optimisation variable is therefore a minimal 6-vector per pose even though the pose itself is a $4\times4$ matrix — no over-parameterisation, no orthonormality constraint, no gimbal lock. For $SE(2)$ the increment is 3-dimensional and the exponential has a simple closed form. The attitude page derives the exp/log maps.

One subtlety that bites in practice: angles must be wrapped when forming residuals. A heading residual of $+359°$ is really $-1°$, and failing to wrap produces an enormous spurious residual that wrenches the graph apart. Every $SE(2)$ implementation needs an explicit wrap in the residual, and $SE(3)$ needs the equivalent care in the $\log$ map near $\pi$.

4 · Gauge freedom

Every edge is a relative measurement, so the objective is invariant to rigidly transforming the entire graph. $\mathbf H$ is therefore singular, with a null space of dimension 3 in $SE(2)$, 6 in $SE(3)$, and 7 in $Sim(3)$ — the same gauge freedom as in bundle adjustment §5. Fix it by anchoring: hold one node fixed (equivalently, add a strong unary prior on it), or add weak priors to all nodes. Without an anchor a direct solver fails outright and an iterative one drifts along the null space doing meaningless work.

5 · Loop closure

5.1 Why it is the whole game

Odometry alone drifts without bound: errors integrate, and nothing in the sequential chain can detect it. A single loop closure changes the topology of the constraint graph from a chain to a cycle, and suddenly the accumulated error is observable — the graph is over-determined, the inconsistency shows up as a residual, and the optimiser distributes the correction over every pose in the loop, weighted by the relative information of the edges. One good loop closure can reduce global error by orders of magnitude.

The simulation demonstrates exactly this, and also the dangerous case. Run it with a false closure and watch the map tear:

• Set iterations to 0 to see raw drifted odometry, then raise it — the loop gap closes and the correction is spread over all poses rather than dumped on the last one.
• Turn on false loop closure. Least squares cannot reject it: RMS error versus truth becomes worse than doing nothing. This is §5.3's motivation, shown rather than asserted.
• Vary loop-closure confidence to see how the information matrix arbitrates between trusting the loop and trusting the odometry.

5.2 Detecting loops

5.3 False loop closures, and robust kernels

Because the graph keeps the measurements, it can do something the EKF fundamentally cannot: revisit the decision. The standard defences:

• Robust kernels. Replace the squared cost with Huber or Cauchy so a large residual has bounded influence. Cheap, effective, and structurally free — the sparsity is unchanged. Best introduced graduated, starting near-quadratic and sharpening, since non-convex kernels otherwise create local minima.
• Switchable constraints. Add a per-edge switch variable $s_{ij} \in [0,1]$ that scales the edge's information, and include a prior pulling it toward 1. The optimiser can then turn an edge off if satisfying it costs more than the prior penalty — outlier rejection as part of the optimisation rather than a preprocessing step.
• Max-mixtures. Model each edge as a mixture of a "good" and a "bad" (broad) Gaussian, and take the max component. Keeps the problem Gaussian and cheap while allowing an edge to be explained away.
• Consistency-based pre-filtering. Accept a loop closure only if it is geometrically consistent with other candidate closures in the same region (RRR, pairwise consistency). Cheap and catches most aliasing.
• Certifiable methods. SE-Sync exploits the problem structure to obtain a globally optimal solution with a certificate, removing the local-minimum worry entirely for the non-robust case.

5.4 Scale drift and $Sim(3)$

For monocular systems there is an extra wrinkle. As explained on the SfM page, scale is unobservable and drifts slowly along the trajectory, so two ends of a loop can be reconstructed at different scales. A rigid $SE(3)$ loop closure cannot absorb that — it insists on a scale-preserving correction that does not exist. The fix is to optimise in $Sim(3)$, adding a per-node scale factor so the 7-DoF correction can rescale as well as rotate and translate. This is what ORB-SLAM does at loop closure, and it is the difference between a monocular loop closing cleanly and the optimiser fighting itself.

6 · Keeping it bounded

The graph grows forever, so a long-lived system needs a strategy. Note these are trades between information and cost, and they are not equivalent:

iSAM2 deserves emphasis: rather than re-solving the whole system each time an edge arrives, it maintains a factorisation in a tree structure (the Bayes tree) and updates only the parts affected by the new measurement, re-linearising selectively. For sequential odometry that is nearly constant time per step; a loop closure touches more of the tree and costs more, which is the correct behaviour. This is what makes the smoothing formulation viable in real time rather than as a batch post-process.

7 · Why the graph formulation won

The decisive rows are the first three. Keeping the measurements means every approximation is revisable: the linearisation can be redone from a better estimate, a bad edge can be down-weighted or removed, and new information can correct old conclusions. Filters make irreversible commitments; smoothers do not. The honest counterpoints are the last two rows — an EKF's bounded execution time and free covariance remain genuine advantages for small, hard-real-time, safety-critical systems, which is why EKF-SLAM is not merely a historical curiosity.

8 · Practical checklist

• Anchor the gauge — fix one node or add a prior, or the solve is singular (§4).
• Wrap angles in residuals. A missing wrap is a classic, spectacular failure.
• Set the information matrices honestly. A loop closure trusted 1000× too much will distort the trajectory to satisfy itself; trusted too little, it will not correct anything. This is the most common source of mediocre results.
• Always verify loop closures geometrically, and use a robust kernel or switchable constraints anyway.
• Use $Sim(3)$ for monocular loop closure to absorb scale drift (§5.4).
• Use keyframes, not every frame.
• Use an established solver — g2o, GTSAM, Ceres. The residuals and Jacobians are yours to get right; the sparse linear algebra is not worth rewriting.
• Use iSAM2 for online operation rather than batch re-solving.
• Marginalise rather than delete when bounding the graph, and sparsify the resulting clique.
• Monitor the per-edge chi-square after optimisation. Edges with persistently large residuals are your bad loop closures, and they are identifiable precisely because the measurements were kept.

9 · Further reading

• Lu & Milios (1997), "Globally Consistent Range Scan Alignment for Environment Mapping," Autonomous Robots 4. The paper that introduced the pose-graph idea.
• Thrun & Montemerlo (2006), "The GraphSLAM Algorithm with Applications to Large-Scale Mapping of Urban Structures," IJRR 25.
• Grisetti, Kümmerle, Stachniss & Burgard (2010), "A Tutorial on Graph-Based SLAM," IEEE ITS Magazine. Start here — the clearest available derivation of the Jacobians and the sparse structure.
• Dellaert & Kaess (2006), "Square Root SAM: Simultaneous Localization and Mapping via Square Root Information Smoothing," IJRR. The information-form/factorisation view.
• Kaess et al. (2012), "iSAM2: Incremental Smoothing and Mapping Using the Bayes Tree," IJRR 31(2).
• Kümmerle et al. (2011), "g2o: A General Framework for Graph Optimization," ICRA.
• Sünderhauf & Protzel (2012), "Switchable Constraints for Robust Pose Graph SLAM," IROS.
• Olson & Agarwal (2013), "Inference on Networks of Mixtures for Robust Robot Mapping," IJRR. Max-mixtures.
• Rosen, Carlone, Bandeira & Leonard (2019), "SE-Sync: A certifiably correct algorithm for synchronization over the special Euclidean group," IJRR.
• Strasdat, Montiel & Davison (2010), "Scale Drift-Aware Large Scale Monocular SLAM," RSS. The $Sim(3)$ treatment.
• Mur-Artal & Tardós (2017), "ORB-SLAM2," IEEE T-RO. A complete, readable system that uses all of the above.
• Cadena et al. (2016), "Past, Present, and Future of Simultaneous Localization and Mapping," IEEE T-RO. The survey that frames the filtering-vs-smoothing question properly.

Related pages: EKF-SLAM · FastSLAM · Bundle adjustment · ICP & registration · Structure from motion · Attitude & rotations