Pose-graph optimization: fit robot positions and headings together

Pose-graph optimization adjusts robot poses so their relative motions better agree with supplied measurements. A small heading correction can also move many position estimates because translations depend on the robot's local axes.

This lesson fits five planar poses, each containing x, y, and yaw. You can inspect every accepted update and see why a lower fitting cost can still produce a worse route.

Build a graph of poses and measurements

Each node stores a pose Xᵢ = (xᵢ, yᵢ, θᵢ). An edge stores a measured relative pose Zᵢⱼ connecting nodes i and j. Odometry often connects consecutive poses; a loop measurement can connect a later pose to an earlier one.

Grisetti and colleagues' graph-based SLAM tutorial separates graph construction from fitting the graph. The optimizer receives constraints and adjusts their variables. It does not establish that two scans depict the same place.

The earlier loop-closure example optimized positions with fixed orientation and global-axis displacements. Here, the unknown headings rotate the displacement predictions. That coupling makes the objective nonlinear.

A pose graph estimates the trajectory from relative-pose constraints. A landmark graph can also include landmark positions as variables, as in the SLAM introduction. This widget contains no landmark variables or raw sensor data.

Predict motion in the starting pose frame

Use Xᵢ = T_WBᵢ to map pose i's body coordinates into world coordinates. Then Xᵢ⁻¹Xⱼ maps body j coordinates into body i coordinates. Its translation locates pose j's origin in pose i's axes.

Writing tᵢ = (xᵢ, yᵢ), the predicted relative translation is:

For example, pose i at (2, 0, 90°) and pose j at (2, 2, 180°) differ by (0, 2) m in world coordinates. Pose i faces north, so the same relative translation is (2, 0) m in its body frame. Their relative yaw is 90°.

Keep this convention attached to every measurement. A body-frame displacement cannot be compared directly with a world-frame position difference. The homogeneous-transformation lesson develops the composition and inverse used here.

Define the residual before minimizing it

The lab uses prediction minus measurement for translation, expressed in body frame i. It wraps the yaw difference to the shortest signed angle:

Compute wrap(a) as atan2(sin a, cos a). For example, a predicted yaw of −179° and a measured yaw of +179° disagree by +2°, not −358°. The residual has a branch cut at ±π; derivatives assume the local step stays away from that cut.

This is a specified coordinate residual for planar poses. Some libraries use a tangent-space residual such as Log(Zᵢⱼ⁻¹Xᵢ⁻¹Xⱼ). Its translation coordinates can differ, so residual definitions and covariance frames must match before comparing implementations.

The GTSAM tutorial shows pose variables, relative-pose factors, and nonlinear optimization. Our implementation makes its own small residual and solver explicit; it does not call GTSAM.

Put meters and radians on stated scales

Translation residuals use meters; yaw residuals use radians. Divide by stated scales before adding their squared values. Each ordinary edge uses σₜ = 0.2 m and σθ = 0.1 rad:

This objective is dimensionless. The corresponding diagonal information matrix is w diag(25, 25, 100), with the component units implied by the residual coordinates. Odometry edges have w = 1; the loop multiplier ranges from 0 to 16.

Under an independent, zero-mean Gaussian residual model, information is inverse covariance and weighted least squares gives a likelihood-based fit. These synthetic observations contain deliberate bias. Their chosen scales set influence; this experiment does not validate a noise model or produce calibrated confidence intervals.

At loop multiplier 4, the same influence would result from dividing its scales by √4: 0.1 m and 0.05 rad. Multiplier 0 removes the loop from the objective.

For the preceding two-pose example, suppose odometry reports (2.1 m, 0 m, π/2 + 0.08 rad). The residual is (−0.1 m, 0 m, −0.08 rad). Its normalized components are (−0.5, 0, −0.8), contributing 0.25 + 0.64 = 0.89 at weight 1.

Anchor the graph's world frame

Relative measurements do not choose an absolute world origin or heading. Apply the same rigid transform G to every pose, and each prediction stays unchanged:

This three-parameter freedom in 2D is the graph's gauge freedom. The lab fixes pose 0 exactly at (0 m, 0 m, 0°) and optimizes the other twelve scalar coordinates. Fixing that pose selects a frame; it does not assert a perfect global position sensor.

The SLAM chapter by Dellaert and Hutchinson explains why a pose graph needs an anchor. Our helper also requires every node to connect to pose 0 through positive-weight edges. A disconnected component would retain its own frame freedom.

Removing this gauge does not prove that a nonlinear graph has one global optimum. Initialization, ambiguous rotations, and incorrect measurements can still affect the result.

Construct the five-pose example

The known reference follows a square with four 2 m sides. Heading indicates the body axes at each recorded pose:

Each supplied odometry edge reports 2.1 m forward and a turn of π/2 + 0.08 rad. Integrating those four readings gives the starting estimate. Its final pose is approximately (0.358177 m, −0.281094 m, 18.334649°).

The correct supplied edge from pose 0 to pose 4 reports (0 m, 0 m, 0°). The deliberately incorrect alternative reports (1 m, 1 m, 60°). Both are direct relative-pose measurements; the lab implements no place recognition or registration that could generate or verify them.

The solver receives the starting poses and measured edges. It never receives the reference poses. The reference defines the synthetic example and supports evaluation afterward, including all five poses with no additional alignment fit.

Linearize and accept a decreasing step

Let r collect all residual components after dividing by their scales and multiplying by √w. Around the current estimate, approximate the effect of a small coordinate change Δ:

A is the residual Jacobian. For example, changing θᵢ changes both components of R(θᵢ)ᵀ(tⱼ − tᵢ). The widget differentiates those expressions analytically.

A damped Gauss–Newton step solves:

Here D contains max((AᵀA)ₖₖ, 1) on its diagonal. The solver updates world x and y additively, adds each yaw change, then wraps the new yaw. It leaves the anchor untouched.

The initial damping is 0.001. A trial must contain finite values and strictly reduce the full nonlinear objective. Rejected trials increase damping tenfold; accepted trials reduce it to 30%, down to a floor of 10⁻¹².

The solver allows up to 12 trials per iteration and 50 accepted iterations. It stops on a small gradient, step, or cost improvement, or reports a limit or failure to find a decreasing step. The displayed trace includes accepted steps only; these numerical checks certify neither a global minimum nor correct associations.

Inspect the route and objective trace

The default correct loop has multiplier 4. Its cost falls from 61.690508 to 2.412962 over four accepted updates. Full-route position RMSE changes from 0.248478 to 0.086258 m, while heading RMSE changes from 11.227634° to 0.603116°.

The remaining loop disagreement is 0.001273 m in translation and 1.053129° in yaw. A finite loop weight allows a compromise with the odometry readings. The graph does not force the endpoint to match the loop exactly.

Use the accepted-step slider to return to step 0, then advance one update at a time. The route plot uses fixed equal meter scales; short lines show headings with schematic lengths. The objective plot rescales its vertical axis for each graph setting.

Set the multiplier to 0. The starting trajectory already fits the four odometry edges, so no update is necessary and cost rounds to zero. Restore multiplier 4, then choose the incorrect loop to inspect how a wrong constraint changes both positions and headings.

Separate a better fit from a better map

For the incorrect loop at multiplier 4, cost falls from 416.840427 to 23.995523. Yet position RMSE rises to 0.737388 m, and heading RMSE rises to 33.765301°. The optimizer successfully fits the supplied objective while moving farther from the reference.

The correct loop also leaves position error because the 2.1 m odometry lengths contain bias. Increasing its multiplier from 4 to 16 improves heading RMSE here but increases position RMSE from 0.086258 to 0.088466 m. A tighter loop fit does not imply every accuracy metric improves.

Compare costs only within the same measurements and weights. Changing a weight changes the objective; changing residual units without changing scales changes the problem. Use trajectory error metrics for a separate, explicit accuracy comparison.

Practical systems need association checks and can add outlier-resistant losses or constraint handling. This simple squared-error solver implements none of those checks. A sparse production solver also exploits graph structure to handle many more poses, a topic developed in the Grisetti tutorial.

Reproduce the nonlinear fit in Python

This standard-library version uses central differences for Jacobians and Gaussian elimination for its small linear solve. Those choices independently check the widget's analytic derivatives and Cholesky solve. The output evaluates each final estimate against the same reference.

from math import atan2, cos, sin, pi, sqrt


def wrap(a):
    return atan2(sin(a), cos(a))


def compose(a, b):
    x, y, t = a
    return [x+cos(t)*b[0]-sin(t)*b[1],
            y+sin(t)*b[0]+cos(t)*b[1], wrap(t+b[2])]


reference = [[0, 0, 0], [2, 0, pi/2], [2, 2, pi],
             [0, 2, -pi/2], [0, 0, 0]]
increment = [2.1, 0, pi/2+.08]
initial = [[0, 0, 0]]
for _ in range(4):
    initial.append(compose(initial[-1], increment))


def residuals(values, loop, weight):
    poses = [[0, 0, 0]] + [values[i:i+3] for i in range(0, 12, 3)]
    edges = [(i, i+1, increment, 1) for i in range(4)]
    if weight:
        edges.append((0, 4, loop, weight))
    residual = []
    for i, j, z, w in edges:
        x, y, t = poses[i]
        dx, dy = poses[j][0]-x, poses[j][1]-y
        residual.extend([sqrt(w)*(cos(t)*dx+sin(t)*dy-z[0])/.2,
                         sqrt(w)*(-sin(t)*dx+cos(t)*dy-z[1])/.2,
                         sqrt(w)*wrap(poses[j][2]-t-z[2])/.1])
    return residual


def solve(a, b):
    augmented = [row[:] + [value] for row, value in zip(a, b)]
    for column in range(len(b)):
        pivot = max(range(column, len(b)), key=lambda i: abs(augmented[i][column]))
        augmented[column], augmented[pivot] = augmented[pivot], augmented[column]
        scale = augmented[column][column]
        augmented[column] = [v/scale for v in augmented[column]]
        for row in range(len(b)):
            if row != column:
                scale = augmented[row][column]
                augmented[row] = [a-scale*c for a, c in zip(augmented[row], augmented[column])]
    return [row[-1] for row in augmented]


def optimize(loop, weight):
    values = [v for pose in initial[1:] for v in pose]
    damping, accepted = .001, 0
    for _ in range(50):
        residual = residuals(values, loop, weight)
        cost = sum(r*r for r in residual)
        # Numerical columns independently check the widget's analytic Jacobian.
        columns = []
        for column in range(12):
            plus, minus = values[:], values[:]
            plus[column] += 1e-6
            minus[column] -= 1e-6
            columns.append([(a-b)/2e-6 for a, b in
                            zip(residuals(plus, loop, weight), residuals(minus, loop, weight))])
        g = [sum(a*b for a, b in zip(col, residual)) for col in columns]
        if max(map(abs, g)) < 1e-9:
            break
        h = [[sum(a*b for a, b in zip(ci, cj)) for cj in columns] for ci in columns]
        success = False
        for _ in range(12):
            a = [[v+(damping*max(h[i][i], 1) if i == j else 0)
                  for j, v in enumerate(row)] for i, row in enumerate(h)]
            delta = solve(a, [-v for v in g])
            candidate = [v+d for v, d in zip(values, delta)]
            for i in (2, 5, 8, 11):
                candidate[i] = wrap(candidate[i])
            new_cost = sum(r*r for r in residuals(candidate, loop, weight))
            if new_cost < cost:
                values, success = candidate, True
                damping = max(damping*.3, 1e-12)
                accepted += 1
                break
            damping *= 10
        if not success or cost-new_cost < 1e-12*(1+new_cost) or max(map(abs, delta)) < 1e-9:
            break
    poses = [[0, 0, 0]] + [values[i:i+3] for i in range(0, 12, 3)]
    position = sqrt(sum((p[0]-q[0])**2+(p[1]-q[1])**2 for p, q in zip(poses, reference))/5)
    heading = sqrt(sum(wrap(p[2]-q[2])**2 for p, q in zip(poses, reference))/5)*180/pi
    cost = sum(r*r for r in residuals(values, loop, weight))
    return cost, position, heading


for label, loop, weight in [('no loop', [0, 0, 0], 0),
                            ('correct', [0, 0, 0], 4),
                            ('incorrect', [1, 1, pi/3], 4)]:
    cost, position, heading = optimize(loop, weight)
    print(f'{label}: cost={cost:.6f}, position={position:.6f} m, heading={heading:.6f} deg')

Output:

no loop: cost=0.000000, position=0.248478 m, heading=11.227634 deg
correct: cost=2.412962, position=0.086258 m, heading=0.603116 deg
incorrect: cost=23.995523, position=0.737388 m, heading=33.765301 deg

Check two graph-optimization claims

Exercise 1. Pose i is (2 m, 0 m, 90°), pose j is (2 m, 2 m, 180°), and the measurement is (2.1 m, 0 m, π/2 + 0.08 rad). Calculate the three residuals and the cost at scales 0.2 m and 0.1 rad. What happens if the edge's information multiplier becomes 4?

Exercise 2. With the loop disabled, the graph has nearly zero measurement cost. Does that prove the trajectory is accurate? Would translating and rotating every pose by the same transform change its relative-pose residuals?

Sources and further study

• Giorgio Grisetti, Rainer Kümmerle, Cyrill Stachniss, and Wolfram Burgard, A Tutorial on Graph-Based SLAM: graph construction, relative constraints, weighted least squares, linearization, and sparse optimization.
• Frank Dellaert, Factor Graphs and GTSAM: pose variables, noise scales, nonlinear optimization, and PoseSLAM examples.
• Frank Dellaert and Seth Hutchinson, Introduction to Robotics and Perception: SLAM: anchored pose constraints and least-squares inference.

Continue with loop closure to examine how a revisit becomes a constraint, then use trajectory error metrics to evaluate the corrected route independently of its fitting cost.