1use std::collections::HashMap;
4
5use nalgebra::{Matrix2, SymmetricEigen, Vector2};
6use rand_distr::{Distribution, Uniform};
7
8#[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
36pub 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 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 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
88pub 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 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 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
181pub 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
202pub 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
237pub 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}