Skip to main content

rust_robotics_mapping/
ray_casting_grid_map.rs

1//! Ray Casting Grid Map for occupancy mapping.
2//!
3//! Based on the work by Atsushi Sakai (@Atsushi_twi) and Ryohei Sasaki (@rsasaki0109).
4
5use nalgebra::DMatrix;
6use std::collections::HashMap;
7use std::f64::consts::PI;
8use std::fs::File;
9use std::io::Write;
10
11/// Grid map extension area \[m\].
12pub const EXTEND_AREA: f64 = 10.0;
13/// Default grid resolution \[m\].
14pub const XY_RESOLUTION: f64 = 0.25;
15/// Default yaw resolution \[rad\] (10 degrees).
16pub const YAW_RESOLUTION: f64 = PI / 18.0;
17
18/// Cell information for ray casting.
19#[derive(Clone)]
20struct CellInfo {
21    ix: usize,
22    iy: usize,
23    distance: f64,
24}
25
26/// Precomputed data for ray casting.
27struct PrecastDB {
28    /// Maps angle_id -> Vec<CellInfo> sorted by distance.
29    cells_by_angle: HashMap<i32, Vec<CellInfo>>,
30}
31
32/// Ray Casting Grid Map for occupancy mapping.
33pub struct RayCastingGridMap {
34    pub data: DMatrix<f64>,
35    pub min_x: f64,
36    pub min_y: f64,
37    pub max_x: f64,
38    pub max_y: f64,
39    pub resolution: f64,
40    pub x_width: usize,
41    pub y_width: usize,
42    precast_db: PrecastDB,
43    origin_ix: usize,
44    origin_iy: usize,
45}
46
47impl RayCastingGridMap {
48    /// Create a new Ray Casting Grid Map.
49    pub fn new(ox: &[f64], oy: &[f64], resolution: f64, yaw_resolution: f64) -> Self {
50        let min_x = ox.iter().cloned().fold(f64::INFINITY, f64::min) - EXTEND_AREA;
51        let min_y = oy.iter().cloned().fold(f64::INFINITY, f64::min) - EXTEND_AREA;
52        let max_x = ox.iter().cloned().fold(f64::NEG_INFINITY, f64::max) + EXTEND_AREA;
53        let max_y = oy.iter().cloned().fold(f64::NEG_INFINITY, f64::max) + EXTEND_AREA;
54
55        let x_width = ((max_x - min_x) / resolution).round() as usize;
56        let y_width = ((max_y - min_y) / resolution).round() as usize;
57
58        let origin_x = (min_x + max_x) / 2.0;
59        let origin_y = (min_y + max_y) / 2.0;
60        let origin_ix = ((origin_x - min_x) / resolution).round() as usize;
61        let origin_iy = ((origin_y - min_y) / resolution).round() as usize;
62
63        let data = DMatrix::from_element(x_width, y_width, 0.0);
64
65        let precast_db = Self::precompute_db(
66            x_width,
67            y_width,
68            origin_ix,
69            origin_iy,
70            resolution,
71            yaw_resolution,
72        );
73
74        let mut grid_map = RayCastingGridMap {
75            data,
76            min_x,
77            min_y,
78            max_x,
79            max_y,
80            resolution,
81            x_width,
82            y_width,
83            precast_db,
84            origin_ix,
85            origin_iy,
86        };
87
88        for (obs_x, obs_y) in ox.iter().zip(oy.iter()) {
89            grid_map.ray_casting(*obs_x, *obs_y, yaw_resolution);
90        }
91
92        grid_map
93    }
94
95    /// Precompute database of cells organized by angle.
96    fn precompute_db(
97        x_width: usize,
98        y_width: usize,
99        origin_ix: usize,
100        origin_iy: usize,
101        resolution: f64,
102        yaw_resolution: f64,
103    ) -> PrecastDB {
104        let mut cells_by_angle: HashMap<i32, Vec<CellInfo>> = HashMap::new();
105
106        for ix in 0..x_width {
107            for iy in 0..y_width {
108                let dx = ix as f64 - origin_ix as f64;
109                let dy = iy as f64 - origin_iy as f64;
110
111                let distance = (dx * dx + dy * dy).sqrt() * resolution;
112                let angle = atan_zero_to_twopi(dy, dx);
113                let angle_id = (angle / yaw_resolution).floor() as i32;
114
115                let cell_info = CellInfo { ix, iy, distance };
116
117                cells_by_angle.entry(angle_id).or_default().push(cell_info);
118            }
119        }
120
121        for cells in cells_by_angle.values_mut() {
122            cells.sort_by(|a, b| a.distance.partial_cmp(&b.distance).unwrap());
123        }
124
125        PrecastDB { cells_by_angle }
126    }
127
128    /// Perform ray casting for a single obstacle.
129    fn ray_casting(&mut self, obs_x: f64, obs_y: f64, yaw_resolution: f64) {
130        let obs_ix = ((obs_x - self.min_x) / self.resolution).round() as i32;
131        let obs_iy = ((obs_y - self.min_y) / self.resolution).round() as i32;
132
133        if obs_ix < 0
134            || obs_ix >= self.x_width as i32
135            || obs_iy < 0
136            || obs_iy >= self.y_width as i32
137        {
138            return;
139        }
140
141        let dx = obs_ix as f64 - self.origin_ix as f64;
142        let dy = obs_iy as f64 - self.origin_iy as f64;
143        let obs_angle = atan_zero_to_twopi(dy, dx);
144        let obs_distance = (dx * dx + dy * dy).sqrt() * self.resolution;
145        let angle_id = (obs_angle / yaw_resolution).floor() as i32;
146
147        if let Some(cells) = self.precast_db.cells_by_angle.get(&angle_id) {
148            for cell in cells {
149                if cell.distance < obs_distance {
150                    self.data[(cell.ix, cell.iy)] = 0.5;
151                } else if (cell.distance - obs_distance).abs() < self.resolution {
152                    self.data[(cell.ix, cell.iy)] = 1.0;
153                    break;
154                }
155            }
156        }
157    }
158
159    /// Get grid index from world coordinates.
160    pub fn get_index(&self, x: f64, y: f64) -> Option<(usize, usize)> {
161        let ix = ((x - self.min_x) / self.resolution).floor() as i32;
162        let iy = ((y - self.min_y) / self.resolution).floor() as i32;
163
164        if ix >= 0 && ix < self.x_width as i32 && iy >= 0 && iy < self.y_width as i32 {
165            Some((ix as usize, iy as usize))
166        } else {
167            None
168        }
169    }
170
171    /// Get occupancy at world coordinates.
172    pub fn get_occupancy(&self, x: f64, y: f64) -> Option<f64> {
173        self.get_index(x, y).map(|(ix, iy)| self.data[(ix, iy)])
174    }
175}
176
177/// Convert atan2 result to 0-2pi range.
178fn atan_zero_to_twopi(y: f64, x: f64) -> f64 {
179    let angle = y.atan2(x);
180    if angle < 0.0 {
181        angle + 2.0 * PI
182    } else {
183        angle
184    }
185}
186
187fn occupancy_style(value: f64) -> (&'static str, f64) {
188    if value >= 0.99 {
189        ("#b91c1c", 0.95)
190    } else if value >= 0.49 {
191        ("#bfdbfe", 0.92)
192    } else {
193        ("#f8fafc", 0.75)
194    }
195}
196
197/// Build an SVG visualization of the ray casting grid map.
198pub fn build_ray_casting_grid_map_svg(
199    grid_map: &RayCastingGridMap,
200    ox: &[f64],
201    oy: &[f64],
202) -> String {
203    let width = 920.0;
204    let height = 700.0;
205    let margin = 72.0;
206    let side_panel_width = 220.0;
207    let available_plot_width = width - 2.0 * margin - side_panel_width;
208    let available_plot_height = height - 2.0 * margin;
209    let x_range = grid_map.max_x - grid_map.min_x;
210    let y_range = grid_map.max_y - grid_map.min_y;
211    let scale = (available_plot_width / x_range)
212        .min(available_plot_height / y_range)
213        .max(1.0);
214    let plot_width = x_range * scale;
215    let plot_height = y_range * scale;
216    let plot_left = margin;
217    let plot_top = margin + (available_plot_height - plot_height) / 2.0;
218    let legend_x = plot_left + plot_width + 36.0;
219    let legend_y = plot_top + 24.0;
220
221    let to_svg_x = |x: f64| plot_left + (x - grid_map.min_x) * scale;
222    let to_svg_y = |y: f64| plot_top + plot_height - (y - grid_map.min_y) * scale;
223    let cell_size = grid_map.resolution * scale;
224    let origin_x = grid_map.min_x + grid_map.origin_ix as f64 * grid_map.resolution;
225    let origin_y = grid_map.min_y + grid_map.origin_iy as f64 * grid_map.resolution;
226
227    let mut svg = String::new();
228    svg.push_str(&format!(
229        "<?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"
230    ));
231    svg.push_str("<rect width='100%' height='100%' fill='white'/>\n");
232    svg.push_str(&format!(
233        "<text x='{:.1}' y='38' text-anchor='middle' font-size='24' font-family='sans-serif' font-weight='700' fill='#111827'>Ray Casting Grid Map</text>\n",
234        width / 2.0
235    ));
236    svg.push_str(&format!(
237        "<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"
238    ));
239
240    let x_tick_start = (grid_map.min_x / 10.0).ceil() as i32;
241    let x_tick_end = (grid_map.max_x / 10.0).floor() as i32;
242    for tick in x_tick_start..=x_tick_end {
243        let x = tick as f64 * 10.0;
244        let sx = to_svg_x(x);
245        svg.push_str(&format!(
246            "<line x1='{sx:.2}' y1='{plot_top:.2}' x2='{sx:.2}' y2='{:.2}' stroke='#ece7dc' stroke-width='1'/>\n",
247            plot_top + plot_height
248        ));
249        svg.push_str(&format!(
250            "<text x='{sx:.2}' y='{:.2}' text-anchor='middle' font-size='14' font-family='sans-serif' fill='#374151'>{x:.0}</text>\n",
251            plot_top + plot_height + 24.0
252        ));
253    }
254
255    let y_tick_start = (grid_map.min_y / 10.0).ceil() as i32;
256    let y_tick_end = (grid_map.max_y / 10.0).floor() as i32;
257    for tick in y_tick_start..=y_tick_end {
258        let y = tick as f64 * 10.0;
259        let sy = to_svg_y(y);
260        svg.push_str(&format!(
261            "<line x1='{plot_left:.2}' y1='{sy:.2}' x2='{:.2}' y2='{sy:.2}' stroke='#ece7dc' stroke-width='1'/>\n",
262            plot_left + plot_width
263        ));
264        svg.push_str(&format!(
265            "<text x='{:.2}' y='{:.2}' text-anchor='end' font-size='14' font-family='sans-serif' fill='#374151'>{y:.0}</text>\n",
266            plot_left - 10.0,
267            sy + 5.0
268        ));
269    }
270
271    for iy in 0..grid_map.y_width {
272        for ix in 0..grid_map.x_width {
273            let value = grid_map.data[(ix, iy)];
274            let x = grid_map.min_x + ix as f64 * grid_map.resolution;
275            let y = grid_map.min_y + iy as f64 * grid_map.resolution;
276            let sx = to_svg_x(x);
277            let sy = to_svg_y(y + grid_map.resolution);
278            let (fill, opacity) = occupancy_style(value);
279            svg.push_str(&format!(
280                "<rect x='{sx:.2}' y='{sy:.2}' width='{cell_size:.2}' height='{cell_size:.2}' fill='{fill}' fill-opacity='{opacity:.3}' stroke='none'/>\n"
281            ));
282        }
283    }
284
285    for (x, y) in ox.iter().zip(oy.iter()) {
286        svg.push_str(&format!(
287            "<circle cx='{:.2}' cy='{:.2}' r='3.8' fill='#111827' stroke='white' stroke-width='1.2'/>\n",
288            to_svg_x(*x),
289            to_svg_y(*y)
290        ));
291    }
292    svg.push_str(&format!(
293        "<circle cx='{:.2}' cy='{:.2}' r='6' fill='#2563eb' stroke='white' stroke-width='2'/>\n",
294        to_svg_x(origin_x),
295        to_svg_y(origin_y)
296    ));
297
298    svg.push_str(&format!(
299        "<rect x='{legend_x:.1}' y='{legend_y:.1}' width='182' height='156' rx='18' fill='white' stroke='#d6d3d1'/>\n"
300    ));
301    let legend_rows = [
302        ("#b91c1c", "Occupied cell"),
303        ("#bfdbfe", "Free space"),
304        ("#f8fafc", "Unknown cell"),
305    ];
306    for (index, (color, label)) in legend_rows.iter().enumerate() {
307        let row_y = legend_y + 28.0 + index as f64 * 26.0;
308        svg.push_str(&format!(
309            "<rect x='{:.1}' y='{:.1}' width='22' height='14' rx='4' fill='{color}' stroke='#cbd5e1'/>\n",
310            legend_x + 14.0,
311            row_y - 10.0
312        ));
313        svg.push_str(&format!(
314            "<text x='{:.1}' y='{:.1}' font-size='16' font-family='sans-serif' fill='#111827'>{label}</text>\n",
315            legend_x + 48.0,
316            row_y + 1.0
317        ));
318    }
319    let obstacle_row_y = legend_y + 112.0;
320    svg.push_str(&format!(
321        "<circle cx='{:.1}' cy='{obstacle_row_y:.1}' r='4.0' fill='#111827' stroke='white' stroke-width='1.2'/>\n",
322        legend_x + 25.0
323    ));
324    svg.push_str(&format!(
325        "<text x='{:.1}' y='{:.1}' font-size='16' font-family='sans-serif' fill='#111827'>Obstacle points</text>\n",
326        legend_x + 48.0,
327        obstacle_row_y + 1.0
328    ));
329    let sensor_row_y = legend_y + 138.0;
330    svg.push_str(&format!(
331        "<circle cx='{:.1}' cy='{sensor_row_y:.1}' r='5.5' fill='#2563eb' stroke='white' stroke-width='1.5'/>\n",
332        legend_x + 25.0
333    ));
334    svg.push_str(&format!(
335        "<text x='{:.1}' y='{:.1}' font-size='16' font-family='sans-serif' fill='#111827'>Sensor origin</text>\n",
336        legend_x + 48.0,
337        sensor_row_y + 1.0
338    ));
339
340    svg.push_str(&format!(
341        "<text x='{:.1}' y='{:.1}' font-size='15' font-family='sans-serif' font-weight='700' fill='#111827'>Occupancy bands</text>\n",
342        legend_x + 18.0,
343        legend_y + 190.0
344    ));
345    svg.push_str(&format!(
346        "<text x='{:.1}' y='{:.1}' font-size='13' font-family='monospace' fill='#374151'>1.0 obstacle</text>\n",
347        legend_x + 18.0,
348        legend_y + 216.0
349    ));
350    svg.push_str(&format!(
351        "<text x='{:.1}' y='{:.1}' font-size='13' font-family='monospace' fill='#374151'>0.5 free</text>\n",
352        legend_x + 18.0,
353        legend_y + 238.0
354    ));
355    svg.push_str(&format!(
356        "<text x='{:.1}' y='{:.1}' font-size='13' font-family='monospace' fill='#374151'>0.0 unknown</text>\n",
357        legend_x + 18.0,
358        legend_y + 260.0
359    ));
360
361    svg.push_str(&format!(
362        "<text x='{:.1}' y='{:.1}' text-anchor='middle' font-size='16' font-family='sans-serif' fill='#111827'>x [m]</text>\n",
363        plot_left + plot_width / 2.0,
364        plot_top + plot_height + 52.0
365    ));
366    svg.push_str(&format!(
367        "<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",
368        plot_left - 48.0,
369        plot_top + plot_height / 2.0,
370        plot_left - 48.0,
371        plot_top + plot_height / 2.0
372    ));
373    svg.push_str("</svg>\n");
374
375    svg
376}
377
378/// Save a ray casting grid map SVG to a file.
379pub fn save_ray_casting_grid_map_svg(
380    filename: &str,
381    grid_map: &RayCastingGridMap,
382    ox: &[f64],
383    oy: &[f64],
384) -> std::io::Result<()> {
385    let svg = build_ray_casting_grid_map_svg(grid_map, ox, oy);
386    let mut file = File::create(filename)?;
387    file.write_all(svg.as_bytes())?;
388    Ok(())
389}
390
391#[cfg(test)]
392mod tests {
393    use super::*;
394
395    #[test]
396    fn test_ray_casting_svg_has_white_background() {
397        let grid_map = RayCastingGridMap::new(&[-1.0, 1.0], &[0.0, 0.0], 1.0, PI / 4.0);
398        let svg = build_ray_casting_grid_map_svg(&grid_map, &[-1.0, 1.0], &[0.0, 0.0]);
399
400        assert!(svg.contains("<rect width='100%' height='100%' fill='white'/>"));
401        assert!(svg.contains("Ray Casting Grid Map"));
402        assert!(svg.contains("Occupied cell"));
403        assert!(svg.contains("Sensor origin"));
404    }
405}