rust_robotics_mapping/
imls.rs1use nalgebra::{Matrix2, SymmetricEigen, Vector2};
4
5#[derive(Debug, Clone, Copy, PartialEq)]
7pub struct ImlsProjection {
8 pub signed_distance: f64,
9 pub normal: Vector2<f64>,
10 pub projected_point: Vector2<f64>,
11 pub support_count: usize,
12}
13
14pub fn estimate_normals_2d(
16 samples: &[Vector2<f64>],
17 radius: f64,
18 min_neighbors: usize,
19) -> Vec<Option<Vector2<f64>>> {
20 let radius_squared = radius * radius;
21 samples
22 .iter()
23 .map(|sample| {
24 let neighbors = samples
25 .iter()
26 .filter(|candidate| (*candidate - sample).norm_squared() <= radius_squared)
27 .copied()
28 .collect::<Vec<_>>();
29 if neighbors.len() < min_neighbors.max(2) {
30 return None;
31 }
32 let center = neighbors.iter().copied().sum::<Vector2<f64>>() / neighbors.len() as f64;
33 let covariance = neighbors.iter().fold(Matrix2::zeros(), |sum, point| {
34 let centered = point - center;
35 sum + centered * centered.transpose()
36 }) / neighbors.len() as f64;
37 let eigen = SymmetricEigen::new(covariance);
38 let index = if eigen.eigenvalues[0] <= eigen.eigenvalues[1] {
39 0
40 } else {
41 1
42 };
43 let mut normal = eigen.eigenvectors.column(index).into_owned().normalize();
44 if normal.y < 0.0 || (normal.y.abs() < 1.0e-12 && normal.x < 0.0) {
46 normal = -normal;
47 }
48 Some(normal)
49 })
50 .collect()
51}
52
53pub fn project_to_imls_2d(
55 query: Vector2<f64>,
56 samples: &[Vector2<f64>],
57 normals: &[Option<Vector2<f64>>],
58 bandwidth: f64,
59 support_radius: f64,
60) -> Option<ImlsProjection> {
61 if samples.len() != normals.len() || bandwidth <= 0.0 || support_radius <= 0.0 {
62 return None;
63 }
64 let radius_squared = support_radius * support_radius;
65 let bandwidth_squared = bandwidth * bandwidth;
66 let mut weight_sum = 0.0;
67 let mut distance_sum = 0.0;
68 let mut gradient = Vector2::zeros();
69 let mut support_count = 0;
70
71 for (sample, normal) in samples.iter().zip(normals) {
72 let Some(normal) = normal else {
73 continue;
74 };
75 let displacement = query - sample;
76 let squared_distance = displacement.norm_squared();
77 if squared_distance > radius_squared {
78 continue;
79 }
80 let weight = (-squared_distance / bandwidth_squared).exp();
81 weight_sum += weight;
82 distance_sum += weight * displacement.dot(normal);
83 gradient += weight * normal;
84 support_count += 1;
85 }
86 if weight_sum <= f64::EPSILON || support_count == 0 {
87 return None;
88 }
89 let signed_distance = distance_sum / weight_sum;
90 let gradient = gradient / weight_sum;
91 let gradient_norm_squared = gradient.norm_squared();
92 if gradient_norm_squared <= 1.0e-12 {
93 return None;
94 }
95 let normal = gradient / gradient_norm_squared.sqrt();
96 let projected_point = query - gradient * (signed_distance / gradient_norm_squared);
97 Some(ImlsProjection {
98 signed_distance,
99 normal,
100 projected_point,
101 support_count,
102 })
103}
104
105#[cfg(test)]
106mod tests {
107 use super::*;
108
109 #[test]
110 fn line_normals_and_projection_are_correct() {
111 let samples = (-10..=10)
112 .map(|index| Vector2::new(index as f64 * 0.1, 0.0))
113 .collect::<Vec<_>>();
114 let normals = estimate_normals_2d(&samples, 0.35, 3);
115 let projection =
116 project_to_imls_2d(Vector2::new(0.1, 0.25), &samples, &normals, 0.3, 0.5).unwrap();
117 assert!((projection.signed_distance - 0.25).abs() < 1.0e-9);
118 assert!(projection.projected_point.y.abs() < 1.0e-9);
119 assert!(projection.normal.y > 0.99);
120 }
121
122 #[test]
123 fn projection_requires_local_support() {
124 let samples = vec![Vector2::new(0.0, 0.0), Vector2::new(1.0, 0.0)];
125 let normals = vec![Some(Vector2::y()), Some(Vector2::y())];
126 assert!(
127 project_to_imls_2d(Vector2::new(10.0, 10.0), &samples, &normals, 1.0, 1.0).is_none()
128 );
129 }
130}