Express relative pose locally
Subtract the two world positions and rotate that displacement into the source pose’s frame. Subtract source heading from target heading, wrapping the result around ±π.
13 / POSE-GRAPH SLAM · LOOP CLOSURE
Revisit a place.
Reconsider the whole path.
A drone has already completed a circular survey. Its pose graph holds 25 historical poses linked by noisy relative-motion constraints. A supplied loop closure relates two visits. Gauss–Newton optimization adjusts the estimated past trajectory to fit those measurements together.
Predict: a loop is attached to the wrong historical pose. Can the optimizer reduce its mathematical cost while making the reconstructed path less accurate?
Name the graph, solver and association
Residuals and primary sources ↗- Problem and algorithm
- Pose-graph SLAM / Gauss–Newton
A SLAM back end estimates recorded planar poses by minimizing a weighted sum of squared relative-pose residuals. Gauss–Newton uses local Jacobians and a backtracking line search.
- State and reference frame
- 25 poses · pose 0 fixed exactly
Each pose contains x, y and heading. The first pose [4, 0, π/2] removes the free global translation and rotation. The remaining 72 coordinates are optimized jointly.
- Constraints and association
- 24 odometry edges + an optional loop
Relative translations are expressed in the source pose’s local frame. Loop identities are supplied: correct 0 → 24, or deliberately wrong 0 → 18 using the same measurement.
- Architecture and execution
- One offline optimizer · one recorded drone
The 24-second survey and noisy measurements stay fixed. Optimizer iterations revise historical estimates; they are not flight ticks. Detailed 3D geometry illustrates planar poses at a fixed 2 m display height.
Input versus evaluator: the optimizer receives measured edges, their declared weights and the fixed first pose. True poses, trajectory error and the knowledge that a loop is wrong belong to the evaluator. No automatic place recognition or outlier rejection is implemented.
THE SURVEY IS RECORDED · NOW OPTIMIZE ITS GRAPH
One constraint can move many past poses.
Green: current estimate. Amber: initial odometry. Arrows show the correction from the initial estimate.
Largest position change: 0 m
Drag to orbit · scroll to zoom · Focus selected pose for a closer view. The single solid drone marks one recorded true pose; graph markers are historical estimates, not other drones.
━ True trajectory / evaluator┄ Initial odometry━ Displayed estimate⌒ Supplied loop edge
OPTIMIZER ITERATION00 accepted steps · survey already recorded
WEIGHTED RESIDUAL COST—Objective of this graph / dimensionless
TRAJECTORY RMSE / EVALUATOR—Position error over the 24 unfixed historical poses
ENDPOINT ERROR / EVALUATOR—Pose 24 heading error
INSPECT A HISTORICAL POSE
One drone, seen at different times.
Selection moves the inspection marker along an already recorded survey. It neither runs the optimizer nor records another flight. Optimizer iterations can revise all 24 unfixed poses together.
Anchor and evaluator reference
Pose 0 is fixed exactly. The displayed solid drone uses the selected true historical pose; its estimated counterpart and initial odometry position can differ. Only the anchor is supplied as exact world information.
Inspect all 25 current historical poses
| Pose | x / m | y / m | Heading / ° |
|---|
INSPECT ONE MEASURED CONSTRAINT
How does this edge disagree?
Translation measurements and residuals use the source pose’s local axes. The angular residual is wrapped to ±π. Whitened components divide by their declared standard deviations; the displayed contribution is their squared sum.
Inspect all constraints and their current costs
| Edge | From → to | Kind | Weighted cost |
|---|
LATEST SOLVER STEP
Linearize, solve, then check the real cost.
Largest current position correction from initial odometry: 0.000 m.
Inspect pose corrections from the latest step
| Pose | Δx / m | Δy / m | Δheading / ° |
|---|
Inspect backtracking trials
| Step fraction α | Trial cost | Armijo bound | Cost − bound | Accepted |
|---|
OPTIMIZATION HISTORY / SAME GRAPH
Lower residual cost is not the same as ground truth.
The objective uses measured constraints. Trajectory RMSE uses evaluator truth. A wrong loop can improve the objective while distorting the path. Before / After changes the scene only; these histories and all inspectors retain the computed result. Costs from different association scenarios minimize different graphs and are not an accuracy ranking.
Solver events and stop conditions
PREDICT · COMPARE · EXPLAIN
A loop needs a correct identity.
Compare integrated odometry alone, the supplied correct loop, and the same measurement attached to a wrong historical pose. Keep the seed fixed; inspect both residuals and physical trajectory error.
Reproduce the reference comparisons
Independent optimization copies use seed 7, the same recorded survey and declared stop conditions. Opening these results leaves the active graph and selection unchanged.
| Association | Stop condition | Iterations | Graph cost | Trajectory RMSE / m | Endpoint / m |
|---|
POSE-GRAPH SLAM / WEIGHTED LEAST SQUARES
The graph remembers.
The optimizer revises.
The previous EKF-SLAM workshop updated a current pose and landmark map. Here the unknowns are historical robot poses. Their relative constraints connect a recorded trajectory, and an iteration can adjust past estimates across the whole graph.
This is the optimization back end of a small pose-graph SLAM example. Detecting a revisit and deciding which poses correspond are separate front-end problems, supplied directly in this lesson.
Weight measurement disagreements
Compare the predicted relative pose with the fixed measured edge. Scale translation and angle by their sensor standard deviations, then sum squared components across edges.
Gauss–Newton with backtracking
Linearize residuals, solve the anchored normal equations and propose a joint pose increment. A backtracking line search checks the actual nonlinear cost before accepting a step.
A solver trusts supplied associations
A wrong loop still contributes a weighted residual. Without a robust loss or outlier detector, the optimizer tries to satisfy it. Numerical convergence does not validate the data association.
pᵢ = [xᵢ, yᵢ]ᵀ · poseᵢ = [xᵢ, yᵢ, θᵢ]
t̂ᵢⱼ = R(θᵢ)ᵀ(pⱼ − pᵢ)
θ̂ᵢⱼ = wrap(θⱼ − θᵢ)
rᵢⱼ = [t̂ᵢⱼ − zᵗᵢⱼ, wrap(θ̂ᵢⱼ − zᶿᵢⱼ)]ᵀ
F(p) = Σ rᵢⱼᵀ Ωᵢⱼ rᵢⱼ
(JᵀΩJ) δ = −JᵀΩr · p′ = p + αδ
t̂ᵢⱼ = R(θᵢ)ᵀ(pⱼ − pᵢ)
θ̂ᵢⱼ = wrap(θⱼ − θᵢ)
rᵢⱼ = [t̂ᵢⱼ − zᵗᵢⱼ, wrap(θ̂ᵢⱼ − zᶿᵢⱼ)]ᵀ
F(p) = Σ rᵢⱼᵀ Ωᵢⱼ rᵢⱼ
(JᵀΩJ) δ = −JᵀΩr · p′ = p + αδ
- zᵢⱼ and Ωᵢⱼ / measurements and weights
- Each edge supplies relative translation and heading. The diagonal information matrix Ω contains inverse variances; translation uses meters and heading uses radians.
- J / residual Jacobian
- Jacobians describe how each edge residual changes with the two connected poses. Pose 0 is omitted from the unknown vector, leaving 72 free coordinates.
- δ and α / local optimization step
- The Gauss–Newton direction is scaled by a backtracking factor α. Accepted poses use wrapped headings; the fixed anchor is never updated.
- Loop closure / supplied association
- The true survey returns near pose 0 at pose 24. The wrong-loop case deliberately attaches that same relative observation to pose 18. Its incorrect endpoint is not exposed to the optimizer as a warning.
Odometry alone can fit a wrong path perfectly
The initial poses are integrated from the same odometry edges. An open chain can therefore have near-zero residual cost even though noise has displaced it from the true path. Another measurement adds information; a low objective alone does not.
No uncertainty ellipse is claimed
The page shows residuals, corrections and errors. It does not interpret the Hessian as a calibrated posterior covariance, perform marginalization, optimize landmarks, recover missed loop associations or guarantee a global minimum.
Primary method context: Grisetti, Kümmerle, Stachniss and Burgard — A Tutorial on Graph-Based SLAM. This educational implementation uses a supplied planar relative-pose graph, one fixed anchor and a small dense Gauss–Newton solver with backtracking. It is not a complete visual, LiDAR or multi-robot SLAM pipeline.