M4-T2 — Pose Graphs

Learning objectives

After this lesson, you should be able to:

• Define the nodes, edges and information matrices of a pose graph.
• Compute the error e_ij of an SE(2) edge from the current poses and the measurement.
• Build a pose graph from odometry and loop-closure measurements and evaluate its total cost.
• Interpret information matrices: what the numbers say about trust in an edge.

The intuition

Picture a necklace of beads: each bead is a place the robot visited, and each string between neighbours is a measurement that says “from here, the next bead is about this far and in this direction.” Hung from the first bead, the necklace is the dead-reckoned path. Now add one more string between the last bead and the first, because the robot recognised the starting place. The necklace no longer fits: some strings must stretch and others bend. Optimisation is finding the shape of the necklace that strains every string as little as possible, with stiffer strings (more confident measurements) allowed to stretch less.

A pose graph is a network of springs. Each edge is a spring whose rest length is the measurement and whose stiffness is the information matrix.

The mathematics

A pose graph has nodes x_i = (x, y, θ), and edges (i, j) each with a measurement z_ij (the pose of node j as seen from node i) and an information matrix Ω_ij = Σ_ij⁻¹.

For a candidate set of poses, the predicted relative pose is X_i⁻¹ X_j (the transform from M2-T1). The edge error is the transform that takes the measurement to the prediction, flattened to a 3-vector:

In components, with d = t_j − t_i and R(·) the rotation matrix:

X_i

the 3×3 matrix of the current pose of node i.

Z_ij, (t_z, θ_z)

the measured relative pose from i to j.

e_ij

zero when the poses agree exactly with the measurement. Its units are metres, metres and radians.

Ω_ij

information matrix (3×3): large entries mean a trusted component. Typical: diag(1/σ_x², 1/σ_y², 1/σ_θ²).

The total cost is

and the maximum-likelihood trajectory minimises it. Each term is a squared Mahalanobis distance: the error measured in units of the measurement’s standard deviation. If the graph is consistent, the minimum F is about the number of constraints minus the number of free variables.

Where edges come from

Without an anchor, the graph defines poses only up to a rigid motion of the whole thing: every constraint is relative. The optimiser fixes the first node (M4-T4).

A worked example: four poses, a drifting square

The robot drives a 1 m square: four poses, each reached from the previous by “1 m forward, then turn 90°”. The odometry has a heading bias: every step measures a turn of 92° instead of 90°. After the fourth pose it recognises the start and measures its pose relative to the start exactly: the loop-closure edge from x₃ to x₀, z = (1, 0, 90°).

1. Initial guess by chaining the odometry. x₀ = (0, 0, 0°); x₁ = (1, 0, 92°); x₂ = (0.965, 0.999, 184°); x₃ = (−0.033, 0.930, 276°). The true x₃ is (0, 1, 270°): the drift is 7.7 cm and 6°.
2. Odometry edges are satisfied by construction: their errors are exactly (0, 0, 0), because the chain was built from them.
3. The loop-closure edge is not. The chain predicts x₀ relative to x₃ as something that differs from the measured (1, 0, 90°) by e = (−0.065 m, +0.072 m, −0.105 rad): 6° in angle and about 10 cm in position.
4. Cost. With Ω = diag(100, 100, 400) (σ = 10 cm, σ_θ = 0.05 rad): F = 100(0.065²) + 100(0.072²) + 400(0.105²) = 0.42 + 0.52 + 4.41 = 5.33. The angle error dominates: 6° is two standard deviations of its edge.
5. What the optimiser will do. It distributes the 6° of excess turning over all four edges instead of pinning it on the loop-closure edge. After two Gauss-Newton iterations the cost falls to 1.11, and the position error of x₃ falls from 7.7 cm to 2.1 cm (M4-T4 shows the iterations).

The remaining cost, 1.11, is not a failure: with four edges (12 constraints) and three free poses (9 unknowns) the expected minimum is about 3; a value near or below that says the odometry’s claimed accuracy was not overconfident.

Engineering implications and limitations

Engineering implications

• Define the error carefully. The edge error depends on the convention (which frame the measurement is in, which composition order). A wrong convention gives a graph that converges to the wrong shape. Test with a trivial graph whose answer you know, like the square above.
• Choose nodes sparsely. Between nodes the information of dozens of scans is folded into one edge. Spacing of 0.3–1 m or a few degrees is common in 2D; keyframes for cameras.
• Set information from evidence. A constant Ω is a crude guess. Scale it by matching quality and by distance; use the Hessian of the matcher when available. Never give a doubtful edge a high information.
• Keep the graph. Serialise nodes, edges and information (g2o format, TORO, or your own). Datasets such as Intel, MIT Killian Court and Manhattan3500 are standard benchmarks.
• Mind the angle. The wrapped angle error is non-smooth at ±π; a graph with large angle errors can start in the wrong basin (good initialisation, M4-T4).

Limits

A pose graph throws away the maps: once poses are optimised, the maps (grids, point clouds) must be re-rendered from the stored scans. It treats edges as independent, which is false when two edges come from the same scan, and assumes Gaussian errors, which a bad loop closure violates (M4-T6). It keeps only relative poses; it cannot represent landmarks unless extended into a factor graph (bundle adjustment, M4-T7).

Common misconceptions

Engineering takeaways

1. A pose graph has poses as nodes and measured relative poses with information matrices as edges.
2. The edge error is t2v(Z⁻¹ X_i⁻¹ X_j): zero when the poses match the measurement.
3. The cost is the sum of squared Mahalanobis errors; the best trajectory minimises it.
4. Odometry edges are consistent by construction; loop closures expose the drift.
5. An anchor is needed because all constraints are relative.

References and further reading

Primary papers and authoritative documentation.

• Lu, F. and Milios, E. (1997). Globally consistent range scan alignment for environment mapping. Autonomous Robots 4(4): the first pose-graph formulation.
• Grisetti, G., Kümmerle, R., Stachniss, C. and Burgard, W. (2010). A tutorial on graph-based SLAM. IEEE Intelligent Transportation Systems Magazine 2(4).
• Thrun, S. and Montemerlo, M. (2006). The GraphSLAM algorithm with applications to large-scale mapping of urban structures. International Journal of Robotics Research 25(5–6).
• Dellaert, F. and Kaess, M. (2006). Square root SAM: simultaneous localization and mapping via square root information smoothing. International Journal of Robotics Research 25(12).
• Carlone, L., Aragues, R., Castellanos, J. A. and Bona, B. (2012). A linear approximation for graph-based simultaneous localization and mapping. Robotics: Science and Systems.

Knowledge check

Three conceptual questions. Write an answer, then reveal the explanation. Your answers stay in your browser.

What’s next

To minimise the cost we need an optimiser. The next lesson derives it in general: Gauss-Newton and Levenberg-Marquardt for nonlinear least squares.

Related content

Nothing below is required to finish this lesson. The last 2 entries are lessons of other courses on this site that teach the background or go further.

• Before
  M4-T1: Front-End and Back-End SLAM

  Split a SLAM system into data-association front end and optimisation back end and say what each owns.

• Lab
  M4-L1: Visualize a Pose Graph

  Build a pose graph from odometry and loop-closure measurements and inspect its edges, residuals and information.

• Probability
  MAP Estimation and Weighted Least Squares

  Why maximising a Gaussian posterior is a weighted least-squares problem.

• Probability
  Factor Graphs

  The probabilistic reading of a graph of constraints.