Generalized ICP (GICP) keeps the ICP loop but changes the residual model. Plain point-to-point ICP treats every correspondence residual with the same isotropic metric:
min_R,t sum ||R p_i + t - q_i||^2
GICP estimates a local covariance around each source point and target point.
For correspondence (p_i, q_i), the residual is:
e_i = R p_i + t - q_i
and the covariance-aware cost is:
e_i^T (C_qi + R C_pi R^T)^-1 e_i
Intuition: if a point lies on a locally planar patch, residuals tangent to the surface are less informative than residuals along the surface normal. The covariance matrix encodes that local shape.
File: pointcloud_geolab/registration/gicp.py
The implementation is intentionally compact:
- Estimate local covariance matrices with the custom KDTree kNN query.
- Find correspondences with the custom KDTree.
- Convert the Mahalanobis residual into scalar correspondence weights.
- Solve each rigid update with weighted SVD.
- Compose transforms until RMSE convergence.
This is not a wrapper around Open3D. Open3D is only used as an optional industrial baseline in benchmarks.
GICP costs more per iteration than point-to-point ICP because every correspondence evaluates a covariance system. It can be more stable on smooth surface data, but it is still local: it needs overlap and a reasonable initial pose. Bad normals/covariances, thin structures, repeated geometry, and very poor initialization remain failure modes.
python examples/gicp_demo.py
python -m pointcloud_geolab benchmark --suite gicp --quick --output outputs/benchmarkspython -m pytest tests/test_gicp.py