Metadata-Version: 2.4
Name: fricp
Version: 0.1.0
Classifier: Development Status :: 4 - Beta
Classifier: Intended Audience :: Science/Research
Classifier: License :: OSI Approved :: GNU General Public License v3 or later (GPLv3+)
Classifier: Programming Language :: Rust
Classifier: Programming Language :: Python :: 3
Classifier: Programming Language :: Python :: Implementation :: CPython
Classifier: Topic :: Scientific/Engineering :: Image Processing
Classifier: Topic :: Scientific/Engineering :: Mathematics
Classifier: Typing :: Typed
Requires-Dist: numpy>=1.21
License-File: LICENSE
Summary: Fast and Robust Iterative Closest Point (FRICP) — rigid registration of 3-D point sets, in Rust
Keywords: icp,registration,point-cloud,slam,alignment,3d
Home-Page: https://github.com/schlegelp/fricp
Author: Philipp Schlegel
Requires-Python: >=3.9
Description-Content-Type: text/markdown; charset=UTF-8; variant=GFM
Project-URL: Homepage, https://github.com/schlegelp/fricp
Project-URL: Issues, https://github.com/schlegelp/fricp/issues
Project-URL: Repository, https://github.com/schlegelp/fricp

# fricp

Fast and Robust Iterative Closest Point — rigid (optionally similarity) registration between two 3-D point sets, in Rust, with Python bindings.

An implementation of [*Fast and Robust Iterative Closest Point*](https://arxiv.org/abs/2007.07627) by Juyong Zhang, Yuxin Yao and Bailin Deng (IEEE TPAMI 2021).

Classical ICP has two well-known weaknesses. It converges linearly, so it takes many iterations. And its squared-distance metric insists that *every* source point be explained by the target, which quietly wrecks the alignment when the clouds only partly overlap or carry outliers. The paper fixes both:

- **Anderson acceleration.** Classical ICP is a majorisation-minimisation algorithm, so it is a fixed-point iteration, so it can be extrapolated from its own history. The extrapolation happens in the Lie algebra `se(3)`, where affine combinations of past iterates are still valid rigid transformations, and an accelerated iterate is kept only if it lowers the target energy — the energy still decreases monotonically.
- **A Welsch robust metric.** Replacing the squared distance with `1 - exp(-r² / 2ν²)` bounds what any single pair can cost, so outliers stop dragging the fit. Majorising it keeps the alignment step a closed-form weighted SVD, and `ν` is annealed from coarse to fine so the solve starts near-global and progressively sheds bad pairs.

## Install

```toml
# Cargo.toml
[dependencies]
fricp = "0.1"
```

```sh
pip install fricp
```

## Use it

```rust
use fricp::{register, Config, Method};

let result = register(&source, &target, &Config::new(Method::RobustIcp))?;

println!("{:?}", result.transform.to_matrix4());
println!("overlap ≈ {:.0}%", result.inlier_ratio * 100.0);
let aligned: Vec<[f64; 3]> = source
    .iter()
    .map(|p| result.transform.transform_point(*p))
    .collect();
```

```python
import fricp

result = fricp.register(source, target, method="robust_icp")

print(result.transform)                      # (4, 4) float64
print(f"overlap ≈ {result.inlier_ratio:.0%}")
aligned = result.apply(source)
```

Point-to-plane needs normals on the target; estimate them if your data has none:

```python
normals = fricp.estimate_normals(target, k=30)
result = fricp.register(source, target, method="robust_point_to_plane",
                        target_normals=normals)
```

## Which method

| Method | Normals | Use when |
|---|---|---|
| `Icp` / `"icp"` | no | the classical baseline, for comparison |
| `FastIcp` / `"fast_icp"` | no | clean, fully overlapping clouds |
| `RobustIcp` / `"robust_icp"` | no | **default** — noise, outliers, partial overlap |
| `PointToPlane` / `"point_to_plane"` | yes | clean scans of smooth surfaces |
| `RobustPointToPlane` / `"robust_point_to_plane"` | yes | usually the most accurate on surface scans |

All of them **refine** an alignment. Like every ICP variant they converge to the nearest local minimum, so where they start matters as much as which one you pick. The default start translates the source so the two centroids coincide; `Initial::Identity` (`init="identity"`) leaves the clouds where they are, and `Initial::Pose` (`init=matrix`) takes a known pose from odometry or a global matcher such as Super4PCS, FPFH + RANSAC, or TEASER++.

## Estimating scale

The fit is rigid by default. Ask for `estimate_scale` and the three point-to-point methods solve a 7-DoF similarity `p ↦ s R p + t` instead, which is what you want when the clouds come from different modalities or carry a unit mismatch:

```rust
let config = Config::new(Method::RobustIcp).with_scale_estimation();
let result = register(&source, &target, &config)?;
println!("scale {:.4}", result.transform.scale());
```

```python
result = fricp.register(source, target, estimate_scale=True)
print(result.scale)
print(result.transform)   # upper-left block is scale * rotation
print(result.rotation)    # the pure rotation, scale divided out
```

The scale comes from Umeyama's closed form, folded into the same weighted SVD the alignment step already computes — so it is the exact minimiser at each step and `robust_icp` keeps its monotone energy decrease. Normalisation divides both clouds by the *same* factor and therefore never affects the result.

**One caveat worth knowing.** The Welsch energy `Σ 1 - exp(-r²/2ν²)` is *minimised* by collapsing the source onto a single target point: drive `s → 0` and every residual vanishes. That degenerate optimum is real, and the small-`ν` stages are where it is most tempting. The scale is therefore clamped into `scale_bounds` — `(0.1, 10.0)` by default — at every iteration. Narrow it when you know the scale better than two orders of magnitude:

```python
result = fricp.register(source, target, scale_bounds=(0.9, 1.1))
```

The point-to-plane methods do not support scale yet; asking for it is an error rather than a silent no-op.

## What it can take

Registering a bumpy sphere against a clipped copy of itself, 6000 points, plus 5% gross outliers. Numbers are the RMS pose error (paper Eq. 12) at convergence, over a cloud of radius ≈ 1:

| True overlap | `icp` | `robust_icp` | `robust_point_to_plane` |
|---:|---:|---:|---:|
| 94% | 0.009 | **0.000** | **0.000** |
| 87% | 0.258 | **0.000** | **0.000** |
| 74% | 0.442 | **0.000** | **0.000** |
| 56% | 0.554 | 0.333 | **0.000** |
| 33% | 0.717 | 0.545 | **0.000** |
| ~0% | 0.911 | 0.740 | 0.255 |

Plain ICP is already lost at 87% overlap. The robust point-to-plane variant holds on down to a third. Below that everything fails, and so does the authors' own C++ implementation on the same data — at that point you need a global method, not a better ICP.

`inlier_ratio` recovers the true overlap closely (0.87, 0.74, 0.57 for the rows above), which makes it a usable confidence signal.

## Correctness

Validated against [the authors' reference C++ implementation](https://github.com/yaoyx689/Fast-Robust-ICP) on its own example data — a 15 446-point target and a 14 806-point source. Largest absolute difference over all 16 entries of the resulting transform:

| Method | Reference method # | max abs difference | this crate |
|---|---:|---:|---|
| `Icp` | 0 | 5.0e-7 | 65 iterations / 0.063 s |
| `FastIcp` | 2 | 4.4e-5 | 34 iterations / 0.045 s |
| `RobustIcp` | 3 | 7.6e-5 | 177 iterations / 0.176 s |
| `PointToPlane` | 4 | 6.5e-6 | 11 iterations / 0.022 s |
| `RobustPointToPlane` | 5 | 4.7e-6 | 58 iterations / 0.089 s |

(Apple M-series, release build. The residual differences come from the two deliberate deviations below, which change the path taken but not the fixed point reached.)

Reproduce it with the bundled example:

```sh
cargo run --release --example register_ply -- target.ply source.ply robust_icp
```

## Deliberate deviations from the reference

Two, both documented in the source:

- **Anderson acceleration runs on the 6-vector twist**, not on the 16 entries of the flattened 4×4 logarithm. The two span the same subspace, but the flattened form stores each rotation entry twice and so silently weights the rotation part of the least-squares problem by two. The energy-decrease safeguard means either choice converges to the same fixed point.
- **The point-to-plane Jacobian uses the correct small-angle limits.** The reference zeroes `∂t/∂δ` when the rotation is near identity; the true limit is `½ (e_j × υ)`. This crate uses Taylor series for all six exponential-map coefficients, which matters precisely near convergence.

Beyond that: the SE(3) logarithm is computed in closed form via a quaternion (stable all the way to a rotation of π) rather than by real Schur decomposition, and the point-to-plane line search backtracks as Algorithm 2 describes rather than trying a single step.

## Details

- **Reproducible.** Results are bit-for-bit identical however many threads rayon uses. Parallel work either writes to disjoint slots or reduces over fixed-size chunks combined in order.
- **Parallel** by default via rayon (`default-features = false` for a serial build). The Python bindings release the GIL, so `register` can be called from a thread pool.
- **Few dependencies:** `nalgebra` and (optionally) `rayon`. The k-d tree is built in.
- **No `unsafe`** in the core crate (`#![forbid(unsafe_code)]`).
- Input is plain `[f64; 3]` — no `nalgebra` types needed at the API boundary, though they are available for callers who want them.

## Development

```sh
cargo test --workspace                      # Rust: unit, integration, doc tests
cargo test -p fricp --no-default-features   # and the serial build

pip install pytest numpy
pip install .                               # or, inside an activated venv:
                                            # maturin develop --release
pytest python/tests
```

## Licence and attribution

GPL-3.0-or-later; see [LICENSE](LICENSE).

The algorithm is due to Zhang, Yao and Deng. Their reference implementation at
[yaoyx689/Fast-Robust-ICP](https://github.com/yaoyx689/Fast-Robust-ICP) is MIT
licensed (© 2020 yaoyuxin) and was used to cross-check this port; its licence
permits the relicensing here.

```bibtex
@article{zhang2021fast,
  title   = {Fast and Robust Iterative Closest Point},
  author  = {Zhang, Juyong and Yao, Yuxin and Deng, Bailin},
  journal = {IEEE Transactions on Pattern Analysis and Machine Intelligence},
  year    = {2021}
}
```

