1use nalgebra::Vector2;
11use rust_robotics_core::Pose2D;
12
13use crate::scan_to_map::{beam_angle, transform_scan_to_world};
14
15const MISS: u8 = 1;
16const HIT: u8 = 2;
17
18struct ScanUpdate {
20 width: usize,
21 marks: Vec<u8>,
22 touched: Vec<usize>,
23}
24
25impl ScanUpdate {
26 fn new(width: usize, height: usize) -> Self {
27 Self {
28 width,
29 marks: vec![0; width * height],
30 touched: Vec::new(),
31 }
32 }
33
34 fn mark(&mut self, (x, y): (usize, usize), mark: u8) {
35 let index = y * self.width + x;
36 if self.marks[index] == 0 {
37 self.touched.push(index);
38 }
39 self.marks[index] = self.marks[index].max(mark);
40 }
41}
42
43#[derive(Debug, Clone, Copy, PartialEq)]
45pub struct OccupancyConfig {
46 pub resolution: f64,
48 pub hit: f32,
50 pub miss: f32,
52 pub clamp: f32,
54 pub occupied_threshold: f32,
56 pub free_threshold: f32,
58}
59
60impl Default for OccupancyConfig {
61 fn default() -> Self {
62 Self {
63 resolution: 0.1,
64 hit: 0.9,
65 miss: -0.4,
66 clamp: 5.0,
67 occupied_threshold: 0.6,
68 free_threshold: -0.3,
69 }
70 }
71}
72
73#[derive(Debug, Clone, Copy, PartialEq, Eq)]
75pub enum CellState {
76 Unknown,
77 Free,
78 Occupied,
79}
80
81#[derive(Debug, Clone, Copy, PartialEq, Eq)]
83pub enum OccupancyEvidence {
84 Hit,
85 Miss,
86}
87
88#[derive(Debug, Clone, Copy, PartialEq)]
90pub struct OccupancyCellUpdate {
91 pub x: usize,
92 pub y: usize,
93 pub evidence: OccupancyEvidence,
94 pub before: f32,
95 pub after: f32,
96}
97
98#[derive(Debug, Clone)]
100pub struct OccupancyGrid {
101 config: OccupancyConfig,
102 origin: Vector2<f64>,
103 width: usize,
104 height: usize,
105 log_odds: Vec<f32>,
106}
107
108impl OccupancyGrid {
109 pub fn new(min: Vector2<f64>, max: Vector2<f64>, config: OccupancyConfig) -> Self {
111 let resolution = config.resolution.max(1.0e-3);
112 let width = (((max.x - min.x) / resolution).ceil() as usize).max(1);
113 let height = (((max.y - min.y) / resolution).ceil() as usize).max(1);
114 Self {
115 config: OccupancyConfig {
116 resolution,
117 ..config
118 },
119 origin: min,
120 width,
121 height,
122 log_odds: vec![0.0; width * height],
123 }
124 }
125
126 pub fn from_scans<'a>(
129 scans: impl IntoIterator<Item = (Pose2D, &'a [Vector2<f64>])> + Clone,
130 margin: f64,
131 config: OccupancyConfig,
132 ) -> Self {
133 let mut min = Vector2::new(f64::INFINITY, f64::INFINITY);
134 let mut max = Vector2::new(f64::NEG_INFINITY, f64::NEG_INFINITY);
135 for (pose, scan) in scans.clone() {
136 let origin = Vector2::new(pose.x, pose.y);
137 min = min.inf(&origin);
138 max = max.sup(&origin);
139 for point in transform_scan_to_world(scan, pose) {
140 min = min.inf(&point);
141 max = max.sup(&point);
142 }
143 }
144 if !min.x.is_finite() {
145 min = Vector2::zeros();
146 max = Vector2::zeros();
147 }
148 let margin = Vector2::new(margin, margin);
149 let mut grid = Self::new(min - margin, max + margin, config);
150 for (pose, scan) in scans {
151 grid.insert_scan(pose, scan);
152 }
153 grid
154 }
155
156 pub fn resolution(&self) -> f64 {
158 self.config.resolution
159 }
160
161 fn diagonal(&self) -> f64 {
163 (self.width as f64).hypot(self.height as f64) * self.config.resolution
164 }
165
166 pub fn size(&self) -> (usize, usize) {
168 (self.width, self.height)
169 }
170
171 pub fn origin(&self) -> Vector2<f64> {
173 self.origin
174 }
175
176 pub fn cell_of(&self, point: Vector2<f64>) -> Option<(usize, usize)> {
178 let x = ((point.x - self.origin.x) / self.config.resolution).floor();
179 let y = ((point.y - self.origin.y) / self.config.resolution).floor();
180 (x >= 0.0 && y >= 0.0 && (x as usize) < self.width && (y as usize) < self.height)
181 .then_some((x as usize, y as usize))
182 }
183
184 pub fn cell_center(&self, x: usize, y: usize) -> Vector2<f64> {
186 self.origin
187 + Vector2::new(
188 (x as f64 + 0.5) * self.config.resolution,
189 (y as f64 + 0.5) * self.config.resolution,
190 )
191 }
192
193 pub fn log_odds(&self, x: usize, y: usize) -> f32 {
195 self.log_odds[y * self.width + x]
196 }
197
198 pub fn state(&self, x: usize, y: usize) -> CellState {
200 let value = self.log_odds[y * self.width + x];
201 if value > self.config.occupied_threshold {
202 CellState::Occupied
203 } else if value < self.config.free_threshold {
204 CellState::Free
205 } else {
206 CellState::Unknown
207 }
208 }
209
210 pub fn state_at(&self, point: Vector2<f64>) -> CellState {
212 self.cell_of(point)
213 .map_or(CellState::Unknown, |(x, y)| self.state(x, y))
214 }
215
216 pub fn occupied_points(&self) -> Vec<Vector2<f64>> {
218 (0..self.height)
219 .flat_map(|y| (0..self.width).map(move |x| (x, y)))
220 .filter(|&(x, y)| self.state(x, y) == CellState::Occupied)
221 .map(|(x, y)| self.cell_center(x, y))
222 .collect()
223 }
224
225 pub fn insert_scan(&mut self, pose: Pose2D, scan_body: &[Vector2<f64>]) {
229 self.insert_scan_observed(pose, scan_body, |_| {});
230 }
231
232 pub fn insert_scan_with_trace(
236 &mut self,
237 pose: Pose2D,
238 scan_body: &[Vector2<f64>],
239 ) -> Vec<OccupancyCellUpdate> {
240 let mut trace = Vec::new();
241 self.insert_scan_observed(pose, scan_body, |update| trace.push(update));
242 trace
243 }
244
245 fn insert_scan_observed(
246 &mut self,
247 pose: Pose2D,
248 scan_body: &[Vector2<f64>],
249 observe: impl FnMut(OccupancyCellUpdate),
250 ) {
251 let Some(start) = self.cell_of(Vector2::new(pose.x, pose.y)) else {
252 return;
253 };
254 let mut update = ScanUpdate::new(self.width, self.height);
255 for point in transform_scan_to_world(scan_body, pose) {
256 let Some(end) = self.cell_of(point) else {
257 continue;
258 };
259 self.walk(&mut update, start, end);
260 update.mark(end, HIT);
261 }
262 self.apply_observed(update, observe);
263 }
264
265 pub fn insert_ranges(&mut self, pose: Pose2D, ranges: &[f64], max_range: f64) {
273 let Some(start) = self.cell_of(Vector2::new(pose.x, pose.y)) else {
274 return;
275 };
276 let max_range = max_range.min(self.diagonal());
279 let count = ranges.len();
280 let spacing = std::f64::consts::TAU / count.max(1) as f64;
281 let sub_rays =
284 ((2.0 * max_range * spacing / self.config.resolution).ceil() as usize).max(1);
285 let mut update = ScanUpdate::new(self.width, self.height);
286 for (beam, &range) in ranges.iter().enumerate() {
287 if range.is_nan() {
288 continue;
289 }
290 let hit = range.is_finite() && range <= max_range;
291 let reach = if hit { range } else { max_range };
292 let center = pose.yaw + beam_angle(beam, count);
293 for sub in 0..sub_rays {
294 let angle = center + spacing * ((sub as f64 + 0.5) / sub_rays as f64 - 0.5);
296 let main = sub == sub_rays / 2;
297 let length = if hit && !main {
300 (reach - self.config.resolution).max(0.0)
301 } else {
302 reach
303 };
304 let Some(end) = self.last_cell_along(pose, angle, length) else {
305 continue;
306 };
307 let ends_inside = self.cell_of(Vector2::new(
308 pose.x + length * angle.cos(),
309 pose.y + length * angle.sin(),
310 )) == Some(end);
311 self.walk(&mut update, start, end);
312 update.mark(
313 end,
314 if hit && main && ends_inside {
315 HIT
316 } else {
317 MISS
318 },
319 );
320 }
321 }
322 self.apply(update);
323 }
324
325 fn last_cell_along(&self, pose: Pose2D, angle: f64, length: f64) -> Option<(usize, usize)> {
327 let end = Vector2::new(pose.x + length * angle.cos(), pose.y + length * angle.sin());
328 if let Some(cell) = self.cell_of(end) {
329 return Some(cell);
330 }
331 let steps = (length / self.config.resolution).ceil() as usize;
332 (0..=steps).rev().find_map(|step| {
333 let t = length * step as f64 / steps.max(1) as f64;
334 self.cell_of(Vector2::new(
335 pose.x + t * angle.cos(),
336 pose.y + t * angle.sin(),
337 ))
338 })
339 }
340
341 fn apply(&mut self, update: ScanUpdate) {
343 self.apply_observed(update, |_| {});
344 }
345
346 fn apply_observed(&mut self, update: ScanUpdate, mut observe: impl FnMut(OccupancyCellUpdate)) {
347 let clamp = self.config.clamp;
348 for index in update.touched {
349 let delta = if update.marks[index] == HIT {
350 self.config.hit
351 } else {
352 self.config.miss
353 };
354 let cell = &mut self.log_odds[index];
355 let before = *cell;
356 *cell = (*cell + delta).clamp(-clamp, clamp);
357 observe(OccupancyCellUpdate {
358 x: index % self.width,
359 y: index / self.width,
360 evidence: if update.marks[index] == HIT {
361 OccupancyEvidence::Hit
362 } else {
363 OccupancyEvidence::Miss
364 },
365 before,
366 after: *cell,
367 });
368 }
369 }
370
371 fn walk(&self, update: &mut ScanUpdate, start: (usize, usize), end: (usize, usize)) {
373 let (mut x, mut y) = (start.0 as i64, start.1 as i64);
374 let (x1, y1) = (end.0 as i64, end.1 as i64);
375 let (dx, dy) = ((x1 - x).abs(), -(y1 - y).abs());
376 let (sx, sy) = (if x < x1 { 1 } else { -1 }, if y < y1 { 1 } else { -1 });
377 let mut error = dx + dy;
378 while (x, y) != (x1, y1) {
379 update.mark((x as usize, y as usize), MISS);
380 let doubled = 2 * error;
381 if doubled >= dy {
382 error += dy;
383 x += sx;
384 }
385 if doubled <= dx {
386 error += dx;
387 y += sy;
388 }
389 }
390 }
391
392 pub fn distance_field(&self, max_distance: f64) -> Vec<f32> {
396 let resolution = self.config.resolution as f32;
397 let cap = max_distance as f32;
398 let mut field: Vec<f32> = (0..self.width * self.height)
399 .map(|index| {
400 let (x, y) = (index % self.width, index / self.width);
401 if self.state(x, y) == CellState::Occupied {
402 0.0
403 } else {
404 cap
405 }
406 })
407 .collect();
408 let straight = resolution;
409 let diagonal = resolution * std::f32::consts::SQRT_2;
410 let (w, h) = (self.width as i64, self.height as i64);
411 let relax = |field: &mut [f32], x: i64, y: i64, offsets: &[(i64, i64, f32)]| {
412 let index = (y * w + x) as usize;
413 for &(dx, dy, cost) in offsets {
414 let (nx, ny) = (x + dx, y + dy);
415 if nx >= 0 && ny >= 0 && nx < w && ny < h {
416 let candidate = field[(ny * w + nx) as usize] + cost;
417 if candidate < field[index] {
418 field[index] = candidate;
419 }
420 }
421 }
422 };
423 let forward = [
424 (-1, 0, straight),
425 (0, -1, straight),
426 (-1, -1, diagonal),
427 (1, -1, diagonal),
428 ];
429 let backward = [
430 (1, 0, straight),
431 (0, 1, straight),
432 (1, 1, diagonal),
433 (-1, 1, diagonal),
434 ];
435 for y in 0..h {
436 for x in 0..w {
437 relax(&mut field, x, y, &forward);
438 }
439 }
440 for y in (0..h).rev() {
441 for x in (0..w).rev() {
442 relax(&mut field, x, y, &backward);
443 }
444 }
445 for value in &mut field {
446 *value = value.min(cap);
447 }
448 field
449 }
450}
451
452#[cfg(test)]
453mod tests {
454 use super::*;
455 use crate::scan_to_map::{ranges_to_points, ray_cast_ranges, LineSegment};
456
457 fn room() -> Vec<LineSegment> {
458 let c = [
459 Vector2::new(-3.0, -2.0),
460 Vector2::new(3.0, -2.0),
461 Vector2::new(3.0, 2.0),
462 Vector2::new(-3.0, 2.0),
463 ];
464 (0..4)
465 .map(|i| LineSegment::new(c[i], c[(i + 1) % 4]))
466 .collect()
467 }
468
469 fn room_grid() -> OccupancyGrid {
470 let pose = Pose2D::new(0.0, 0.0, 0.3);
471 let scan = ranges_to_points(&ray_cast_ranges(pose, &room(), 360, 10.0));
472 OccupancyGrid::from_scans([(pose, scan.as_slice())], 0.5, OccupancyConfig::default())
473 }
474
475 #[test]
476 fn sparse_trace_matches_legacy_cells_transforms_and_clamps() {
477 let mut traced = OccupancyGrid::new(
478 Vector2::zeros(),
479 Vector2::new(8.0, 8.0),
480 OccupancyConfig {
481 resolution: 0.5,
482 ..OccupancyConfig::default()
483 },
484 );
485 let mut legacy = traced.clone();
486 let pose = Pose2D::new(2.25, 2.25, std::f64::consts::FRAC_PI_2);
487 let scan = [
488 Vector2::new(3.0, 0.0),
489 Vector2::new(2.0, -1.0),
490 Vector2::new(99.0, 0.0),
491 ];
492 for _ in 0..20 {
493 let before = traced.log_odds.clone();
494 let updates = traced.insert_scan_with_trace(pose, &scan);
495 legacy.insert_scan(pose, &scan);
496 assert_eq!(traced.log_odds, legacy.log_odds);
497 let mut seen = std::collections::BTreeSet::new();
498 for u in updates {
499 let index = u.y * 16 + u.x;
500 assert!(seen.insert(index));
501 assert_eq!(u.before, before[index]);
502 assert_eq!(u.after, traced.log_odds[index]);
503 let delta = if u.evidence == OccupancyEvidence::Hit {
504 0.9
505 } else {
506 -0.4
507 };
508 assert_eq!(u.after, (u.before + delta).clamp(-5.0, 5.0));
509 }
510 for (index, value) in before.iter().enumerate() {
511 if !seen.contains(&index) {
512 assert_eq!(*value, traced.log_odds[index]);
513 }
514 }
515 }
516 assert_eq!(traced.log_odds(4, 10), 5.0);
517 let before = traced.log_odds.clone();
518 assert!(traced.insert_scan_with_trace(pose, &[]).is_empty());
519 assert!(traced
520 .insert_scan_with_trace(Pose2D::new(-1.0, 0.0, 0.0), &scan)
521 .is_empty());
522 assert_eq!(before, traced.log_odds);
523 }
524
525 #[test]
526 fn sparse_trace_resolves_hits_once_even_with_duplicates_and_grazing_beams() {
527 let mut grid = OccupancyGrid::new(
528 Vector2::zeros(),
529 Vector2::new(12.0, 8.0),
530 OccupancyConfig {
531 resolution: 0.5,
532 ..OccupancyConfig::default()
533 },
534 );
535 let pose = Pose2D::new(2.25, 4.25, 0.0);
536 let scan = [Vector2::new(4.0, 0.0), Vector2::new(7.0, 0.2)];
537 let mut duplicated = grid.clone();
538 for step in 1..=10 {
539 let trace = grid.insert_scan_with_trace(pose, &scan);
540 let other =
541 duplicated.insert_scan_with_trace(pose, &[scan[0], scan[1], scan[0], scan[1]]);
542 assert_eq!(trace, other);
543 assert_eq!(grid.log_odds, duplicated.log_odds);
544 let hits: Vec<_> = trace.iter().filter(|u| (u.x, u.y) == (12, 8)).collect();
545 assert_eq!(hits.len(), 1);
546 assert_eq!(hits[0].evidence, OccupancyEvidence::Hit);
547 assert!(hits[0].after >= hits[0].before);
548 if step >= 6 {
549 assert_eq!(hits[0].after, 5.0);
550 }
551 }
552 }
553
554 #[test]
555 fn walls_are_occupied_inside_is_free_outside_is_unknown() {
556 let grid = room_grid();
557 assert_eq!(grid.state_at(Vector2::new(3.0, 0.5)), CellState::Occupied);
558 assert_eq!(grid.state_at(Vector2::new(-1.0, 2.0)), CellState::Occupied);
559 assert_eq!(grid.state_at(Vector2::new(1.0, 0.5)), CellState::Free);
560 assert_eq!(grid.state_at(Vector2::new(0.0, 0.0)), CellState::Free);
561 assert_eq!(grid.state_at(Vector2::new(3.4, 0.0)), CellState::Unknown);
562 assert_eq!(grid.state_at(Vector2::new(50.0, 0.0)), CellState::Unknown);
563 }
564
565 #[test]
566 fn occupied_points_lie_on_the_walls() {
567 let grid = room_grid();
568 let points = grid.occupied_points();
569 assert!(points.len() > 100);
570 for point in points {
571 let to_wall = (3.0 - point.x.abs()).min(2.0 - point.y.abs()).abs();
572 assert!(to_wall < 0.15, "{point:?}");
573 }
574 }
575
576 #[test]
577 fn beams_without_a_return_clear_open_space() {
578 let wall = [LineSegment::new(
580 Vector2::new(3.0, -1.0),
581 Vector2::new(3.0, 1.0),
582 )];
583 let pose = Pose2D::new(0.0, 0.0, 0.0);
584 let ranges = ray_cast_ranges(pose, &wall, 360, 5.0);
585 let mut grid = OccupancyGrid::new(
586 Vector2::new(-6.0, -6.0),
587 Vector2::new(6.0, 6.0),
588 OccupancyConfig::default(),
589 );
590 grid.insert_ranges(pose, &ranges, 5.0);
591 let wall_cells = [Vector2::new(2.95, 0.35), Vector2::new(3.05, 0.35)];
593 assert!(wall_cells
594 .iter()
595 .any(|point| grid.state_at(*point) == CellState::Occupied));
596 assert_eq!(grid.state_at(Vector2::new(-4.0, 0.0)), CellState::Free);
597 assert_eq!(grid.state_at(Vector2::new(0.0, 4.5)), CellState::Free);
598 assert_eq!(grid.state_at(Vector2::new(-5.5, 0.0)), CellState::Unknown);
599 assert_eq!(grid.state_at(Vector2::new(4.0, 0.0)), CellState::Unknown);
600 }
601
602 #[test]
603 fn an_unlimited_max_range_is_bounded_by_the_grid() {
604 let pose = Pose2D::new(0.0, 0.0, 0.0);
605 let mut grid = OccupancyGrid::new(
606 Vector2::new(-6.0, -6.0),
607 Vector2::new(6.0, 6.0),
608 OccupancyConfig::default(),
609 );
610 grid.insert_ranges(pose, &[f64::INFINITY; 90], f64::INFINITY);
612 assert_eq!(grid.state_at(Vector2::new(-5.5, 0.0)), CellState::Free);
613 assert_eq!(grid.state_at(Vector2::new(5.5, 5.5)), CellState::Free);
614 }
615
616 #[test]
617 fn distance_field_grows_away_from_walls() {
618 let grid = room_grid();
619 let field = grid.distance_field(2.0);
620 let (w, _) = grid.size();
621 let at = |p: Vector2<f64>| {
622 let (x, y) = grid.cell_of(p).unwrap();
623 field[y * w + x]
624 };
625 assert_eq!(at(Vector2::new(3.0, 0.5)), 0.0);
626 let one_meter = at(Vector2::new(2.0, 0.0));
627 assert!((one_meter - 1.0).abs() < 0.15, "{one_meter}");
628 assert!(at(Vector2::new(0.0, 0.0)) > 1.8);
629 }
630}