[Feature]: Point-to-Plane ICP

🚀 Feature Description

Add point-to-plane Iterative Closest Point (ICP) for 3D point cloud registration, along with the necessary normal estimation (PCA-based) as a prerequisite.

This extends the existing kornia-icp crate, which currently only implements point-to-point ICP, with a more advanced algorithm that converges significantly faster on planar surfaces.

---

📂 Feature Category

Feature Detection/Matching

💡 Motivation

What problem does this feature solve?

The current icp_vanilla implementation in kornia-3d uses the point-to-point distance metric. On planar surfaces (walls, floors, LiDAR scans of buildings), this metric converges slowly because points can "slide" along the surface without significantly changing the alignment error. This makes the algorithm inefficient for real-world robotics and SLAM applications.

Why is this feature needed?

Point-to-plane ICP is a well-established improvement that converges ~2× faster on planar surfaces[reference:1]. It is the standard algorithm used in robotics, LiDAR odometry, and SLAM pipelines. Adding this feature makes kornia-rs more competitive with libraries like Open3D, PCL, and libpointmatcher for 3D point cloud registration.

Related Issue: None yet; this is the feature request.

---

💭 Proposed Solution

Add two new capabilities in a single PR:

Phase 1: Normal Estimation

• Add estimate_normals function that computes surface normals using PCA on k-nearest neighbors
• This is a prerequisite for point-to-plane ICP and also useful for other 3D vision tasks

Phase 2: Point-to-Plane ICP

• Add icp_point_to_plane function that aligns point clouds using the point-to-plane metric
• Uses normals from Phase 1
• Includes Levenberg-Marquardt damping for numerical stability

API Design:

// Phase 1: Normal estimation
pub fn estimate_normals(
    cloud: &PointCloud,
    k: usize,
) -> Result<PointCloud, NormalEstimationError>

// Phase 2: Point-to-plane ICP
pub fn icp_point_to_plane(
    source: &PointCloud,
    target: &PointCloud,
    initial_rot: [[f64; 3]; 3],
    initial_trans: [f64; 3],
    criteria: ICPConvergenceCriteria,
) -> Result<ICPResult, Box<dyn std::error::Error>>

### 📚 Library Reference

### 📚 Library Reference

**Existing Implementations:**

- **Open3D:** `open3d::pipelines::registration::TransformationEstimationPointToPlane` class

- **PCL (Point Cloud Library):** `pcl::IterativeClosestPointWithNormals<PointSource, PointTarget, Scalar>` — Special case of ICP that uses point-to-plane distances by default.

- **libpointmatcher:** Point-to-plane variant used in the minimization step, allowing points to "slide" along planes.

- **OpenCV:** ICP point-to-plane odometry algorithm implementation (used in visual odometry).

**Key Mathematical Formulas:**

| Component | Formula |
|-----------|---------|
| Residual (point-to-plane distance) | `r = (R·p + t - q) · n` |
| Jacobian (rotation part) | `J_rot = (R·p + t) × n` |
| Jacobian (translation part) | `J_trans = -n` |
| Full Jacobian row | `J = [J_rot, J_trans]` |
| Linear system | `A·x = -b` where `x = [α, β, γ, tx, ty, tz]` |
| Rodrigues formula | Converts `(α, β, γ)` to rotation matrix |

**Why This Implementation Follows Established Algorithms:**

- The residual `r = (R·p + t - q) · n` is the standard Chen-Medioni point-to-plane distance.
- The Jacobian `J = [(R·p + t) × n, -n]` is the standard linearization of the point-to-plane error.
- The linear system `A·x = -b` is solved via QR decomposition (matching Open3D's approach).
- Levenberg-Marquardt damping is used for numerical stability (matching PCL's robust ICP variants).

---




### 🔄 Alternatives Considered

_No response_

### 🎯 Use Cases

_No response_

### 📝 Additional Context



**Both Phase 1 and Phase 2 will be submitted in a single PR.**

**Performance Comparison (Local Benchmarks):**

| Test | Point-to-Point | Point-to-Plane | Speedup |
|------|----------------|----------------|---------|
| 100 points | 53.4 ms | 53.4 ms | ~1.0× |
| 400 points | 404.4 ms | 214.6 ms | ~1.88× |
| 900 points | 1045.2 ms | 536.4 ms | ~1.95× |

**Test Coverage:**

- Normal estimation: flat plane, tilted plane, edge cases
- Point-to-plane ICP: flat plane, tilted plane, noise robustness, sphere, edge cases
- Iteration count comparison: point-to-point vs point-to-plane on both flat and curved surfaces

**Implementation Status:** Complete
- [x] Phase 1: `estimate_normals` in `normal_estimation.rs`
- [x] Phase 2: `fit_transformation_point_to_plane` in `registration/ops.rs`
- [x] Phase 2: `find_correspondences_with_indices` in `registration/ops.rs`
- [x] Phase 2: `compute_point_to_plane_rmse` in `registration/ops.rs`
- [x] Phase 2: `icp_point_to_plane` in `registration/icp_vanilla.rs`



### 🤝 Contribution Intent

- [x] I plan to submit a PR to implement this feature
- [ ] I'm requesting this feature but not planning to implement it