explainer
ICP failure cases: when a smaller residual gives the wrong pose
Diagnose ICP failure cases with repeated geometry, partial overlap, and unmatched points. Compare the fitting residual with a known pose error, change the starting estimate, and see which evidence each metric uses.
What you will learn
- Separate nearest-neighbor fitting residual from error against an independent reference.
- Explain how repeated geometry gives two zero-residual alignments.
- Predict why unmatched source points can move a correct initial pose.
- Compare controlled runs without changing the meaning of their metrics.
- Choose diagnostic checks before using a scan match as a pose constraint.
Before you start
Recognize an ICP failure despite convergence
ICP can reach zero fitting error at the wrong physical location. A repeated wall feature can match perfectly while the estimated translation misses the generating pose by meters.
This lesson tests that distinction with actual point-to-point ICP iterations. You will also remove target observations and add unmatched source points. Both changes can make ICP move away from a correct starting pose.
The ICP introduction explains how matching and rigid fitting alternate. Here, the main question is how to judge the resulting pose. All datasets and reference transforms below are synthetic teaching examples.
State exactly what the residual measures
Let pᵢ be a source point and qⱼ a target point, both measured in meters. The transform T maps source coordinates into the target frame: Tpᵢ = R(θ)pᵢ + t. Positive rotation is counterclockwise.
The lab minimizes this directed mean squared distance:
J(T) = (1/N) Σᵢ minⱼ ‖Tpᵢ − qⱼ‖²
Every source point contributes once, including added outliers. Each chooses its nearest target. Several sources can choose the same target, and some targets can receive no matches.
The blue trace shows √J, the all-source nearest RMSE, in meters. There is no distance gate, trimmed subset, normal estimate, or robust loss in this lab. Its denominator is the number of source points: six normally, eight with outliers.
For these fixed point sets, each exact rigid fit cannot increase the current paired squared error. Choosing nearest neighbors again cannot increase it either. This explains the nonincreasing objective, up to floating-point roundoff; it says nothing about the physical identity of those neighbors. Besl and McKay's original paper develops ICP's distance-convergence argument.
Keep reference errors separate from the fit
We generate the six original points with a known transform T⋆: zero rotation and translation (0.5, −0.25) m. Their identities let us measure a second quantity:
reference RMSE = √[(1/6) Σᵢ₌₁⁶ ‖Tpᵢ − T⋆pᵢ‖²]
This amber diagnostic always uses the same six original source points. It includes points whose target observations we later remove and excludes the two added source outliers. ICP never receives these identities or their hidden reference positions.
Two further readouts describe the transform directly:
- Translation error: ‖t − t*‖, in meters, for the stated source-frame origin.
- Rotation error: the absolute wrapped angular difference, in degrees, between 0° and 180°.
Reference-point RMSE measures the effect of both rotation and translation on this six-point geometry. It does not equal translation error in general. These reference metrics require information that a robot usually lacks during operation; they help evaluate this controlled example.
Repeat the geometry and change the starting pose
The repeated scene contains two identical target patches, separated by 3 m along target-frame x. The source observes only one patch. We know it came from the left patch because we generated it that way.
Unlabeled point geometry supports both alignments equally well. Starting with x translation 3.3 m places the source near the right patch. ICP reaches x = 3.5 m and zero residual, with a 3 m translation error.
Change to Repeated: near start. The exact same source and target sets now start at x = 0.3 m. ICP reaches the generating translation x = 0.5 m.
The initial estimate selects between these solutions. It does not add distinguishing detail to the point sets. An independent motion estimate, a larger view containing a unique feature, or another observation can supply information the repeated patch lacks.
Calculate a zero-residual wrong alignment
The six source coordinates are:
(0, 0), (0.8, 0), (1.4, 0.3),
(1.2, 1.1), (0.3, 1.4), (-0.3, 0.5) meters
For source point (0.8, 0), its reference location is (1.3, −0.25) m. The repeated patch contains another target at (4.3, −0.25) m.
At the wrong initial translation (3.3, −0.25) m, this point lands at (4.1, −0.25) m. Its nearest neighbor lies 0.2 m to the right. All six source points have the same paired offset, so the exact fit adds (0.2, 0) m with no rotation.
After that update:
- Each transformed source coincides with its wrong-patch target: nearest RMSE = 0 m.
- Each lies 3 m from its own reference identity: reference RMSE = √(6 × 3² / 6) = 3 m.
- The estimated translation is (3.5, −0.25) m: translation error = 3 m and rotation error = 0°.
The residual falls from 0.2 m to zero while the reference error grows from 2.8 m to 3 m. Here the wrong alignment is also a global minimum of the stated objective. Calling every ICP failure a local minimum would miss this ambiguity.
Remove overlap without removing source points
The Partial overlap preset keeps only the first three target observations. All six original source points remain. This is 50% retained point correspondence in a constructed dataset, not a measured percentage of surface area.
The preset starts at the exact generating pose. The three missing observations force their source points to choose different targets. ICP can reduce that error by moving the whole source set.
The initial squared nearest distances are 0, 0, 0, 0.68, 2.05, and 0.34 m². Thus the initial nearest RMSE is √(3.07/6) = 0.715309 m, even though the initial reference error is zero.
After fitting, nearest RMSE falls to 0.541354 m. Reference-point RMSE rises to 0.478316 m, and translation error reaches 0.569608 m. A correct initial pose cannot compensate for an objective that forces nonexistent correspondences.
Real scans lose overlap through field of view and occlusion. The LiDAR scan model shows how an obstacle hides a surface. This experiment removes points directly; it does not simulate visibility or scan motion.
Watch unmatched points pull the fit
The Source outliers preset restores all six target observations and adds source points (3, 2) and (3.5, 2.8) m. They have no generating target counterpart. ICP still assigns each one a nearest neighbor and includes it in the fit.
At the correct initial pose, those two squared distances are 4.05 and 8.18 m². The six original points contribute zero. The all-source nearest RMSE is √(12.23/8) = 1.236426 m.
The final nearest RMSE is lower, at 0.965647 m. The reference-point RMSE over the original six points rises to 1.098949 m. Squared residuals give distant unmatched points enough influence to move the entire fit.
Distance gates, overlap-aware selection, and robust losses change how a registration method treats suspect pairs. Each adds assumptions or parameters that need evaluation. The Open3D robust-kernel tutorial demonstrates downweighting large residuals for point-to-plane ICP; this lab retains ordinary unweighted point-to-point fitting.
A residual-based rule also faces the repeated-patch case: every wrong-patch residual is zero. Extra geometric or motion information must resolve that ambiguity.
Compare the controlled failure cases
Try this sequence:
- Repeated: wrong start: step once. Check the falling blue residual against the rising amber reference error.
- Repeated: near start: finish the run. Both point sets stay unchanged; only the initial estimate differs.
- Partial overlap: note the zero initial reference error, then finish. Missing targets pull the pose away.
- Source outliers: note that the objective includes eight points while the reference diagnostic still uses six.
The initial y translation stays at −0.25 m. All subsequent rigid updates can change x, y, and rotation. The initial-rotation control lets you test how changed nearest matches affect the result.
Each setting change starts a new run at iteration zero. Reset restores the repeated scene with the wrong start. Charts keep their axes fixed during a run and show only the iterations you have reached.
The solver stops after a tiny rigid update, an undetermined rotation, or 30 updates. A tiny update establishes a stopping condition. Check the reference errors before interpreting it as a correct pose.
Check a scan match before using its pose
A useful diagnostic record states what the algorithm fitted and what independent evidence agrees with it:
- Record source and target counts, retained pairs, rejection rules, units, and the starting transform.
- Inspect where matches lie. Many-to-one matches or a small shared patch can hide missing coverage.
- Compare starts using the same point sets and objective. Different outcomes reveal sensitivity; identical outcomes alone cannot prove correctness.
- Compare a motion estimate with separate measurements when available, accounting for their errors and shared inputs.
- Report failures alongside successful alignments when testing a method on representative data.
Metric names alone can conceal different populations. Open3D's registration example reports an inlier RMSE associated with a distance threshold. That value and this lab's all-source RMSE summarize different point selections.
For a sequence of poses, use trajectory-error metrics with explicit timestamp associations and frame alignment. A scan-pair residual does not measure drift along a complete route. In pose-graph optimization, an incorrect relative-pose constraint can affect estimates beyond the original pair.
Reproduce the failures in Python
This standard-library program uses the same generated data and directed nearest-neighbor objective. It fits the original source directly to the current matched targets on every iteration. That gives the same absolute rigid fit as composing a fit of the currently transformed source.
from math import atan2, cos, hypot, sin, sqrt
original = [(0, 0), (.8, 0), (1.4, .3),
(1.2, 1.1), (.3, 1.4), (-.3, .5)]
reference = [(x + .5, y - .25) for x, y in original]
def move(points, pose):
tx, ty, angle = pose
c, s = cos(angle), sin(angle)
return [(c*x - s*y + tx, s*x + c*y + ty) for x, y in points]
def squared(p, q):
return (p[0] - q[0])**2 + (p[1] - q[1])**2
def fit(source, target):
n = len(source)
px, py = (sum(p[k] for p in source) / n for k in (0, 1))
qx, qy = (sum(q[k] for q in target) / n for k in (0, 1))
a = sum((x-px)*(u-qx) + (y-py)*(v-qy)
for (x, y), (u, v) in zip(source, target))
b = sum((x-px)*(v-qy) - (y-py)*(u-qx)
for (x, y), (u, v) in zip(source, target))
if hypot(a, b) < 1e-12:
raise ValueError("Rotation undetermined")
angle = atan2(b, a)
c, s = cos(angle), sin(angle)
return qx-c*px+s*py, qy-s*px-c*py, angle
cases = [
("repeated-wrong", True, 6, False, 3.3),
("repeated-near", True, 6, False, .3),
("complete", False, 6, False, .3),
("cropped", False, 3, False, .5),
("outliers", False, 6, True, .5),
]
for name, repeated, retained, outliers, initial_x in cases:
source = original + ([(3, 2), (3.5, 2.8)] if outliers else [])
target = reference[:retained]
if repeated:
target = target + [(x + 3, y) for x, y in target]
pose = (initial_x, -.25, 0)
for _ in range(30):
points = move(source, pose)
pairs = [min(target, key=lambda q: squared(p, q)) for p in points]
updated = fit(source, pairs)
change = max(abs(a-b) for a, b in zip(updated, pose))
pose = updated
if change < 1e-10:
break
points = move(source, pose)
nearest = sqrt(sum(min(squared(p, q) for q in target)
for p in points) / len(points))
error = sqrt(sum(squared(p, q) for p, q in zip(points[:6], reference)) / 6)
translation = hypot(pose[0] - .5, pose[1] + .25)
print(f"{name}: nearest={nearest:.6f}, reference={error:.6f}, "
f"translation={translation:.6f}")
Expected output:
repeated-wrong: nearest=0.000000, reference=3.000000, translation=3.000000
repeated-near: nearest=0.000000, reference=0.000000, translation=0.000000
complete: nearest=0.000000, reference=0.000000, translation=0.000000
cropped: nearest=0.541354, reference=0.478316, translation=0.569608
outliers: nearest=0.965647, reference=1.098949, translation=1.092925
The compact Python stopping test compares absolute pose coordinates and uses a fixed degeneracy threshold for these small examples. The widget checks incremental translation and wrapped rotation with a scale-aware rigid-fit check. Neither stopping test certifies the physical match.
Check two diagnostic claims
Exercise 1: Duplicate the target patch 4 m to the right, keeping the generating translation at (0.5, −0.25) m. Suppose ICP aligns all six source points perfectly to that duplicate. What are its nearest RMSE, reference-point RMSE, translation error, and rotation error?
Check the repeated-patch errors
The estimated translation is (4.5, −0.25) m, with zero rotation. Every source point reaches a target, so nearest RMSE is 0 m. Each point misses its reference by 4 m, so reference-point RMSE is 4 m. Translation error is 4 m and rotation error is 0°.
Exercise 2: Six original source points match perfectly. Two added source outliers have nearest-target distances 2 m and 3 m. What all-source nearest RMSE does this lab report? What reference-point RMSE does it report before the pose changes?
Check the metric populations
The objective includes eight sources: nearest RMSE = √[(6 × 0² + 2² + 3²)/8] = √(13/8) = 1.274755 m. The reference diagnostic includes only the six original identities, so it is 0 m. A zero reference error here uses the known generating pose; the robot cannot infer it from the residual alone.
Sources and further study
- Besl and McKay, A Method for Registration of 3-D Shapes, 1992: the original ICP method and its distance-convergence analysis. The planar data and failure calculations in this lesson are original constructions.
- Open3D, ICP registration: nearest-neighbor fitting, point-to-point and point-to-plane objectives, initial alignment, and thresholded evaluation.
- Open3D, Robust kernels: an implemented point-to-plane example that reduces large-residual influence. Its method differs from the unweighted point-to-point solver used here.
Continue with pose-graph optimization. Track what happens when a single relative-pose measurement disagrees with the rest of a route.