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;
23use std::f64;
24
25// ICP parameters
26const EPS: f64 = 0.0001;
27const MAX_ITER: usize = 100;
28const INLIER_DISTANCE_THRESHOLD: f64 = 0.05;
29
30pub struct ICPResult {
31    pub rotation: DMatrix<f64>,
32    pub translation: DVector<f64>,
33    pub iterations: usize,
34    /// Sum of nearest-neighbor distances (legacy; scales with point count).
35    pub final_error: f64,
36    /// Mean nearest-neighbor distance \[m/point\]: `final_error / point_count`.
37    pub final_error_mean: f64,
38    /// Mean nearest-neighbor distance before the first ICP update \[m/point\].
39    pub initial_error_mean: f64,
40    /// Median nearest-neighbor distance after applying the final transform.
41    pub final_error_median: f64,
42    /// 90th percentile nearest-neighbor distance after applying the final transform.
43    pub final_error_p90: f64,
44    /// Fraction of final associations below 5 cm. Useful as a simple ICP quality diagnostic.
45    pub inlier_ratio_5cm: f64,
46    /// Relative reduction from initial to final mean error. Negative values are clamped to 0.
47    pub relative_error_reduction: f64,
48    pub point_count: usize,
49    pub converged: bool,
50}
51
52/// Main ICP matching function
53///
54/// # Arguments
55/// * `previous_points` - Points from the previous frame (2×N or 3×N matrix)
56/// * `current_points` - Points from the current frame (2×N or 3×N matrix)
57///
58/// # Returns
59/// * `ICPResult` containing rotation matrix, translation vector, and convergence info
60pub fn icp_matching(previous_points: &DMatrix<f64>, current_points: &DMatrix<f64>) -> ICPResult {
61    let mut h_matrix: Option<DMatrix<f64>> = None;
62    let mut d_error = f64::INFINITY;
63    let mut pre_error = f64::INFINITY;
64    let mut initial_error = f64::NAN;
65    let mut count = 0;
66    let mut current_pts = current_points.clone();
67
68    while d_error >= EPS {
69        count += 1;
70
71        let (indexes, error) = nearest_neighbor_association(previous_points, &current_pts);
72        if initial_error.is_nan() {
73            initial_error = error;
74        }
75        let previous_indexed = select_columns(previous_points, &indexes);
76        let (rt, tt) = svd_motion_estimation(&previous_indexed, &current_pts);
77
78        // Update current points: current_points = (Rt @ current_points) + Tt
79        let rotated_pts = &rt * &current_pts;
80        current_pts = rotated_pts.add_scalar_to_each_column(&tt);
81
82        d_error = pre_error - error;
83
84        if d_error < 0.0 {
85            break;
86        }
87
88        pre_error = error;
89        h_matrix = Some(update_homogeneous_matrix(h_matrix, &rt, &tt));
90
91        if d_error <= EPS || count >= MAX_ITER {
92            break;
93        }
94    }
95
96    let h = h_matrix.unwrap_or_else(|| {
97        DMatrix::identity(previous_points.nrows() + 1, previous_points.nrows() + 1)
98    });
99    let dim = previous_points.nrows();
100
101    let rotation = h.view((0, 0), (dim, dim)).into_owned();
102    let translation = h.column(dim).rows(0, dim).into_owned();
103
104    let point_count = current_points.ncols().max(1);
105    let final_error_mean = pre_error / point_count as f64;
106    let initial_error_mean = initial_error / point_count as f64;
107    let final_distances = nearest_neighbor_distances(previous_points, &current_pts);
108    let final_error_median = percentile(final_distances.clone(), 0.50);
109    let final_error_p90 = percentile(final_distances.clone(), 0.90);
110    let inlier_count = final_distances
111        .iter()
112        .filter(|distance| **distance <= INLIER_DISTANCE_THRESHOLD)
113        .count();
114    let inlier_ratio_5cm = if final_distances.is_empty() {
115        0.0
116    } else {
117        inlier_count as f64 / final_distances.len() as f64
118    };
119    let relative_error_reduction = if initial_error_mean.is_finite() && initial_error_mean > 0.0 {
120        ((initial_error_mean - final_error_mean) / initial_error_mean).max(0.0)
121    } else {
122        0.0
123    };
124
125    ICPResult {
126        rotation,
127        translation,
128        iterations: count,
129        final_error: pre_error,
130        final_error_mean,
131        initial_error_mean,
132        final_error_median,
133        final_error_p90,
134        inlier_ratio_5cm,
135        relative_error_reduction,
136        point_count,
137        converged: d_error <= EPS && count < MAX_ITER,
138    }
139}
140
141/// Update the homogeneous transformation matrix
142fn update_homogeneous_matrix(
143    h_in: Option<DMatrix<f64>>,
144    r: &DMatrix<f64>,
145    t: &DVector<f64>,
146) -> DMatrix<f64> {
147    let r_size = r.nrows();
148    let mut h = DMatrix::zeros(r_size + 1, r_size + 1);
149
150    // Set rotation part
151    h.view_mut((0, 0), (r_size, r_size)).copy_from(r);
152    // Set translation part
153    h.view_mut((0, r_size), (r_size, 1)).copy_from(&t.column(0));
154    // Set homogeneous coordinate
155    h[(r_size, r_size)] = 1.0;
156
157    match h_in {
158        None => h,
159        Some(h_prev) => &h_prev * &h,
160    }
161}
162
163/// Find nearest neighbor associations between point sets using KdTree
164fn nearest_neighbor_association(
165    previous_points: &DMatrix<f64>,
166    current_points: &DMatrix<f64>,
167) -> (Vec<usize>, f64) {
168    let dim = previous_points.nrows();
169
170    // Build KdTree from previous_points (reference point cloud)
171    if dim == 2 {
172        let prev_svecs: Vec<SVector<f64, 2>> = (0..previous_points.ncols())
173            .map(|i| SVector::<f64, 2>::from([previous_points[(0, i)], previous_points[(1, i)]]))
174            .collect();
175        let tree = KdTree::new(&prev_svecs, 2);
176
177        let mut indexes = Vec::with_capacity(current_points.ncols());
178        let mut error = 0.0;
179
180        for j in 0..current_points.ncols() {
181            let query = SVector::<f64, 2>::from([current_points[(0, j)], current_points[(1, j)]]);
182            let (argmin, sq_dist) = tree.search(&query);
183            indexes.push(argmin.unwrap_or(0));
184            error += sq_dist.sqrt();
185        }
186        (indexes, error)
187    } else {
188        // Fallback to brute-force for non-2D cases
189        let mut indexes = Vec::with_capacity(current_points.ncols());
190        let mut error = 0.0;
191
192        for j in 0..current_points.ncols() {
193            let current_point = current_points.column(j);
194            let mut min_dist = f64::INFINITY;
195            let mut best_idx = 0;
196
197            for i in 0..previous_points.ncols() {
198                let prev_point = previous_points.column(i);
199                let dist = (current_point - prev_point).norm();
200                if dist < min_dist {
201                    min_dist = dist;
202                    best_idx = i;
203                }
204            }
205            indexes.push(best_idx);
206            error += min_dist;
207        }
208        (indexes, error)
209    }
210}
211
212fn nearest_neighbor_distances(
213    previous_points: &DMatrix<f64>,
214    current_points: &DMatrix<f64>,
215) -> Vec<f64> {
216    let dim = previous_points.nrows();
217
218    if dim == 2 {
219        let prev_svecs: Vec<SVector<f64, 2>> = (0..previous_points.ncols())
220            .map(|i| SVector::<f64, 2>::from([previous_points[(0, i)], previous_points[(1, i)]]))
221            .collect();
222        let tree = KdTree::new(&prev_svecs, 2);
223
224        (0..current_points.ncols())
225            .map(|j| {
226                let query =
227                    SVector::<f64, 2>::from([current_points[(0, j)], current_points[(1, j)]]);
228                let (_, sq_dist) = tree.search(&query);
229                sq_dist.sqrt()
230            })
231            .collect()
232    } else {
233        let mut distances = Vec::with_capacity(current_points.ncols());
234        for j in 0..current_points.ncols() {
235            let current_point = current_points.column(j);
236            let mut min_dist = f64::INFINITY;
237
238            for i in 0..previous_points.ncols() {
239                let prev_point = previous_points.column(i);
240                let dist = (current_point - prev_point).norm();
241                if dist < min_dist {
242                    min_dist = dist;
243                }
244            }
245            distances.push(min_dist);
246        }
247        distances
248    }
249}
250
251fn percentile(mut values: Vec<f64>, q: f64) -> f64 {
252    values.retain(|value| value.is_finite());
253    if values.is_empty() {
254        return f64::NAN;
255    }
256    values.sort_by(|a, b| a.total_cmp(b));
257    let clamped_q = q.clamp(0.0, 1.0);
258    let idx = ((values.len() - 1) as f64 * clamped_q).round() as usize;
259    values[idx]
260}
261
262/// Select columns from matrix based on indices
263fn select_columns(matrix: &DMatrix<f64>, indices: &[usize]) -> DMatrix<f64> {
264    let mut result = DMatrix::zeros(matrix.nrows(), indices.len());
265    for (j, &idx) in indices.iter().enumerate() {
266        result.set_column(j, &matrix.column(idx));
267    }
268    result
269}
270
271/// Add scalar to each column of a matrix (for translation)
272trait AddScalarToEachColumn {
273    fn add_scalar_to_each_column(&self, scalar: &DVector<f64>) -> Self;
274}
275
276impl AddScalarToEachColumn for DMatrix<f64> {
277    fn add_scalar_to_each_column(&self, scalar: &DVector<f64>) -> Self {
278        let mut result = self.clone();
279        for j in 0..result.ncols() {
280            for i in 0..result.nrows() {
281                result[(i, j)] += scalar[i];
282            }
283        }
284        result
285    }
286}
287
288/// SVD-based motion estimation for 2D points
289fn svd_motion_estimation(
290    previous_points: &DMatrix<f64>,
291    current_points: &DMatrix<f64>,
292) -> (DMatrix<f64>, DVector<f64>) {
293    let n_points = previous_points.ncols();
294
295    // Calculate centroids
296    let mut pm_x = 0.0;
297    let mut pm_y = 0.0;
298    let mut cm_x = 0.0;
299    let mut cm_y = 0.0;
300
301    for j in 0..n_points {
302        pm_x += previous_points[(0, j)];
303        pm_y += previous_points[(1, j)];
304        cm_x += current_points[(0, j)];
305        cm_y += current_points[(1, j)];
306    }
307
308    pm_x /= n_points as f64;
309    pm_y /= n_points as f64;
310    cm_x /= n_points as f64;
311    cm_y /= n_points as f64;
312
313    // Shift points to centroids
314    let mut p_shift = DMatrix::zeros(2, n_points);
315    let mut c_shift = DMatrix::zeros(2, n_points);
316
317    for j in 0..n_points {
318        p_shift[(0, j)] = previous_points[(0, j)] - pm_x;
319        p_shift[(1, j)] = previous_points[(1, j)] - pm_y;
320        c_shift[(0, j)] = current_points[(0, j)] - cm_x;
321        c_shift[(1, j)] = current_points[(1, j)] - cm_y;
322    }
323
324    // Calculate cross-covariance matrix W = c_shift * p_shift^T
325    let w = &c_shift * p_shift.transpose();
326
327    // SVD decomposition
328    let svd = w.svd(true, true);
329    let u = svd.u.unwrap();
330    let v_t = svd.v_t.unwrap();
331
332    // Calculate rotation: R = V * U^T
333    let r = &v_t.transpose() * &u.transpose();
334
335    // Calculate translation: t = pm - R * cm
336    let cm_vec = DVector::from_vec(vec![cm_x, cm_y]);
337    let pm_vec = DVector::from_vec(vec![pm_x, pm_y]);
338    let r_cm = &r * &cm_vec;
339    let t = &pm_vec - &r_cm;
340
341    (r, t)
342}
343
344/// Generate random 2D point cloud for testing
345pub fn generate_2d_points(n_points: usize, field_length: f64) -> DMatrix<f64> {
346    let mut rng = rand::rng();
347    let mut points = DMatrix::zeros(2, n_points);
348
349    for j in 0..n_points {
350        points[(0, j)] = (rng.random::<f64>() - 0.5) * field_length;
351        points[(1, j)] = (rng.random::<f64>() - 0.5) * field_length;
352    }
353
354    points
355}
356
357/// Apply 2D transformation to points
358pub fn apply_2d_transformation(
359    points: &DMatrix<f64>,
360    translation: &Vector2<f64>,
361    rotation_angle: f64,
362) -> DMatrix<f64> {
363    let cos_theta = rotation_angle.cos();
364    let sin_theta = rotation_angle.sin();
365    let rotation = Matrix2::new(cos_theta, -sin_theta, sin_theta, cos_theta);
366
367    let mut transformed = DMatrix::zeros(2, points.ncols());
368
369    for j in 0..points.ncols() {
370        let point = Vector2::new(points[(0, j)], points[(1, j)]);
371        let rotated = rotation * point;
372        transformed[(0, j)] = rotated.x + translation.x;
373        transformed[(1, j)] = rotated.y + translation.y;
374    }
375
376    transformed
377}
378
379#[cfg(test)]
380mod tests {
381    use super::*;
382    use nalgebra::Vector2;
383    use rand::{rngs::StdRng, Rng, SeedableRng};
384
385    fn generate_seeded_2d_points(n_points: usize, field_length: f64, seed: u64) -> DMatrix<f64> {
386        let mut rng = StdRng::seed_from_u64(seed);
387        let mut points = DMatrix::zeros(2, n_points);
388
389        for j in 0..n_points {
390            points[(0, j)] = (rng.random::<f64>() - 0.5) * field_length;
391            points[(1, j)] = (rng.random::<f64>() - 0.5) * field_length;
392        }
393
394        points
395    }
396
397    #[test]
398    fn test_icp_simple_translation() {
399        let n_points = 50;
400        let previous_points = generate_seeded_2d_points(n_points, 20.0, 7);
401        let translation = Vector2::new(1.0, 2.0);
402        let current_points = apply_2d_transformation(&previous_points, &translation, 0.0);
403
404        let result = icp_matching(&previous_points, &current_points);
405
406        assert!(result.converged);
407        assert!((result.translation[0] + translation.x).abs() < 0.1);
408        assert!((result.translation[1] + translation.y).abs() < 0.1);
409        assert!(result.initial_error_mean.is_finite());
410        assert!(result.final_error_median.is_finite());
411        assert!(result.final_error_p90.is_finite());
412        assert!((0.0..=1.0).contains(&result.inlier_ratio_5cm));
413        assert!((0.0..=1.0).contains(&result.relative_error_reduction));
414    }
415
416    #[test]
417    fn test_icp_rotation_and_translation() {
418        let n_points = 50;
419        let previous_points = generate_seeded_2d_points(n_points, 20.0, 42);
420        let translation = Vector2::new(0.5, 1.5);
421        let rotation_angle = 0.2; // ~11.5 degrees
422        let current_points =
423            apply_2d_transformation(&previous_points, &translation, rotation_angle);
424
425        let result = icp_matching(&previous_points, &current_points);
426
427        assert!(result.converged);
428        assert!(result.final_error_mean < 0.5);
429    }
430}
rust_robotics_slam/icp_matching.rs

rust_robotics_slam/
icp_matching.rs
