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
9use nalgebra::{DMatrix, DVector, Matrix2, SVector, Vector2};
21use nearest_neighbor::KdTree;
22use rand::Rng;
23
24const 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 pub final_error: f64,
35 pub final_error_mean: f64,
37 pub initial_error_mean: f64,
39 pub final_error_median: f64,
41 pub final_error_p90: f64,
43 pub inlier_ratio_5cm: f64,
45 pub relative_error_reduction: f64,
47 pub point_count: usize,
48 pub converged: bool,
49}
50
51#[derive(Debug, Clone)]
55pub struct ICPTraceFrame {
56 pub points: DMatrix<f64>,
57 pub transform: DMatrix<f64>,
59 pub correspondences: Vec<usize>,
61 pub mean_error: f64,
62}
63
64pub 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
91pub 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 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, ¤t_pts);
122 if initial_error.is_nan() {
123 initial_error = error;
124 }
125 if error > pre_error + EPS {
126 if let Some((h, pts)) = last_good.take() {
129 h_matrix = h;
130 current_pts = pts;
131 }
132 break;
133 }
134 observe(¤t_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, ¤t_pts);
147
148 let rotated_pts = &rt * ¤t_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, ¤t_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
203fn 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 h.view_mut((0, 0), (r_size, r_size)).copy_from(r);
218 h.view_mut((0, r_size), (r_size, 1)).copy_from(&t.column(0));
220 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
229fn 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 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 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
328fn 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
337trait 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
354fn 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 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 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 let w = &c_shift * p_shift.transpose();
392
393 let svd = w.svd(true, true);
395 let u = svd.u.unwrap();
396 let v_t = svd.v_t.unwrap();
397
398 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 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
416pub 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
429pub 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 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, ¤t);
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, ¤t);
559 let aligned =
560 (&result.rotation * ¤t).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, ¤t_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; let current_points =
607 apply_2d_transformation(&previous_points, &translation, rotation_angle);
608
609 let result = icp_matching(&previous_points, ¤t_points);
610
611 assert!(result.converged);
612 assert!(result.final_error_mean < 0.5);
613 }
614}