spatialrust_registration/
point_to_plane.rs

1use spatialrust_core::{HasNormals3, HasPositions3, PointCloud, SpatialError, SpatialResult};
2use spatialrust_math::{
3    solve_linear_system, Isometry3, LeastSquaresResult, Quat, TransformPoint, Vec3,
4};
5use spatialrust_search::{KdTree, NearestNeighborIndex};
6
7use crate::registration::{PointCloudRegistration, RegistrationResult};
8
9/// Configuration for point-to-plane ICP.
10///
11/// Point-to-plane ICP minimizes the distance from each transformed source point
12/// to the tangent plane of its target correspondence, which converges faster and
13/// more accurately than point-to-point ICP on locally planar surfaces. The target
14/// cloud must carry normals (e.g. from normal estimation).
15#[derive(Clone, Copy, Debug, PartialEq)]
16pub struct PointToPlaneIcpConfig {
17    /// Maximum number of ICP iterations.
18    pub max_iterations: usize,
19    /// Maximum correspondence distance.
20    pub max_correspondence_distance: f32,
21    /// Stop when the transform update is smaller than this threshold.
22    pub transformation_epsilon: f64,
23    /// Stop when the point-to-plane fitness is smaller than this threshold.
24    pub fitness_epsilon: f64,
25    /// Minimum number of correspondences required per iteration.
26    pub min_correspondences: usize,
27    /// Initial transform guess mapping source into target frame.
28    pub initial_guess: Isometry3<f32>,
29}
30
31impl Default for PointToPlaneIcpConfig {
32    fn default() -> Self {
33        Self {
34            max_iterations: 50,
35            max_correspondence_distance: 1.0,
36            transformation_epsilon: 1e-8,
37            fitness_epsilon: 1e-6,
38            min_correspondences: 6,
39            initial_guess: Isometry3::identity(),
40        }
41    }
42}
43
44impl PointToPlaneIcpConfig {
45    /// Creates a config with the given correspondence distance.
46    #[must_use]
47    pub fn with_correspondence_distance(max_correspondence_distance: f32) -> Self {
48        Self { max_correspondence_distance, ..Self::default() }
49    }
50}
51
52/// Point-to-plane ICP registration.
53#[derive(Clone, Copy, Debug, PartialEq)]
54pub struct PointToPlaneIcp {
55    config: PointToPlaneIcpConfig,
56}
57
58impl PointToPlaneIcp {
59    /// Creates a point-to-plane ICP algorithm from config.
60    #[must_use]
61    pub const fn new(config: PointToPlaneIcpConfig) -> Self {
62        Self { config }
63    }
64
65    /// Returns the config.
66    #[must_use]
67    pub const fn config(&self) -> PointToPlaneIcpConfig {
68        self.config
69    }
70
71    /// Aligns `source` to `target` minimizing point-to-plane distance.
72    ///
73    /// `target` must provide normals.
74    pub fn align_with_diagnostics(
75        &self,
76        source: &PointCloud,
77        target: &PointCloud,
78    ) -> SpatialResult<RegistrationResult> {
79        if source.is_empty() || target.is_empty() {
80            return Err(SpatialError::InvalidArgument(
81                "ICP requires non-empty source and target point clouds".to_owned(),
82            ));
83        }
84
85        let (source_x, source_y, source_z) = source.positions3()?;
86        let (target_x, target_y, target_z) = target.positions3()?;
87        let (normal_x, normal_y, normal_z) = target.normals3()?;
88        let tree = KdTree::from_slices(target_x, target_y, target_z);
89        let max_distance_squared =
90            self.config.max_correspondence_distance * self.config.max_correspondence_distance;
91
92        let mut transform = self.config.initial_guess;
93        let mut transformed = vec![Vec3::new(0.0, 0.0, 0.0); source.len()];
94        apply_transform(&mut transformed, source_x, source_y, source_z, transform);
95
96        let mut iterations = 0usize;
97        let mut converged = false;
98
99        for _ in 0..self.config.max_iterations {
100            iterations += 1;
101
102            // Accumulate the 6x6 normal equations for the linearized problem.
103            let mut ata = [[0.0_f64; 6]; 6];
104            let mut atb = [0.0_f64; 6];
105            let mut count = 0usize;
106
107            for point in &transformed {
108                let Some(neighbor) = tree.nearest_one(point.x, point.y, point.z) else {
109                    continue;
110                };
111                if neighbor.distance_squared > max_distance_squared {
112                    continue;
113                }
114                let q = Vec3::new(
115                    target_x[neighbor.index],
116                    target_y[neighbor.index],
117                    target_z[neighbor.index],
118                );
119                let n = Vec3::new(
120                    normal_x[neighbor.index],
121                    normal_y[neighbor.index],
122                    normal_z[neighbor.index],
123                );
124
125                // Jacobian row for x = [rx, ry, rz, tx, ty, tz]: [p x n, n].
126                let c = point.cross(n);
127                let row = [
128                    f64::from(c.x),
129                    f64::from(c.y),
130                    f64::from(c.z),
131                    f64::from(n.x),
132                    f64::from(n.y),
133                    f64::from(n.z),
134                ];
135                let residual = f64::from((*point - q).dot(n));
136
137                for i in 0..6 {
138                    atb[i] -= row[i] * residual;
139                    for j in 0..6 {
140                        ata[i][j] += row[i] * row[j];
141                    }
142                }
143                count += 1;
144            }
145
146            if count < self.config.min_correspondences {
147                return Err(SpatialError::InvalidArgument(format!(
148                    "point-to-plane ICP found only {} correspondences, minimum is {}",
149                    count, self.config.min_correspondences
150                )));
151            }
152
153            let a_rows: Vec<Vec<f64>> = ata.iter().map(|row| row.to_vec()).collect();
154            let LeastSquaresResult::Solved(solution) = solve_linear_system(a_rows, atb.to_vec())
155            else {
156                return Err(SpatialError::InvalidArgument(
157                    "point-to-plane ICP normal equations were singular".to_owned(),
158                ));
159            };
160
161            let delta = delta_from_solution(&solution);
162            transform = delta.compose(transform);
163            apply_transform(&mut transformed, source_x, source_y, source_z, transform);
164
165            if update_magnitude(&solution) < self.config.transformation_epsilon {
166                converged = true;
167                break;
168            }
169            let fitness = point_to_plane_fitness(
170                &transformed,
171                target_x,
172                target_y,
173                target_z,
174                normal_x,
175                normal_y,
176                normal_z,
177                &tree,
178                max_distance_squared,
179            );
180            if fitness < self.config.fitness_epsilon {
181                converged = true;
182                break;
183            }
184        }
185
186        Ok(RegistrationResult {
187            transform,
188            fitness: point_to_plane_fitness(
189                &transformed,
190                target_x,
191                target_y,
192                target_z,
193                normal_x,
194                normal_y,
195                normal_z,
196                &tree,
197                max_distance_squared,
198            ),
199            iterations,
200            converged,
201        })
202    }
203}
204
205impl PointCloudRegistration for PointToPlaneIcp {
206    fn name(&self) -> &'static str {
207        "PointToPlaneIcp"
208    }
209
210    fn align(&self, source: &PointCloud, target: &PointCloud) -> SpatialResult<RegistrationResult> {
211        self.align_with_diagnostics(source, target)
212    }
213}
214
215fn apply_transform(
216    transformed: &mut [Vec3<f32>],
217    source_x: &[f32],
218    source_y: &[f32],
219    source_z: &[f32],
220    transform: Isometry3<f32>,
221) {
222    for (index, point) in transformed.iter_mut().enumerate() {
223        *point =
224            transform.transform_point(Vec3::new(source_x[index], source_y[index], source_z[index]));
225    }
226}
227
228/// Builds the incremental isometry from the linearized rotation/translation solution.
229fn delta_from_solution(solution: &[f64]) -> Isometry3<f32> {
230    let (rx, ry, rz) = (solution[0], solution[1], solution[2]);
231    let angle = (rx * rx + ry * ry + rz * rz).sqrt();
232    let rotation = if angle > 1e-12 {
233        let axis = Vec3::new((rx / angle) as f32, (ry / angle) as f32, (rz / angle) as f32);
234        Quat::from_axis_angle(axis, angle as f32)
235    } else {
236        Quat::<f32>::identity()
237    };
238    let translation = Vec3::new(solution[3] as f32, solution[4] as f32, solution[5] as f32);
239    Isometry3::new(rotation, translation)
240}
241
242fn update_magnitude(solution: &[f64]) -> f64 {
243    solution.iter().map(|value| value * value).sum::<f64>().sqrt()
244}
245
246#[allow(clippy::too_many_arguments)]
247fn point_to_plane_fitness(
248    transformed: &[Vec3<f32>],
249    target_x: &[f32],
250    target_y: &[f32],
251    target_z: &[f32],
252    normal_x: &[f32],
253    normal_y: &[f32],
254    normal_z: &[f32],
255    tree: &KdTree,
256    max_distance_squared: f32,
257) -> f64 {
258    let mut sum = 0.0_f64;
259    let mut count = 0usize;
260    for point in transformed {
261        let Some(neighbor) = tree.nearest_one(point.x, point.y, point.z) else {
262            continue;
263        };
264        if neighbor.distance_squared > max_distance_squared {
265            continue;
266        }
267        let q =
268            Vec3::new(target_x[neighbor.index], target_y[neighbor.index], target_z[neighbor.index]);
269        let n =
270            Vec3::new(normal_x[neighbor.index], normal_y[neighbor.index], normal_z[neighbor.index]);
271        let residual = f64::from((*point - q).dot(n));
272        sum += residual * residual;
273        count += 1;
274    }
275    if count == 0 {
276        return f64::MAX;
277    }
278    sum / count as f64
279}
280
281#[cfg(test)]
282mod tests {
283    use super::{PointToPlaneIcp, PointToPlaneIcpConfig};
284    use crate::registration::PointCloudRegistration;
285    use crate::transform::transform_point_cloud;
286    use spatialrust_core::{DType, FieldSemantic, PointCloudBuilder, PointField, PointSchema};
287    use spatialrust_math::{Isometry3, Quat, TransformPoint, Vec3};
288
289    fn schema_with_normals() -> PointSchema {
290        PointSchema::new()
291            .with_field(PointField::scalar("x", FieldSemantic::PositionX, DType::F32))
292            .with_field(PointField::scalar("y", FieldSemantic::PositionY, DType::F32))
293            .with_field(PointField::scalar("z", FieldSemantic::PositionZ, DType::F32))
294            .with_field(PointField::scalar("normal_x", FieldSemantic::NormalX, DType::F32))
295            .with_field(PointField::scalar("normal_y", FieldSemantic::NormalY, DType::F32))
296            .with_field(PointField::scalar("normal_z", FieldSemantic::NormalZ, DType::F32))
297    }
298
299    /// Two perpendicular faces with normals, giving full 6-DoF constraint.
300    fn box_corner() -> spatialrust_core::PointCloud {
301        let mut builder = PointCloudBuilder::new(schema_with_normals());
302        for i in 0..8 {
303            for j in 0..8 {
304                let (a, b) = (i as f32 * 0.1, j as f32 * 0.1);
305                // floor z=0, normal +Z
306                builder.push_point([a, b, 0.0, 0.0, 0.0, 1.0]).unwrap();
307                // wall y=0, normal +Y
308                builder.push_point([a, 0.0, b + 0.05, 0.0, 1.0, 0.0]).unwrap();
309                // wall x=0, normal +X
310                builder.push_point([0.0, a + 0.05, b + 0.05, 1.0, 0.0, 0.0]).unwrap();
311            }
312        }
313        builder.build().unwrap()
314    }
315
316    #[test]
317    fn aligns_rotated_and_translated_source() {
318        let target = box_corner();
319        let misalignment = Isometry3::new(
320            Quat::from_axis_angle(Vec3::new(0.0, 0.0, 1.0), 0.08),
321            Vec3::new(0.02, -0.015, 0.01),
322        );
323        let source = transform_point_cloud(&target, misalignment).unwrap();
324
325        let icp = PointToPlaneIcp::new(PointToPlaneIcpConfig {
326            max_correspondence_distance: 0.2,
327            max_iterations: 40,
328            ..PointToPlaneIcpConfig::default()
329        });
330        let result = icp.align(&source, &target).unwrap();
331        assert!(result.fitness < 1e-5, "fitness too high: {}", result.fitness);
332
333        // Composing the recovered transform with the misalignment restores points.
334        let composed = result.transform.compose(misalignment);
335        let probe = Vec3::new(0.3, 0.4, 0.2);
336        let restored = composed.transform_point(probe);
337        assert!((restored.x - probe.x).abs() < 5e-3);
338        assert!((restored.y - probe.y).abs() < 5e-3);
339        assert!((restored.z - probe.z).abs() < 5e-3);
340    }
341
342    #[test]
343    fn requires_target_normals() {
344        let mut builder = PointCloudBuilder::xyz();
345        builder.push_point([0.0, 0.0, 0.0]).unwrap();
346        let cloud = builder.build().unwrap();
347        let icp = PointToPlaneIcp::new(PointToPlaneIcpConfig::default());
348        assert!(icp.align(&cloud, &cloud).is_err());
349    }
350}
spatialrust_registration/point_to_plane.rs

spatialrust_registration/
point_to_plane.rs
