Skip to main content

rust_robotics_slam/
lidar_occupancy.rs

1//! Log-odds occupancy grid built from LiDAR scans at known poses, e.g. the
2//! node scans of [`crate::lidar_graph_slam::LidarGraphSlam`] at their
3//! optimized poses.
4//!
5//! Every beam marks the cells it passes through as free and its end cell as
6//! occupied (Bresenham ray tracing). The grid answers occupancy queries for
7//! planning and provides a distance-to-nearest-obstacle field for scan
8//! likelihoods ([`OccupancyGrid::distance_field`]).
9
10use 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
18/// The cells one scan observed; a hit overrides a miss.
19struct 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/// Log-odds increments and thresholds.
44#[derive(Debug, Clone, Copy, PartialEq)]
45pub struct OccupancyConfig {
46    /// Cell edge \[m\].
47    pub resolution: f64,
48    /// Log-odds added to the end cell of a beam.
49    pub hit: f32,
50    /// Log-odds added to cells a beam passes through (negative).
51    pub miss: f32,
52    /// Log-odds clamp, so a cell can still change its mind.
53    pub clamp: f32,
54    /// Cells above this log-odds are occupied.
55    pub occupied_threshold: f32,
56    /// Cells below this log-odds are free.
57    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/// State of one cell.
74#[derive(Debug, Clone, Copy, PartialEq, Eq)]
75pub enum CellState {
76    Unknown,
77    Free,
78    Occupied,
79}
80
81/// Evidence applied to a cell after resolving all beams in one scan.
82#[derive(Debug, Clone, Copy, PartialEq, Eq)]
83pub enum OccupancyEvidence {
84    Hit,
85    Miss,
86}
87
88/// One actual log-odds update. Each cell appears at most once per scan.
89#[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/// A fixed-size log-odds occupancy grid over an axis-aligned world rectangle.
99#[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    /// Creates an all-unknown grid covering `[min, max]` (world, meters).
110    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    /// Builds a grid from body-frame scans taken at `poses`, sized to fit
127    /// every pose and scan point plus `margin` meters.
128    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    /// Cell edge \[m\].
157    pub fn resolution(&self) -> f64 {
158        self.config.resolution
159    }
160
161    /// Length of the grid's diagonal \[m\].
162    fn diagonal(&self) -> f64 {
163        (self.width as f64).hypot(self.height as f64) * self.config.resolution
164    }
165
166    /// `(width, height)` in cells.
167    pub fn size(&self) -> (usize, usize) {
168        (self.width, self.height)
169    }
170
171    /// World position of the grid corner (cell `(0, 0)`'s lower-left corner).
172    pub fn origin(&self) -> Vector2<f64> {
173        self.origin
174    }
175
176    /// Cell containing `point`, if inside the grid.
177    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    /// World center of cell `(x, y)`.
185    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    /// Log-odds of cell `(x, y)` being occupied (0 = unknown).
194    pub fn log_odds(&self, x: usize, y: usize) -> f32 {
195        self.log_odds[y * self.width + x]
196    }
197
198    /// State of cell `(x, y)`.
199    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    /// State of the cell containing `point` (`Unknown` outside the grid).
211    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    /// World centers of every occupied cell.
217    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    /// Integrates one body-frame scan taken at `pose`. Each cell is updated
226    /// at most once per scan, and a hit wins over a miss, so a wall cell is
227    /// not erased by neighboring beams that graze it.
228    pub fn insert_scan(&mut self, pose: Pose2D, scan_body: &[Vector2<f64>]) {
229        self.insert_scan_observed(pose, scan_body, |_| {});
230    }
231
232    /// Integrates a sparse return scan and records the updates actually
233    /// applied, including clamping. Missing observations produce no updates;
234    /// use [`Self::insert_ranges`] for a full range scan with no-return beams.
235    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    /// Integrates a full 360° range scan taken at `pose` (beam order of
266    /// [`crate::scan_to_map::ray_cast_ranges`]). Beams without a return
267    /// (non-finite or beyond `max_range`) still clear the cells out to
268    /// `max_range`, so open space becomes known free instead of unknown (NaN
269    /// beams are skipped entirely, e.g. ones that hit a moving object), and
270    /// free-only sub-rays fill the angular gaps between beams so far cells are
271    /// not left as unknown speckles (which would look like frontiers).
272    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        // No ray can usefully reach past the grid; this also bounds the
277        // sub-ray count for an unlimited (infinite) `max_range`.
278        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        // Half-cell ray spacing at max range: 8-connected lines one cell
282        // apart can leave moiré gaps between them.
283        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                // Sub-rays span the half-open sector around the beam.
295                let angle = center + spacing * ((sub as f64 + 0.5) / sub_rays as f64 - 0.5);
296                let main = sub == sub_rays / 2;
297                // Off-center sub-rays stop a cell short of a wall they may
298                // not actually reach, and never mark it occupied.
299                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    /// The last grid cell along a ray of `length` from `pose` at `angle`.
326    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    /// Applies one scan's marks: +hit on hit cells, +miss on the others.
342    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    /// Marks every cell from `start` up to (not including) `end` as free.
372    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    /// Distance \[m\] from every cell center to the nearest occupied cell
393    /// center (two-pass chamfer approximation, exact along axes and
394    /// diagonals), capped at `max_distance`. Row-major, `width × height`.
395    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        // Only the wall at x = 3 is in range; every other beam has no return.
579        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        // The wall lies on a cell boundary; its hits land on either side.
592        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        // No returns at all, no range limit: clears the whole grid, and returns.
611        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}