ICP algorithm explained: align two scans step by step

The ICP algorithm aligns two point sets by repeating two steps: match nearby points, then fit a rotation and translation. Its name, iterative closest point, describes that loop. A good starting pose matters because the nearest point can belong to the wrong part of the scene.

Here you can inspect every update in a small planar example. You will also produce a perfect point-set overlap with the wrong rotation, which exposes a limit that a residual alone cannot resolve. Download the Python exercise to run the same point sets and check the results on your computer.

What the ICP algorithm aligns

Suppose a robot observes a wall from two positions. The same wall has different coordinates in the two sensor frames. Registration seeks a transform that expresses one set of points in the other's frame.

ICP is a local registration method. The Open3D tutorial starts with two clouds and an initial transform, then refines their alignment. An odometry estimate or a global registration method can supply that initial pose.

This lesson uses generated 2D point sets with complete overlap and a known transform. They let us check the answer exactly. They represent the geometric registration problem without simulating a LiDAR's rays, scan timing, or missing returns.

Define the transform and objective

Call the moving source points pᵢ and the fixed target points qⱼ. Coordinates use meters. The transform T(p) = R(θ)p + t maps source coordinates into the target frame, with counterclockwise angle θ and translation t = (tₓ, tᵧ).

The rotation preserves lengths and orientation. This model permits no scaling, reflection, or bending. Review coordinate frames if the direction of the mapping feels unclear.

For N source points, define the directed mean-squared nearest-point objective:

F has units of m²; root-mean-square error, or RMSE, uses meters. Every source point contributes once. A target point may receive several matches or none, so swapping source and target can change this objective.

Match each point to its nearest neighbor

Apply the current transform, then find the nearest target to each transformed source point. Write that target's index as cᵢ = argminⱼ ‖T(pᵢ) − qⱼ‖². Hold these choices fixed during the next fit.

Nearest-neighbor matching uses geometry at the current pose. It has no knowledge of which generated point originally produced each target. The lab resolves exact distance ties by choosing the earliest target index.

Several source points can choose the same target. That is valid for this directed objective, though it can yield a poor alignment. The lab's Distinct target matches readout helps you spot that collapse.

Fit one planar rotation and translation

Let aᵢ be the currently transformed source points and bᵢ their chosen targets. Compute their centroids ā and b̄. Center the pairs: uᵢ = aᵢ − ā, vᵢ = bᵢ − b̄.

For a trial rotation ΔR, the best translation is Δt = b̄ − ΔR ā. Substituting it leaves a centered least-squares problem. Expanding the squared distances shows that minimizing them means maximizing A cos Δθ + B sin Δθ, where:

This solves the fixed-pair planar fit analytically. It gives the best proper rotation for those pairs, even when they are the wrong pairs. In higher dimensions, singular value decomposition provides a common route to the rigid fit.

If A and B are both zero, the objective supplies no preferred rotation. The lab also stops when their magnitude is too small relative to the point spread to resolve reliably in floating-point arithmetic. It reports Rotation undetermined and keeps the previous pose.

Calculate a fixed-pair example

Take these three known pairs, in meters:

Source aᵢ	Matched target bᵢ
(0, 0)	(1, 2)
(1, 0)	(1, 3)
(0, 1)	(0, 2)

Their centroids are ā = (1/3, 1/3) and b̄ = (2/3, 7/3). Centering and summing gives A = 0 and B = 4/3 m². Thus Δθ = atan2(4/3, 0) = 90°.

That rotation maps ā to (−1/3, 1/3). Subtracting from b̄ gives Δt = (1, 2) m. Check the third point: R(90°)(0, 1) + (1, 2) = (0, 2), exactly its target.

All three residuals are zero. This calculation assumes the listed pairings; it does not show that nearest-neighbor matching will find them from any initial pose.

Compose the update and repeat

The fit maps the current source points toward their targets. Compose it on the left of the existing transform:

Simply adding translations misses the rotation of the old translation vector. The transform-composition lesson explains why order matters. Apply the combined transform to the original source points, find new nearest neighbors, and repeat.

With exact nearest neighbors, fixed point sets, and an exact least-squares fit, this objective cannot increase in exact arithmetic. The fit cannot worsen the current pairs' squared error. Rematching can only choose equal or shorter distances at the new pose.

This monotonic decrease does not promise the globally best alignment. Besl and McKay's original ICP paper analyzes convergence of the distance objective; initialization still matters. Our lab stops after a tiny update or 30 iterations, and floating-point rounding can cause negligible residual changes.

Step through ICP in the lab

Start with Nearby start. The ten-point target comes from rotating the source by 20°, then translating by (0.6, −0.2) m. ICP starts at zero rotation and translation, without receiving that answer.

The initial nearest-point RMSE is 0.562084 m. After one update it falls to 0.432199 m. By the third update the estimate reaches the known transform within rounding; a fourth tiny update triggers the stopping rule.

Planar point-to-point registration

Watch ICP match, fit, and repeat

Align generated source points with a fixed target. Each step chooses nearest neighbors, fits a rigid update, and finds the next set of matches.

ICP exampleNearby startBad initial rotationSymmetric room

Initial rotation (deg)

0

Initial translation x (m)

0

Initial translation y (m)

0

Changing the initial pose starts a new run. The target uses a known 20° rotation and translation (0.6, -0.2) m. ICP receives only the two point sets and your initial pose. The known transform supplies the error readouts.

ICP iteration

0 of 4

Step ICPFinish ICPReset ICP experiment

Current points and nearest neighbors

Iteration

0

Nearest-point RMSE (m)

0.562084

Translation error (m)

0.632456

Rotation error (deg)

20.000000

Estimated rotation (deg)

0.000000

Estimated translation x (m)

0.000000

Estimated translation y (m)

0.000000

Distinct target matches

7 of 10

Ready. Inspect the matches, then apply the next rigid update.

Residual after each completed update

RMSE averages all source-to-nearest-target squared distances, then takes the square root. Translation error compares the estimated and known translation vectors; rotation error is the smallest absolute angle difference, from 0° to 180°. A real scan pair usually lacks these known-transform checks.

The correspondence lines show nearest neighbors at the displayed pose, ready for the next fit. Changing an initial-pose control starts a new run. Step tolerance reached means the incremental translation and angle are below 10⁻¹⁰ m and 10⁻¹⁰ rad; it does not certify a correct pose.

Separate a low residual from the right pose

Select Bad initial rotation and finish the run. RMSE decreases from 0.695486 m to 0.366997 m, but the final rotation error is 128.866330° and translation error is 0.405761 m. More copies of the same tiny update cannot repair those matches.

Now select Symmetric room. Its point pattern repeats after a half-turn. The initial −160° estimate and the true 20° transform produce the same unlabeled point set, so RMSE rounds to 0.000000 m while rotation error remains 180°.

The known generating transform lets this lesson measure those errors. Real scan pairs usually lack that reference. Geometry alone cannot distinguish perfectly symmetric alternatives; another observation or prior is needed to resolve them.

Check overlap, geometry, and model limits

The lab isolates an unweighted, point-to-point objective. Practical scan matching also needs choices about data quality and correspondence selection:

• Partial overlap: unmatched source regions still pull on our all-point fit. A distance gate or trimming changes which pairs contribute and requires separate overlap checks.
• Outliers and moving objects: a squared-distance cost gives large residuals strong influence. Rejecting inconsistent matches or using a suitable robust loss changes that influence.
• Repeated or weak geometry: a corridor can admit competing alignments. A small residual cannot supply information the scene lacks.
• Scan motion: points collected at different times can violate the single-rigid-transform assumption. This lab treats each set as an instantaneous observation.

Point-to-plane ICP uses target surface normals and minimizes the displacement along those normals. In a planar setting, its counterpart fits point-to-line distances. Open3D documents both objectives; our widget implements point-to-point updates only.

Registration supplies one geometric relationship. A complete SLAM system must also manage observations over time, estimate a map and poses, and handle inconsistent constraints. A scan-matching result can complement wheel odometry, but neither a low residual nor this teaching demo validates a full SLAM system.

Continue with ICP failure cases to compare repeated geometry, cropped target coverage, and source outliers. The experiments show correspondence RMSE beside known-transform error, including a zero-residual alignment displaced by 3 m.

Run the downloadable Python exercise

Download the ICP exercise ZIP, then extract it. The folder contains a standard-library Python solver, the point sets, expected output, and a short README. It uses the same three presets as the lab: nearby start, bad initial rotation, and symmetric room.

You can also inspect or save the individual files:

• icp.py: the solver, command-line entry point, and sanity checks.
• points.json: explicit source and target coordinates, initial poses, and the separate reference transform.
• expected-output.txt: output captured from the checked run.
• README.md: commands, coordinate conventions, and limits.

Keep the four files together. Open a terminal in the extracted folder and run:

python3 icp.py --check

On Windows, use py -3 in place of python3 if that is how you launch Python. The script requires Python 3.10 or later and installs no packages. Validation used Python 3.14.3 on macOS ARM64 and Python 3.12.3 in an Ubuntu ARM64 container.

The checks cover the hand-worked triangle, transform composition, and the expected outcomes of the three presets. A passing check means the exercise reproduces those cases. The bad-start and symmetry cases still demonstrate incorrect poses.

These rounded results compare nearest-point RMSE with rotation error against the known transform. The download reports translation error too.

Preset	RMSE (m)	Rotation error
Nearby start	≈0	≈0°
Bad start	0.367	128.866°
Symmetric room	≈0	180°

The nearby run recovers 20° and (0.6, −0.2) m. Its first update changes RMSE from 0.562084 m to 0.432199 m, matching the lab. Compare the printed output with expected-output.txt; tiny floating-point differences can occur across runtimes.

Inspect the updates

Run one case with its iteration trace:

python3 icp.py --case nearby --trace
python3 icp.py --case bad-start --trace
python3 icp.py --case symmetric --trace

The downloadable solver fits the currently transformed points, composes that update on the left, and rematches. It uses the lab's scale-aware rotation check, a 30-update limit, and separate translation and rotation tolerances of 10⁻¹⁰ m and 10⁻¹⁰ rad. The trace includes the final tiny update that triggers the stopping rule.

points.json stores the target coordinates directly. The solver receives those coordinates, the source points, and an initial pose. Reporting uses the known generating transform to calculate pose errors; the solver never receives it. Coordinates use meters; positive angles turn counterclockwise from the source frame into the target frame. The script stores angles in radians and prints degrees.

Explain a failure before changing the solver

Compare the bad-start trace with the nearby trace. The fitting residual falls in both. Only the nearby run reaches the generating pose. Then inspect the symmetric run: the unlabeled geometry fits even when the rotation differs by a half-turn.

These are deterministic synthetic point sets with complete overlap, without noise or sensor timing. The package does not contain the cropped-overlap and outlier experiments from the separate ICP failure-cases lesson. Continue there to see what changes when the available points no longer support the same fit.

Try two alignment checks

Exercise 1: The current transform rotates 90° and translates by (1, 0) m. A fitted update also rotates 90° and translates by (2, −1) m. What is the combined transform, and where does it send (1, 0)?

Check the transform composition

The combined rotation is 180°. Rotate the old translation: R(90°)(1, 0) = (0, 1). Add the update translation to get (2, 0) m. The point (1, 0) then becomes (−1, 0) + (2, 0) = (1, 0) m.

Exercise 2: A source set contains (−1, 0) and (1, 0). The target contains (2, 0) and (4, 0). The generating transform is a zero rotation plus (3, 0) m translation. What RMSE results from a 180° rotation with the same translation, and what is its rotation error?

Check the symmetric alignment

The transformed source points become (4, 0) and (2, 0). Each has a target at zero distance, so nearest-point RMSE is zero. Rotation error against the generating transform is 180°. The unlabeled geometry cannot tell the two solutions apart.

An aligned scan pair can supply a relative-motion constraint in SLAM, where many measurements connect robot positions and the map. The loop-closure lesson shows how an added revisit constraint changes an entire route, including what happens when that constraint is wrong.

A registration residual measures agreement between the points used in the fit. To evaluate a sequence of estimated robot poses against a separate reference, continue with trajectory-error metrics.

The LiDAR scan lesson shows how range and bearing generate scan points and how an obstacle hides surfaces behind it. To represent free, occupied, and unobserved space across observations, continue with occupancy-grid mapping.

Sources and further study

• Besl and McKay, A Method for Registration of 3-D Shapes, 1992: the original ICP formulation and distance-convergence analysis. The small planar dataset and worked examples here are original teaching constructions.
• Open3D, ICP registration: a runnable library example, initial-alignment assumptions, and point-to-point versus point-to-plane objectives. Its thresholded inlier metrics differ from this lab's all-source-point RMSE.

Continue with SVD to connect centered point pairs to matrix factorization. Then return to wheel odometry and ask which motion errors a scan of your chosen geometry could actually reveal.