Skip to main content

rust_robotics_mapping/
ndt.rs

1//! Normal Distribution Transform (NDT) mapping algorithm.
2
3use std::collections::HashMap;
4
5use nalgebra::{Matrix2, SymmetricEigen, Vector2};
6use rand_distr::{Distribution, Uniform};
7
8/// A single cell in the NDT grid, storing distribution parameters.
9#[derive(Debug, Clone)]
10pub struct NDTGrid {
11    pub n_points: usize,
12    pub mean_x: f64,
13    pub mean_y: f64,
14    pub center_grid_x: f64,
15    pub center_grid_y: f64,
16    pub covariance: Matrix2<f64>,
17    pub eig_values: Vector2<f64>,
18    pub eig_vec: Matrix2<f64>,
19}
20
21impl Default for NDTGrid {
22    fn default() -> Self {
23        Self {
24            n_points: 0,
25            mean_x: 0.0,
26            mean_y: 0.0,
27            center_grid_x: 0.0,
28            center_grid_y: 0.0,
29            covariance: Matrix2::zeros(),
30            eig_values: Vector2::zeros(),
31            eig_vec: Matrix2::zeros(),
32        }
33    }
34}
35
36/// A 2D grid map that stores an `NDTGrid` in each cell.
37pub struct GridMap {
38    pub width: usize,
39    pub height: usize,
40    pub resolution: f64,
41    pub center_x: f64,
42    pub center_y: f64,
43    pub data: Vec<NDTGrid>,
44}
45
46impl GridMap {
47    pub fn new(width: usize, height: usize, resolution: f64, center_x: f64, center_y: f64) -> Self {
48        let data = vec![NDTGrid::default(); width * height];
49        GridMap {
50            width,
51            height,
52            resolution,
53            center_x,
54            center_y,
55            data,
56        }
57    }
58
59    /// Convert (x, y) in world coordinates to a single index in the grid.
60    /// Returns `None` if out of bounds.
61    pub fn calc_grid_index_from_xy_pos(&self, x: f64, y: f64) -> Option<usize> {
62        let gx = (x - self.center_x) / self.resolution + (self.width as f64) / 2.0;
63        let gy = (y - self.center_y) / self.resolution + (self.height as f64) / 2.0;
64
65        let ix = gx.floor() as isize;
66        let iy = gy.floor() as isize;
67
68        if ix < 0 || iy < 0 || ix >= self.width as isize || iy >= self.height as isize {
69            return None;
70        }
71        let idx = (iy as usize) * self.width + (ix as usize);
72        Some(idx)
73    }
74
75    /// Convert a single index to the center-world-coordinates of that grid cell.
76    pub fn calc_grid_central_xy_position_from_grid_index(&self, idx: usize) -> (f64, f64) {
77        let row = idx / self.width;
78        let col = idx % self.width;
79
80        let cx = (col as f64 + 0.5) - (self.width as f64) / 2.0;
81        let cy = (row as f64 + 0.5) - (self.height as f64) / 2.0;
82        let x = self.center_x + cx * self.resolution;
83        let y = self.center_y + cy * self.resolution;
84        (x, y)
85    }
86}
87
88/// Normal Distribution Transform (NDT) Map
89pub struct NDTMap {
90    pub min_n_points: usize,
91    pub resolution: f64,
92    pub grid_map: GridMap,
93    pub grid_index_map: HashMap<usize, Vec<usize>>,
94    pub ox: Vec<f64>,
95    pub oy: Vec<f64>,
96}
97
98impl NDTMap {
99    pub fn new(ox: &[f64], oy: &[f64], resolution: f64) -> Self {
100        let min_x = ox.iter().cloned().fold(f64::INFINITY, f64::min);
101        let max_x = ox.iter().cloned().fold(f64::NEG_INFINITY, f64::max);
102        let min_y = oy.iter().cloned().fold(f64::INFINITY, f64::min);
103        let max_y = oy.iter().cloned().fold(f64::NEG_INFINITY, f64::max);
104
105        let width = ((max_x - min_x) / resolution).ceil() as usize + 3;
106        let height = ((max_y - min_y) / resolution).ceil() as usize + 3;
107        let center_x = ox.iter().sum::<f64>() / (ox.len() as f64);
108        let center_y = oy.iter().sum::<f64>() / (oy.len() as f64);
109
110        let grid_map = GridMap::new(width, height, resolution, center_x, center_y);
111
112        let mut ndt_map = NDTMap {
113            min_n_points: 3,
114            resolution,
115            grid_map,
116            grid_index_map: HashMap::new(),
117            ox: ox.to_vec(),
118            oy: oy.to_vec(),
119        };
120        ndt_map.grid_index_map = ndt_map.create_grid_index_map(ox, oy);
121        ndt_map.construct_grid_map();
122        ndt_map
123    }
124
125    /// Build each cell's NDTGrid from the points that fall in it.
126    fn construct_grid_map(&mut self) {
127        for (grid_index, inds) in &self.grid_index_map {
128            let mut ndt = NDTGrid {
129                n_points: inds.len(),
130                ..NDTGrid::default()
131            };
132            if ndt.n_points >= self.min_n_points {
133                let mut sum_x = 0.0;
134                let mut sum_y = 0.0;
135                for &i in inds {
136                    sum_x += self.ox[i];
137                    sum_y += self.oy[i];
138                }
139                let mean_x = sum_x / (inds.len() as f64);
140                let mean_y = sum_y / (inds.len() as f64);
141
142                ndt.mean_x = mean_x;
143                ndt.mean_y = mean_y;
144
145                let (cx, cy) = self
146                    .grid_map
147                    .calc_grid_central_xy_position_from_grid_index(*grid_index);
148                ndt.center_grid_x = cx;
149                ndt.center_grid_y = cy;
150
151                let mut xs = Vec::with_capacity(inds.len());
152                let mut ys = Vec::with_capacity(inds.len());
153                for &i in inds {
154                    xs.push(self.ox[i]);
155                    ys.push(self.oy[i]);
156                }
157                let cov = compute_covariance_2d(&xs, &ys, mean_x, mean_y);
158                ndt.covariance = cov;
159
160                let sym_eig = SymmetricEigen::new(cov);
161                ndt.eig_values = sym_eig.eigenvalues;
162                ndt.eig_vec = sym_eig.eigenvectors;
163
164                self.grid_map.data[*grid_index] = ndt;
165            }
166        }
167    }
168
169    /// Return (grid_index -> Vec<point_index>) mapping.
170    fn create_grid_index_map(&self, ox: &[f64], oy: &[f64]) -> HashMap<usize, Vec<usize>> {
171        let mut map: HashMap<usize, Vec<usize>> = HashMap::new();
172        for i in 0..ox.len() {
173            if let Some(idx) = self.grid_map.calc_grid_index_from_xy_pos(ox[i], oy[i]) {
174                map.entry(idx).or_default().push(i);
175            }
176        }
177        map
178    }
179}
180
181/// Compute a 2x2 covariance matrix from a set of points, given precomputed means.
182pub fn compute_covariance_2d(xs: &[f64], ys: &[f64], mean_x: f64, mean_y: f64) -> Matrix2<f64> {
183    let n = xs.len() as f64;
184    if n <= 1.0 {
185        return Matrix2::zeros();
186    }
187
188    let mut sxx = 0.0;
189    let mut syy = 0.0;
190    let mut sxy = 0.0;
191    for (&x, &y) in xs.iter().zip(ys.iter()) {
192        let dx = x - mean_x;
193        let dy = y - mean_y;
194        sxx += dx * dx;
195        syy += dy * dy;
196        sxy += dx * dy;
197    }
198    let denom = n - 1.0;
199    Matrix2::new(sxx / denom, sxy / denom, sxy / denom, syy / denom)
200}
201
202/// Generates dummy corridor-like data with random noise.
203pub fn create_dummy_observation_data() -> (Vec<f64>, Vec<f64>) {
204    let mut ox = Vec::new();
205    let mut oy = Vec::new();
206
207    for y in -50..50 {
208        ox.push(-20.0);
209        oy.push(y as f64);
210    }
211    for y in -50..0 {
212        ox.push(20.0);
213        oy.push(y as f64);
214    }
215    for x in 20..50 {
216        ox.push(x as f64);
217        oy.push(0.0);
218    }
219    for x in 20..50 {
220        ox.push(x as f64);
221        oy.push(x as f64 / 2.0 + 10.0);
222    }
223    for y in 20..50 {
224        ox.push(20.0);
225        oy.push(y as f64);
226    }
227
228    let mut rng = rand::rng();
229    let uni = Uniform::new(0.0, 1.0).expect("valid uniform noise range");
230    for i in 0..ox.len() {
231        ox[i] += uni.sample(&mut rng);
232        oy[i] += uni.sample(&mut rng);
233    }
234    (ox, oy)
235}
236
237/// Generate (x_vals, y_vals) describing an ellipse for the given mean and covariance.
238pub fn covariance_ellipse_points(
239    mean_x: f64,
240    mean_y: f64,
241    cov: &Matrix2<f64>,
242    n_points: usize,
243) -> (Vec<f64>, Vec<f64>) {
244    let sym = SymmetricEigen::new(*cov);
245    let vals = sym.eigenvalues;
246    let vecs = sym.eigenvectors;
247
248    let lambda1 = vals[0].max(1e-6);
249    let lambda2 = vals[1].max(1e-6);
250
251    let r1 = lambda1.sqrt();
252    let r2 = lambda2.sqrt();
253
254    let mut xs = Vec::with_capacity(n_points);
255    let mut ys = Vec::with_capacity(n_points);
256
257    for i in 0..n_points {
258        let theta = 2.0 * std::f64::consts::PI * (i as f64) / (n_points as f64);
259        let px = r1 * theta.cos();
260        let py = r2 * theta.sin();
261
262        let rotated = vecs * Vector2::new(px, py);
263
264        xs.push(mean_x + rotated[0]);
265        ys.push(mean_y + rotated[1]);
266    }
267
268    (xs, ys)
269}
270
271#[cfg(test)]
272mod tests {
273    use super::*;
274
275    #[test]
276    fn test_ndt_map_creation() {
277        let (ox, oy) = create_dummy_observation_data();
278        let ndt_map = NDTMap::new(&ox, &oy, 10.0);
279        assert!(!ndt_map.grid_index_map.is_empty());
280        assert!(!ndt_map.grid_map.data.is_empty());
281    }
282
283    #[test]
284    fn test_covariance_2d() {
285        let xs = vec![1.0, 2.0, 3.0];
286        let ys = vec![4.0, 5.0, 6.0];
287        let cov = compute_covariance_2d(&xs, &ys, 2.0, 5.0);
288        assert!((cov[(0, 0)] - 1.0).abs() < 1e-10);
289        assert!((cov[(1, 1)] - 1.0).abs() < 1e-10);
290        assert!((cov[(0, 1)] - 1.0).abs() < 1e-10);
291    }
292
293    #[test]
294    fn test_covariance_ellipse_points() {
295        let cov = Matrix2::new(1.0, 0.0, 0.0, 1.0);
296        let (xs, ys) = covariance_ellipse_points(0.0, 0.0, &cov, 100);
297        assert_eq!(xs.len(), 100);
298        assert_eq!(ys.len(), 100);
299    }
300}