spatialrust_registration/
icp.rs

1use spatialrust_core::{HasPositions3, PointCloud, SpatialError, SpatialResult};
2use spatialrust_math::{Isometry3, TransformPoint, Vec3};
3use spatialrust_search::{KdTree, Neighbor};
4
5use crate::kabsch::estimate_rigid_transform;
6use crate::registration::{PointCloudRegistration, RegistrationResult};
7
8/// Why a successful ICP call stopped; convergence does not certify the pose.
9#[derive(Clone, Copy, Debug, PartialEq, Eq)]
10pub enum IcpStopReason {
11    /// The absolute mean squared correspondence distance was small enough.
12    FitnessThreshold,
13    /// Both translation length and rotation angle of the update were small enough.
14    TransformThreshold,
15    /// The iteration budget was exhausted without meeting a convergence threshold.
16    IterationLimit,
17}
18
19/// Measurements for one completed update (no additional nearest-neighbor pass).
20#[derive(Clone, Copy, Debug, PartialEq)]
21pub struct IcpIteration {
22    /// One-based update number.
23    pub iteration: usize,
24    /// Gated correspondences used to estimate this update.
25    pub correspondences: usize,
26    /// Gated correspondences after applying the update and rematching.
27    pub evaluated_correspondences: usize,
28    /// Post-update mean squared distance, or f64::MAX if there are no matches.
29    pub fitness: f64,
30    /// Previous post-update fitness minus this fitness; None for the first update.
31    /// Membership can change, so this is not an error on a fixed set of points.
32    pub fitness_change: Option<f64>,
33    /// Update translation length in the input coordinate units.
34    pub translation_delta: f64,
35    /// Shortest update rotation angle in radians.
36    pub rotation_delta_radians: f64,
37}
38
39/// Opt-in ICP history. Ordinary alignment does not allocate this history.
40#[derive(Clone, Debug, PartialEq)]
41pub struct IcpDiagnostics {
42    /// Ordinary registration result from the same updates.
43    pub result: RegistrationResult,
44    /// One row per completed update, in execution order.
45    pub history: Vec<IcpIteration>,
46    /// Criterion that stopped this successful call.
47    pub stop_reason: IcpStopReason,
48}
49
50/// Configuration for point-to-point ICP.
51#[derive(Clone, Copy, Debug, PartialEq)]
52pub struct IcpConfig {
53    /// Maximum number of ICP iterations.
54    pub max_iterations: usize,
55    /// Maximum correspondence distance.
56    pub max_correspondence_distance: f32,
57    /// Stop when both translation length (coordinate units) and rotation angle
58    /// (radians) of the transform update are smaller than this threshold.
59    pub transformation_epsilon: f64,
60    /// Stop when the mean squared correspondence distance is below this threshold.
61    pub fitness_epsilon: f64,
62    /// Minimum number of correspondences required per iteration.
63    pub min_correspondences: usize,
64    /// Initial transform guess mapping source into target frame.
65    pub initial_guess: Isometry3<f32>,
66}
67
68impl Default for IcpConfig {
69    fn default() -> Self {
70        Self {
71            max_iterations: 50,
72            max_correspondence_distance: 1.0,
73            transformation_epsilon: 1e-8,
74            fitness_epsilon: 1e-6,
75            min_correspondences: 3,
76            initial_guess: Isometry3::identity(),
77        }
78    }
79}
80
81impl IcpConfig {
82    /// Creates a config with the given correspondence distance.
83    #[must_use]
84    pub fn with_correspondence_distance(max_correspondence_distance: f32) -> Self {
85        Self { max_correspondence_distance, ..Self::default() }
86    }
87}
88
89/// Explicit unit-aware stopping thresholds; zero disables the corresponding test.
90#[derive(Clone, Copy, Debug, PartialEq)]
91pub struct IcpConvergenceCriteria {
92    /// Translation update length threshold in input coordinate units.
93    pub translation_epsilon: f64,
94    /// Shortest rotation update threshold in radians.
95    pub rotation_epsilon: f64,
96    /// Absolute mean squared correspondence distance threshold.
97    pub fitness_epsilon: f64,
98}
99
100/// Point-to-point ICP registration.
101#[derive(Clone, Copy, Debug, PartialEq)]
102pub struct IcpRegistration {
103    config: IcpConfig,
104    criteria: IcpConvergenceCriteria,
105    trim_fraction: f64,
106}
107
108impl IcpRegistration {
109    /// Creates an ICP registration algorithm from config.
110    #[must_use]
111    pub const fn new(config: IcpConfig) -> Self {
112        Self {
113            config,
114            criteria: IcpConvergenceCriteria {
115                translation_epsilon: config.transformation_epsilon,
116                rotation_epsilon: config.transformation_epsilon,
117                fitness_epsilon: config.fitness_epsilon,
118            },
119            trim_fraction: 1.0,
120        }
121    }
122
123    /// Returns the base ICP config; explicit convergence overrides are separate.
124    #[must_use]
125    pub const fn config(&self) -> IcpConfig {
126        self.config
127    }
128
129    /// Overrides stopping thresholds without changing the base config or defaults.
130    /// Invalid thresholds are rejected when alignment is called.
131    #[must_use]
132    pub const fn with_convergence_criteria(mut self, criteria: IcpConvergenceCriteria) -> Self {
133        self.criteria = criteria;
134        self
135    }
136
137    /// Returns the effective stopping thresholds, including overrides.
138    #[must_use]
139    pub const fn convergence_criteria(&self) -> IcpConvergenceCriteria {
140        self.criteria
141    }
142
143    /// Retains floor(fraction * gated correspondences) lowest-distance pairs
144    /// for each update. Equal distances keep source input order. The default 1.0
145    /// preserves ordinary ICP. Invalid fractions and fewer than the configured
146    /// minimum retained pairs are rejected during alignment.
147    ///
148    /// Fitness and fitness stopping still evaluate every post-update gated pair;
149    /// they do not silently use a smaller trimmed denominator. Trimming assumes
150    /// larger residuals are less useful; it can discard informative correct pairs
151    /// when the initial pose is poor, and it does not resolve repeated geometry.
152    #[must_use]
153    pub const fn with_trim_fraction(mut self, fraction: f64) -> Self {
154        self.trim_fraction = fraction;
155        self
156    }
157
158    /// Returns the fraction of gated pairs retained for transform estimation.
159    #[must_use]
160    pub const fn trim_fraction(&self) -> f64 {
161        self.trim_fraction
162    }
163
164    /// Aligns `source` to `target` using iterative closest point.
165    pub fn align_with_diagnostics(
166        &self,
167        source: &PointCloud,
168        target: &PointCloud,
169    ) -> SpatialResult<RegistrationResult> {
170        self.align_impl(source, target, |_| {}, Ok).map(|(result, _, _)| result)
171    }
172
173    /// Aligns with an opt-in history; errors use the same path as ordinary alignment.
174    pub fn align_with_trace(
175        &self,
176        source: &PointCloud,
177        target: &PointCloud,
178    ) -> SpatialResult<IcpDiagnostics> {
179        let mut history = Vec::new();
180        let (result, stop_reason, _) =
181            self.align_impl(source, target, |row| history.push(row), Ok)?;
182        Ok(IcpDiagnostics { result, history, stop_reason })
183    }
184
185    /// Aligns with explicit GPU query uploads/readbacks and a resident target.
186    /// Both update matching and post-update fitness matching run on GPU.
187    /// Transform application, trimming, Kabsch, f64 fitness accumulation and
188    /// convergence remain on CPU. No automatic CPU fallback is performed.
189    #[cfg(feature = "register-icp-gpu")]
190    pub fn align_with_gpu_correspondences(
191        &self,
192        runtime: &spatialrust_gpu::WgpuRuntime,
193        source: &PointCloud,
194        target: &PointCloud,
195    ) -> SpatialResult<GpuIcpDiagnostics> {
196        let mut history = Vec::new();
197        let (result, stop_reason, search) = self.align_impl(
198            source,
199            target,
200            |row| history.push(row),
201            |tree| {
202                let target = spatialrust_gpu::GpuKdTree::upload(runtime, &tree)?;
203                let transfers = target.upload_transfers();
204                Ok(GpuCorrespondences { runtime, target, transfers, batches: 0 })
205            },
206        )?;
207        Ok(GpuIcpDiagnostics {
208            diagnostics: IcpDiagnostics { result, history, stop_reason },
209            transfers: search.transfers,
210            correspondence_batches: search.batches,
211        })
212    }
213
214    fn validate_inputs(&self, source: &PointCloud, target: &PointCloud) -> SpatialResult<()> {
215        if !self.trim_fraction.is_finite() || self.trim_fraction <= 0.0 || self.trim_fraction > 1.0
216        {
217            return Err(SpatialError::InvalidArgument(
218                "trim_fraction must be finite and in (0, 1]".to_owned(),
219            ));
220        }
221        let max_distance_squared =
222            self.config.max_correspondence_distance * self.config.max_correspondence_distance;
223        if self.config.max_correspondence_distance <= 0.0
224            || !max_distance_squared.is_finite()
225            || max_distance_squared == 0.0
226        {
227            return Err(SpatialError::InvalidArgument(
228                "max_correspondence_distance must be positive with finite nonzero f32 square"
229                    .to_owned(),
230            ));
231        }
232        if self.config.max_iterations == 0 || self.config.min_correspondences < 3 {
233            return Err(SpatialError::InvalidArgument(
234                "ICP requires max_iterations >= 1 and min_correspondences >= 3".to_owned(),
235            ));
236        }
237        for (name, value) in [
238            ("translation_epsilon", self.criteria.translation_epsilon),
239            ("rotation_epsilon", self.criteria.rotation_epsilon),
240            ("fitness_epsilon", self.criteria.fitness_epsilon),
241        ] {
242            if !value.is_finite() || value < 0.0 {
243                return Err(SpatialError::InvalidArgument(format!(
244                    "{name} must be finite and nonnegative"
245                )));
246            }
247        }
248        if source.is_empty() || target.is_empty() {
249            return Err(SpatialError::InvalidArgument(
250                "ICP requires non-empty source and target point clouds".to_owned(),
251            ));
252        }
253
254        let (source_x, source_y, source_z) = source.positions3()?;
255        let (target_x, target_y, target_z) = target.positions3()?;
256        for (name, x, y, z) in
257            [("source", source_x, source_y, source_z), ("target", target_x, target_y, target_z)]
258        {
259            if x.len() < 3 || x.iter().chain(y).chain(z).any(|v| !v.is_finite()) {
260                return Err(SpatialError::InvalidArgument(format!(
261                    "{name} requires at least 3 finite XYZ points"
262                )));
263            }
264        }
265        validate_icp_transform(self.config.initial_guess)?;
266        Ok(())
267    }
268
269    fn align_impl<S: IcpCorrespondences>(
270        &self,
271        source: &PointCloud,
272        target: &PointCloud,
273        mut observe: impl FnMut(IcpIteration),
274        make_search: impl FnOnce(KdTree) -> SpatialResult<S>,
275    ) -> SpatialResult<(RegistrationResult, IcpStopReason, S)> {
276        self.validate_inputs(source, target)?;
277        let max_distance_squared =
278            self.config.max_correspondence_distance * self.config.max_correspondence_distance;
279        let (source_x, source_y, source_z) = source.positions3()?;
280        let (target_x, target_y, target_z) = target.positions3()?;
281        let mut search = make_search(KdTree::from_slices(target_x, target_y, target_z))?;
282
283        let mut transform = self.config.initial_guess;
284        let mut transformed = Vec::with_capacity(source.len());
285        for index in 0..source.len() {
286            transformed.push(Vec3::new(source_x[index], source_y[index], source_z[index]));
287        }
288        apply_transform_in_place(&mut transformed, source_x, source_y, source_z, transform);
289        validate_transformed_points(&transformed)?;
290
291        let mut iterations = 0usize;
292        let mut converged = false;
293        let mut stop_reason = IcpStopReason::IterationLimit;
294        let mut previous_fitness = None;
295
296        for _ in 0..self.config.max_iterations {
297            iterations += 1;
298            let mut pairs_source = Vec::new();
299            let mut pairs_target = Vec::new();
300            let mut ranked_pairs = Vec::new();
301
302            search.visit(&transformed, max_distance_squared, |point, neighbor| {
303                if neighbor.distance_squared <= max_distance_squared {
304                    pairs_source.push(*point);
305                    pairs_target.push(Vec3::new(
306                        target_x[neighbor.index],
307                        target_y[neighbor.index],
308                        target_z[neighbor.index],
309                    ));
310                    if self.trim_fraction < 1.0 {
311                        ranked_pairs.push((neighbor.distance_squared, pairs_source.len() - 1));
312                    }
313                }
314            })?;
315
316            if pairs_source.len() < self.config.min_correspondences {
317                return Err(SpatialError::InvalidArgument(format!(
318                    "ICP found only {} correspondences, minimum is {}",
319                    pairs_source.len(),
320                    self.config.min_correspondences
321                )));
322            }
323
324            if self.trim_fraction < 1.0 {
325                let retained = (pairs_source.len() as f64 * self.trim_fraction).floor() as usize;
326                if retained < self.config.min_correspondences {
327                    return Err(SpatialError::InvalidArgument(format!(
328                        "trim_fraction retains only {retained} of {} correspondences, minimum is {}",
329                        pairs_source.len(), self.config.min_correspondences
330                    )));
331                }
332                ranked_pairs.sort_unstable_by(|a, b| a.0.total_cmp(&b.0).then(a.1.cmp(&b.1)));
333                pairs_source = ranked_pairs[..retained]
334                    .iter()
335                    .map(|(_, index)| pairs_source[*index])
336                    .collect();
337                pairs_target = ranked_pairs[..retained]
338                    .iter()
339                    .map(|(_, index)| pairs_target[*index])
340                    .collect();
341            }
342
343            let Some(delta) = estimate_rigid_transform(&pairs_source, &pairs_target) else {
344                return Err(SpatialError::InvalidArgument(
345                    "ICP failed to estimate a rigid transform".to_owned(),
346                ));
347            };
348
349            validate_icp_transform(delta)?;
350            transform = delta.compose(transform);
351            validate_icp_transform(transform)?;
352            apply_transform_in_place(&mut transformed, source_x, source_y, source_z, transform);
353            validate_transformed_points(&transformed)?;
354
355            let (fitness, evaluated_correspondences) = evaluate_fitness(
356                &transformed,
357                target_x,
358                target_y,
359                target_z,
360                &mut search,
361                max_distance_squared,
362            )?;
363            let (translation_delta, rotation_delta_radians) = transform_delta_magnitudes(delta);
364            observe(IcpIteration {
365                iteration: iterations,
366                correspondences: pairs_source.len(),
367                evaluated_correspondences,
368                fitness,
369                fitness_change: previous_fitness.map(|previous| previous - fitness),
370                translation_delta,
371                rotation_delta_radians,
372            });
373            previous_fitness = Some(fitness);
374            if fitness < self.criteria.fitness_epsilon {
375                converged = true;
376                stop_reason = IcpStopReason::FitnessThreshold;
377                break;
378            }
379            if translation_delta < self.criteria.translation_epsilon
380                && rotation_delta_radians < self.criteria.rotation_epsilon
381            {
382                converged = true;
383                stop_reason = IcpStopReason::TransformThreshold;
384                break;
385            }
386        }
387
388        Ok((
389            RegistrationResult {
390                transform,
391                fitness: evaluate_fitness(
392                    &transformed,
393                    target_x,
394                    target_y,
395                    target_z,
396                    &mut search,
397                    max_distance_squared,
398                )?
399                .0,
400                iterations,
401                converged,
402            },
403            stop_reason,
404            search,
405        ))
406    }
407}
408
409impl PointCloudRegistration for IcpRegistration {
410    fn name(&self) -> &'static str {
411        "IcpRegistration"
412    }
413
414    fn align(&self, source: &PointCloud, target: &PointCloud) -> SpatialResult<RegistrationResult> {
415        self.align_with_diagnostics(source, target)
416    }
417}
418
419fn apply_transform_in_place(
420    transformed: &mut [Vec3<f32>],
421    source_x: &[f32],
422    source_y: &[f32],
423    source_z: &[f32],
424    transform: Isometry3<f32>,
425) {
426    for (index, point) in transformed.iter_mut().enumerate() {
427        *point =
428            transform.transform_point(Vec3::new(source_x[index], source_y[index], source_z[index]));
429    }
430}
431
432fn evaluate_fitness<S: IcpCorrespondences>(
433    transformed: &[Vec3<f32>],
434    target_x: &[f32],
435    target_y: &[f32],
436    target_z: &[f32],
437    search: &mut S,
438    max_distance_squared: f32,
439) -> SpatialResult<(f64, usize)> {
440    let mut sum = 0.0_f64;
441    let mut count = 0usize;
442    search.visit(transformed, max_distance_squared, |point, neighbor| {
443        if neighbor.distance_squared <= max_distance_squared {
444            let dx = f64::from(point.x - target_x[neighbor.index]);
445            let dy = f64::from(point.y - target_y[neighbor.index]);
446            let dz = f64::from(point.z - target_z[neighbor.index]);
447            sum += dx * dx + dy * dy + dz * dz;
448            count += 1;
449        }
450    })?;
451    Ok((if count == 0 { f64::MAX } else { sum / count as f64 }, count))
452}
453
454#[cfg(test)]
455fn transform_delta_below_epsilon(delta: Isometry3<f32>, epsilon: f64) -> bool {
456    let (translation_norm, rotation_angle) = transform_delta_magnitudes(delta);
457    translation_norm < epsilon && rotation_angle < epsilon
458}
459
460fn validate_transformed_points(points: &[Vec3<f32>]) -> SpatialResult<()> {
461    if points.iter().any(|p| !p.x.is_finite() || !p.y.is_finite() || !p.z.is_finite()) {
462        return Err(SpatialError::InvalidArgument(
463            "ICP transform overflowed coordinates".to_owned(),
464        ));
465    }
466    Ok(())
467}
468
469fn validate_icp_transform(transform: Isometry3<f32>) -> SpatialResult<()> {
470    let t = transform.translation();
471    let r = transform.rotation();
472    let norm = f64::from(r.x).hypot(f64::from(r.y)).hypot(f64::from(r.z)).hypot(f64::from(r.w));
473    if !t.x.is_finite()
474        || !t.y.is_finite()
475        || !t.z.is_finite()
476        || !norm.is_finite()
477        || (norm - 1.0).abs() > 1e-4
478    {
479        return Err(SpatialError::InvalidArgument(
480            "ICP requires a finite transform with unit quaternion".to_owned(),
481        ));
482    }
483    Ok(())
484}
485
486fn transform_delta_magnitudes(delta: Isometry3<f32>) -> (f64, f64) {
487    let translation = delta.translation();
488    let translation_norm =
489        f64::from(translation.x).hypot(f64::from(translation.y)).hypot(f64::from(translation.z));
490    let rotation = delta.rotation();
491    let vector_norm =
492        f64::from(rotation.x).hypot(f64::from(rotation.y)).hypot(f64::from(rotation.z));
493    // atan2 retains small angles when the f32 scalar component rounds to one.
494    // abs(w) gives the same shortest angle for equivalent q and -q rotations.
495    let rotation_angle = 2.0 * vector_norm.atan2(f64::from(rotation.w).abs());
496    (translation_norm, rotation_angle)
497}
498
499trait IcpCorrespondences {
500    fn visit(
501        &mut self,
502        points: &[Vec3<f32>],
503        bound: f32,
504        consume: impl FnMut(&Vec3<f32>, Neighbor),
505    ) -> SpatialResult<()>;
506}
507
508impl IcpCorrespondences for KdTree {
509    fn visit(
510        &mut self,
511        points: &[Vec3<f32>],
512        bound: f32,
513        mut consume: impl FnMut(&Vec3<f32>, Neighbor),
514    ) -> SpatialResult<()> {
515        for point in points {
516            if let Some(neighbor) = self.nearest_one_within(point.x, point.y, point.z, bound) {
517                consume(point, neighbor);
518            }
519        }
520        Ok(())
521    }
522}
523
524/// GPU correspondence receipt with the ordinary ICP trace and CPU pose solver.
525#[cfg(feature = "register-icp-gpu")]
526#[derive(Clone, Debug, PartialEq)]
527pub struct GpuIcpDiagnostics {
528    /// Pose, fitness, convergence and per-update trace.
529    pub diagnostics: IcpDiagnostics,
530    /// Explicit target/query upload and nearest-result download bytes.
531    pub transfers: spatialrust_core::TransferStats,
532    /// Completed GPU nearest-point batches (update, fitness, final fitness).
533    pub correspondence_batches: usize,
534}
535
536#[cfg(feature = "register-icp-gpu")]
537struct GpuCorrespondences<'a> {
538    runtime: &'a spatialrust_gpu::WgpuRuntime,
539    target: spatialrust_gpu::GpuKdTree,
540    transfers: spatialrust_core::TransferStats,
541    batches: usize,
542}
543
544#[cfg(feature = "register-icp-gpu")]
545impl IcpCorrespondences for GpuCorrespondences<'_> {
546    fn visit(
547        &mut self,
548        points: &[Vec3<f32>],
549        bound: f32,
550        mut consume: impl FnMut(&Vec3<f32>, Neighbor),
551    ) -> SpatialResult<()> {
552        let queries: Vec<_> = points.iter().map(|p| [p.x, p.y, p.z]).collect();
553        let (neighbors, transfers) =
554            self.target.nearest_one_within_download(self.runtime, &queries, bound)?;
555        self.transfers.record(
556            spatialrust_core::TransferDirection::HostToDevice,
557            transfers.host_to_device_bytes(),
558        );
559        self.transfers.record(
560            spatialrust_core::TransferDirection::DeviceToHost,
561            transfers.device_to_host_bytes(),
562        );
563        self.batches += 1;
564        for (point, neighbor) in points.iter().zip(neighbors) {
565            if let Some(neighbor) = neighbor {
566                consume(point, neighbor);
567            }
568        }
569        Ok(())
570    }
571}
572
573#[cfg(feature = "register-icp-gpu")]
574#[path = "icp_reduction.rs"]
575mod reduction;
576#[cfg(feature = "register-icp-gpu")]
577pub use reduction::GpuIcpReductionDiagnostics;
578
579#[cfg(test)]
580mod tests {
581    use super::{
582        transform_delta_below_epsilon, IcpConfig, IcpConvergenceCriteria, IcpRegistration,
583        IcpStopReason,
584    };
585    use crate::registration::PointCloudRegistration;
586    use crate::transform::transform_point_cloud;
587    use spatialrust_core::{HasPositions3, PointCloudBuilder, StandardSchemas};
588    use spatialrust_math::{Isometry3, Quat, TransformPoint, Vec3};
589
590    #[test]
591    fn convergence_requires_small_translation_and_rotation() {
592        let zero = Vec3::new(0.0, 0.0, 0.0);
593        let rotation = Quat::from_axis_angle(Vec3::new(0.0, 0.0, 1.0), 1e-4);
594        assert_eq!(rotation.w, 1.0); // acos(w) would incorrectly report zero.
595        assert!(!transform_delta_below_epsilon(Isometry3::new(rotation, zero), 1e-6));
596        let opposite = Quat::new(-rotation.x, -rotation.y, -rotation.z, -rotation.w);
597        assert!(!transform_delta_below_epsilon(Isometry3::new(opposite, zero), 1e-6));
598        let tiny = Quat::from_axis_angle(Vec3::new(1.0, 0.0, 0.0), 1e-8);
599        assert!(transform_delta_below_epsilon(
600            Isometry3::new(tiny, Vec3::new(1e-8, 0.0, 0.0)),
601            1e-6
602        ));
603        assert!(!transform_delta_below_epsilon(
604            Isometry3::new(tiny, Vec3::new(1e-4, 0.0, 0.0)),
605            1e-6
606        ));
607        assert!(!transform_delta_below_epsilon(Isometry3::identity(), 0.0));
608    }
609
610    fn plane_cloud() -> spatialrust_core::PointCloud {
611        let mut builder = PointCloudBuilder::new(StandardSchemas::point_xyz());
612        for x in 0..6 {
613            for y in 0..6 {
614                for z in 0..3 {
615                    builder
616                        .push_point([x as f32 * 0.05, y as f32 * 0.05, z as f32 * 0.05])
617                        .unwrap();
618                }
619            }
620        }
621        builder.build().unwrap()
622    }
623
624    #[test]
625    fn trimming_rejects_invalid_fractions_and_insufficient_retained_pairs() {
626        let cloud = plane_cloud();
627        for fraction in [0.0, -0.1, 1.1, f64::NAN, f64::INFINITY, 1e-300] {
628            let registration =
629                IcpRegistration::new(IcpConfig::default()).with_trim_fraction(fraction);
630            assert_eq!(registration.trim_fraction().is_nan(), fraction.is_nan());
631            assert!(registration
632                .align_with_trace(&cloud, &cloud)
633                .unwrap_err()
634                .to_string()
635                .contains("trim_fraction"));
636        }
637        let registration = IcpRegistration::new(IcpConfig::default());
638        assert_eq!(registration.trim_fraction(), 1.0);
639        assert_eq!(
640            registration.align(&cloud, &cloud).unwrap(),
641            registration.with_trim_fraction(1.0).align(&cloud, &cloud).unwrap()
642        );
643    }
644
645    #[test]
646    fn trimming_uses_clean_pairs_but_fitness_keeps_all_gated_pairs() {
647        let mut builder = PointCloudBuilder::new(StandardSchemas::point_xyz());
648        for x in 0..4 {
649            for y in 0..4 {
650                for z in 0..4 {
651                    builder.push_point([x as f32, y as f32, z as f32]).unwrap();
652                }
653            }
654        }
655        let target = builder.build().unwrap();
656        let (x, y, z) = target.positions3().unwrap();
657        let mut builder = PointCloudBuilder::new(StandardSchemas::point_xyz());
658        for i in 0..64 {
659            builder.push_point([x[i], y[i], z[i]]).unwrap();
660        }
661        for i in 0..16 {
662            builder.push_point([x[i] + 0.125, y[i], z[i]]).unwrap();
663        }
664        let source = builder.build().unwrap();
665        let config = IcpConfig {
666            max_iterations: 1,
667            max_correspondence_distance: 0.3,
668            ..IcpConfig::default()
669        };
670        let plain = IcpRegistration::new(config).align(&source, &target).unwrap();
671        let trimmed = IcpRegistration::new(config)
672            .with_trim_fraction(0.8)
673            .align_with_trace(&source, &target)
674            .unwrap();
675        assert_eq!(trimmed.history[0].correspondences, 64);
676        assert_eq!(trimmed.history[0].evaluated_correspondences, 80);
677        assert_eq!(trimmed.result.transform, Isometry3::identity());
678        assert!((trimmed.result.fitness - 0.003125).abs() < 1e-12);
679        assert!(plain.fitness < trimmed.result.fitness);
680        assert!(plain.transform.translation().x.abs() > 0.01);
681    }
682
683    #[test]
684    fn rejects_invalid_rust_configuration_and_xyz() {
685        let cloud = plane_cloud();
686        for gate in [0.0, -1.0, f32::NAN, f32::INFINITY, 1e30, 1e-30] {
687            assert!(IcpRegistration::new(IcpConfig {
688                max_correspondence_distance: gate,
689                ..IcpConfig::default()
690            })
691            .align(&cloud, &cloud)
692            .is_err());
693        }
694        for config in [
695            IcpConfig { max_iterations: 0, ..IcpConfig::default() },
696            IcpConfig { min_correspondences: 2, ..IcpConfig::default() },
697            IcpConfig { transformation_epsilon: f64::NAN, ..IcpConfig::default() },
698            IcpConfig { fitness_epsilon: -1.0, ..IcpConfig::default() },
699            IcpConfig {
700                initial_guess: Isometry3::new(
701                    Quat::new(0.0, 0.0, 0.0, 0.0),
702                    Vec3::new(0.0, 0.0, 0.0),
703                ),
704                ..IcpConfig::default()
705            },
706        ] {
707            assert!(IcpRegistration::new(config).align_with_trace(&cloud, &cloud).is_err());
708        }
709        let mut builder = PointCloudBuilder::new(StandardSchemas::point_xyz());
710        for p in [[0.0, 0.0, 0.0], [1.0, 0.0, 0.0], [0.0, f32::NAN, 1.0]] {
711            builder.push_point(p).unwrap();
712        }
713        let invalid = builder.build().unwrap();
714        assert!(IcpRegistration::new(IcpConfig::default()).align(&invalid, &cloud).is_err());
715        assert!(IcpRegistration::new(IcpConfig::default()).align(&cloud, &invalid).is_err());
716    }
717
718    #[test]
719    fn rejects_transformed_coordinate_overflow() {
720        let mut builder = PointCloudBuilder::new(StandardSchemas::point_xyz());
721        for y in [0.0, 1.0, 2.0] {
722            builder.push_point([f32::MAX, y, 0.0]).unwrap();
723        }
724        let source = builder.build().unwrap();
725        let registration = IcpRegistration::new(IcpConfig {
726            initial_guess: Isometry3::new(Quat::<f32>::identity(), Vec3::new(f32::MAX, 0.0, 0.0)),
727            ..IcpConfig::default()
728        });
729        assert!(registration
730            .align(&source, &plane_cloud())
731            .unwrap_err()
732            .to_string()
733            .contains("overflowed"));
734    }
735
736    #[test]
737    fn separate_thresholds_and_zero_disabled_tests_are_honored() {
738        let cloud = plane_cloud();
739        let base = IcpConfig { max_iterations: 2, ..IcpConfig::default() };
740        let disabled = IcpConvergenceCriteria {
741            translation_epsilon: 0.0,
742            rotation_epsilon: 0.0,
743            fitness_epsilon: 0.0,
744        };
745        let registration = IcpRegistration::new(base).with_convergence_criteria(disabled);
746        assert_eq!(registration.config(), base);
747        assert_eq!(registration.convergence_criteria(), disabled);
748        let trace = registration.align_with_trace(&cloud, &cloud).unwrap();
749        assert_eq!(trace.stop_reason, IcpStopReason::IterationLimit);
750        assert_eq!(trace.result.iterations, 2);
751        let criteria = IcpConvergenceCriteria {
752            translation_epsilon: 1e-3,
753            rotation_epsilon: 1e-4,
754            ..disabled
755        };
756        let trace = registration
757            .with_convergence_criteria(criteria)
758            .align_with_trace(&cloud, &cloud)
759            .unwrap();
760        assert_eq!(trace.stop_reason, IcpStopReason::TransformThreshold);
761        for invalid in [f64::NAN, f64::INFINITY, -1.0] {
762            assert!(registration
763                .with_convergence_criteria(IcpConvergenceCriteria {
764                    rotation_epsilon: invalid,
765                    ..criteria
766                })
767                .align(&cloud, &cloud)
768                .is_err());
769        }
770    }
771
772    #[test]
773    fn pure_rotation_does_not_report_convergence_after_one_update() {
774        let mut builder = PointCloudBuilder::new(StandardSchemas::point_xyz());
775        for point in [
776            [1.0, 0.0, 0.0],
777            [-1.0, 0.0, 0.0],
778            [0.0, 2.0, 0.0],
779            [0.0, -2.0, 0.0],
780            [0.0, 0.0, 3.0],
781            [0.0, 0.0, -3.0],
782        ] {
783            builder.push_point(point).unwrap();
784        }
785        let target = builder.build().unwrap();
786        let source = transform_point_cloud(
787            &target,
788            Isometry3::new(
789                Quat::from_axis_angle(Vec3::new(0.0, 0.0, 1.0), 0.05),
790                Vec3::new(0.0, 0.0, 0.0),
791            ),
792        )
793        .unwrap();
794        let config = IcpConfig {
795            max_iterations: 1,
796            max_correspondence_distance: 0.2,
797            transformation_epsilon: 1e-5,
798            fitness_epsilon: 0.0,
799            ..IcpConfig::default()
800        };
801        let first = IcpRegistration::new(config).align(&source, &target).unwrap();
802        assert!(first.fitness < 1e-10);
803        assert!(!first.converged);
804        let settled = IcpRegistration::new(IcpConfig { max_iterations: 3, ..config })
805            .align(&source, &target)
806            .unwrap();
807        assert!(settled.converged);
808        assert!(settled.iterations > 1);
809        let trace = IcpRegistration::new(IcpConfig { max_iterations: 3, ..config })
810            .align_with_trace(&source, &target)
811            .unwrap();
812        assert_eq!(trace.result, settled);
813        assert_eq!(trace.stop_reason, IcpStopReason::TransformThreshold);
814        assert_eq!(trace.history.len(), settled.iterations);
815        assert!(trace.history[0].rotation_delta_radians > 0.04);
816        assert!(trace.history[0].translation_delta < config.transformation_epsilon);
817        assert_eq!(trace.history[0].fitness_change, None);
818        for rows in trace.history.windows(2) {
819            assert_eq!(rows[1].fitness_change, Some(rows[0].fitness - rows[1].fitness));
820        }
821        let capped = IcpRegistration::new(config).align_with_trace(&source, &target).unwrap();
822        assert_eq!(capped.stop_reason, IcpStopReason::IterationLimit);
823        assert_eq!(capped.result, first);
824    }
825
826    #[test]
827    fn aligns_translated_source_to_target() {
828        let target = plane_cloud();
829        let shift = Isometry3::new(Quat::<f32>::identity(), Vec3::new(0.02, -0.01, 0.0));
830        let source = transform_point_cloud(&target, shift).unwrap();
831
832        let registration = IcpRegistration::new(IcpConfig {
833            max_correspondence_distance: 0.1,
834            max_iterations: 30,
835            ..IcpConfig::default()
836        });
837        let result = registration.align(&source, &target).unwrap();
838        assert!(result.fitness < 1e-4);
839        assert!(result.converged);
840        let trace = registration.align_with_trace(&source, &target).unwrap();
841        assert_eq!(trace.result, result);
842        assert_eq!(trace.history.last().unwrap().fitness, result.fitness);
843        assert_eq!(trace.stop_reason, IcpStopReason::FitnessThreshold);
844
845        let composed = result.transform.compose(shift);
846        let probe = Vec3::new(0.2, 0.3, 0.0);
847        let restored = composed.transform_point(probe);
848        assert!((restored.x - probe.x).abs() < 5e-3);
849        assert!((restored.y - probe.y).abs() < 5e-3);
850    }
851
852    #[test]
853    fn aligns_rotated_and_translated_source() {
854        let target = plane_cloud();
855        let misalignment = Isometry3::new(
856            Quat::from_axis_angle(Vec3::new(0.0, 0.0, 1.0), 0.15),
857            Vec3::new(0.02, 0.01, 0.0),
858        );
859        let source = transform_point_cloud(&target, misalignment).unwrap();
860
861        let registration = IcpRegistration::new(IcpConfig {
862            max_correspondence_distance: 0.1,
863            max_iterations: 40,
864            ..IcpConfig::default()
865        });
866        let result = registration.align(&source, &target).unwrap();
867        assert!(result.fitness < 1e-3);
868
869        let composed = result.transform.compose(misalignment);
870        let probe = Vec3::new(0.15, 0.25, 0.0);
871        let restored = composed.transform_point(probe);
872        assert!((restored.x - probe.x).abs() < 1e-2);
873        assert!((restored.y - probe.y).abs() < 1e-2);
874    }
875}
spatialrust_registration/icp.rs

spatialrust_registration/
icp.rs
