Skip to the lesson
FixTheRobot Explore Robotics / Localization & SLAM / M4 Graph-Based SLAM / M4-T4

Localization & SLAM · Theory · M4

M4-T4 — Graph Optimization

Gauss-Newton on a pose graph: build one block of H and b per edge, fix the anchor, solve, update.

  • Lesson 4 of 8
  • Module 4: Graph-Based SLAM
  • Theory
  • About 9 minutes to read

M4-T3 gave the general algorithm; M4-T2 gave the pose-graph error. This lesson joins them. Each edge contributes four small blocks to a big sparse matrix; solving it moves every pose a little; a few iterations later the loop is closed. The details that cost people days are the Jacobians, the anchor, and updating angles correctly.

Learning objectives

After this lesson, you should be able to:

  • Write the Jacobians of the SE(2) edge error with respect to the two poses.
  • Build H and b by accumulating blocks per edge, and explain the sparsity.
  • Explain the gauge freedom and fix it with an anchor.
  • Run an optimisation iteration on a small loop and interpret the change in cost and poses.

The problem and the intuition

Take the necklace of beads again. For each string you can ask: “if I nudged each of its two beads a little, how would its strain change?” That is the Jacobian of one edge. Collect all those answers into one big linear problem: nudges for all beads that, together, reduce the total strain the most. Solve it, apply the nudges, and recompute. Each string only involves two beads, so the big problem is mostly empty and can be solved fast.

Graph optimisation is Gauss-Newton where every edge is a tiny problem touching only two poses.

Sparse structure of the pose-graph normal equationsA ten by ten block grid. Blue blocks are non-zero: the diagonal, the two neighbouring diagonals from the odometry edges, and four off-diagonal blocks from two loop closures. Most of the matrix is empty. H = Σ over edges of (Jᵀ Ω J) An edge between i and j adds blocks (i,i), (i,j),(j,i) and (j,j) only. Odometry: the band along the diagonal.Loop closures: the isolated off-diagonal blocks. A sparse Cholesky factorisation exploits this:cost grows far slower than N³.
The normal-equation matrix of a pose graph: a band from the odometry and isolated blocks from the loop closures.

The algorithm: Gauss-Newton on SE(2) pose graphs

Problem. Given the graph of M4-T2, find the poses x0…xn−1 that minimise F(x) = Σ eijᵀ Ωij eij.

Assumptions. A reasonable initial guess (the odometry chain); Gaussian errors; correct constraints (M4-T5 and M4-T6 relax this); one node fixed to remove the gauge freedom.

Jacobians of one edge

With Ri = R(θi), Rz = R(θz), d = tj − ti and exy = Rzᵀ (Riᵀ d − tz), eθ = θj − θi − θz:

SE(2) edge Jacobians
Aij=∂e∂xi=[−Rz⊤Ri⊤Rz⊤ dRi⊤dθi d0⊤−1]A_{ij} = \frac{\partial \mathbf e}{\partial \mathbf x_i} = \begin{bmatrix}-\mathbf R_z^{\top}\mathbf R_i^{\top} & \mathbf R_z^{\top}\,\dfrac{\mathrm d\mathbf R_i^{\top}}{\mathrm d\theta_i}\,\mathbf d\\ \mathbf 0^{\top} & -1\end{bmatrix}
Bij=∂e∂xj=[Rz⊤Ri⊤00⊤1]B_{ij} = \frac{\partial \mathbf e}{\partial \mathbf x_j} = \begin{bmatrix}\mathbf R_z^{\top}\mathbf R_i^{\top} & \mathbf 0\\ \mathbf 0^{\top} & 1\end{bmatrix}
Derivative of the rotation
dRi⊤dθi=[−sin⁡θicos⁡θi−cos⁡θi−sin⁡θi]\frac{\mathrm d\mathbf R_i^{\top}}{\mathrm d\theta_i} = \begin{bmatrix}-\sin\theta_i & \cos\theta_i\\ -\cos\theta_i & -\sin\theta_i\end{bmatrix}

Aij is 3×3 and acts on node i’s (x, y, θ); Bij on node j’s. All other nodes do not appear in the edge, so the rest of the Jacobian row is zero.

Building and solving the system

  1. Zero H (3n × 3n) and b (3n).
  2. For each edge (i, j): compute e = eij, A, B. Add to the four blocks and two gradient blocks: Hii += AᵀΩA, Hij += AᵀΩB, Hji += BᵀΩA, Hjj += BᵀΩB; bi += AᵀΩe, bj += BᵀΩe.
  3. Fix the gauge. Add a strong prior to node 0 (for example H00 += 10⁶ I), or remove its rows and columns.
  4. Solve H Δ = −b with a sparse Cholesky factorisation.
  5. Update every pose xk ← xk ⊞ Δk and wrap the angle. Recompute F. Repeat until the step is tiny or F stops decreasing (LM damping if it does not).

The gauge freedom

Every error depends only on relative poses. If all poses are moved by the same rigid motion, no error changes: H has a 3-dimensional null space (in 2D). Without an anchor it is singular and the solve fails or returns an arbitrary shift. The anchor is not an approximation: it chooses which of the equivalent solutions to report.

Aij, Bij
3×3 Jacobian blocks of edge (i, j) with respect to nodes i and j.
H, b
the Gauss-Newton system: H = Σ JᵀΩJ, b = Σ JᵀΩe, assembled block by block.
⊞
the manifold update: for SE(2) with a rotation angle it is addition followed by angle wrapping.
anchor
a prior on one node (or the removal of it from the unknowns) that removes the gauge freedom.

Strengths, limits and failure modes

  • Efficient: the sparsity gives near-linear cost with good variable ordering (COLAMD, AMD); incremental solvers (iSAM2) reuse earlier work.
  • Local: needs a decent initial guess. Large rotation errors can trap it in a bad minimum; rotation-first initialisation helps (Carlone et al., 2015).
  • Fragile to bad edges: one wrong loop closure with high information bends the map (M4-T5, M4-T6).
  • Overconfident information makes one edge dominate, while too little information leaves the loop uncorrected.
  • Conditioning: long chains with few loops are ill-conditioned along the chain; use damping and good units.

A worked example: closing the square

The four-pose square of M4-T2: odometry with a 2° heading bias per step, then a loop closure from x3 to x0. Ω = diag(100, 100, 400) on every edge; x0 is anchored.

  1. Before. The three odometry edges have e = 0 and the loop edge e = (−0.065, 0.072, −0.105). F = 5.33.
  2. Assemble. Four edges contribute 16 blocks into a 12 × 12 matrix H, which is block-tridiagonal plus the (3, 0) block from the loop edge. With x0 anchored, the 9 × 9 system for x1, x2, x3 is solved.
  3. First iteration. The step has norm 0.120: it turns the headings of x1, x2 and x3 by −1.5°, −3.1° and −4.6° (progressively more along the chain: about 1.5° per edge) and moves x3 by 6.7 cm. The cost falls from 5.33 to 1.114.
  4. Second iteration. Norm 0.0055, cost 1.1108. Third: change below 10⁻⁵. The problem is nearly linear, so Gauss-Newton converges in two steps.
  5. After. The poses are x1 = (0.996, 0.004, 90.5°), x2 = (0.983, 1.008, −179.0°), x3 = (−0.021, 0.996, −88.5°). Their position errors against the true square are 0.6, 1.9 and 2.1 cm, down from 0, 3.5 and 7.7 cm: the largest error drops 3.7-fold. The optimiser spread the 6° of excess rotation over all steps, about 1.5° each, instead of letting it pile up at the end.
  6. Why the cost stays at 1.11. The residuals are spread over the four edges at about the noise level: each edge’s error is now below one standard deviation, which is what a consistent solution looks like.

The lab asks you to implement exactly this loop and run it on a drifted loop with a hundred poses.

Engineering implications and limitations

Engineering implications

  • Unit-test the Jacobians with finite differences on random poses: a sign error in one block makes the optimiser “converge” to a distorted map.
  • Use a library for production (g2o, GTSAM, Ceres, SE-Sync) but know what it does: they take the same graph, the same errors and the same information.
  • Iterate with a trust region (LM, dogleg) rather than plain Gauss-Newton on real data.
  • Report χ² and the number of iterations. A cost far above the expected (constraints minus unknowns) points to a bad edge or an optimistic information matrix; a very small cost may mean the information is too low.
  • Optimise incrementally online (iSAM2) or on keyframe batches, and in a background thread. Do not stall the tracker waiting for it.
  • Anchor in the right place. The first pose is conventional. If a GPS or a known landmark is available, add it as a prior edge instead of fixing node 0: it also fixes the global frame.

Limits

The method assumes small errors for the linearisation. A badly rotated initial guess may need many LM iterations or fail; the 3D version has additional singularities in the rotation parameterisation (use the exponential map). Marginalising old nodes (sliding-window smoothers) introduces fill-in and approximation. And the optimiser assumes the graph’s edges are independent: reusing the same scan in two edges double-counts it.

Common misconceptions

“The optimiser knows which edge is wrong.”

It only minimises the weighted sum. A wrong edge pulls the solution toward itself in proportion to its information.

“Fixing node 0 is an arbitrary hack.”

It is required: without it H is singular because all constraints are relative.

“The loop closure is corrected at the loop-closure pose.”

The correction is spread over the whole loop in proportion to the information of the edges.

“More iterations make it more accurate.”

After convergence the step is zero. A final cost that is too high is a modelling problem, not an iteration-count problem.

Engineering takeaways

  1. Each edge contributes four 3×3 blocks to H and two to b; the result is sparse (band plus loop-closure blocks).
  2. The SE(2) Jacobians are A = [−RzᵀR_iᵀ, Rzᵀ(dR_iᵀ/dθ)d; 0, −1] and B = [RzᵀR_iᵀ, 0; 0, 1].
  3. A gauge freedom (three degrees in 2D) requires an anchor; the anchor chooses a solution, not an approximation.
  4. Solve with a sparse Cholesky, update poses on the manifold, wrap angles, and iterate with LM damping.
  5. The optimiser spreads loop-closure error across the loop according to the information of the edges.

References and further reading

Established fundamentals first; certifiable and learned back ends (Module 8) are active research.

Knowledge check

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

Question 1

An optimisation fails with a singular matrix when no node is fixed. Why, and does fixing node 0 change the answer?

Explanation

All errors depend only on relative poses, so moving every pose by the same rigid motion changes nothing: H has a null space and cannot be inverted. Fixing node 0 selects one of the equivalent solutions; the shape of the optimised trajectory is unchanged, only its placement in the world frame.

Question 2

After optimisation of the square loop, the poses x1 and x2, which are far from the loop-closure edge, also moved. Why?

Explanation

The loop closure creates an inconsistency that can only be removed by changing the chain: the optimiser distributes the correction over every edge according to its information. Every pose in the loop shares some of the change; those at the far end move most because errors accumulate along the chain.

Question 3

The final cost of an optimised graph is 50 times the expected value (constraints minus unknowns). Name two possible causes.

Explanation

A wrong constraint (for example a false loop closure) that cannot be reconciled, or information matrices that are too optimistic (the measurements are less accurate than claimed). A third possibility is a local minimum from a bad initial guess.

What’s next

The graph optimiser is only as good as its loop closures. Next: how a robot decides that it has returned to a place, and what it costs to be wrong.

Practice this lesson

Correct Accumulated Trajectory Drift

Implement Gauss-Newton on a pose graph and bring a drifted loop trajectory back together.

Next lesson

M4-T5 — Loop Closure

Explain loop-closure detection and verification, and the cost of accepting a false closure.

Nothing below is required to finish this lesson. The last entry is a lesson of other courses on this site that teach the background or go further.