Loop Closure Detection

mediumFREESLAMCV: Structure from Motion and SLAM

Detect potential loop closures based on pose proximity.

Loop closure is one of the most critical components of SLAM. It occurs when a robot returns to a previously visited location, allowing it to correct accumulated drift.

Simple loop closure detection checks if the current pose is close to any past pose:

Compute distance to each past pose
If distance < threshold, it's a potential loop closure candidate

In practice, loop closure also uses appearance-based methods (comparing images or point clouds), but geometric proximity is a necessary first filter.

Example

Input

detect_loop_closure([0, 0, 0], [[10, 0, 0], [0.5, 0.5, 0], [5, 5, 0]], 1.0)

Output

[1]

Reasoning

Inputs: current_pose is $(x, y, \theta) = (0, 0, 0)$ , past_poses holds three earlier poses, and distance_thresh is $1.0$ . Position is what matters for "have I been here before?", so heading $\theta$ plays no part in the distance.
Math: for past pose $i$ at $(x_i, y_i)$ , the planar Euclidean distance is $d_i = \sqrt{(x - x_i)^2 + (y - y_i)^2}$ .
Pose 0 at $(10, 0)$ : $d_0 = \sqrt{100 + 0} = 10$ , far outside the radius.
Pose 1 at $(0.5, 0.5)$ : $d_1 = \sqrt{0.25 + 0.25} = \sqrt{0.5} \approx 0.707 < 1.0$ , a candidate.
Pose 2 at $(5, 5)$ : $d_2 = \sqrt{25 + 25} \approx 7.071$ , too far.
Output: the positions (not coordinates) of the qualifying poses in their original order, so [1]. Notice the test is strict: a pose at exactly distance $1.0$ would not count, and if nothing is close the result is an empty list.

Constraints

current_pose: [x, y, theta] current robot pose
past_poses: list of [x, y, theta] previously visited poses
distance_thresh: maximum distance for loop closure candidate
Return list of indices of potential loop closures

Learn this in

Structure from Motion and SLAMCV Study Plan Visual SLAM & Localization3D Computer Vision

More from CV: Structure from Motion and SLAM

Map Point Visibility Testhard Triangulation of 3D Pointsmedium PnP Pose from Correspondencesmedium Checkerboard Corner Detectionhard Range-Bearing Observationmedium

Concept primer

To solve this problem, you just need to use geometry on poses and a distance threshold; no heavy SLAM theory is required in code, but understanding the context helps.

1. Background Knowledge

In SLAM (Simultaneous Localization and Mapping), a robot estimates its own trajectory and builds a map at the same time. Because odometry and sensor measurements are noisy, the estimated pose drifts over time (small errors accumulate into large trajectory errors).

A loop closure happens when the robot revisits a previously seen place. Detecting this lets the system add a strong constraint between the current pose and a past pose in the pose graph, which allows global optimization (e.g., pose graph optimization) to correct accumulated drift. In practice, loop closure detection often uses appearance (images, point clouds, descriptors), but a basic and very common first filter is pose proximity: if the current pose is close in space to some past pose, it is a loop candidate.

In this problem, you are focusing on that geometric filter: given a list of past robot poses and the current pose, detect which past poses are within some distance threshold and thus mark them as potential loop closures.

2. Algorithm / Approach

General pattern:

Treat each pose as a point in space (often just its position, e.g., $(x, y)$ or $(x, y, z)$ ).
For the current pose, compute its Euclidean distance to every previous pose.
If the distance is less than a given threshold $d_{$ \text{th} $}$ , consider that previous pose a loop closure candidate.
Optionally exclude very recent poses (to avoid trivial near-duplicates in time).

This is a simple linear scan + distance check over an array/list of past poses.

3. Step-by-Step Strategy

Assuming poses are in 2D with coordinates $(x_i, y_i)$ :

Inputs you likely have:

poses: an array/list of past poses, e.g. poses[0..N-1], each with coordinates (x, y) (and maybe orientation, which you can ignore for proximity).
current_pose: the current pose (x_c, y_c).
threshold: a float value $d_{$ \text{th} $}$ defining “close enough”.

Initialize a result container:

For example, candidates = [] to store indices or IDs of loop closure candidates.

Iterate over past poses:

For each index i in 0..N-1:
Extract pose poses[i] = (x_i, y_i).

Compute distance:

Compute squared distance to avoid a sqrt (optional optimization):
dx = x_i - x_c
dy = y_i - y_c
dist_sq = dxdx + dydy
Compare with threshold_sq = threshold * threshold.

Threshold check:

If dist_sq < threshold_sq, then i is a loop closure candidate.
Append i (or the pose itself) to candidates.

Return or output candidates:

The function returns the list of indices / poses that are within threshold.

Example sketch in Python-like pseudocode:

def detect_loop_closures(poses, current_pose, threshold):
 x_c, y_c = current_pose
 threshold_sq = threshold * threshold
 candidates = []

 for i, (x_i, y_i) in enumerate(poses):
 dx = x_i - x_c
 dy = y_i - y_c
 dist_sq = dx*dx + dy*dy
 if dist_sq < threshold_sq:
 candidates.append(i)

 return candidates

If 3D, just add dz similarly.

4. Common Pitfalls

Including too-recent poses: Consecutive poses are naturally close; they should not be considered loop closures. Often you ignore the last $k$ poses (e.g., skip indices N-k... N-1) to avoid trivial matches.
Choosing a bad threshold:
Too small: you miss real loops.
Too large: almost everything becomes a candidate, causing many false positives downstream.
Ignoring units or scaling:
Ensure all poses and the threshold are in the same units (meters, not mixed with pixels, etc.).
Unnecessary sqrt cost:
Using actual Euclidean distance (with sqrt) in every iteration is slower; compare squared distances instead.
Mixing position and orientation incorrectly:
For this simple geometric filter, you typically use only position; if you include orientation, do it separately and carefully (orientation difference is not Euclidean).

5. Time & Space Complexity

Time Complexity:
You compare the current pose to each of the $N$ past poses.
Each comparison is $O(1)$ .
Total: $O(N)$ per loop-closure query.
Space Complexity:
You store the poses: $O(N)$ (given).
Extra space for candidate indices is at most $O(N)$ in the worst case (if all poses are within threshold).
The algorithm’s additional working space is $O(N)$ including output, or $O(1)$ if you only count the number of candidates or stream them out.

solution.py

Python 3.13

Test results