Skip to main content

rust_robotics_mapping/
occupancy_grid_map.rs

1//! Occupancy Grid Mapping with log-odds update.
2//!
3//! Implements a 2-D occupancy grid that fuses range sensor scans using
4//! Bayesian log-odds updates and Bresenham ray casting.
5
6/// Configuration parameters for [`OccupancyGridMap`].
7#[derive(Clone, Debug)]
8pub struct OccupancyGridConfig {
9    /// Cell size \[m\].
10    pub resolution: f64,
11    /// Number of cells along the x-axis.
12    pub width: usize,
13    /// Number of cells along the y-axis.
14    pub height: usize,
15    /// Log-odds value assigned to every cell at initialisation.
16    pub prior_log_odds: f64,
17    /// Log-odds increment added when a cell is observed as occupied.
18    pub occupied_log_odds: f64,
19    /// Log-odds decrement added when a cell is observed as free.
20    pub free_log_odds: f64,
21    /// Upper saturation limit for log-odds values.
22    pub max_log_odds: f64,
23    /// Lower saturation limit for log-odds values.
24    pub min_log_odds: f64,
25}
26
27impl Default for OccupancyGridConfig {
28    fn default() -> Self {
29        Self {
30            resolution: 0.5,
31            width: 100,
32            height: 100,
33            prior_log_odds: 0.0,
34            occupied_log_odds: 0.85,
35            free_log_odds: -0.4,
36            max_log_odds: 5.0,
37            min_log_odds: -5.0,
38        }
39    }
40}
41
42/// Occupancy grid map stored as log-odds values.
43///
44/// The grid is indexed as `grid[ix][iy]` where `ix` is the column (x-axis)
45/// and `iy` is the row (y-axis).  World coordinate `(0, 0)` maps to the
46/// centre of the grid.
47pub struct OccupancyGridMap {
48    /// Log-odds grid: `grid[ix][iy]`.
49    pub grid: Vec<Vec<f64>>,
50    /// Configuration used to build this map.
51    pub config: OccupancyGridConfig,
52}
53
54impl OccupancyGridMap {
55    /// Create a new occupancy grid initialised to `config.prior_log_odds`.
56    pub fn new(config: OccupancyGridConfig) -> Self {
57        let grid = vec![vec![config.prior_log_odds; config.height]; config.width];
58        Self { grid, config }
59    }
60
61    /// Update the map from a single 2-D laser scan.
62    ///
63    /// # Parameters
64    /// - `robot_x`, `robot_y` – robot position in world coordinates \[m\].
65    /// - `robot_yaw` – robot heading \[rad\].
66    /// - `scan_ranges` – measured ranges for each beam \[m\].
67    /// - `angle_min` – angle of the first beam relative to the robot heading \[rad\].
68    /// - `angle_increment` – angular step between consecutive beams \[rad\].
69    pub fn update_with_scan(
70        &mut self,
71        robot_x: f64,
72        robot_y: f64,
73        robot_yaw: f64,
74        scan_ranges: &[f64],
75        angle_min: f64,
76        angle_increment: f64,
77    ) {
78        let origin = match self.world_to_grid(robot_x, robot_y) {
79            Some(o) => o,
80            None => return,
81        };
82
83        for (i, &range) in scan_ranges.iter().enumerate() {
84            if range <= 0.0 || !range.is_finite() {
85                continue;
86            }
87
88            let angle = robot_yaw + angle_min + i as f64 * angle_increment;
89            let end_x = robot_x + range * angle.cos();
90            let end_y = robot_y + range * angle.sin();
91
92            let end_cell = self.world_to_grid(end_x, end_y);
93
94            let end_ix = end_cell.map(|(ix, _)| ix as i32).unwrap_or({
95                // Clamp endpoint to grid boundary along the ray direction.
96                let ix = (end_x / self.config.resolution + self.config.width as f64 / 2.0).round()
97                    as i32;
98                ix.clamp(0, self.config.width as i32 - 1)
99            });
100            let end_iy = end_cell.map(|(_, iy)| iy as i32).unwrap_or({
101                let iy = (end_y / self.config.resolution + self.config.height as f64 / 2.0).round()
102                    as i32;
103                iy.clamp(0, self.config.height as i32 - 1)
104            });
105
106            // Collect cells along the ray (excluding endpoint).
107            let ray_cells = bresenham_line(origin.0 as i32, origin.1 as i32, end_ix, end_iy);
108
109            let free_delta = self.config.free_log_odds;
110            let occ_delta = self.config.occupied_log_odds;
111            let min_l = self.config.min_log_odds;
112            let max_l = self.config.max_log_odds;
113            let w = self.config.width;
114            let h = self.config.height;
115
116            // Mark free cells along the ray (all cells except the last).
117            let ray_len = ray_cells.len();
118            for &(cx, cy) in ray_cells.iter().take(ray_len.saturating_sub(1)) {
119                if cx >= 0 && cx < w as i32 && cy >= 0 && cy < h as i32 {
120                    let l = &mut self.grid[cx as usize][cy as usize];
121                    *l = (*l + free_delta).clamp(min_l, max_l);
122                }
123            }
124
125            // Mark endpoint as occupied (only if it falls inside the grid).
126            if let Some((ex, ey)) = end_cell {
127                let l = &mut self.grid[ex][ey];
128                *l = (*l + occ_delta).clamp(min_l, max_l);
129            }
130        }
131    }
132
133    /// Convert a log-odds value at `(ix, iy)` to an occupancy probability.
134    ///
135    /// Returns a value in `[0, 1]` via `1 - 1 / (1 + exp(l))`.
136    pub fn get_probability(&self, ix: usize, iy: usize) -> f64 {
137        let l = self.grid[ix][iy];
138        1.0 - 1.0 / (1.0 + l.exp())
139    }
140
141    /// Convert world coordinates to grid indices.
142    ///
143    /// Returns `None` when the point lies outside the grid.
144    pub fn world_to_grid(&self, x: f64, y: f64) -> Option<(usize, usize)> {
145        let ix = (x / self.config.resolution + self.config.width as f64 / 2.0).floor() as i32;
146        let iy = (y / self.config.resolution + self.config.height as f64 / 2.0).floor() as i32;
147
148        if ix >= 0 && ix < self.config.width as i32 && iy >= 0 && iy < self.config.height as i32 {
149            Some((ix as usize, iy as usize))
150        } else {
151            None
152        }
153    }
154
155    /// Return `true` when the occupancy probability at `(ix, iy)` exceeds
156    /// `threshold`.
157    pub fn is_occupied(&self, ix: usize, iy: usize, threshold: f64) -> bool {
158        self.get_probability(ix, iy) > threshold
159    }
160}
161
162/// Bresenham's line algorithm returning all grid cells from `(x0, y0)` to
163/// `(x1, y1)`, inclusive of both endpoints.
164fn bresenham_line(x0: i32, y0: i32, x1: i32, y1: i32) -> Vec<(i32, i32)> {
165    let mut cells = Vec::new();
166
167    let dx = (x1 - x0).abs();
168    let dy = (y1 - y0).abs();
169    let sx = if x0 < x1 { 1_i32 } else { -1 };
170    let sy = if y0 < y1 { 1_i32 } else { -1 };
171
172    let mut x = x0;
173    let mut y = y0;
174    let mut err = dx - dy;
175
176    loop {
177        cells.push((x, y));
178        if x == x1 && y == y1 {
179            break;
180        }
181        let e2 = 2 * err;
182        if e2 > -dy {
183            err -= dy;
184            x += sx;
185        }
186        if e2 < dx {
187            err += dx;
188            y += sy;
189        }
190    }
191
192    cells
193}
194
195#[cfg(test)]
196mod tests {
197    use super::*;
198
199    /// Prior probability from log-odds 0 should be 0.5.
200    #[test]
201    fn test_initial_grid_is_prior_probability() {
202        let map = OccupancyGridMap::new(OccupancyGridConfig::default());
203        let p = map.get_probability(50, 50);
204        println!("Prior probability at (50,50): {p}");
205        assert!((p - 0.5).abs() < 1e-10, "Expected 0.5 but got {p}");
206    }
207
208    /// After a scan that places an obstacle directly in front of the robot,
209    /// the endpoint cell should have probability > 0.5.
210    #[test]
211    fn test_update_marks_occupied_cell() {
212        let mut map = OccupancyGridMap::new(OccupancyGridConfig::default());
213        // Robot at origin, single beam pointing along +x at 5 m.
214        map.update_with_scan(0.0, 0.0, 0.0, &[5.0], 0.0, 0.0);
215
216        let (ex, ey) = map.world_to_grid(5.0, 0.0).unwrap();
217        let p = map.get_probability(ex, ey);
218        println!("Occupied cell probability at ({ex},{ey}): {p}");
219        assert!(p > 0.5, "Expected occupied (p > 0.5) but got {p}");
220    }
221
222    /// Cells between the robot and the obstacle should be updated as free
223    /// (probability < 0.5) after several identical scans.
224    #[test]
225    fn test_update_marks_free_cells_along_ray() {
226        let mut map = OccupancyGridMap::new(OccupancyGridConfig::default());
227        // Apply the same scan multiple times to overcome the prior.
228        for _ in 0..5 {
229            map.update_with_scan(0.0, 0.0, 0.0, &[5.0], 0.0, 0.0);
230        }
231
232        // A cell at 2 m along the ray should be free.
233        let (fx, fy) = map.world_to_grid(2.0, 0.0).unwrap();
234        let p = map.get_probability(fx, fy);
235        println!("Free cell probability at ({fx},{fy}): {p}");
236        assert!(p < 0.5, "Expected free (p < 0.5) but got {p}");
237    }
238
239    /// `world_to_grid` should round-trip correctly for a simple coordinate.
240    #[test]
241    fn test_world_to_grid_conversion() {
242        let cfg = OccupancyGridConfig::default();
243        let map = OccupancyGridMap::new(cfg.clone());
244
245        // Origin should map to the centre of the grid.
246        let (cx, cy) = map.world_to_grid(0.0, 0.0).unwrap();
247        println!("Grid centre: ({cx}, {cy})");
248        assert_eq!(cx, cfg.width / 2);
249        assert_eq!(cy, cfg.height / 2);
250
251        // A point outside the grid should return None.
252        let outside = map.world_to_grid(1_000.0, 1_000.0);
253        assert!(outside.is_none());
254    }
255}