M11-T4 — Nonlinear Least Squares and Pose Graph Optimization

Learning objectives

After this lesson, you should be able to:

• Define the pose-graph objective with relative-pose residuals, and run Gauss–Newton on it.
• Explain the gauge freedom and how an anchor removes it.
• Show how a loop closure corrects accumulated drift along the whole path.
• Compare squared, Huber and Cauchy losses against false loop closures, and state what each can and cannot do.

The intuition

The robot of the left panel drives a square: 40 poses, 1 m between them, a quarter turn at each corner. It estimates each step from odometry, with a little noise (5 cm, 0.02 rad). Adding the steps (dead reckoning, the orange dashed line) gives a path that does not return to where it started: the last pose is 0.73 m from the truth, and the path misses the start by 0.74 m. Then the robot recognises its starting place and obtains a measurement: pose 39 is 1 m from pose 0, with a small error (2 cm).

Each measurement is a factor on two poses; the objective is the sum of their squared residuals, each weighted by its inverse covariance. Gauss–Newton linearises it, solves the sparse system and repeats. The loop closure is very precise (2 cm), so it carries a large weight and the cost drops from 1 538 to 24.6 after one iteration and to 1.5 after three. The estimate (blue) closes the square and lies on the truth: the rms position error drops from 0.52 to 0.22 m, and the last pose is 3.6 cm off. The correction is spread over the whole path, each pose moving a little, in proportion to how much its odometry is trusted.

Without the loop closure, optimisation changes nothing: the odometry is a chain, and a chain has a unique solution that fits every edge exactly (the optimised error, 0.522 m, equals the dead-reckoning error). The graph needs a cycle to carry information against drift.

The right panel shows the danger. Add five false loop closures, each claiming that two distant poses are within a metre or two of each other. With the squared loss (the maximum likelihood objective) the optimiser tries to satisfy them all and distorts the path: 1.54 m of error, three times the dead-reckoning error. The Huber loss, which limits the pull of a large residual to linear growth, is no better here (1.94 m). The Cauchy loss, whose pull decreases for very large residuals, ignores the false constraints and recovers 0.19 m, as good as the 0.22 m obtained with the one good closure alone: the five false constraints are rejected and the good one retained.

A pose graph needs loop closures to correct drift, and a single wrong one can ruin it. Robust losses are the defence, and the choice of loss is a choice between convexity and the ability to reject.

Definitions

Pose graph

Variables: poses $x_i\in SE(2)$ (x, y, heading). Factors: relative-pose measurements $z_{ij}$ between two poses.

Edge residual

$\mathbf{e}_{ij}(x)=z_{ij}\ominus(x_j\ominus x_i)$: the measured relative pose minus the relative pose implied by the estimates, with the heading difference wrapped.

Loop closure

An edge between two poses that are far apart in time but close in space, found by recognising a place seen before.

Gauge freedom

Moving or rotating the whole graph rigidly leaves every relative residual unchanged, so the objective has a zero-curvature direction (three in 2-D). An anchor on one pose, or a prior, removes it.

Robust loss $\rho$

A function that replaces the square in the objective, $\sum_k\rho(\lVert\mathbf{e}_k\rVert_{\Sigma_k^{-1}})$, so that large residuals have less influence.

IRLS

Iteratively reweighted least squares: each iteration is a weighted Gauss–Newton step with weights $w_k=\rho'(s_k)/s_k$ evaluated at the current residuals.

Derivation: the linearised residual, the gauge, and the weights of a robust loss

The Gauss–Newton system for a pose edge

The residual of an edge depends only on the two poses it joins, $\mathbf{e}_{ij}(x_i,x_j)$. Linearise around the current estimate: $\mathbf{e}_{ij}(x+\Delta)\approx\mathbf{e}_{ij}+A_{ij}\Delta_i+B_{ij}\Delta_j$, with $A_{ij}=\partial\mathbf{e}/\partial x_i$ and $B_{ij}=\partial\mathbf{e}/\partial x_j$ (3 × 3 matrices; the program evaluates them by finite differences).

1. $$H_{ii}\mathrel{+}=A^\top\Omega A,\quad H_{jj}\mathrel{+}=B^\top\Omega B,\quad H_{ij}\mathrel{+}=A^\top\Omega B,\quad H_{ji}\mathrel{+}=B^\top\Omega A$$

   Each edge adds four 3 × 3 blocks to the matrix: the factor-graph assembly of lesson 3.

2. $$\mathbf{b}_i\mathrel{+}=A^\top\Omega\,\mathbf{e},\qquad\mathbf{b}_j\mathrel{+}=B^\top\Omega\,\mathbf{e}$$

   The right-hand side collects the gradient.

3. $$H\,\Delta=-\mathbf{b},\qquad x\leftarrow x\oplus\Delta$$

   Solve and update the poses; the heading increments are wrapped.

The gauge

A rigid transformation $T$ applied to every pose leaves each relative pose $x_j\ominus x_i$ unchanged, so every residual is unchanged. Hence $H\,\delta=0$ for the three directions of rigid motion: $H$ is singular and the step is not unique. The first pose is therefore held fixed (adding a large number to its diagonal block, or removing it from the unknowns): the problem is then the MAP problem with an infinitely confident prior on the first pose, the prior $x_0$ of lesson 2. Note what the anchor does to the answer: nothing, up to a rigid motion of the whole map. What it does to the covariance: the uncertainty then grows with the graph distance from the anchor, as in the example (0.01 m at the anchor to 0.2 m at p5 in lesson 3).

Robust losses by reweighting

For a loss $\rho(s)$ applied to the Mahalanobis size $s=\lVert\mathbf{e}\rVert_\Omega$, the gradient of the objective is the squared-loss gradient multiplied by $\rho'(s)/(2s)$.

1. $$\nabla\sum_k\rho(s_k)=\sum_k\frac{\rho'(s_k)}{s_k}\,J_k^\top\Omega_k\mathbf{e}_k$$

   By the chain rule, with $\nabla s=J^\top\Omega\mathbf{e}/s$.

2. $$w_k=\frac{\rho'(s_k)}{2s_k}:\qquad w_\mathrm{Huber}=\min\!\bigl(1,\,k/s\bigr),\quad w_\mathrm{Cauchy}=\frac{1}{1+s^2/k^2}$$

   The weight of an edge in the next step. For Huber the weight of a large residual is k/s, so its influence $w\,s=k$ is capped but not zero. For Cauchy the weight is $1/(1+s^2/k^2)$ and the influence $s/(1+s^2/k^2)$ falls to zero for large s.

3. $$\text{influence}(s)=w(s)\,s:\quad \text{L2: }s;\ \text{Huber: }\min(s,k);\ \text{Cauchy: }\frac{s}{1+s^2/k^2}$$

   The influence is how hard an edge pulls on the solution. A false loop closure has a huge s (here 75 to 150 standard deviations).

With five outliers of 75 to 150 standard deviations in each coordinate, a Huber edge still pulls with strength $k$ (here 3), about three times what an honest edge (s ≈ 1) pulls with, and five of them together outweigh the good loop closure and the odometry. The Cauchy edge pulls with strength about $k^2/s$, near zero. The price of that rejection is that the objective is not convex: IRLS from a poor starting point can lock onto a wrong solution (or reject a correct constraint that looks wrong at the start), so robust losses need a reasonable initial estimate (here the dead-reckoned one) or a graduated schedule.

L2 trusts everything and is distorted by one liar; Huber bounds the lie’s influence but does not remove it; Cauchy and its relatives ignore large lies, at the price of non-convexity.

A worked example

A square loop of 40 poses (1 m steps, quarter turns every 10 poses). Odometry edges have noise 5 cm, 5 cm and 0.02 rad; the loop closure 2 cm, 2 cm and 0.01 rad; the first pose is anchored.

1. Dead reckoning. Position error 0.522 m (rms over the poses), the last pose 0.725 m off, and the loop closure disagrees with it by 0.736 m.
2. Optimising without and with the loop closure. With the odometry alone the optimum is dead reckoning: 0.522 m. With the loop closure the cost per iteration is 1 537.8, 24.6, 1.5, 1.5, 1.5 (converged after three steps) and the rms error 0.218 m; the last pose is 0.036 m off.
3. Five false loop closures. Each links two poses that are 15 to 29 steps apart and claims a relative position that is 1.5 to 3 m wrong in each coordinate. Pose error (rms): squared loss 1.543 m; Huber 2.014, 1.942 and 1.883 for k = 1, 3, 5; Cauchy 0.211, 0.192 and 0.333 for k = 1, 3, 5. The Cauchy values with k ≤ 3 are about as good as the 0.218 m obtained with the single good loop closure alone, and k = 5 starts to let the outliers back in.

import math
from robotprob import Rng, solve, mean, wrap_angle

# ground truth: a robot drives a square loop of 40 poses (10 per side, 1 m steps, a 90 degree turn at each corner)
def compose(a, b):
    c, s = math.cos(a[2]), math.sin(a[2])
    return [a[0] + c * b[0] - s * b[1], a[1] + s * b[0] + c * b[1], wrap_angle(a[2] + b[2])]
def relative(a, b):                                     # the pose of b in the frame of a
    c, s = math.cos(a[2]), math.sin(a[2]); dx, dy = b[0] - a[0], b[1] - a[1]
    return [c * dx + s * dy, -s * dx + c * dy, wrap_angle(b[2] - a[2])]
N = 40
truth = [[0.0, 0.0, 0.0]]
for i in range(1, N):
    step = [1.0, 0.0, math.pi / 2 if i % 10 == 0 else 0.0]
    truth.append(compose(truth[-1], step))
SIG = (0.05, 0.05, 0.02)                                # noise std of an odometry edge (x, y, theta)
rng = Rng(7)
def noisy(rel, sig):
    return [rel[0] + rng.gauss(0.0, sig[0]), rel[1] + rng.gauss(0.0, sig[1]), wrap_angle(rel[2] + rng.gauss(0.0, sig[2]))]
edges = [(i, i + 1, noisy(relative(truth[i], truth[i + 1]), SIG), SIG) for i in range(N - 1)]
loop = (N - 1, 0, noisy(relative(truth[N - 1], truth[0]), (0.02, 0.02, 0.01)), (0.02, 0.02, 0.01))     # the loop closure: the start is recognised again

def error(a, b, z):                                     # the residual of an edge: the measured relative pose against the one the estimate implies
    r = relative(a, b)
    return [r[0] - z[0], r[1] - z[1], wrap_angle(r[2] - z[2])]

def optimise(edges, init, loss='l2', iters=12, k=1.0, verbose=False):
    x = [list(p) for p in init]
    n = len(x)
    history = []
    for it in range(iters):
        A = [[0.0] * (3 * n) for _ in range(3 * n)]; b = [0.0] * (3 * n)
        cost = 0.0
        # the anchor: the first pose is held at its initial value (the gauge freedom of the problem)
        for d in range(3): A[d][d] += 1e8
        for (i, j, z, sg) in edges:
            e = error(x[i], x[j], z)
            w = [1.0 / s ** 2 for s in sg]
            m2 = sum(w[d] * e[d] ** 2 for d in range(3))              # the squared Mahalanobis size of the residual
            cost += m2
            if loss == 'huber':
                f = 1.0 if m2 <= k * k else k / math.sqrt(m2)
            elif loss == 'cauchy':
                f = 1.0 / (1.0 + m2 / (k * k))
            else:
                f = 1.0
            J = []                                                     # numerical Jacobian of the residual with respect to the 6 pose variables
            for var, idx in ((i, 0), (j, 3)):
                for d in range(3):
                    xp = [list(p) for p in (x[i], x[j])]
                    xp[0 if idx == 0 else 1][d] += 1e-6
                    ep = error(xp[0], xp[1], z)
                    J.append([(ep[c] - e[c]) / 1e-6 for c in range(3)])
            # J[col][row]: assemble J^T W J and J^T W e
            cols = [3 * i + d for d in range(3)] + [3 * j + d for d in range(3)]
            for p in range(6):
                for q in range(6):
                    A[cols[p]][cols[q]] += f * sum(J[p][c] * w[c] * J[q][c] for c in range(3))
                b[cols[p]] -= f * sum(J[p][c] * w[c] * e[c] for c in range(3))
        history.append(cost)
        dx = solve(A, b)
        for i in range(n):
            x[i] = [x[i][0] + dx[3 * i], x[i][1] + dx[3 * i + 1], wrap_angle(x[i][2] + dx[3 * i + 2])]
        if max(abs(v) for v in dx) < 1e-7: break
    return x, history

def rmse(x):
    return math.sqrt(mean([(p[0] - t[0]) ** 2 + (p[1] - t[1]) ** 2 for p, t in zip(x, truth)]))

# dead reckoning: integrate the odometry edges
dead = [[0.0, 0.0, 0.0]]
for (i, j, z, sg) in edges: dead.append(compose(dead[-1], z))
print('poses:', N, ' odometry edges:', len(edges), ' loop closures: 1')
print('dead reckoning: position error rms', round(rmse(dead), 3), ' last pose off by', round(math.hypot(dead[-1][0] - truth[-1][0], dead[-1][1] - truth[-1][1]), 3), ' loop closure disagrees with it by', round(math.hypot(*error(dead[-1], dead[0], loop[2])[:2]), 3))
x1, h1 = optimise(edges, dead, 'l2')
x2, h2 = optimise(edges + [loop], dead, 'l2')
print('without the loop closure (optimised):  rms', round(rmse(x1), 3))
print('with the loop closure, cost per iteration:', [round(c, 1) for c in h2[:7]])
print('with the loop closure: rms', round(rmse(x2), 3), ' last pose off by', round(math.hypot(x2[-1][0] - truth[-1][0], x2[-1][1] - truth[-1][1]), 3))

# wrong loop closures: five false ones, each claiming that two distant poses are close together
bad = []
for (i, j) in ((7, 31), (12, 27), (3, 22), (16, 35), (9, 38)):
    z = relative(truth[i], truth[j]); z = [z[0] + rng.uniform(1.5, 3.0), z[1] + rng.uniform(1.5, 3.0), wrap_angle(z[2] + rng.uniform(-1.0, 1.0))]
    bad.append((i, j, z, (0.02, 0.02, 0.01)))
print('with 5 false loop closures added to the good one:')
print('loss          pose rms error (m)')
for loss, k in (('l2', 1.0), ('huber', 1.0), ('huber', 3.0), ('huber', 5.0), ('cauchy', 1.0), ('cauchy', 3.0), ('cauchy', 5.0)):
    x, h = optimise(edges + [loop] + bad, dead, loss, iters=25, k=k)
    print((loss + ' k=' + str(k)).ljust(12), str(round(rmse(x), 3)).rjust(10))

poses: 40  odometry edges: 39  loop closures: 1
dead reckoning: position error rms 0.522  last pose off by 0.725  loop closure disagrees with it by 0.736
without the loop closure (optimised):  rms 0.522
with the loop closure, cost per iteration: [1537.8, 24.6, 1.5, 1.5, 1.5]
with the loop closure: rms 0.218  last pose off by 0.036
with 5 false loop closures added to the good one:
loss          pose rms error (m)
l2 k=1.0          1.543
huber k=1.0       2.014
huber k=3.0       1.942
huber k=5.0       1.883
cauchy k=1.0      0.211
cauchy k=3.0      0.192
cauchy k=5.0      0.333

In robotics

The standard SLAM back end

Pose-graph optimisation is the core of modern LiDAR and visual SLAM systems: a front end produces odometry and loop-closure edges, the back end optimises them. Packages such as g2o, GTSAM, Ceres and the SLAM Toolbox implement this with sparse Cholesky factorisation. The SLAM course covers the pipeline in its back-end lessons, and the Navigation course uses the resulting maps.

Making loop closures safe

• Verify before accepting: geometric verification of the scan match, a minimum score, consistency with the current pose uncertainty (a Mahalanobis gate as in Module 8).
• Use a robust loss on loop-closure edges (Cauchy or a switchable constraint, or dynamic covariance scaling), not on odometry.
• Check consistency across several closures: a set of closures that agree with each other (pairwise consistency maximisation) is far less likely to be false than one that stands alone.
• Keep a copy of the graph so that a bad closure found later can be removed and the graph re-optimised.

Gauge and the covariance

Because of the gauge, the absolute uncertainty of a pose is only meaningful relative to the anchor. After optimisation the covariance of the poses relative to the first one grows with the graph distance from it; the covariance of one pose relative to another nearby one stays small. Report relative uncertainty for local decisions.

Practical checks

• Start from a good initial estimate (odometry); Gauss–Newton on a graph with large heading errors can converge to a local minimum.
• Check the Jacobians with finite differences, and wrap angles in residuals and updates.
• Look at the cost per edge after optimisation: a few edges with large residuals mark inconsistent measurements.
• Use Levenberg–Marquardt or dogleg steps for robustness on hard problems.

Common misconceptions

Key takeaways

1. Pose-graph optimisation minimises the sum of squared relative-pose residuals, weighted by the inverse covariances, by Gauss–Newton on a sparse system.
2. A loop closure creates a cycle and corrects drift along the whole path: the rms error fell from 0.52 to 0.22 m and the last pose from 0.725 to 0.036 m off. Without a cycle the optimum equals dead reckoning.
3. The objective is invariant under rigid motion of the graph: an anchor on one pose removes the gauge freedom, and absolute covariances are relative to it.
4. False loop closures wreck a squared-loss solution (1.54 m); Huber caps their pull but did not rescue it (1.94 m); Cauchy rejected them (0.19 m).
5. Redescending losses are non-convex and need a good initial guess; verify loop closures before accepting them.

References and further reading

Each source is marked free or paid. Free to read does not mean free to reuse: the course resources page lists every source with its licence.

• F. Dellaert and M. Kaess, “Factor Graphs for Robot Perception”, Foundations and Trends in Robotics 6 (1–2), 2017 , including robust losses and pose-graph optimisation. Free to read as the authors’ PDF; copyrighted.
• G. Grisetti, R. Kümmerle, C. Stachniss and W. Burgard, “A Tutorial on Graph-Based SLAM”, IEEE Intelligent Transportation Systems Magazine 2 (4), 2010. Paid or institutional access; the authors’ copies were not checked here.
• C. Stachniss, “Graph-based SLAM using Pose Graphs” (video lecture) . Free to watch.
• T. D. Barfoot, State Estimation for Robotics, 2nd edition, 2024 , the nonlinear batch estimation and the robust cost functions of chapter 5. Free to read as the author’s PDF; the book is copyrighted.

Knowledge check

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

What’s next

This is the end of Module 11. Module 12 collects the advanced topics that the earlier modules needed: angles and rotations as manifolds, the error-state Kalman filter, smoothing with future measurements, and decisions under uncertainty with POMDPs.

Related content

Nothing below is required to finish this lesson.

• Before
  M11-T3: Factor Graphs

  Draw the factor graph of a localization or SLAM problem, write the function it represents, and explain why its sparsity matters.

• Lab
  M11-L1: Occupancy Grid and Pose Graph SLAM

  The map of the lab has a doubled wall where the loop should close. Build the grid in log odds, find why it smears, and correct the trajectory with a pose graph before mapping again.

Other courses on this site treat neighbouring topics from their own angle:

• SLAM M4-T2: Pose Graphs
• SLAM M4-L1: Visualize a Pose Graph
• UAV M4-T12: Pose graph optimization
• SLAM: Robust Optimization