Loop closure in SLAM: why revisiting a place changes the map

A robot drives around a room and returns to its starting place. Its motion estimate says it stopped somewhere else. A verified revisit gives the system new evidence about the route it already traveled.

Loop closure adds a constraint between observations taken at different times in a revisited area. That constraint can change earlier position estimates and the map they support. This lesson makes the correction visible, then shows what happens when the supplied revisit is wrong.

What a loop closure adds

Odometry links each estimated pose to the next one. Small motion errors can accumulate along that chain. A later observation of a familiar place can supply another relationship between two poses.

The GTSAM PoseSLAM tutorial illustrates this extra link in a graph. Nodes represent poses; edges represent measurement constraints. The optimizer adjusts estimates to make the measurements more consistent together.

A revisit usually supplies a relative pose with uncertainty. It need not mean that the robot returned to exactly the same point or orientation. Our example supplies a simpler equality: two selected positions should coincide.

Separate recognition, verification, and correction

Three jobs support a useful loop closure:

1. Candidate recognition: find an earlier observation that might show the same place.
2. Geometric verification: test whether observations support a plausible relative transform.
3. Global correction: add an accepted constraint and update connected estimates.

Similar-looking corridors can produce a misleading candidate. Geometry supplies additional checks, including whether enough corresponding features support one transform. ICP can help align suitable scans, though a small alignment residual alone cannot establish a correct match.

The robotics textbook’s SLAM chapter connects scan matching, loop constraints, and graph optimization. This experiment starts with a supplied constraint. It implements the correction step; it performs no image retrieval, feature matching, or geometric acceptance test.

Build a small translation-only graph

Use nine positions, numbered 0 through 8. Position 0 stays fixed at (0, 0) meters. Each odometry edge measures the displacement from one position to the next.

Every displacement uses the same fixed global axes. We omit orientation and its uncertainty. This assumption makes the optimization linear. The pose-graph optimization lesson estimates planar positions and headings together from local-frame measurements, using a nonlinear solver.

The supplied loop says p₈ − pⱼ = (0, 0). The correct example uses j = 0. The false example uses j = 4, claiming that the last position matches the opposite corner.

Anchoring one position removes an ambiguity: shifting every estimate together leaves all relative displacements unchanged. The textbook’s graph example discusses this need for an anchor. Here the anchor is exact, and this lesson draws the route without estimating landmarks.

Watch odometry drift around a square

The known synthetic route follows a two-meter square. These positions provide evaluation data:

Pose:  0      1      2      3      4      5      6      7      8
(x,y): (0,0)  (1,0)  (2,0)  (2,1)  (2,2)  (1,2)  (0,2)  (0,1)  (0,0)

Add (0.10, 0.05) m to every true displacement to create biased odometry measurements. Integrating eight of those measurements ends at (0.80, 0.40) m. The distance from the estimated endpoint to the start is:

The error also affects intermediate positions. At pose i, the position error vector is i × (0.10, 0.05) m. Wheel odometry explores the motion measurements that can produce drift in a physical system.

Give the measurements a weighted cost

Let dᵢ be measured displacement i, and pᵢ the position we want to estimate. A residual measures disagreement between an estimated displacement and its measurement. We minimize this cost while holding p₀ fixed:

Each odometry edge has weight 1. The relative loop weight w sets the cost of violating the supplied equality. Weight 0 removes its influence.

All residuals use meters, so this normalized cost uses m². It is a sum of weighted squared residuals. The widget separately reports a route RMSE: the square root of the mean squared position error across all nine known synthetic poses, including the anchor.

The solver receives odometry and the supplied loop only. Known positions enter the evaluation afterward. A physical route usually lacks this exact reference for checking its error.

Distribute the correction around the loop

Consider the correct loop with w = 4. Its initial residual vector is s = (0.80, 0.40) m. Equal odometry weights make the optimal correction identical on each of the eight edges.

Call the subtracted correction c. The cost becomes 8‖c‖² + w‖s − 8c‖². Setting its derivative to zero gives:

At w = 4, c is approximately (0.096970, 0.048485) m. Subtract it from each measured displacement. Pose i moves by −i c, so every free position changes.

The final estimate becomes (0.024242, 0.012121) m. A finite weight allows some disagreement: the remaining loop gap is 0.027104 m. Full-route RMSE falls from 0.532291 m to 0.016130 m.

The widget solves the graph’s linear equations directly. This small formula supplies an independent check. Changing an estimate revises the recorded route; it does not move the robot.

When a mapping system revises poses, it can also revise where the associated observations belong in the map.

Interpret the loop weight carefully

A larger weight makes the optimizer work harder to satisfy the loop. Under independent, isotropic Gaussian measurement errors, inverse variance supplies one weighting rule:

For that model, w = 4 corresponds to a loop standard deviation half the odometry standard deviation. The GTSAM tutorial uses measurement noise models when constructing graph factors.

Our measurements contain a deliberate, repeated bias. They do not sample independent Gaussian errors. Treat the slider as a chosen relative penalty; it supplies neither a confidence percentage nor an estimated covariance.

Weight changes also change the objective itself. Compare cost before and after at the same weight. Comparing raw costs from different weights answers a different question.

Fit the wrong revisit

Select the false constraint: pose 8 supposedly revisits pose 4. Those true positions lie at opposite corners. The optimizer still tries to satisfy the supplied equality.

At w = 4, weighted cost drops from 23.200000 to 1.364706 m². Yet full-route RMSE grows from 0.532291 to 1.506228 m. The last four edges change to pull the endpoint toward the wrong corner; positions 0 through 4 keep their odometry estimates.

The optimizer improves consistency with bad input. A low cost cannot certify the association that produced that input.

Sünderhauf and Protzel’s switchable-constraints work addresses false loop closures by extending the optimization model. Our plain squared-error graph has no such outlier mechanism. Increasing the false loop’s weight strengthens its distortion.

Compare the two supplied constraints

Start with the correct constraint at weight 4. Compare the route error with the loop residual: they measure different quantities. The corrected route still disagrees slightly with the loop and the odometry.

Set the weight to 0. Both route estimates coincide because odometry alone determines this anchored chain. Then select the false constraint and restore weight 4 to see the route error increase even as cost falls.

A supplied constraint changes the graph

Correct the route, or trust the wrong revisit

Optimize nine planar positions with fixed orientation and global-axis displacements. Recognition and geometric verification are outside this experiment.

Loop constraintCorrect: pose 8 revisits pose 0False: pose 8 revisits pose 4

Relative loop weight

4

Every odometry edge has weight 1. The supplied loop says its two positions have zero relative translation. Weight 0 omits that edge; a larger weight makes disagreement with it more expensive. These labels disclose the synthetic truth, which the solver cannot see.

Constraint used

Pose 8 to pose 0

Weighted cost before (m²)

3.200000

Weighted cost after (m²)

0.096970

Loop residual before (m)

0.894427

Loop residual after (m)

0.027104

Odometry RMSE after (m)

0.108415

Full-route RMSE before (m)

0.532291

Full-route RMSE after (m)

0.016130

Final x after (m)

0.024242

Final y after (m)

0.012121

This supplied association is correct. The optimizer distributes its correction around the route.

Cost compares the same weighted constraints before and after each solve. Changing the weight changes the objective. Full-route RMSE compares all nine estimates with known synthetic positions; it is not a measurement residual or a reported uncertainty.

Reset loop closure experiment

The gray route is known synthetic truth. Amber squares show the initial estimate, and blue circles show the optimized estimate. The second chart checks every position, including places far from the constrained endpoint.

Reproduce the correction in Python

This standard-library script uses the closed-form solution for this chain and one zero-displacement loop. For a loop from j to 8, only its 8 − j edges need correction. The widget solves the equivalent weighted linear system.

from math import hypot, sqrt

truth = [(0, 0), (1, 0), (2, 0), (2, 1), (2, 2),
         (1, 2), (0, 2), (0, 1), (0, 0)]
deltas = [(b[0] - a[0] + 0.1, b[1] - a[1] + 0.05)
          for a, b in zip(truth, truth[1:])]
before = [(0.0, 0.0)]
for dx, dy in deltas:
    x, y = before[-1]
    before.append((x + dx, y + dy))


def route_rmse(points):
    return sqrt(sum((x - tx)**2 + (y - ty)**2
                    for (x, y), (tx, ty) in zip(points, truth)) / 9)


def cost(points, j, weight):
    odometry = sum((b[0] - a[0] - d[0])**2
                   + (b[1] - a[1] - d[1])**2
                   for a, b, d in zip(points, points[1:], deltas))
    gap = hypot(points[8][0] - points[j][0],
                points[8][1] - points[j][1])
    return odometry + weight * gap**2


for name, j, weight in [("none", 0, 0), ("correct", 0, 4),
                        ("false", 4, 4)]:
    n = 8 - j
    scale = weight / (1 + n * weight)
    cx = scale * (before[8][0] - before[j][0])
    cy = scale * (before[8][1] - before[j][1])
    after = [(x - max(0, i - j) * cx, y - max(0, i - j) * cy)
             for i, (x, y) in enumerate(before)]
    print(f"{name}: cost {cost(before, j, weight):.6f} -> "
          f"{cost(after, j, weight):.6f}")
    print(f"route RMSE {route_rmse(before):.6f} -> "
          f"{route_rmse(after):.6f}")

Expected output:

none: cost 0.000000 -> 0.000000
route RMSE 0.532291 -> 0.532291
correct: cost 3.200000 -> 0.096970
route RMSE 0.532291 -> 0.016130
false: cost 23.200000 -> 1.364706
route RMSE 0.532291 -> 1.506228

Try it yourself

Exercise 1. Use the correct constraint with weight 1. The initial residual is (0.8, 0.4) m. Calculate the remaining endpoint vector and its distance from the start.

Check the weight-one correction

The correction per edge is s / 9. Eight edges subtract 8s / 9, leaving s / 9 = (0.088889, 0.044444) m at the endpoint. Its length is √0.8 / 9 = 0.099381 m.

Every position error in this particular biased example shrinks by the same factor of 9. Full-route RMSE becomes 0.532291 / 9 ≈ 0.059143 m.

Exercise 2. A candidate loop produces lower cost after optimization, but its association has never passed a geometric check. Does the cost reduction establish that the robot revisited that place? Use the false-constraint experiment to justify your answer.

Check the false-loop reasoning

No. The optimizer reduces the cost of the constraints it receives, including a false one. Here cost falls from 23.200000 to 1.364706 m² while full-route error rises to 1.506228 m.

Candidate recognition and geometric verification supply evidence about the association before it enters this simple optimizer. Additional outlier handling can reduce the effect of bad constraints. A cost reduction alone provides neither check.

A corrected endpoint can hide errors elsewhere along a route. The trajectory-error lesson compares every matched position and separates position error from relative translation and rotation error.

Sources and further study

• Frank Dellaert, Factor Graphs and GTSAM: graph factors, measurement noise, and a PoseSLAM loop-closure example.
• Frank Dellaert and Seth Hutchinson, Introduction to Robotics and Perception: SLAM: scan matching, anchored pose graphs, least squares, and nonlinear pose optimization.
• Niko Sünderhauf and Peter Protzel, Switchable Constraints for Robust SLAM: why false loop constraints require more than ordinary squared-error fitting.

Continue with SLAM explained to connect this route correction to localization, mapping, and the measurements that support both.