Skip to main content

rust_robotics_mapping/
imls.rs

1//! Implicit Moving Least Squares (IMLS) surfaces for 2D point sets.
2
3use nalgebra::{Matrix2, SymmetricEigen, Vector2};
4
5/// Local IMLS surface evaluation and first-order projection.
6#[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
14/// Estimates a PCA normal around every sample using neighbors within `radius`.
15pub 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            // Deterministic orientation makes signed distances reproducible.
45            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
53/// Evaluates and projects onto an IMLS surface.
54pub 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}