# ICP: align two point sets in Python

Run the three examples from [the ICP algorithm lesson](https://www.dustinturner.ai/writing/icp-algorithm) with Python 3.10 or later. No packages, installation steps, or network access are needed.

Keep these four files together after extracting the download:

- `icp.py`: the solver, reporting, and checks.
- `points.json`: explicit source and target coordinates, initial poses, and separate evaluation metadata.
- `expected-output.txt`: output captured by running `python3 icp.py --check` with Python 3.14.3.
- `README.md`: these instructions.

## Run it

Open a terminal in the extracted `icp-algorithm` folder:

```sh
python3 icp.py --check
```

This prints all three cases, then checks a hand-worked triangle fit, transform composition, tie handling, a degenerate fit, and fixed numerical benchmarks. A passing run ends with `Checks passed`; a failed check or invalid input exits with a nonzero status. The checks use ordinary conditions, so they also run when Python optimization is enabled.

To inspect one case and every iteration:

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

`python3 icp.py` prints all three summaries without the checks. Combining `--case` with `--check` prints the chosen case while checking all three.

## Read the result

The pose maps source coordinates into the target frame: **T(p) = R(theta)p + (x, y)**. Coordinates, translations, and RMSE use meters. JSON and solver angles use counterclockwise radians; printed angles use degrees. RMSE is the square root of the mean squared distance from each transformed source point to its nearest target. It is directed: a target can receive several matches or none.

| Case | Final rotation | Final translation (m) | Final RMSE (m) | Rotation error | Updates |
| --- | --- | --- | --- | --- | --- |
| `nearby` | 20.000000° | (0.600000, −0.200000) | 0.000000 | 0.000000° | 4 |
| `bad-start` | 148.866330° | (0.539248, −0.601187) | 0.366997 | 128.866330° | 5 |
| `symmetric` | −160.000000° | (0.600000, −0.200000) | 0.000000 | 180.000000° | 1 |

The nearby start recovers the generating pose. The bad start settles at incorrect matches; its translation error is 0.405761 m. The symmetric pattern gives a nearly zero residual at a rotation that differs from the generating pose by half a turn. Residual and pose error answer different questions. Printed zeroes are rounded floating-point values.

## Inspect the data and solver

These are original synthetic point sets from the lesson, with complete overlap and no noise. The asymmetric set contains ten points; the symmetric set contains eight. The stored targets were generated once using a 20° rotation and a (0.6, −0.2) m translation. They are included explicitly so the solver reads an observed target array rather than creating its own answer.

`run_icp(source, target, initial)` uses only the supplied coordinates and initial pose. It does not read `evaluation.reference_transform`. Reporting and checks use that separate reference to measure pose error. The arrays retain their construction order, but nearest-neighbor matching ignores that pairing and uses geometry alone. Exact distance ties keep the first target index.

Each iteration fits the current transformed points to their nearest targets, composes that increment on the left, then rematches. Both the incremental translation and wrapped rotation must be below 10⁻¹⁰ m and 10⁻¹⁰ rad to stop early. The default limit is 30 updates. A scale-aware floating-point check stops a fit when its rotation is undetermined. These choices match the lesson's interactive lab, including the final tiny update in the history.

For the independent fixed-pair check, (0, 0), (1, 0), (0, 1) map to (1, 2), (1, 3), (0, 2). The answer is a 90° rotation and a (1, 2) m translation. Those known pairs test the rigid fit; they do not promise that ICP will discover those pairs from any starting pose.

## Try a change

After reproducing the baseline, change one initial pose under `cases` in `points.json` and run that case with `--trace`. Keep the two point arrays fixed. Does the residual decrease, and does the recovered pose remain correct?

The bundled checks intentionally describe the original examples. If you alter a dataset or initial pose, a benchmark failure can be expected. Preserve an unmodified copy for comparison; do not use the reference transform to steer the solver.

## Limits

This is an unweighted 2D point-to-point teaching example. It has no outlier rejection, overlap filtering, surface normals, scan timing, sensor model, or global initialization. It is not a full SLAM system or evidence of robot performance. A tiny update indicates a stopping condition, not correct registration. Continue with [ICP failure cases](https://www.dustinturner.ai/writing/icp-failure-cases) to study cropped coverage, outliers, and repeated geometry.