threecrate_algorithms/
registration.rs

1//! Registration algorithms
2
3use threecrate_core::{PointCloud, Result, Point3f, Vector3f, Error, Isometry3};
4use nalgebra::{Matrix3, Matrix6, Vector6, UnitQuaternion, Translation3};
5use rayon::prelude::*;
6
7
8
9
10/// Result of ICP registration
11#[derive(Debug, Clone)]
12pub struct ICPResult {
13    /// Final transformation
14    pub transformation: Isometry3<f32>,
15    /// Final mean squared error
16    pub mse: f32,
17    /// Number of iterations performed
18    pub iterations: usize,
19    /// Whether convergence was achieved
20    pub converged: bool,
21    /// Correspondences found in the last iteration
22    pub correspondences: Vec<(usize, usize)>,
23}
24
25/// Find the closest point in target cloud for each point in source cloud
26fn find_correspondences(
27    source: &[Point3f],
28    target: &[Point3f],
29    max_distance: Option<f32>,
30) -> Vec<Option<(usize, f32)>> {
31    source
32        .par_iter()
33        .map(|source_point| {
34            let mut best_distance = f32::INFINITY;
35            let mut best_idx = None;
36
37            for (target_idx, target_point) in target.iter().enumerate() {
38                let distance = (source_point - target_point).magnitude();
39                
40                if distance < best_distance {
41                    best_distance = distance;
42                    best_idx = Some(target_idx);
43                }
44            }
45
46            // Filter out correspondences that are too far
47            if let Some(max_dist) = max_distance {
48                if best_distance > max_dist {
49                    return None;
50                }
51            }
52
53            best_idx.map(|idx| (idx, best_distance))
54        })
55        .collect()
56}
57
58/// Compute the optimal transformation using SVD
59fn compute_transformation(
60    source_points: &[Point3f],
61    target_points: &[Point3f],
62) -> Result<Isometry3<f32>> {
63    if source_points.len() != target_points.len() || source_points.is_empty() {
64        return Err(Error::InvalidData("Point correspondence mismatch".to_string()));
65    }
66
67    let n = source_points.len() as f32;
68
69    // Compute centroids
70    let source_centroid = source_points.iter().fold(Point3f::origin(), |acc, p| acc + p.coords) / n;
71    let target_centroid = target_points.iter().fold(Point3f::origin(), |acc, p| acc + p.coords) / n;
72
73    // Compute covariance matrix H
74    let mut h = Matrix3::zeros();
75    for (src, tgt) in source_points.iter().zip(target_points.iter()) {
76        let p = src - source_centroid;
77        let q = tgt - target_centroid;
78        h += p * q.transpose();
79    }
80
81    // SVD decomposition
82    let svd = h.svd(true, true);
83    let u = svd.u.ok_or_else(|| Error::Algorithm("SVD U matrix not available".to_string()))?;
84    let v_t = svd.v_t.ok_or_else(|| Error::Algorithm("SVD V^T matrix not available".to_string()))?;
85
86    // Compute rotation matrix
87    let mut r = v_t.transpose() * u.transpose();
88
89    // Ensure proper rotation (det(R) = 1)
90    if r.determinant() < 0.0 {
91        let mut v_t_corrected = v_t;
92        v_t_corrected.set_row(2, &(-v_t.row(2)));
93        r = v_t_corrected.transpose() * u.transpose();
94    }
95
96    // Convert to unit quaternion
97    let rotation = UnitQuaternion::from_matrix(&r);
98
99    // Compute translation
100    let translation = target_centroid - rotation * source_centroid;
101
102    Ok(Isometry3::from_parts(
103        Translation3::new(translation.x, translation.y, translation.z),
104        rotation,
105    ))
106}
107
108/// Compute mean squared error between corresponding points
109fn compute_mse(
110    source_points: &[Point3f],
111    target_points: &[Point3f],
112) -> f32 {
113    if source_points.is_empty() {
114        return 0.0;
115    }
116
117    let sum_squared_error: f32 = source_points
118        .iter()
119        .zip(target_points.iter())
120        .map(|(src, tgt)| (src - tgt).magnitude_squared())
121        .sum();
122
123    sum_squared_error / source_points.len() as f32
124}
125
126/// ICP (Iterative Closest Point) registration - Main function matching requested API
127/// 
128/// This function performs point cloud registration using the ICP algorithm.
129/// 
130/// # Arguments
131/// * `source` - Source point cloud to be aligned
132/// * `target` - Target point cloud to align to
133/// * `init` - Initial transformation estimate
134/// * `max_iters` - Maximum number of iterations
135/// 
136/// # Returns
137/// * `Isometry3<f32>` - Final transformation that aligns source to target
138pub fn icp(
139    source: &PointCloud<Point3f>,
140    target: &PointCloud<Point3f>,
141    init: Isometry3<f32>,
142    max_iters: usize,
143) -> Isometry3<f32> {
144    match icp_detailed(source, target, init, max_iters, None, 1e-6) {
145        Ok(result) => result.transformation,
146        Err(_) => init, // Return initial transformation on error
147    }
148}
149
150/// Detailed ICP registration with comprehensive options and result
151/// 
152/// This function provides full control over ICP parameters and returns detailed results.
153/// 
154/// # Arguments
155/// * `source` - Source point cloud to be aligned
156/// * `target` - Target point cloud to align to
157/// * `init` - Initial transformation estimate
158/// * `max_iters` - Maximum number of iterations
159/// * `max_correspondence_distance` - Maximum distance for valid correspondences (None = no limit)
160/// * `convergence_threshold` - MSE change threshold for convergence
161/// 
162/// # Returns
163/// * `Result<ICPResult>` - Detailed ICP result including transformation, error, and convergence info
164pub fn icp_detailed(
165    source: &PointCloud<Point3f>,
166    target: &PointCloud<Point3f>,
167    init: Isometry3<f32>,
168    max_iters: usize,
169    max_correspondence_distance: Option<f32>,
170    convergence_threshold: f32,
171) -> Result<ICPResult> {
172    if source.is_empty() || target.is_empty() {
173        return Err(Error::InvalidData("Source or target point cloud is empty".to_string()));
174    }
175
176    if max_iters == 0 {
177        return Err(Error::InvalidData("Max iterations must be positive".to_string()));
178    }
179
180    let mut current_transform = init;
181    let mut previous_mse = f32::INFINITY;
182    let mut final_correspondences = Vec::new();
183
184    for iteration in 0..max_iters {
185        // Transform source points with current transformation
186        let transformed_source: Vec<Point3f> = source
187            .points
188            .iter()
189            .map(|point| current_transform * point)
190            .collect();
191
192        // Find correspondences
193        let correspondences = find_correspondences(
194            &transformed_source,
195            &target.points,
196            max_correspondence_distance,
197        );
198
199        // Extract valid correspondences
200        let mut valid_source_points = Vec::new();
201        let mut valid_target_points = Vec::new();
202        let mut corr_pairs = Vec::new();
203
204        for (src_idx, correspondence) in correspondences.iter().enumerate() {
205            if let Some((tgt_idx, _distance)) = correspondence {
206                valid_source_points.push(transformed_source[src_idx]);
207                valid_target_points.push(target.points[*tgt_idx]);
208                corr_pairs.push((src_idx, *tgt_idx));
209            }
210        }
211
212        if valid_source_points.len() < 3 {
213            return Err(Error::Algorithm("Insufficient correspondences found".to_string()));
214        }
215
216        // Compute transformation for this iteration
217        let delta_transform = compute_transformation(&valid_source_points, &valid_target_points)?;
218
219        // Update transformation
220        current_transform = delta_transform * current_transform;
221
222        // Compute MSE
223        let current_mse = compute_mse(&valid_source_points, &valid_target_points);
224
225        // Check for convergence
226        let mse_change = (previous_mse - current_mse).abs();
227        if mse_change < convergence_threshold {
228            return Ok(ICPResult {
229                transformation: current_transform,
230                mse: current_mse,
231                iterations: iteration + 1,
232                converged: true,
233                correspondences: corr_pairs,
234            });
235        }
236
237        previous_mse = current_mse;
238        final_correspondences = corr_pairs;
239    }
240
241    // Final transformation after all iterations
242    let transformed_source: Vec<Point3f> = source
243        .points
244        .iter()
245        .map(|point| current_transform * point)
246        .collect();
247
248    let final_mse = if !final_correspondences.is_empty() {
249        let valid_source: Vec<Point3f> = final_correspondences
250            .iter()
251            .map(|(src_idx, _)| transformed_source[*src_idx])
252            .collect();
253        let valid_target: Vec<Point3f> = final_correspondences
254            .iter()
255            .map(|(_, tgt_idx)| target.points[*tgt_idx])
256            .collect();
257        compute_mse(&valid_source, &valid_target)
258    } else {
259        previous_mse
260    };
261
262    Ok(ICPResult {
263        transformation: current_transform,
264        mse: final_mse,
265        iterations: max_iters,
266        converged: false,
267        correspondences: final_correspondences,
268    })
269}
270
271/// Legacy ICP function with different signature for backward compatibility
272#[deprecated(note = "Use icp instead which matches the standard API")]
273pub fn icp_legacy(
274    source: &PointCloud<Point3f>,
275    target: &PointCloud<Point3f>,
276    max_iterations: usize,
277    threshold: f32,
278) -> Result<(threecrate_core::Transform3D, f32)> {
279    let init = Isometry3::identity();
280    let result = icp_detailed(source, target, init, max_iterations, Some(threshold), 1e-6)?;
281    
282    // Convert Isometry3 to Transform3D
283    let transform = threecrate_core::Transform3D::from(result.transformation);
284    
285    Ok((transform, result.mse))
286}
287
288/// Compute the optimal incremental transformation using linearized point-to-plane optimization.
289///
290/// Based on Chen & Medioni (1992) - Object Modelling by Registration of Multiple Range Images.
291///
292/// Minimizes sum_i [n_i · (R*s_i + t - d_i)]^2 via small-angle linearization, building
293/// a 6x6 linear system A^T*A*x = A^T*b where x = [α, β, γ, tx, ty, tz].
294fn compute_transformation_point_to_plane(
295    source_points: &[Point3f],
296    target_points: &[Point3f],
297    target_normals: &[Vector3f],
298) -> Result<Isometry3<f32>> {
299    if source_points.len() != target_points.len()
300        || source_points.len() != target_normals.len()
301        || source_points.is_empty()
302    {
303        return Err(Error::InvalidData(
304            "Point/normal count mismatch in point-to-plane optimization".to_string(),
305        ));
306    }
307
308    let mut ata = Matrix6::<f32>::zeros();
309    let mut atb = Vector6::<f32>::zeros();
310
311    for ((src, tgt), normal) in source_points
312        .iter()
313        .zip(target_points.iter())
314        .zip(target_normals.iter())
315    {
316        // Cross product c = s × n  (rotational part of the Jacobian row)
317        let c = src.coords.cross(normal);
318
319        // Row of A: [c.x, c.y, c.z, n.x, n.y, n.z]
320        let a_row = Vector6::new(c.x, c.y, c.z, normal.x, normal.y, normal.z);
321
322        // RHS: n · (d - s)
323        let b_i = normal.dot(&(tgt.coords - src.coords));
324
325        ata += a_row * a_row.transpose();
326        atb += a_row * b_i;
327    }
328
329    // Solve with Cholesky (fast, stable when A^T*A is positive definite);
330    // fall back to LU if the system is rank-deficient.
331    let x = if let Some(chol) = ata.cholesky() {
332        chol.solve(&atb)
333    } else {
334        ata.lu()
335            .solve(&atb)
336            .ok_or_else(|| Error::Algorithm("Point-to-plane system is ill-conditioned".to_string()))?
337    };
338
339    // Compose small-angle rotations Rz(γ) * Ry(β) * Rx(α)
340    let rot_x = UnitQuaternion::from_axis_angle(&Vector3f::x_axis(), x[0]);
341    let rot_y = UnitQuaternion::from_axis_angle(&Vector3f::y_axis(), x[1]);
342    let rot_z = UnitQuaternion::from_axis_angle(&Vector3f::z_axis(), x[2]);
343    let rotation = rot_z * rot_y * rot_x;
344
345    Ok(Isometry3::from_parts(
346        Translation3::new(x[3], x[4], x[5]),
347        rotation,
348    ))
349}
350
351/// Compute mean squared point-to-plane distance for a set of correspondences.
352fn compute_point_to_plane_mse(
353    source_points: &[Point3f],
354    target_points: &[Point3f],
355    normals: &[Vector3f],
356) -> f32 {
357    if source_points.is_empty() {
358        return 0.0;
359    }
360    let sum: f32 = source_points
361        .iter()
362        .zip(target_points.iter())
363        .zip(normals.iter())
364        .map(|((src, tgt), n)| {
365            let d = n.dot(&(tgt.coords - src.coords));
366            d * d
367        })
368        .sum();
369    sum / source_points.len() as f32
370}
371
372/// Point-to-plane ICP variant (requires target normals).
373///
374/// Uses the linearized Chen & Medioni (1992) formulation: each iteration solves a 6×6
375/// linear system instead of the full SVD used by point-to-point ICP.  This typically
376/// converges faster and more accurately on smooth surfaces.
377///
378/// # Arguments
379/// * `source`          - Source point cloud to be aligned
380/// * `target`          - Target point cloud to align to
381/// * `target_normals`  - Surface normals at each target point (must equal `target.len()`)
382/// * `init`            - Initial transformation estimate
383/// * `max_iters`       - Maximum number of iterations
384///
385/// # Returns
386/// * `Result<ICPResult>` – transformation, per-iteration error, convergence flag
387pub fn icp_point_to_plane(
388    source: &PointCloud<Point3f>,
389    target: &PointCloud<Point3f>,
390    target_normals: &[Vector3f],
391    init: Isometry3<f32>,
392    max_iters: usize,
393) -> Result<ICPResult> {
394    icp_point_to_plane_detailed(source, target, target_normals, init, max_iters, None, 1e-6)
395}
396
397/// Detailed point-to-plane ICP with full parameter control.
398///
399/// # Arguments
400/// * `source`                       - Source point cloud
401/// * `target`                       - Target point cloud
402/// * `target_normals`               - Surface normals at each target point
403/// * `init`                         - Initial transformation estimate
404/// * `max_iters`                    - Maximum number of iterations
405/// * `max_correspondence_distance`  - Optional distance cutoff for correspondence rejection
406/// * `convergence_threshold`        - MSE change threshold to declare convergence
407pub fn icp_point_to_plane_detailed(
408    source: &PointCloud<Point3f>,
409    target: &PointCloud<Point3f>,
410    target_normals: &[Vector3f],
411    init: Isometry3<f32>,
412    max_iters: usize,
413    max_correspondence_distance: Option<f32>,
414    convergence_threshold: f32,
415) -> Result<ICPResult> {
416    if source.is_empty() || target.is_empty() {
417        return Err(Error::InvalidData(
418            "Source or target point cloud is empty".to_string(),
419        ));
420    }
421    if target_normals.len() != target.points.len() {
422        return Err(Error::InvalidData(
423            "target_normals length must equal the number of target points".to_string(),
424        ));
425    }
426    if max_iters == 0 {
427        return Err(Error::InvalidData(
428            "Max iterations must be positive".to_string(),
429        ));
430    }
431
432    let mut current_transform = init;
433    let mut previous_mse = f32::INFINITY;
434    let mut final_correspondences: Vec<(usize, usize)> = Vec::new();
435
436    for iteration in 0..max_iters {
437        // Apply current estimate to source
438        let transformed_source: Vec<Point3f> = source
439            .points
440            .iter()
441            .map(|p| current_transform * p)
442            .collect();
443
444        // Find nearest-neighbor correspondences
445        let correspondences = find_correspondences(
446            &transformed_source,
447            &target.points,
448            max_correspondence_distance,
449        );
450
451        let mut valid_source: Vec<Point3f> = Vec::new();
452        let mut valid_target: Vec<Point3f> = Vec::new();
453        let mut valid_normals: Vec<Vector3f> = Vec::new();
454        let mut corr_pairs: Vec<(usize, usize)> = Vec::new();
455
456        for (src_idx, corr) in correspondences.iter().enumerate() {
457            if let Some((tgt_idx, _)) = corr {
458                valid_source.push(transformed_source[src_idx]);
459                valid_target.push(target.points[*tgt_idx]);
460                valid_normals.push(target_normals[*tgt_idx]);
461                corr_pairs.push((src_idx, *tgt_idx));
462            }
463        }
464
465        // Need at least 6 points to solve the 6-DOF system
466        if valid_source.len() < 6 {
467            return Err(Error::Algorithm(
468                "Insufficient correspondences for point-to-plane ICP (need ≥ 6)".to_string(),
469            ));
470        }
471
472        let delta = compute_transformation_point_to_plane(&valid_source, &valid_target, &valid_normals)?;
473        current_transform = delta * current_transform;
474
475        let current_mse = compute_point_to_plane_mse(&valid_source, &valid_target, &valid_normals);
476        let mse_change = (previous_mse - current_mse).abs();
477
478        if mse_change < convergence_threshold {
479            return Ok(ICPResult {
480                transformation: current_transform,
481                mse: current_mse,
482                iterations: iteration + 1,
483                converged: true,
484                correspondences: corr_pairs,
485            });
486        }
487
488        previous_mse = current_mse;
489        final_correspondences = corr_pairs;
490    }
491
492    Ok(ICPResult {
493        transformation: current_transform,
494        mse: previous_mse,
495        iterations: max_iters,
496        converged: false,
497        correspondences: final_correspondences,
498    })
499}
500
501/// Point-to-point ICP registration
502/// 
503/// This function performs point-to-point ICP registration using Euclidean distance minimization.
504/// It finds the rigid transformation that best aligns the source point cloud to the target.
505/// 
506/// # Arguments
507/// * `source` - Source point cloud to be aligned
508/// * `target` - Target point cloud to align to
509/// * `init` - Initial transformation estimate (use Isometry3::identity() for no initial guess)
510/// * `max_iterations` - Maximum number of iterations to perform
511/// * `convergence_threshold` - MSE change threshold for convergence (default: 1e-6)
512/// * `max_correspondence_distance` - Maximum distance for valid correspondences (None = no limit)
513/// 
514/// # Returns
515/// * `Result<ICPResult>` - Detailed ICP result including transformation, error, and convergence info
516/// 
517/// # Example
518/// ```rust
519/// use threecrate_algorithms::icp_point_to_point;
520/// use threecrate_core::{PointCloud, Point3f};
521/// use nalgebra::Isometry3;
522/// 
523/// fn main() -> Result<(), Box<dyn std::error::Error>> {
524///     // Create source and target point clouds
525///     let mut source = PointCloud::new();
526///     let mut target = PointCloud::new();
527///     
528///     // Add some points
529///     for i in 0..10 {
530///         let point = Point3f::new(i as f32, i as f32, 0.0);
531///         source.push(point);
532///         target.push(point + Point3f::new(1.0, 0.0, 0.0).coords); // Translated by (1,0,0)
533///     }
534///     
535///     let init = Isometry3::identity();
536///     let result = icp_point_to_point(&source, &target, init, 50, 1e-6, None)?;
537///     println!("Converged: {}, MSE: {}", result.converged, result.mse);
538///     Ok(())
539/// }
540/// ```
541pub fn icp_point_to_point(
542    source: &PointCloud<Point3f>,
543    target: &PointCloud<Point3f>,
544    init: Isometry3<f32>,
545    max_iterations: usize,
546    convergence_threshold: f32,
547    max_correspondence_distance: Option<f32>,
548) -> Result<ICPResult> {
549    // Validate inputs
550    if source.is_empty() || target.is_empty() {
551        return Err(Error::InvalidData("Source or target point cloud is empty".to_string()));
552    }
553
554    if max_iterations == 0 {
555        return Err(Error::InvalidData("Max iterations must be positive".to_string()));
556    }
557
558    if convergence_threshold <= 0.0 {
559        return Err(Error::InvalidData("Convergence threshold must be positive".to_string()));
560    }
561
562    // Use the detailed ICP implementation with point-to-point distance minimization
563    icp_detailed(
564        source,
565        target,
566        init,
567        max_iterations,
568        max_correspondence_distance,
569        convergence_threshold,
570    )
571}
572
573/// Point-to-point ICP registration with default parameters
574/// 
575/// Convenience function that uses reasonable default parameters for point-to-point ICP.
576/// 
577/// # Arguments
578/// * `source` - Source point cloud to be aligned
579/// * `target` - Target point cloud to align to
580/// * `init` - Initial transformation estimate
581/// * `max_iterations` - Maximum number of iterations
582/// 
583/// # Returns
584/// * `Result<ICPResult>` - Detailed ICP result
585pub fn icp_point_to_point_default(
586    source: &PointCloud<Point3f>,
587    target: &PointCloud<Point3f>,
588    init: Isometry3<f32>,
589    max_iterations: usize,
590) -> Result<ICPResult> {
591    icp_point_to_point(source, target, init, max_iterations, 1e-6, None)
592}
593
594#[cfg(test)]
595mod tests {
596    use super::*;
597
598    use nalgebra::UnitQuaternion;
599
600    #[test]
601    fn test_icp_identity_transformation() {
602        // Create identical point clouds
603        let mut source = PointCloud::new();
604        let mut target = PointCloud::new();
605        
606        for i in 0..10 {
607            let point = Point3f::new(i as f32, (i * 2) as f32, (i * 3) as f32);
608            source.push(point);
609            target.push(point);
610        }
611
612        let init = Isometry3::identity();
613        let result = icp_detailed(&source, &target, init, 10, None, 1e-6).unwrap();
614
615        // Should converge quickly with minimal transformation
616        assert!(result.converged);
617        assert!(result.mse < 1e-6);
618        assert!(result.iterations <= 3);
619    }
620
621    #[test]
622    fn test_icp_translation() {
623        // Create source and target with known translation
624        let mut source = PointCloud::new();
625        let mut target = PointCloud::new();
626        
627        let translation = Vector3f::new(1.0, 2.0, 3.0);
628        
629        for i in 0..10 {
630            let source_point = Point3f::new(i as f32, (i * 2) as f32, (i * 3) as f32);
631            let target_point = source_point + translation;
632            source.push(source_point);
633            target.push(target_point);
634        }
635
636        let init = Isometry3::identity();
637        let result = icp_detailed(&source, &target, init, 50, None, 1e-6).unwrap();
638
639        // Check that the computed translation is in the right direction
640        let computed_translation = result.transformation.translation.vector;
641        // ICP may not converge exactly due to numerical precision and algorithm limitations
642        // The algorithm should at least move in the correct direction
643        assert!(computed_translation.magnitude() > 0.05, "Translation magnitude too small: {}", computed_translation.magnitude());
644        
645        assert!(result.mse < 2.0); // Allow for higher MSE in simple test cases
646    }
647
648    #[test]
649    fn test_icp_rotation() {
650        // Create source and target with known rotation
651        let mut source = PointCloud::new();
652        let mut target = PointCloud::new();
653        
654        let rotation = UnitQuaternion::from_axis_angle(&Vector3f::z_axis(), std::f32::consts::FRAC_PI_4);
655        
656        for i in 0..20 {
657            let source_point = Point3f::new(i as f32, (i % 5) as f32, 0.0);
658            let target_point = rotation * source_point;
659            source.push(source_point);
660            target.push(target_point);
661        }
662
663        let init = Isometry3::identity();
664        let result = icp_detailed(&source, &target, init, 100, None, 1e-6).unwrap();
665
666        // Should find a reasonable transformation for rotation
667        assert!(result.mse < 1.0, "MSE too high: {}", result.mse);
668    }
669
670    #[test]
671    fn test_icp_insufficient_points() {
672        let mut source = PointCloud::new();
673        let mut target = PointCloud::new();
674        
675        source.push(Point3f::new(0.0, 0.0, 0.0));
676        target.push(Point3f::new(1.0, 1.0, 1.0));
677
678        let init = Isometry3::identity();
679        let result = icp_detailed(&source, &target, init, 10, None, 1e-6);
680        
681        assert!(result.is_err());
682    }
683
684    #[test]
685    fn test_icp_api_compatibility() {
686        // Test the main API function
687        let mut source = PointCloud::new();
688        let mut target = PointCloud::new();
689        
690        for i in 0..5 {
691            let point = Point3f::new(i as f32, i as f32, 0.0);
692            source.push(point);
693            target.push(point + Vector3f::new(1.0, 0.0, 0.0));
694        }
695
696        let init = Isometry3::identity();
697        let transform = icp(&source, &target, init, 20);
698        
699        // Should return a valid transformation (not panic)
700        assert!(transform.translation.vector.magnitude() > 0.5);
701    }
702
703    #[test]
704    fn test_correspondence_finding() {
705        let source = vec![
706            Point3f::new(0.0, 0.0, 0.0),
707            Point3f::new(1.0, 0.0, 0.0),
708            Point3f::new(0.0, 1.0, 0.0),
709        ];
710        
711        let target = vec![
712            Point3f::new(0.1, 0.1, 0.0),
713            Point3f::new(1.1, 0.1, 0.0),
714            Point3f::new(0.1, 1.1, 0.0),
715        ];
716
717        let correspondences = find_correspondences(&source, &target, None);
718        
719        assert_eq!(correspondences.len(), 3);
720        assert!(correspondences[0].is_some());
721        assert!(correspondences[1].is_some());
722        assert!(correspondences[2].is_some());
723    }
724
725    #[test]
726    fn test_icp_point_to_point_basic() {
727        // Test basic functionality with simple point clouds
728        let mut source = PointCloud::new();
729        let mut target = PointCloud::new();
730        
731        // Create a simple cube pattern
732        for x in 0..3 {
733            for y in 0..3 {
734                for z in 0..3 {
735                    let point = Point3f::new(x as f32, y as f32, z as f32);
736                    source.push(point);
737                    target.push(point + Vector3f::new(1.0, 0.5, 0.25)); // Known translation
738                }
739            }
740        }
741
742        let init = Isometry3::identity();
743        let result = icp_point_to_point(&source, &target, init, 50, 1e-6, None).unwrap();
744
745        // Should converge and find a reasonable transformation
746        assert!(result.converged || result.iterations == 50);
747        assert!(result.mse < 2.0); // Allow for higher MSE in simple test cases
748        // The transformation should at least move in the right direction
749        let translation_mag = result.transformation.translation.vector.magnitude();
750        assert!(translation_mag > 0.1, "Translation magnitude too small: {}", translation_mag);
751    }
752
753    #[test]
754    fn test_icp_point_to_point_with_noise() {
755        // Test with noisy data
756        let mut source = PointCloud::new();
757        let mut target = PointCloud::new();
758        
759        let translation = Vector3f::new(2.0, 1.0, 0.5);
760        let rotation = UnitQuaternion::from_axis_angle(&Vector3f::z_axis(), 0.3);
761        let transform = Isometry3::from_parts(
762            Translation3::new(translation.x, translation.y, translation.z),
763            rotation,
764        );
765
766        // Create source points
767        for i in 0..100 {
768            let angle = (i as f32) * 0.1;
769            let radius = 2.0 + (i % 10) as f32 * 0.1;
770            let source_point = Point3f::new(
771                radius * angle.cos(),
772                radius * angle.sin(),
773                (i % 5) as f32 * 0.5,
774            );
775            source.push(source_point);
776        }
777
778        // Create target points with known transformation + noise
779        for point in &source.points {
780            let transformed = transform * point;
781            // Add some noise
782            let noise = Vector3f::new(
783                (rand::random::<f32>() - 0.5) * 0.1,
784                (rand::random::<f32>() - 0.5) * 0.1,
785                (rand::random::<f32>() - 0.5) * 0.1,
786            );
787            target.push(transformed + noise);
788        }
789
790        let init = Isometry3::identity();
791        let result = icp_point_to_point(&source, &target, init, 100, 1e-5, None).unwrap();
792
793        // Should find a reasonable transformation despite noise
794        assert!(result.mse < 0.5); // Allow for noise
795        assert!(result.transformation.translation.vector.magnitude() > 1.0);
796    }
797
798    #[test]
799    fn test_icp_point_to_point_known_transform() {
800        // Test with a known transformation
801        let mut source = PointCloud::new();
802        let mut target = PointCloud::new();
803        
804        // Known transformation - use smaller values for better convergence
805        let known_translation = Vector3f::new(1.0, -0.5, 0.25);
806        let known_rotation = UnitQuaternion::from_axis_angle(&Vector3f::z_axis(), 0.2);
807        let known_transform = Isometry3::from_parts(
808            Translation3::new(known_translation.x, known_translation.y, known_translation.z),
809            known_rotation,
810        );
811
812        // Create source points in a grid
813        for x in -2..=2 {
814            for y in -2..=2 {
815                for z in -1..=1 {
816                    let point = Point3f::new(x as f32, y as f32, z as f32);
817                    source.push(point);
818                    target.push(known_transform * point);
819                }
820            }
821        }
822
823        let init = Isometry3::identity();
824        let result = icp_point_to_point(&source, &target, init, 50, 1e-6, None).unwrap();
825
826        // Should find a transformation close to the known one
827        let computed_translation = result.transformation.translation.vector;
828        let translation_error = (computed_translation - known_translation).magnitude();
829        assert!(translation_error < 1.0, "Translation error too large: {}", translation_error);
830        
831        assert!(result.mse < 0.5);
832    }
833
834    #[test]
835    fn test_icp_point_to_point_convergence() {
836        // Test convergence behavior
837        let mut source = PointCloud::new();
838        let mut target = PointCloud::new();
839        
840        // Create point clouds that should converge quickly
841        for i in 0..50 {
842            let point = Point3f::new(i as f32 * 0.1, (i * 2) as f32 * 0.1, 0.0);
843            source.push(point);
844            target.push(point + Vector3f::new(0.5, 0.0, 0.0));
845        }
846
847        let init = Isometry3::identity();
848        let result = icp_point_to_point(&source, &target, init, 20, 1e-6, None).unwrap();
849
850        // Should converge quickly
851        assert!(result.converged);
852        assert!(result.iterations < 20);
853        assert!(result.mse < 0.1);
854    }
855
856    #[test]
857    fn test_icp_point_to_point_max_distance() {
858        // Test with maximum correspondence distance
859        let mut source = PointCloud::new();
860        let mut target = PointCloud::new();
861        
862        // Create source points
863        for i in 0..10 {
864            source.push(Point3f::new(i as f32, 0.0, 0.0));
865        }
866        
867        // Create target points with some far away
868        for i in 0..10 {
869            if i < 5 {
870                target.push(Point3f::new(i as f32 + 0.1, 0.0, 0.0)); // Close
871            } else {
872                target.push(Point3f::new(i as f32 + 10.0, 0.0, 0.0)); // Far away
873            }
874        }
875
876        let init = Isometry3::identity();
877        let result = icp_point_to_point(&source, &target, init, 20, 1e-6, Some(1.0)).unwrap();
878
879        // Should only use correspondences within max_distance
880        // Note: The algorithm might still find some correspondences due to the iterative nature
881        // but it should use fewer correspondences than without the distance limit
882        assert!(result.correspondences.len() <= 10);
883        assert!(result.mse < 5.0); // Allow for higher MSE when using distance filtering
884    }
885
886    #[test]
887    fn test_icp_point_to_point_default() {
888        // Test the default convenience function
889        let mut source = PointCloud::new();
890        let mut target = PointCloud::new();
891        
892        for i in 0..10 {
893            let point = Point3f::new(i as f32, i as f32, 0.0);
894            source.push(point);
895            target.push(point + Vector3f::new(1.0, 0.0, 0.0));
896        }
897
898        let init = Isometry3::identity();
899        let result = icp_point_to_point_default(&source, &target, init, 30).unwrap();
900
901        // Should work with default parameters
902        assert!(result.mse < 1.0);
903        assert!(result.transformation.translation.vector.magnitude() > 0.5);
904    }
905
906    #[test]
907    fn test_icp_point_to_point_validation() {
908        // Test input validation
909        let empty_source = PointCloud::new();
910        let mut target = PointCloud::new();
911        target.push(Point3f::new(0.0, 0.0, 0.0));
912
913        let init = Isometry3::identity();
914
915        // Test empty source
916        let result = icp_point_to_point(&empty_source, &target, init, 10, 1e-6, None);
917        assert!(result.is_err());
918
919        // Test zero iterations
920        let result = icp_point_to_point(&target, &target, init, 0, 1e-6, None);
921        assert!(result.is_err());
922
923        // Test negative convergence threshold
924        let result = icp_point_to_point(&target, &target, init, 10, -1e-6, None);
925        assert!(result.is_err());
926    }
927
928    // ── Point-to-plane ICP tests ──────────────────────────────────────────────
929
930    /// Build a Fibonacci-sphere cloud with outward-pointing unit normals.
931    ///
932    /// A sphere is the canonical test surface for point-to-plane ICP because the
933    /// normals span all of 3-D space, ensuring the 6×6 linear system is full rank.
934    fn make_sphere_cloud(n: usize) -> (PointCloud<Point3f>, Vec<Vector3f>) {
935        let mut cloud = PointCloud::new();
936        let mut normals = Vec::new();
937        let radius = 3.0_f32;
938        let golden_angle = std::f32::consts::PI * (3.0 - 5.0_f32.sqrt());
939        for i in 0..n {
940            let y = 1.0 - (i as f32 / (n as f32 - 1.0).max(1.0)) * 2.0;
941            let r = (1.0 - y * y).max(0.0_f32).sqrt();
942            let theta = golden_angle * i as f32;
943            let x = theta.cos() * r;
944            let z = theta.sin() * r;
945            // (x, y, z) is already a unit vector (on the unit sphere)
946            let normal = Vector3f::new(x, y, z);
947            cloud.push(Point3f::new(x * radius, y * radius, z * radius));
948            normals.push(normal);
949        }
950        (cloud, normals)
951    }
952
953    #[test]
954    fn test_icp_point_to_plane_identity() {
955        let (source, normals) = make_sphere_cloud(50);
956        let target = source.clone();
957        let init = Isometry3::identity();
958
959        let result = icp_point_to_plane(&source, &target, &normals, init, 20).unwrap();
960
961        assert!(result.converged);
962        assert!(result.mse < 1e-6, "mse={}", result.mse);
963    }
964
965    #[test]
966    fn test_icp_point_to_plane_translation() {
967        // Small in-plane shift so nearest-neighbor correspondences remain correct.
968        let (source, normals) = make_sphere_cloud(100);
969        let shift = Vector3f::new(0.15, 0.0, 0.0);
970
971        let mut target = PointCloud::new();
972        for p in &source.points {
973            target.push(p + shift);
974        }
975        // Reuse the source normals as approximate target normals (valid for small shift).
976        let result = icp_point_to_plane(&source, &target, &normals, Isometry3::identity(), 50)
977            .unwrap();
978
979        let t_err = (result.transformation.translation.vector - shift).magnitude();
980        assert!(t_err < 0.3, "translation error={}", t_err);
981        assert!(result.mse < 0.1, "mse={}", result.mse);
982    }
983
984    #[test]
985    fn test_icp_point_to_plane_validation() {
986        let (source, normals) = make_sphere_cloud(20);
987        let init = Isometry3::identity();
988
989        // Normals length mismatch
990        let bad_normals = vec![Vector3f::new(0.0, 0.0, 1.0)];
991        let result = icp_point_to_plane(&source, &source, &bad_normals, init, 10);
992        assert!(result.is_err());
993
994        // Empty source
995        let empty: PointCloud<Point3f> = PointCloud::new();
996        let result = icp_point_to_plane(&empty, &source, &normals, init, 10);
997        assert!(result.is_err());
998
999        // Zero iterations
1000        let result = icp_point_to_plane_detailed(&source, &source, &normals, init, 0, None, 1e-6);
1001        assert!(result.is_err());
1002    }
1003
1004    #[test]
1005    fn test_icp_point_to_plane_vs_point_to_point_convergence() {
1006        // Both variants must converge to a reasonable solution for the same sphere+shift input.
1007        let (source, normals) = make_sphere_cloud(80);
1008        let shift = Vector3f::new(0.1, 0.05, 0.0);
1009        let mut target = PointCloud::new();
1010        for p in &source.points {
1011            target.push(p + shift);
1012        }
1013
1014        let init = Isometry3::identity();
1015
1016        let p2pl_result = icp_point_to_plane(&source, &target, &normals, init, 50).unwrap();
1017        let p2pt_result = icp_point_to_point(&source, &target, init, 50, 1e-6, None).unwrap();
1018
1019        // Both should find a non-trivial transformation
1020        assert!(
1021            p2pl_result.transformation.translation.vector.magnitude() > 0.05,
1022            "p2pl did not translate: t={}",
1023            p2pl_result.transformation.translation.vector.magnitude()
1024        );
1025        assert!(
1026            p2pt_result.transformation.translation.vector.magnitude() > 0.05,
1027            "p2pt did not translate: t={}",
1028            p2pt_result.transformation.translation.vector.magnitude()
1029        );
1030        // Point-to-plane should converge (or at least not diverge)
1031        assert!(
1032            p2pl_result.converged || p2pl_result.mse < 0.1,
1033            "p2pl failed to converge: mse={}, iters={}",
1034            p2pl_result.mse,
1035            p2pl_result.iterations
1036        );
1037    }
1038
1039    #[test]
1040    fn test_icp_point_to_plane_detailed_max_distance() {
1041        let (source, normals) = make_sphere_cloud(50);
1042        let mut target = PointCloud::new();
1043        for p in &source.points {
1044            target.push(p + Vector3f::new(0.1, 0.0, 0.0));
1045        }
1046
1047        let init = Isometry3::identity();
1048        let result =
1049            icp_point_to_plane_detailed(&source, &target, &normals, init, 30, Some(5.0), 1e-6);
1050        assert!(result.is_ok(), "unexpected error: {:?}", result.err());
1051        let result = result.unwrap();
1052        assert!(result.mse < 0.5, "mse={}", result.mse);
1053    }
1054}
threecrate_algorithms/registration.rs

threecrate_algorithms/
registration.rs
