M4-T11: Loop closure

Learning objectives

After this lesson, you should be able to:

• Describe place recognition with bag-of-words (visual) and global scan descriptors (LiDAR).
• Make a descriptor invariant to heading and compare descriptors by a shifted distance.
• Explain geometric verification and why it is mandatory.
• Measure precision and recall of a recogniser and choose a threshold.

Conceptual explanation

Loop closure has two stages. Recognition proposes candidate revisits quickly from a database of past places: visual bag-of-words (DBoW2, DBoW3, NetVLAD descriptors) or LiDAR global descriptors (Scan Context, which encodes the polar structure of a scan). Verification computes the relative pose between the current scan or frame and the candidate (RANSAC on feature matches, or ICP) and checks that it is consistent and that enough points agree; only then is a pose-graph edge added (lesson M4-T12).

Engineering intuition

Many buildings look alike: identical corridors, repeated pillars, uniform fields. Appearance-based recognition cannot tell them apart and will propose confident wrong candidates (perceptual aliasing). Verification catches most by demanding that the geometry actually align with many inliers; the remaining risk is handled by robust back-ends. The cost of a false positive (a folded map) far exceeds the cost of a missed loop (some residual drift), so thresholds are set for high precision, accepting lower recall.

Mathematical formulation

A ring–sector descriptor: divide the scan into $R$ range rings and $S$ azimuth sectors; the entry is a summary (for example the maximum height, or a point count) in each bin. A rotation of the sensor about the vertical axis shifts the columns cyclically, so the distance is minimised over column shifts:

Precision $P$ = TP/(TP + FP) and recall $R$ = TP/(TP + FN) over candidate pairs; choose the threshold to reach the required precision (for example 100% on the validation set), then report the recall achieved. Verification inlier ratio and the residual after alignment are the standard accept tests; a loop with a large relative-pose residual or a heading that disagrees with the descriptor’s yaw estimate is rejected.

$R$, $S$

number of rings and sectors.

k*

best column shift (heading difference).

Architecture and data flow

Keyframes → descriptor → database (excluding recent frames, to avoid trivial matches) → candidates → verification → pose-graph edge with covariance → optimisation → map correction published as the map-to-odom transform. In a UAV stack, run recognition on a separate thread and never block odometry. A loop correction must not jump the controller’s frame (lesson M4-T1).

Real-world UAV applications

• Drift removal in LiDAR and visual SLAM (ORB-SLAM3, LIO-SAM).
• Multi-session localisation against a prior map.
• Map merging between vehicles.
• Relocalisation after tracking loss.

Implementation example

This runs as shown: 2-D range scans taken from random poses in a room with obstacles are encoded by a ring–sector descriptor; a revisit with a different heading is recognised (and the heading recovered) while a different place with similar structure is rejected at the chosen threshold.

import numpy as np
rng = np.random.default_rng(4)
R_RINGS, S_SECT, MAXR = 10, 60, 20.0

def make_world(seed):
    r = np.random.default_rng(seed); return [(r.uniform(-12, 12), r.uniform(-12, 12), r.uniform(0.6, 2.0)) for _ in range(40)]   # circular obstacles
def scan_points(world, pose):                                       # points hit by 360 beams, in the sensor frame
    pts = []
    for a in np.linspace(0, 2 * np.pi, 720, endpoint=False):
        d = np.array([np.cos(a + pose[2]), np.sin(a + pose[2])]); best = MAXR
        for (cx, cy, rad) in world:
            f = np.array([cx, cy]) - pose[:2]; t = f @ d
            if t > 0 and (f @ f - t * t) < rad * rad: best = min(best, t - np.sqrt(rad * rad - (f @ f - t * t)))
        if best < MAXR: pts.append(best * np.array([np.cos(a), np.sin(a)]))
    return np.array(pts) + 0.03 * rng.standard_normal((len(pts), 2))

def descriptor(P):                                                  # rings x sectors count of points
    r = np.hypot(P[:, 0], P[:, 1]); a = (np.arctan2(P[:, 1], P[:, 0]) % (2 * np.pi))
    D, _, _ = np.histogram2d(np.minimum((r / MAXR * R_RINGS).astype(int), R_RINGS - 1), (a / (2 * np.pi) * S_SECT).astype(int) % S_SECT, bins=[R_RINGS, S_SECT], range=[[0, R_RINGS], [0, S_SECT]])
    return (D > 0).astype(float)

def distance(A, B):
    best = (9.0, 0)
    for k in range(S_SECT):
        Bk = np.roll(B, k, axis=1); num = (A * Bk).sum(0); den = np.linalg.norm(A, axis=0) * np.linalg.norm(Bk, axis=0)
        cos = np.where(den > 0, num / np.maximum(den, 1e-9), 0.0); d = np.mean(1 - cos)
        best = min(best, (d, k))
    return best

world = make_world(1)
A = descriptor(scan_points(world, np.array([2.0, 1.0, 0.0])))                       # place A
A_rev = descriptor(scan_points(world, np.array([2.1, 0.9, 1.3])))                   # the same place revisited, rotated by 1.3 rad (about 74 degrees)
others = [descriptor(scan_points(world, np.array([x, y, 0.0]))) for x, y in ((-8, -6), (7, -9), (-5, 8), (9, 8), (-10, 0))]
d_same, k = distance(A, A_rev); d_other = min(distance(A, O)[0] for O in others)
print(f"revisit distance {d_same:.3f} (yaw shift {k} sectors = {k * 360 / S_SECT:.0f} deg), best different-place distance {d_other:.3f}")
yaw = (k * 360 / S_SECT) % 360; true_yaw = np.degrees(1.3)
assert d_same < 0.6 * d_other                                        # the revisit is much closer than any other place
assert min(abs(yaw - true_yaw), abs(360 - yaw - true_yaw)) < 20       # and the recovered heading difference is right (up to the shift direction convention)

Common engineering mistakes

Failure modes

Debugging approach

1. Evaluate precision and recall on a sequence with known revisits.
2. Inspect accepted loops visually (overlay).
3. Plot descriptor distance for true and false pairs.
4. Check the verification residual distribution.

Practical exercises

1. Compute precision and recall of the example for a range of thresholds.
2. Add a second building with the same layout and see the false-positive rate.
3. Add geometric verification by ICP on the point sets.
4. Explain why the time-exclusion window is needed.

Interview questions

Write an answer in your own words, then reveal the model answer. Nothing is sent anywhere.

References and further reading

Primary papers, official documentation and standards. Each claim above that is specific to a tool or version points here.

• G. Kim, A. Kim, “Scan Context: egocentric spatial descriptor for place recognition within 3D point cloud map”, IROS 2018
• D. Gálvez-López, J. Tardós, “Bags of binary words for fast place recognition in image sequences”, IEEE T-RO, 2012
• R. Arandjelović et al., “NetVLAD: CNN architecture for weakly supervised place recognition”, IEEE TPAMI, 2018

What’s next