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 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

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

pip install fricp

Use it

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();

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:

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

Which method

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:

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

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:

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:

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 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:

(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:

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

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.

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

@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}
}