Skip to main content

rust_robotics_slam/
icp_matching.rs

1#![allow(
2    dead_code,
3    clippy::items_after_test_module,
4    clippy::needless_borrows_for_generic_args,
5    clippy::ptr_arg,
6    clippy::legacy_numeric_constants
7)]
8
9/*!
10 * Iterative Closest Point (ICP) SLAM implementation
11 *
12 * This module implements the ICP algorithm for point cloud registration.
13 * It finds the optimal transformation (rotation and translation) between
14 * two point sets by iteratively minimizing the distance between corresponding points.
15 *
16 * Ported from PythonRobotics
17 * Original authors: Atsushi Sakai (@Atsushi_twi), Göktuğ Karakaşlı, Shamil Gemuev
18 */
19
20use nalgebra::{DMatrix, DVector, Matrix2, SVector, Vector2};
21use nearest_neighbor::KdTree;
22use rand::Rng;
23
24// ICP parameters
25const EPS: f64 = 0.0001;
26const MAX_ITER: usize = 100;
27const INLIER_DISTANCE_THRESHOLD: f64 = 0.05;
28
29pub struct ICPResult {
30    pub rotation: DMatrix<f64>,
31    pub translation: DVector<f64>,
32    pub iterations: usize,
33    /// Sum of nearest-neighbor distances (legacy; scales with point count).
34    pub final_error: f64,
35    /// Mean nearest-neighbor distance \[m/point\]: `final_error / point_count`.
36    pub final_error_mean: f64,
37    /// Mean nearest-neighbor distance before the first ICP update \[m/point\].
38    pub initial_error_mean: f64,
39    /// Median nearest-neighbor distance after applying the final transform.
40    pub final_error_median: f64,
41    /// 90th percentile nearest-neighbor distance after applying the final transform.
42    pub final_error_p90: f64,
43    /// Fraction of final associations below 5 cm. Useful as a simple ICP quality diagnostic.
44    pub inlier_ratio_5cm: f64,
45    /// Relative reduction from initial to final mean error. Negative values are clamped to 0.
46    pub relative_error_reduction: f64,
47    pub point_count: usize,
48    pub converged: bool,
49}
50
51/// One accepted ICP alignment, including the initial alignment. The points,
52/// nearest-neighbor indices and error all describe this same state, before
53/// the next rigid fit. Rejected divergent updates are not included.
54#[derive(Debug, Clone)]
55pub struct ICPTraceFrame {
56    pub points: DMatrix<f64>,
57    /// Homogeneous transform from the original current points to `points`.
58    pub transform: DMatrix<f64>,
59    /// Destination index for each current point (many-to-one is allowed).
60    pub correspondences: Vec<usize>,
61    pub mean_error: f64,
62}
63
64/// Run the same solver as [`icp_matching`], recording its accepted states.
65/// Inputs have the same requirements: nonempty finite 2xN point matrices.
66/// Frame zero is the input cloud with identity transform;
67/// the last frame describes the returned result. The legacy `iterations`
68/// count also includes an attempted assessment that rejects a divergent fit.
69pub fn icp_matching_with_trace(
70    previous_points: &DMatrix<f64>,
71    current_points: &DMatrix<f64>,
72) -> (ICPResult, Vec<ICPTraceFrame>) {
73    let mut frames = Vec::new();
74    let result = icp_matching_observed(
75        previous_points,
76        current_points,
77        |points, h, pairs, error| {
78            frames.push(ICPTraceFrame {
79                points: points.clone(),
80                transform: h
81                    .cloned()
82                    .unwrap_or_else(|| DMatrix::identity(points.nrows() + 1, points.nrows() + 1)),
83                correspondences: pairs.to_vec(),
84                mean_error: error / points.ncols().max(1) as f64,
85            });
86        },
87    );
88    (result, frames)
89}
90
91/// Main ICP matching function
92///
93/// # Arguments
94/// * `previous_points` - Points from the previous frame (2×N matrix)
95/// * `current_points` - Points from the current frame (2×N matrix)
96///
97/// # Returns
98/// * `ICPResult` containing rotation matrix, translation vector, and convergence info
99pub fn icp_matching(previous_points: &DMatrix<f64>, current_points: &DMatrix<f64>) -> ICPResult {
100    icp_matching_observed(previous_points, current_points, |_, _, _, _| {})
101}
102
103fn icp_matching_observed(
104    previous_points: &DMatrix<f64>,
105    current_points: &DMatrix<f64>,
106    mut observe: impl FnMut(&DMatrix<f64>, Option<&DMatrix<f64>>, &[usize], f64),
107) -> ICPResult {
108    let mut h_matrix: Option<DMatrix<f64>> = None;
109    let mut pre_error = f64::INFINITY;
110    let mut initial_error = f64::NAN;
111    let mut count = 0;
112    let mut converged = false;
113    let mut current_pts = current_points.clone();
114    // The transform and points before the last step, to undo a step that
115    // made the match worse.
116    let mut last_good: Option<(Option<DMatrix<f64>>, DMatrix<f64>)> = None;
117
118    loop {
119        count += 1;
120
121        let (indexes, error) = nearest_neighbor_association(previous_points, &current_pts);
122        if initial_error.is_nan() {
123            initial_error = error;
124        }
125        if error > pre_error + EPS {
126            // The last step diverged: return the alignment before it, so the
127            // transform, the points and the diagnostics all describe it.
128            if let Some((h, pts)) = last_good.take() {
129                h_matrix = h;
130                current_pts = pts;
131            }
132            break;
133        }
134        observe(&current_pts, h_matrix.as_ref(), &indexes, error);
135        let d_error = pre_error - error;
136        pre_error = error;
137        if d_error.abs() <= EPS {
138            converged = true;
139            break;
140        }
141        if count >= MAX_ITER {
142            break;
143        }
144
145        let previous_indexed = select_columns(previous_points, &indexes);
146        let (rt, tt) = svd_motion_estimation(&previous_indexed, &current_pts);
147
148        // Update current points: current_points = (Rt @ current_points) + Tt
149        let rotated_pts = &rt * &current_pts;
150        let next_pts = rotated_pts.add_scalar_to_each_column(&tt);
151        let next_h = update_homogeneous_matrix(h_matrix.clone(), &rt, &tt);
152        last_good = Some((
153            h_matrix.replace(next_h),
154            std::mem::replace(&mut current_pts, next_pts),
155        ));
156    }
157
158    let h = h_matrix.unwrap_or_else(|| {
159        DMatrix::identity(previous_points.nrows() + 1, previous_points.nrows() + 1)
160    });
161    let dim = previous_points.nrows();
162
163    let rotation = h.view((0, 0), (dim, dim)).into_owned();
164    let translation = h.column(dim).rows(0, dim).into_owned();
165
166    let point_count = current_points.ncols().max(1);
167    let final_error_mean = pre_error / point_count as f64;
168    let initial_error_mean = initial_error / point_count as f64;
169    let final_distances = nearest_neighbor_distances(previous_points, &current_pts);
170    let final_error_median = percentile(final_distances.clone(), 0.50);
171    let final_error_p90 = percentile(final_distances.clone(), 0.90);
172    let inlier_count = final_distances
173        .iter()
174        .filter(|distance| **distance <= INLIER_DISTANCE_THRESHOLD)
175        .count();
176    let inlier_ratio_5cm = if final_distances.is_empty() {
177        0.0
178    } else {
179        inlier_count as f64 / final_distances.len() as f64
180    };
181    let relative_error_reduction = if initial_error_mean.is_finite() && initial_error_mean > 0.0 {
182        ((initial_error_mean - final_error_mean) / initial_error_mean).max(0.0)
183    } else {
184        0.0
185    };
186
187    ICPResult {
188        rotation,
189        translation,
190        iterations: count,
191        final_error: pre_error,
192        final_error_mean,
193        initial_error_mean,
194        final_error_median,
195        final_error_p90,
196        inlier_ratio_5cm,
197        relative_error_reduction,
198        point_count,
199        converged,
200    }
201}
202
203/// Composes one ICP step `(r, t)` after the transform accumulated so far.
204///
205/// Each step moves the already-transformed points, `p ← r p + t`, so the
206/// total is `H_step · H_prev` (not `H_prev · H_step`, whose translation is
207/// wrong whenever the steps rotate).
208fn update_homogeneous_matrix(
209    h_in: Option<DMatrix<f64>>,
210    r: &DMatrix<f64>,
211    t: &DVector<f64>,
212) -> DMatrix<f64> {
213    let r_size = r.nrows();
214    let mut h = DMatrix::zeros(r_size + 1, r_size + 1);
215
216    // Set rotation part
217    h.view_mut((0, 0), (r_size, r_size)).copy_from(r);
218    // Set translation part
219    h.view_mut((0, r_size), (r_size, 1)).copy_from(&t.column(0));
220    // Set homogeneous coordinate
221    h[(r_size, r_size)] = 1.0;
222
223    match h_in {
224        None => h,
225        Some(h_prev) => &h * &h_prev,
226    }
227}
228
229/// Find nearest neighbor associations between point sets using KdTree
230fn nearest_neighbor_association(
231    previous_points: &DMatrix<f64>,
232    current_points: &DMatrix<f64>,
233) -> (Vec<usize>, f64) {
234    let dim = previous_points.nrows();
235
236    // Build KdTree from previous_points (reference point cloud)
237    if dim == 2 {
238        let prev_svecs: Vec<SVector<f64, 2>> = (0..previous_points.ncols())
239            .map(|i| SVector::<f64, 2>::from([previous_points[(0, i)], previous_points[(1, i)]]))
240            .collect();
241        let tree = KdTree::new(&prev_svecs, 2);
242
243        let mut indexes = Vec::with_capacity(current_points.ncols());
244        let mut error = 0.0;
245
246        for j in 0..current_points.ncols() {
247            let query = SVector::<f64, 2>::from([current_points[(0, j)], current_points[(1, j)]]);
248            let (argmin, sq_dist) = tree.search(&query);
249            indexes.push(argmin.unwrap_or(0));
250            error += sq_dist.sqrt();
251        }
252        (indexes, error)
253    } else {
254        // Fallback to brute-force for non-2D cases
255        let mut indexes = Vec::with_capacity(current_points.ncols());
256        let mut error = 0.0;
257
258        for j in 0..current_points.ncols() {
259            let current_point = current_points.column(j);
260            let mut min_dist = f64::INFINITY;
261            let mut best_idx = 0;
262
263            for i in 0..previous_points.ncols() {
264                let prev_point = previous_points.column(i);
265                let dist = (current_point - prev_point).norm();
266                if dist < min_dist {
267                    min_dist = dist;
268                    best_idx = i;
269                }
270            }
271            indexes.push(best_idx);
272            error += min_dist;
273        }
274        (indexes, error)
275    }
276}
277
278fn nearest_neighbor_distances(
279    previous_points: &DMatrix<f64>,
280    current_points: &DMatrix<f64>,
281) -> Vec<f64> {
282    let dim = previous_points.nrows();
283
284    if dim == 2 {
285        let prev_svecs: Vec<SVector<f64, 2>> = (0..previous_points.ncols())
286            .map(|i| SVector::<f64, 2>::from([previous_points[(0, i)], previous_points[(1, i)]]))
287            .collect();
288        let tree = KdTree::new(&prev_svecs, 2);
289
290        (0..current_points.ncols())
291            .map(|j| {
292                let query =
293                    SVector::<f64, 2>::from([current_points[(0, j)], current_points[(1, j)]]);
294                let (_, sq_dist) = tree.search(&query);
295                sq_dist.sqrt()
296            })
297            .collect()
298    } else {
299        let mut distances = Vec::with_capacity(current_points.ncols());
300        for j in 0..current_points.ncols() {
301            let current_point = current_points.column(j);
302            let mut min_dist = f64::INFINITY;
303
304            for i in 0..previous_points.ncols() {
305                let prev_point = previous_points.column(i);
306                let dist = (current_point - prev_point).norm();
307                if dist < min_dist {
308                    min_dist = dist;
309                }
310            }
311            distances.push(min_dist);
312        }
313        distances
314    }
315}
316
317fn percentile(mut values: Vec<f64>, q: f64) -> f64 {
318    values.retain(|value| value.is_finite());
319    if values.is_empty() {
320        return f64::NAN;
321    }
322    values.sort_by(|a, b| a.total_cmp(b));
323    let clamped_q = q.clamp(0.0, 1.0);
324    let idx = ((values.len() - 1) as f64 * clamped_q).round() as usize;
325    values[idx]
326}
327
328/// Select columns from matrix based on indices
329fn select_columns(matrix: &DMatrix<f64>, indices: &[usize]) -> DMatrix<f64> {
330    let mut result = DMatrix::zeros(matrix.nrows(), indices.len());
331    for (j, &idx) in indices.iter().enumerate() {
332        result.set_column(j, &matrix.column(idx));
333    }
334    result
335}
336
337/// Add scalar to each column of a matrix (for translation)
338trait AddScalarToEachColumn {
339    fn add_scalar_to_each_column(&self, scalar: &DVector<f64>) -> Self;
340}
341
342impl AddScalarToEachColumn for DMatrix<f64> {
343    fn add_scalar_to_each_column(&self, scalar: &DVector<f64>) -> Self {
344        let mut result = self.clone();
345        for j in 0..result.ncols() {
346            for i in 0..result.nrows() {
347                result[(i, j)] += scalar[i];
348            }
349        }
350        result
351    }
352}
353
354/// SVD-based motion estimation for 2D points
355fn svd_motion_estimation(
356    previous_points: &DMatrix<f64>,
357    current_points: &DMatrix<f64>,
358) -> (DMatrix<f64>, DVector<f64>) {
359    let n_points = previous_points.ncols();
360
361    // Calculate centroids
362    let mut pm_x = 0.0;
363    let mut pm_y = 0.0;
364    let mut cm_x = 0.0;
365    let mut cm_y = 0.0;
366
367    for j in 0..n_points {
368        pm_x += previous_points[(0, j)];
369        pm_y += previous_points[(1, j)];
370        cm_x += current_points[(0, j)];
371        cm_y += current_points[(1, j)];
372    }
373
374    pm_x /= n_points as f64;
375    pm_y /= n_points as f64;
376    cm_x /= n_points as f64;
377    cm_y /= n_points as f64;
378
379    // Shift points to centroids
380    let mut p_shift = DMatrix::zeros(2, n_points);
381    let mut c_shift = DMatrix::zeros(2, n_points);
382
383    for j in 0..n_points {
384        p_shift[(0, j)] = previous_points[(0, j)] - pm_x;
385        p_shift[(1, j)] = previous_points[(1, j)] - pm_y;
386        c_shift[(0, j)] = current_points[(0, j)] - cm_x;
387        c_shift[(1, j)] = current_points[(1, j)] - cm_y;
388    }
389
390    // Calculate cross-covariance matrix W = c_shift * p_shift^T
391    let w = &c_shift * p_shift.transpose();
392
393    // SVD decomposition
394    let svd = w.svd(true, true);
395    let u = svd.u.unwrap();
396    let v_t = svd.v_t.unwrap();
397
398    // Calculate rotation: R = V * U^T, flipping the weakest axis when that
399    // would be a reflection (det = -1) rather than a rotation (Kabsch).
400    let mut v = v_t.transpose();
401    if (&v * u.transpose()).determinant() < 0.0 {
402        let last = v.ncols() - 1;
403        v.column_mut(last).neg_mut();
404    }
405    let r = &v * &u.transpose();
406
407    // Calculate translation: t = pm - R * cm
408    let cm_vec = DVector::from_vec(vec![cm_x, cm_y]);
409    let pm_vec = DVector::from_vec(vec![pm_x, pm_y]);
410    let r_cm = &r * &cm_vec;
411    let t = &pm_vec - &r_cm;
412
413    (r, t)
414}
415
416/// Generate random 2D point cloud for testing
417pub fn generate_2d_points(n_points: usize, field_length: f64) -> DMatrix<f64> {
418    let mut rng = rand::rng();
419    let mut points = DMatrix::zeros(2, n_points);
420
421    for j in 0..n_points {
422        points[(0, j)] = (rng.random::<f64>() - 0.5) * field_length;
423        points[(1, j)] = (rng.random::<f64>() - 0.5) * field_length;
424    }
425
426    points
427}
428
429/// Apply 2D transformation to points
430pub fn apply_2d_transformation(
431    points: &DMatrix<f64>,
432    translation: &Vector2<f64>,
433    rotation_angle: f64,
434) -> DMatrix<f64> {
435    let cos_theta = rotation_angle.cos();
436    let sin_theta = rotation_angle.sin();
437    let rotation = Matrix2::new(cos_theta, -sin_theta, sin_theta, cos_theta);
438
439    let mut transformed = DMatrix::zeros(2, points.ncols());
440
441    for j in 0..points.ncols() {
442        let point = Vector2::new(points[(0, j)], points[(1, j)]);
443        let rotated = rotation * point;
444        transformed[(0, j)] = rotated.x + translation.x;
445        transformed[(1, j)] = rotated.y + translation.y;
446    }
447
448    transformed
449}
450
451#[cfg(test)]
452mod tests {
453    use super::*;
454    use nalgebra::Vector2;
455    use rand::{rngs::StdRng, Rng, SeedableRng};
456
457    #[test]
458    fn tracing_preserves_the_legacy_solver_result_and_input_clouds() {
459        let target = generate_seeded_2d_points(36, 8.0, 81);
460        let source = apply_2d_transformation(&target, &Vector2::new(0.3, -0.2), 0.15);
461        let saved = source.clone();
462        let expected = icp_matching(&target, &source);
463        let (actual, frames) = icp_matching_with_trace(&target, &source);
464        assert_eq!(source, saved);
465        assert_eq!(actual.rotation, expected.rotation);
466        assert_eq!(actual.translation, expected.translation);
467        assert_eq!(actual.iterations, expected.iterations);
468        assert_eq!(actual.final_error, expected.final_error);
469        assert_eq!(actual.converged, expected.converged);
470        assert_eq!(frames[0].points, source);
471        assert_eq!(frames[0].transform, DMatrix::identity(3, 3));
472        let last = frames.last().unwrap();
473        assert!((last.mean_error - actual.final_error_mean).abs() < 1e-12);
474        assert_eq!(last.transform.view((0, 0), (2, 2)), actual.rotation);
475        assert_eq!(last.transform.view((0, 2), (2, 1)), actual.translation);
476    }
477
478    #[test]
479    fn every_traced_state_pairs_the_actual_transformed_points_with_nearest_targets() {
480        let target = generate_seeded_2d_points(48, 9.0, 7);
481        let source = apply_2d_transformation(&target, &Vector2::new(0.6, -0.4), 0.22);
482        let (_, frames) = icp_matching_with_trace(&target, &source);
483        assert!(frames.len() > 2);
484        for f in &frames {
485            assert_eq!(f.correspondences.len(), source.ncols());
486            let mut total = 0.0;
487            for (j, &i) in f.correspondences.iter().enumerate() {
488                let point =
489                    &f.transform * DVector::from_vec(vec![source[(0, j)], source[(1, j)], 1.0]);
490                assert!((point[0] - f.points[(0, j)]).abs() < 1e-10);
491                assert!((point[1] - f.points[(1, j)]).abs() < 1e-10);
492                let d = (f.points.column(j) - target.column(i)).norm();
493                let min = (0..target.ncols())
494                    .map(|k| (f.points.column(j) - target.column(k)).norm())
495                    .fold(f64::INFINITY, f64::min);
496                assert!((d - min).abs() < 1e-12);
497                total += d;
498            }
499            assert!((total / source.ncols() as f64 - f.mean_error).abs() < 1e-12);
500            assert!((f.transform.view((0, 0), (2, 2)).determinant() - 1.0).abs() < 1e-10);
501        }
502    }
503
504    #[test]
505    fn rejected_divergent_fits_do_not_appear_in_the_trace_or_final_transform() {
506        let mut rejected = 0;
507        for seed in 0..64 {
508            let target = generate_seeded_2d_points(24, 10.0, seed);
509            let source = apply_2d_transformation(&target, &Vector2::new(1.1, -0.7), 0.4);
510            let (result, frames) = icp_matching_with_trace(&target, &source);
511            assert!(frames.len() == result.iterations || frames.len() + 1 == result.iterations);
512            if frames.len() < result.iterations {
513                rejected += 1;
514            }
515            for pair in frames.windows(2) {
516                assert!(pair[1].mean_error <= pair[0].mean_error + EPS / source.ncols() as f64);
517            }
518            let last = frames.last().unwrap();
519            assert!((last.mean_error - result.final_error_mean).abs() < 1e-12);
520            for j in 0..source.ncols() {
521                let p = &result.rotation * source.column(j) + &result.translation;
522                assert!((p - last.points.column(j)).norm() < 1e-10);
523            }
524        }
525        assert!(rejected > 0, "fixtures must exercise rollback");
526    }
527
528    #[test]
529    fn motion_estimation_returns_a_rotation_not_a_reflection() {
530        // `previous` is `current` mirrored across the x axis: the best
531        // least-squares fit is a reflection, which must not be returned.
532        let current = DMatrix::from_row_slice(2, 4, &[0.0, 2.0, 3.0, 0.5, 0.0, 0.5, 1.5, 2.0]);
533        let mut previous = current.clone();
534        previous.row_mut(1).neg_mut();
535        let (r, _) = svd_motion_estimation(&previous, &current);
536        assert!(
537            (r.determinant() - 1.0).abs() < 1e-9,
538            "det = {}",
539            r.determinant()
540        );
541    }
542
543    #[test]
544    fn the_reported_errors_describe_the_returned_transform() {
545        let mut rng = StdRng::seed_from_u64(7);
546        let previous = DMatrix::from_fn(2, 60, |_, _| rng.random_range(-5.0..5.0));
547        for seed in 0..20 {
548            let mut rng = StdRng::seed_from_u64(seed);
549            let yaw: f64 = rng.random_range(-0.3..0.3);
550            let (sin, cos) = yaw.sin_cos();
551            let rotation = DMatrix::from_row_slice(2, 2, &[cos, -sin, sin, cos]);
552            let shift = DVector::from_vec(vec![rng.random_range(-0.5..0.5), 0.3]);
553            let mut current = (&rotation * &previous).add_scalar_to_each_column(&shift);
554            current
555                .iter_mut()
556                .for_each(|v| *v += rng.random_range(-0.05..0.05));
557
558            let result = icp_matching(&previous, &current);
559            let aligned =
560                (&result.rotation * &current).add_scalar_to_each_column(&result.translation);
561            let distances = nearest_neighbor_distances(&previous, &aligned);
562            let mean = distances.iter().sum::<f64>() / distances.len() as f64;
563            assert!((mean - result.final_error_mean).abs() < 1e-9, "seed {seed}");
564            assert!((percentile(distances, 0.5) - result.final_error_median).abs() < 1e-9);
565            assert!((result.rotation.determinant() - 1.0).abs() < 1e-9);
566        }
567    }
568
569    fn generate_seeded_2d_points(n_points: usize, field_length: f64, seed: u64) -> DMatrix<f64> {
570        let mut rng = StdRng::seed_from_u64(seed);
571        let mut points = DMatrix::zeros(2, n_points);
572
573        for j in 0..n_points {
574            points[(0, j)] = (rng.random::<f64>() - 0.5) * field_length;
575            points[(1, j)] = (rng.random::<f64>() - 0.5) * field_length;
576        }
577
578        points
579    }
580
581    #[test]
582    fn test_icp_simple_translation() {
583        let n_points = 50;
584        let previous_points = generate_seeded_2d_points(n_points, 20.0, 7);
585        let translation = Vector2::new(1.0, 2.0);
586        let current_points = apply_2d_transformation(&previous_points, &translation, 0.0);
587
588        let result = icp_matching(&previous_points, &current_points);
589
590        assert!(result.converged);
591        assert!((result.translation[0] + translation.x).abs() < 0.1);
592        assert!((result.translation[1] + translation.y).abs() < 0.1);
593        assert!(result.initial_error_mean.is_finite());
594        assert!(result.final_error_median.is_finite());
595        assert!(result.final_error_p90.is_finite());
596        assert!((0.0..=1.0).contains(&result.inlier_ratio_5cm));
597        assert!((0.0..=1.0).contains(&result.relative_error_reduction));
598    }
599
600    #[test]
601    fn test_icp_rotation_and_translation() {
602        let n_points = 50;
603        let previous_points = generate_seeded_2d_points(n_points, 20.0, 42);
604        let translation = Vector2::new(0.5, 1.5);
605        let rotation_angle = 0.2; // ~11.5 degrees
606        let current_points =
607            apply_2d_transformation(&previous_points, &translation, rotation_angle);
608
609        let result = icp_matching(&previous_points, &current_points);
610
611        assert!(result.converged);
612        assert!(result.final_error_mean < 0.5);
613    }
614}