Pose-Graph SLAM: Loop-Closure Optimization

Idle

Drive

Parameters

Odometry noise 1.5%

Loop-closure accuracy 90%

Drive speed 1.0x

Live State

Nodes placed

0 / 48

Drift at closure

–

Graph residual

–

Opt. iterations

Trajectory RMSE vs ground truth

–

Legend

Ground-truth path (hidden from robot) Raw odometry (dead reckoning) Pose-graph estimate Loop-closure constraint

How it works

Each robot pose is a rigid-body transform in the plane, x_i = (x, y, θ) ∈ SE(2). Odometry gives a noisy estimate of the relative motion between consecutive poses, composed forward (dead reckoning):

x_{i+1} = x_i ⊕ δ_i
x' = x + δx·cosθ − δy·sinθ
y' = y + δx·sinθ + δy·cosθ
θ' = θ + δθ

Small per-step noise in δ compounds every step, so the estimated trajectory drifts further from the truth the longer the loop runs — this is the red path. A loop closure is an independent measurement (place recognition / scan matching) linking the current pose back to an earlier one, giving one more relative-pose constraint that the dead-reckoning chain never had.

Pose-graph SLAM finds the poses that best satisfy every constraint at once, minimizing the sum of squared relative-pose errors over the whole graph:

min Σ_(i,j)∈E  ‖ (x_j ⊖ x_i) − z_ij ‖²_Ω

where z_ij is the measured relative pose for edge (i,j) and Ω its information (inverse-covariance) weight. Real systems (g2o, GTSAM, Cartographer) solve this with sparse nonlinear least squares — Gauss–Newton or Levenberg–Marquardt over the sparse information matrix. This demo uses the same cost function but a lighter Gauss–Seidel relaxation: each edge repeatedly nudges its two nodes toward agreement, spreading the loop-closure correction back along the whole chain — visually the same outcome (drift collapses, trajectory becomes globally consistent) with a much simpler solver.

Odometry noise — per-step Gaussian noise on the simulated wheel/IMU odometry; higher values drift faster.
Loop-closure accuracy — how precise the place-recognition measurement is once the robot returns near the start; a stronger (lower-noise) constraint pulls harder during optimization.
Run Loop-Closure Optimization — animates Gauss–Seidel sweeps over every odometry edge plus the one loop-closure edge until the residual converges.

⚠ Couldn't load the 3D engineThree.js failed to load from the CDN. Check your connection and reload.