1use nalgebra::DMatrix;
9use std::collections::VecDeque;
10use std::fs::File;
11use std::io::Write;
12
13pub const EXTEND_AREA: f64 = 1.0;
15
16pub const FREE: f64 = 0.0;
18pub const UNKNOWN: f64 = 0.5;
20pub const OCCUPIED: f64 = 1.0;
22
23#[derive(Debug, Clone, Copy, PartialEq, Eq)]
25pub enum GridMapMethod {
26 Bresenham,
28 FloodFill,
30}
31
32#[derive(Debug, Clone)]
34struct GridMapConfig {
35 min_x: f64,
36 min_y: f64,
37 max_x: f64,
38 max_y: f64,
39 x_width: usize,
40 y_width: usize,
41}
42
43pub struct LidarGridMap {
50 pub data: DMatrix<f64>,
51 pub min_x: f64,
52 pub min_y: f64,
53 pub max_x: f64,
54 pub max_y: f64,
55 pub resolution: f64,
56 pub x_width: usize,
57 pub y_width: usize,
58 pub center_ix: usize,
59 pub center_iy: usize,
60}
61
62impl LidarGridMap {
63 pub fn new(ox: &[f64], oy: &[f64], resolution: f64, method: GridMapMethod) -> Self {
71 assert_eq!(ox.len(), oy.len(), "ox and oy must have the same length");
72 assert!(resolution > 0.0, "resolution must be positive");
73
74 let config = calc_grid_map_config(ox, oy, resolution);
75
76 let center_ix = ((-config.min_x) / resolution).round() as usize;
78 let center_iy = ((-config.min_y) / resolution).round() as usize;
79
80 let mut data = DMatrix::from_element(config.x_width, config.y_width, UNKNOWN);
81
82 match method {
83 GridMapMethod::Bresenham => {
84 generate_bresenham(&mut data, ox, oy, &config, resolution, center_ix, center_iy);
85 }
86 GridMapMethod::FloodFill => {
87 generate_flood_fill(&mut data, ox, oy, &config, resolution, center_ix, center_iy);
88 }
89 }
90
91 LidarGridMap {
92 data,
93 min_x: config.min_x,
94 min_y: config.min_y,
95 max_x: config.max_x,
96 max_y: config.max_y,
97 resolution,
98 x_width: config.x_width,
99 y_width: config.y_width,
100 center_ix,
101 center_iy,
102 }
103 }
104
105 pub fn from_scan(
113 angles: &[f64],
114 distances: &[f64],
115 resolution: f64,
116 method: GridMapMethod,
117 ) -> Self {
118 assert_eq!(
119 angles.len(),
120 distances.len(),
121 "angles and distances must have the same length"
122 );
123 let ox: Vec<f64> = angles
124 .iter()
125 .zip(distances.iter())
126 .map(|(a, d)| a.sin() * d)
127 .collect();
128 let oy: Vec<f64> = angles
129 .iter()
130 .zip(distances.iter())
131 .map(|(a, d)| a.cos() * d)
132 .collect();
133 Self::new(&ox, &oy, resolution, method)
134 }
135
136 pub fn get_index(&self, x: f64, y: f64) -> Option<(usize, usize)> {
138 let ix = ((x - self.min_x) / self.resolution).floor() as i32;
139 let iy = ((y - self.min_y) / self.resolution).floor() as i32;
140
141 if ix >= 0 && ix < self.x_width as i32 && iy >= 0 && iy < self.y_width as i32 {
142 Some((ix as usize, iy as usize))
143 } else {
144 None
145 }
146 }
147
148 pub fn get_occupancy(&self, x: f64, y: f64) -> Option<f64> {
150 self.get_index(x, y).map(|(ix, iy)| self.data[(ix, iy)])
151 }
152}
153
154fn calc_grid_map_config(ox: &[f64], oy: &[f64], resolution: f64) -> GridMapConfig {
156 let min_x = ox.iter().cloned().fold(f64::INFINITY, f64::min).floor() - EXTEND_AREA / 2.0;
157 let min_y = oy.iter().cloned().fold(f64::INFINITY, f64::min).floor() - EXTEND_AREA / 2.0;
158 let max_x = ox.iter().cloned().fold(f64::NEG_INFINITY, f64::max).ceil() + EXTEND_AREA / 2.0;
159 let max_y = oy.iter().cloned().fold(f64::NEG_INFINITY, f64::max).ceil() + EXTEND_AREA / 2.0;
160
161 let x_width = ((max_x - min_x) / resolution).round() as usize;
162 let y_width = ((max_y - min_y) / resolution).round() as usize;
163
164 GridMapConfig {
165 min_x,
166 min_y,
167 max_x,
168 max_y,
169 x_width,
170 y_width,
171 }
172}
173
174fn bresenham(start: (i32, i32), end: (i32, i32)) -> Vec<(i32, i32)> {
176 let (mut x1, mut y1) = start;
177 let (mut x2, mut y2) = end;
178 let dx = x2 - x1;
179 let dy = y2 - y1;
180 let is_steep = dy.abs() > dx.abs();
181
182 if is_steep {
183 std::mem::swap(&mut x1, &mut y1);
184 std::mem::swap(&mut x2, &mut y2);
185 }
186
187 let mut swapped = false;
188 if x1 > x2 {
189 std::mem::swap(&mut x1, &mut x2);
190 std::mem::swap(&mut y1, &mut y2);
191 swapped = true;
192 }
193
194 let dx = x2 - x1;
195 let dy = y2 - y1;
196 let mut error = dx / 2;
197 let y_step = if y1 < y2 { 1 } else { -1 };
198
199 let mut y = y1;
200 let mut points = Vec::new();
201 for x in x1..=x2 {
202 let coord = if is_steep { (y, x) } else { (x, y) };
203 points.push(coord);
204 error -= dy.abs();
205 if error < 0 {
206 y += y_step;
207 error += dx;
208 }
209 }
210
211 if swapped {
212 points.reverse();
213 }
214 points
215}
216
217fn generate_bresenham(
219 data: &mut DMatrix<f64>,
220 ox: &[f64],
221 oy: &[f64],
222 config: &GridMapConfig,
223 resolution: f64,
224 center_ix: usize,
225 center_iy: usize,
226) {
227 let xw = config.x_width;
228 let yw = config.y_width;
229
230 for (&x, &y) in ox.iter().zip(oy.iter()) {
231 let ix = ((x - config.min_x) / resolution).round() as i32;
232 let iy = ((y - config.min_y) / resolution).round() as i32;
233
234 let laser_beams = bresenham((center_ix as i32, center_iy as i32), (ix, iy));
235
236 for &(bx, by) in &laser_beams {
238 if bx >= 0 && bx < xw as i32 && by >= 0 && by < yw as i32 {
239 data[(bx as usize, by as usize)] = FREE;
240 }
241 }
242
243 for di in 0..=1_i32 {
245 for dj in 0..=1_i32 {
246 let oi = ix + di;
247 let oj = iy + dj;
248 if oi >= 0 && oi < xw as i32 && oj >= 0 && oj < yw as i32 {
249 data[(oi as usize, oj as usize)] = OCCUPIED;
250 }
251 }
252 }
253 }
254}
255
256fn generate_flood_fill(
258 data: &mut DMatrix<f64>,
259 ox: &[f64],
260 oy: &[f64],
261 config: &GridMapConfig,
262 resolution: f64,
263 center_ix: usize,
264 center_iy: usize,
265) {
266 let xw = config.x_width;
267 let yw = config.y_width;
268
269 let mut prev_ix = center_ix as i32 - 1;
271 let mut prev_iy = center_iy as i32;
272 for (&x, &y) in ox.iter().zip(oy.iter()) {
273 let ix = ((x - config.min_x) / resolution).round() as i32;
274 let iy = ((y - config.min_y) / resolution).round() as i32;
275
276 let line = bresenham((prev_ix, prev_iy), (ix, iy));
277 for &(bx, by) in &line {
278 if bx >= 0 && bx < xw as i32 && by >= 0 && by < yw as i32 {
279 data[(bx as usize, by as usize)] = FREE;
280 }
281 }
282
283 prev_ix = ix;
284 prev_iy = iy;
285 }
286
287 flood_fill(center_ix, center_iy, data);
289
290 for (&x, &y) in ox.iter().zip(oy.iter()) {
292 let ix = ((x - config.min_x) / resolution).round() as i32;
293 let iy = ((y - config.min_y) / resolution).round() as i32;
294 for di in 0..=1_i32 {
295 for dj in 0..=1_i32 {
296 let oi = ix + di;
297 let oj = iy + dj;
298 if oi >= 0 && oi < xw as i32 && oj >= 0 && oj < yw as i32 {
299 data[(oi as usize, oj as usize)] = OCCUPIED;
300 }
301 }
302 }
303 }
304}
305
306fn flood_fill(start_x: usize, start_y: usize, data: &mut DMatrix<f64>) {
309 let (sx, sy) = data.shape();
310 let mut queue = VecDeque::new();
311 queue.push_back((start_x, start_y));
312
313 while let Some((nx, ny)) = queue.pop_back() {
314 if nx > 0 && (data[(nx - 1, ny)] - UNKNOWN).abs() < f64::EPSILON {
316 data[(nx - 1, ny)] = FREE;
317 queue.push_front((nx - 1, ny));
318 }
319 if nx < sx - 1 && (data[(nx + 1, ny)] - UNKNOWN).abs() < f64::EPSILON {
321 data[(nx + 1, ny)] = FREE;
322 queue.push_front((nx + 1, ny));
323 }
324 if ny > 0 && (data[(nx, ny - 1)] - UNKNOWN).abs() < f64::EPSILON {
326 data[(nx, ny - 1)] = FREE;
327 queue.push_front((nx, ny - 1));
328 }
329 if ny < sy - 1 && (data[(nx, ny + 1)] - UNKNOWN).abs() < f64::EPSILON {
331 data[(nx, ny + 1)] = FREE;
332 queue.push_front((nx, ny + 1));
333 }
334 }
335}
336
337fn occupancy_style(value: f64) -> (&'static str, f64) {
338 if value >= 0.99 {
339 ("#b91c1c", 0.95) } else if value >= 0.49 {
341 ("#94a3b8", 0.60) } else {
343 ("#f0fdf4", 0.85) }
345}
346
347pub fn build_lidar_grid_map_svg(grid_map: &LidarGridMap, ox: &[f64], oy: &[f64]) -> String {
349 let width = 920.0;
350 let height = 700.0;
351 let margin = 72.0;
352 let side_panel_width = 220.0;
353 let available_plot_width = width - 2.0 * margin - side_panel_width;
354 let available_plot_height = height - 2.0 * margin;
355 let x_range = grid_map.max_x - grid_map.min_x;
356 let y_range = grid_map.max_y - grid_map.min_y;
357 let scale = (available_plot_width / x_range)
358 .min(available_plot_height / y_range)
359 .max(1.0);
360 let plot_width = x_range * scale;
361 let plot_height = y_range * scale;
362 let plot_left = margin;
363 let plot_top = margin + (available_plot_height - plot_height) / 2.0;
364 let legend_x = plot_left + plot_width + 36.0;
365 let legend_y = plot_top + 24.0;
366
367 let to_svg_x = |x: f64| plot_left + (x - grid_map.min_x) * scale;
368 let to_svg_y = |y: f64| plot_top + plot_height - (y - grid_map.min_y) * scale;
369 let cell_size = grid_map.resolution * scale;
370 let origin_x = grid_map.min_x + grid_map.center_ix as f64 * grid_map.resolution;
371 let origin_y = grid_map.min_y + grid_map.center_iy as f64 * grid_map.resolution;
372
373 let mut svg = String::new();
374 svg.push_str(&format!(
375 "<?xml version='1.0' encoding='UTF-8'?>\n<svg xmlns='http://www.w3.org/2000/svg' width='{width}' height='{height}' viewBox='0 0 {width} {height}'>\n"
376 ));
377 svg.push_str("<rect width='100%' height='100%' fill='white'/>\n");
378 svg.push_str(&format!(
379 "<text x='{:.1}' y='38' text-anchor='middle' font-size='24' font-family='sans-serif' font-weight='700' fill='#111827'>LIDAR to Grid Map</text>\n",
380 width / 2.0
381 ));
382 svg.push_str(&format!(
383 "<rect x='{plot_left:.1}' y='{plot_top:.1}' width='{plot_width:.1}' height='{plot_height:.1}' fill='#fffdf7' stroke='#d6d3d1' stroke-width='2'/>\n"
384 ));
385
386 let tick_step = pick_tick_step(x_range);
388 let x_tick_start = (grid_map.min_x / tick_step).ceil() as i32;
389 let x_tick_end = (grid_map.max_x / tick_step).floor() as i32;
390 for tick in x_tick_start..=x_tick_end {
391 let x = tick as f64 * tick_step;
392 let sx = to_svg_x(x);
393 svg.push_str(&format!(
394 "<line x1='{sx:.2}' y1='{plot_top:.2}' x2='{sx:.2}' y2='{:.2}' stroke='#ece7dc' stroke-width='1'/>\n",
395 plot_top + plot_height
396 ));
397 svg.push_str(&format!(
398 "<text x='{sx:.2}' y='{:.2}' text-anchor='middle' font-size='14' font-family='sans-serif' fill='#374151'>{x:.1}</text>\n",
399 plot_top + plot_height + 24.0
400 ));
401 }
402
403 let y_tick_step = pick_tick_step(y_range);
405 let y_tick_start = (grid_map.min_y / y_tick_step).ceil() as i32;
406 let y_tick_end = (grid_map.max_y / y_tick_step).floor() as i32;
407 for tick in y_tick_start..=y_tick_end {
408 let y = tick as f64 * y_tick_step;
409 let sy = to_svg_y(y);
410 svg.push_str(&format!(
411 "<line x1='{plot_left:.2}' y1='{sy:.2}' x2='{:.2}' y2='{sy:.2}' stroke='#ece7dc' stroke-width='1'/>\n",
412 plot_left + plot_width
413 ));
414 svg.push_str(&format!(
415 "<text x='{:.2}' y='{:.2}' text-anchor='end' font-size='14' font-family='sans-serif' fill='#374151'>{y:.1}</text>\n",
416 plot_left - 10.0,
417 sy + 5.0
418 ));
419 }
420
421 for iy in 0..grid_map.y_width {
423 for ix in 0..grid_map.x_width {
424 let value = grid_map.data[(ix, iy)];
425 let x = grid_map.min_x + ix as f64 * grid_map.resolution;
426 let y = grid_map.min_y + iy as f64 * grid_map.resolution;
427 let sx = to_svg_x(x);
428 let sy = to_svg_y(y + grid_map.resolution);
429 let (fill, opacity) = occupancy_style(value);
430 svg.push_str(&format!(
431 "<rect x='{sx:.2}' y='{sy:.2}' width='{cell_size:.2}' height='{cell_size:.2}' fill='{fill}' fill-opacity='{opacity:.3}' stroke='none'/>\n"
432 ));
433 }
434 }
435
436 for (x, y) in ox.iter().zip(oy.iter()) {
438 svg.push_str(&format!(
439 "<circle cx='{:.2}' cy='{:.2}' r='3.8' fill='#111827' stroke='white' stroke-width='1.2'/>\n",
440 to_svg_x(*x),
441 to_svg_y(*y)
442 ));
443 }
444
445 svg.push_str(&format!(
447 "<circle cx='{:.2}' cy='{:.2}' r='6' fill='#2563eb' stroke='white' stroke-width='2'/>\n",
448 to_svg_x(origin_x),
449 to_svg_y(origin_y)
450 ));
451
452 svg.push_str(&format!(
454 "<rect x='{legend_x:.1}' y='{legend_y:.1}' width='182' height='180' rx='18' fill='white' stroke='#d6d3d1'/>\n"
455 ));
456 let legend_rows = [
457 ("#b91c1c", "Occupied cell"),
458 ("#94a3b8", "Unknown cell"),
459 ("#f0fdf4", "Free space"),
460 ];
461 for (index, (color, label)) in legend_rows.iter().enumerate() {
462 let row_y = legend_y + 28.0 + index as f64 * 26.0;
463 svg.push_str(&format!(
464 "<rect x='{:.1}' y='{:.1}' width='22' height='14' rx='4' fill='{color}' stroke='#cbd5e1'/>\n",
465 legend_x + 14.0,
466 row_y - 10.0
467 ));
468 svg.push_str(&format!(
469 "<text x='{:.1}' y='{:.1}' font-size='16' font-family='sans-serif' fill='#111827'>{label}</text>\n",
470 legend_x + 48.0,
471 row_y + 1.0
472 ));
473 }
474 let obstacle_row_y = legend_y + 112.0;
475 svg.push_str(&format!(
476 "<circle cx='{:.1}' cy='{obstacle_row_y:.1}' r='4.0' fill='#111827' stroke='white' stroke-width='1.2'/>\n",
477 legend_x + 25.0
478 ));
479 svg.push_str(&format!(
480 "<text x='{:.1}' y='{:.1}' font-size='16' font-family='sans-serif' fill='#111827'>Obstacle points</text>\n",
481 legend_x + 48.0,
482 obstacle_row_y + 1.0
483 ));
484 let sensor_row_y = legend_y + 138.0;
485 svg.push_str(&format!(
486 "<circle cx='{:.1}' cy='{sensor_row_y:.1}' r='5.5' fill='#2563eb' stroke='white' stroke-width='1.5'/>\n",
487 legend_x + 25.0
488 ));
489 svg.push_str(&format!(
490 "<text x='{:.1}' y='{:.1}' font-size='16' font-family='sans-serif' fill='#111827'>Sensor origin</text>\n",
491 legend_x + 48.0,
492 sensor_row_y + 1.0
493 ));
494
495 svg.push_str(&format!(
497 "<text x='{:.1}' y='{:.1}' text-anchor='middle' font-size='16' font-family='sans-serif' fill='#111827'>x [m]</text>\n",
498 plot_left + plot_width / 2.0,
499 plot_top + plot_height + 52.0
500 ));
501 svg.push_str(&format!(
502 "<text x='{:.1}' y='{:.1}' text-anchor='middle' font-size='16' font-family='sans-serif' fill='#111827' transform='rotate(-90, {:.1}, {:.1})'>y [m]</text>\n",
503 plot_left - 48.0,
504 plot_top + plot_height / 2.0,
505 plot_left - 48.0,
506 plot_top + plot_height / 2.0
507 ));
508 svg.push_str("</svg>\n");
509
510 svg
511}
512
513fn pick_tick_step(range: f64) -> f64 {
515 if range > 20.0 {
516 10.0
517 } else if range > 4.0 {
518 2.0
519 } else if range > 1.0 {
520 0.5
521 } else {
522 0.1
523 }
524}
525
526pub fn save_lidar_grid_map_svg(
528 filename: &str,
529 grid_map: &LidarGridMap,
530 ox: &[f64],
531 oy: &[f64],
532) -> std::io::Result<()> {
533 let svg = build_lidar_grid_map_svg(grid_map, ox, oy);
534 let mut file = File::create(filename)?;
535 file.write_all(svg.as_bytes())?;
536 Ok(())
537}
538
539#[cfg(test)]
540mod tests {
541 use super::*;
542 use std::f64::consts::PI;
543
544 fn circular_scan(n: usize, radius: f64) -> (Vec<f64>, Vec<f64>) {
546 let angles: Vec<f64> = (0..n).map(|i| 2.0 * PI * i as f64 / n as f64).collect();
547 let distances: Vec<f64> = vec![radius; n];
548 (angles, distances)
549 }
550
551 #[test]
552 fn test_bresenham_horizontal() {
553 let pts = bresenham((0, 0), (5, 0));
554 assert_eq!(pts.len(), 6);
555 assert_eq!(pts[0], (0, 0));
556 assert_eq!(pts[5], (5, 0));
557 }
558
559 #[test]
560 fn test_bresenham_vertical() {
561 let pts = bresenham((0, 0), (0, 5));
562 assert_eq!(pts.len(), 6);
563 assert_eq!(pts[0], (0, 0));
564 assert_eq!(pts[5], (0, 5));
565 }
566
567 #[test]
568 fn test_bresenham_diagonal() {
569 let pts = bresenham((0, 0), (3, 3));
570 assert_eq!(pts.len(), 4);
571 for (i, &(x, y)) in pts.iter().enumerate() {
572 assert_eq!(x, i as i32);
573 assert_eq!(y, i as i32);
574 }
575 }
576
577 #[test]
578 fn test_bresenham_reversed() {
579 let fwd = bresenham((1, 1), (5, 3));
580 let rev = bresenham((5, 3), (1, 1));
581 assert_eq!(fwd.len(), rev.len());
583 for (a, b) in fwd.iter().zip(rev.iter().rev()) {
584 assert_eq!(a, b);
585 }
586 }
587
588 #[test]
589 fn test_grid_map_bresenham_basic() {
590 let (angles, distances) = circular_scan(36, 5.0);
591 let grid = LidarGridMap::from_scan(&angles, &distances, 0.5, GridMapMethod::Bresenham);
592
593 assert!(grid.x_width > 0);
594 assert!(grid.y_width > 0);
595
596 let origin_val = grid.data[(grid.center_ix, grid.center_iy)];
598 assert!(
599 origin_val < 0.01,
600 "origin should be free, got {}",
601 origin_val
602 );
603
604 let occupied_count = grid.data.iter().filter(|&&v| v >= 0.99).count();
606 assert!(occupied_count > 0, "expected some occupied cells");
607 }
608
609 #[test]
610 fn test_grid_map_flood_fill_basic() {
611 let (angles, distances) = circular_scan(36, 5.0);
612 let grid = LidarGridMap::from_scan(&angles, &distances, 0.5, GridMapMethod::FloodFill);
613
614 let origin_val = grid.data[(grid.center_ix, grid.center_iy)];
616 assert!(
617 origin_val < 0.01,
618 "origin should be free, got {}",
619 origin_val
620 );
621
622 let occupied_count = grid.data.iter().filter(|&&v| v >= 0.99).count();
624 assert!(occupied_count > 0, "expected some occupied cells");
625 }
626
627 #[test]
628 fn test_grid_map_from_cartesian() {
629 let ox = vec![1.0, 0.0, -1.0, 0.0];
630 let oy = vec![0.0, 1.0, 0.0, -1.0];
631 let grid = LidarGridMap::new(&ox, &oy, 0.5, GridMapMethod::Bresenham);
632
633 for (&x, &y) in ox.iter().zip(oy.iter()) {
635 assert!(grid.get_index(x, y).is_some());
636 }
637 }
638
639 #[test]
640 fn test_get_occupancy_out_of_bounds() {
641 let grid = LidarGridMap::new(&[1.0], &[1.0], 0.5, GridMapMethod::Bresenham);
642 assert!(grid.get_occupancy(1000.0, 1000.0).is_none());
643 }
644
645 #[test]
646 fn test_svg_output() {
647 let ox = vec![1.0, 0.0, -1.0, 0.0];
648 let oy = vec![0.0, 1.0, 0.0, -1.0];
649 let grid = LidarGridMap::new(&ox, &oy, 0.5, GridMapMethod::Bresenham);
650 let svg = build_lidar_grid_map_svg(&grid, &ox, &oy);
651
652 assert!(svg.contains("<rect width='100%' height='100%' fill='white'/>"));
653 assert!(svg.contains("LIDAR to Grid Map"));
654 assert!(svg.contains("Occupied cell"));
655 assert!(svg.contains("Free space"));
656 assert!(svg.contains("Sensor origin"));
657 }
658
659 #[test]
660 fn test_both_methods_produce_occupied_and_free() {
661 let (angles, distances) = circular_scan(72, 3.0);
662
663 for method in [GridMapMethod::Bresenham, GridMapMethod::FloodFill] {
664 let grid = LidarGridMap::from_scan(&angles, &distances, 0.25, method);
665 let free_count = grid.data.iter().filter(|&&v| v < 0.01).count();
666 let occupied_count = grid.data.iter().filter(|&&v| v >= 0.99).count();
667 assert!(free_count > 0, "{:?}: expected free cells", method);
668 assert!(occupied_count > 0, "{:?}: expected occupied cells", method);
669 }
670 }
671}