Skip to main content

rust_robotics_mapping/
gaussian_grid_map.rs

1//! Gaussian 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::fs::File;
7use std::io::Write;
8
9/// Grid map extension area \[m\].
10pub const EXTEND_AREA: f64 = 10.0;
11/// Default grid resolution \[m\].
12pub const XY_RESOLUTION: f64 = 0.5;
13/// Default standard deviation for the Gaussian kernel.
14pub const STD: f64 = 5.0;
15
16/// Gaussian Grid Map for occupancy mapping.
17pub struct GaussianGridMap {
18    pub data: DMatrix<f64>,
19    pub min_x: f64,
20    pub min_y: f64,
21    pub max_x: f64,
22    pub max_y: f64,
23    pub resolution: f64,
24    pub x_width: usize,
25    pub y_width: usize,
26}
27
28impl GaussianGridMap {
29    /// Create a new Gaussian Grid Map from obstacle positions.
30    pub fn new(ox: &[f64], oy: &[f64], resolution: f64, std_dev: f64) -> Self {
31        let min_x = ox.iter().cloned().fold(f64::INFINITY, f64::min) - EXTEND_AREA;
32        let min_y = oy.iter().cloned().fold(f64::INFINITY, f64::min) - EXTEND_AREA;
33        let max_x = ox.iter().cloned().fold(f64::NEG_INFINITY, f64::max) + EXTEND_AREA;
34        let max_y = oy.iter().cloned().fold(f64::NEG_INFINITY, f64::max) + EXTEND_AREA;
35
36        let x_width = ((max_x - min_x) / resolution).round() as usize;
37        let y_width = ((max_y - min_y) / resolution).round() as usize;
38
39        let mut data = DMatrix::from_element(x_width, y_width, 0.0);
40
41        for ix in 0..x_width {
42            for iy in 0..y_width {
43                let x = min_x + ix as f64 * resolution;
44                let y = min_y + iy as f64 * resolution;
45
46                let min_dist = ox
47                    .iter()
48                    .zip(oy.iter())
49                    .map(|(ox_i, oy_i)| ((x - ox_i).powi(2) + (y - oy_i).powi(2)).sqrt())
50                    .fold(f64::INFINITY, f64::min);
51
52                let pdf = 1.0 - normal_cdf(min_dist, 0.0, std_dev);
53                data[(ix, iy)] = pdf;
54            }
55        }
56
57        GaussianGridMap {
58            data,
59            min_x,
60            min_y,
61            max_x,
62            max_y,
63            resolution,
64            x_width,
65            y_width,
66        }
67    }
68
69    /// Get grid index from world coordinates.
70    pub fn get_index(&self, x: f64, y: f64) -> Option<(usize, usize)> {
71        let ix = ((x - self.min_x) / self.resolution).floor() as i32;
72        let iy = ((y - self.min_y) / self.resolution).floor() as i32;
73
74        if ix >= 0 && ix < self.x_width as i32 && iy >= 0 && iy < self.y_width as i32 {
75            Some((ix as usize, iy as usize))
76        } else {
77            None
78        }
79    }
80
81    /// Get probability at world coordinates.
82    pub fn get_probability(&self, x: f64, y: f64) -> Option<f64> {
83        self.get_index(x, y).map(|(ix, iy)| self.data[(ix, iy)])
84    }
85
86    /// Get world coordinates from grid index.
87    pub fn get_xy(&self, ix: usize, iy: usize) -> (f64, f64) {
88        let x = self.min_x + ix as f64 * self.resolution;
89        let y = self.min_y + iy as f64 * self.resolution;
90        (x, y)
91    }
92}
93
94/// Standard normal CDF using error function approximation.
95pub fn normal_cdf(x: f64, mean: f64, std_dev: f64) -> f64 {
96    let z = (x - mean) / (std_dev * (2.0_f64).sqrt());
97    0.5 * (1.0 + erf(z))
98}
99
100/// Error function approximation (Horner's method).
101pub fn erf(x: f64) -> f64 {
102    let a1 = 0.254829592;
103    let a2 = -0.284496736;
104    let a3 = 1.421413741;
105    let a4 = -1.453152027;
106    let a5 = 1.061405429;
107    let p = 0.3275911;
108
109    let sign = if x < 0.0 { -1.0 } else { 1.0 };
110    let x = x.abs();
111
112    let t = 1.0 / (1.0 + p * x);
113    let y = 1.0 - (((((a5 * t + a4) * t) + a3) * t + a2) * t + a1) * t * (-x * x).exp();
114
115    sign * y
116}
117
118fn lerp_rgb(start: (u8, u8, u8), end: (u8, u8, u8), t: f64) -> (u8, u8, u8) {
119    let t = t.clamp(0.0, 1.0);
120    let blend = |a: u8, b: u8| -> u8 { (a as f64 + (b as f64 - a as f64) * t).round() as u8 };
121    (
122        blend(start.0, end.0),
123        blend(start.1, end.1),
124        blend(start.2, end.2),
125    )
126}
127
128fn gaussian_heat_style(probability: f64, max_probability: f64) -> (String, f64) {
129    if max_probability <= 0.0 || probability <= 0.0 {
130        return ("#fffaf0".to_string(), 0.0);
131    }
132
133    let t = (probability / max_probability).sqrt().clamp(0.0, 1.0);
134    let rgb = if t < 0.5 {
135        lerp_rgb((255, 250, 240), (191, 219, 254), t * 2.0)
136    } else {
137        lerp_rgb((191, 219, 254), (30, 64, 175), (t - 0.5) * 2.0)
138    };
139
140    (
141        format!("#{:02x}{:02x}{:02x}", rgb.0, rgb.1, rgb.2),
142        0.14 + 0.82 * t,
143    )
144}
145
146/// Build an SVG visualization of the Gaussian grid map.
147pub fn build_gaussian_grid_map_svg(grid_map: &GaussianGridMap, ox: &[f64], oy: &[f64]) -> String {
148    let width = 920.0;
149    let height = 700.0;
150    let margin = 72.0;
151    let side_panel_width = 220.0;
152    let available_plot_width = width - 2.0 * margin - side_panel_width;
153    let available_plot_height = height - 2.0 * margin;
154    let x_range = grid_map.max_x - grid_map.min_x;
155    let y_range = grid_map.max_y - grid_map.min_y;
156    let scale = (available_plot_width / x_range)
157        .min(available_plot_height / y_range)
158        .max(1.0);
159    let plot_width = x_range * scale;
160    let plot_height = y_range * scale;
161    let plot_left = margin;
162    let plot_top = margin + (available_plot_height - plot_height) / 2.0;
163    let legend_x = plot_left + plot_width + 36.0;
164    let legend_y = plot_top + 24.0;
165
166    let to_svg_x = |x: f64| plot_left + (x - grid_map.min_x) * scale;
167    let to_svg_y = |y: f64| plot_top + plot_height - (y - grid_map.min_y) * scale;
168
169    let max_probability = grid_map.data.iter().copied().fold(0.0, f64::max);
170    let cell_size = grid_map.resolution * scale;
171
172    let mut svg = String::new();
173    svg.push_str(&format!(
174        "<?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"
175    ));
176    svg.push_str("<rect width='100%' height='100%' fill='white'/>\n");
177    svg.push_str(&format!(
178        "<text x='{:.1}' y='38' text-anchor='middle' font-size='24' font-family='sans-serif' font-weight='700' fill='#111827'>Gaussian Grid Map</text>\n",
179        width / 2.0
180    ));
181    svg.push_str(&format!(
182        "<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"
183    ));
184
185    let x_tick_start = (grid_map.min_x / 5.0).ceil() as i32;
186    let x_tick_end = (grid_map.max_x / 5.0).floor() as i32;
187    for tick in x_tick_start..=x_tick_end {
188        let x = tick as f64 * 5.0;
189        let sx = to_svg_x(x);
190        svg.push_str(&format!(
191            "<line x1='{sx:.2}' y1='{plot_top:.2}' x2='{sx:.2}' y2='{:.2}' stroke='#ece7dc' stroke-width='1'/>\n",
192            plot_top + plot_height
193        ));
194        svg.push_str(&format!(
195            "<text x='{sx:.2}' y='{:.2}' text-anchor='middle' font-size='14' font-family='sans-serif' fill='#374151'>{x:.0}</text>\n",
196            plot_top + plot_height + 24.0
197        ));
198    }
199
200    let y_tick_start = (grid_map.min_y / 5.0).ceil() as i32;
201    let y_tick_end = (grid_map.max_y / 5.0).floor() as i32;
202    for tick in y_tick_start..=y_tick_end {
203        let y = tick as f64 * 5.0;
204        let sy = to_svg_y(y);
205        svg.push_str(&format!(
206            "<line x1='{plot_left:.2}' y1='{sy:.2}' x2='{:.2}' y2='{sy:.2}' stroke='#ece7dc' stroke-width='1'/>\n",
207            plot_left + plot_width
208        ));
209        svg.push_str(&format!(
210            "<text x='{:.2}' y='{:.2}' text-anchor='end' font-size='14' font-family='sans-serif' fill='#374151'>{y:.0}</text>\n",
211            plot_left - 10.0,
212            sy + 5.0
213        ));
214    }
215
216    for iy in 0..grid_map.y_width {
217        for ix in 0..grid_map.x_width {
218            let probability = grid_map.data[(ix, iy)];
219            if probability <= max_probability * 0.01 {
220                continue;
221            }
222            let x = grid_map.min_x + ix as f64 * grid_map.resolution;
223            let y = grid_map.min_y + iy as f64 * grid_map.resolution;
224            let sx = to_svg_x(x);
225            let sy = to_svg_y(y + grid_map.resolution);
226            let (fill, opacity) = gaussian_heat_style(probability, max_probability);
227            svg.push_str(&format!(
228                "<rect x='{sx:.2}' y='{sy:.2}' width='{cell_size:.2}' height='{cell_size:.2}' fill='{fill}' fill-opacity='{opacity:.3}' stroke='none'/>\n"
229            ));
230        }
231    }
232
233    for (x, y) in ox.iter().zip(oy.iter()) {
234        svg.push_str(&format!(
235            "<circle cx='{:.2}' cy='{:.2}' r='5.5' fill='#111827' stroke='white' stroke-width='1.8'/>\n",
236            to_svg_x(*x),
237            to_svg_y(*y)
238        ));
239    }
240
241    svg.push_str(&format!(
242        "<rect x='{legend_x:.1}' y='{legend_y:.1}' width='182' height='118' rx='18' fill='white' stroke='#d6d3d1'/>\n"
243    ));
244    let swatches = [
245        ("#1e40af", "High probability"),
246        ("#93c5fd", "Medium probability"),
247        ("#fffaf0", "Low probability"),
248    ];
249    for (index, (color, label)) in swatches.iter().enumerate() {
250        let row_y = legend_y + 28.0 + index as f64 * 26.0;
251        svg.push_str(&format!(
252            "<rect x='{:.1}' y='{:.1}' width='22' height='14' rx='4' fill='{color}' stroke='#cbd5e1'/>\n",
253            legend_x + 14.0,
254            row_y - 10.0
255        ));
256        svg.push_str(&format!(
257            "<text x='{:.1}' y='{:.1}' font-size='16' font-family='sans-serif' fill='#111827'>{label}</text>\n",
258            legend_x + 48.0,
259            row_y + 1.0
260        ));
261    }
262    let obstacle_row_y = legend_y + 28.0 + swatches.len() as f64 * 26.0;
263    svg.push_str(&format!(
264        "<circle cx='{:.1}' cy='{obstacle_row_y:.1}' r='5.5' fill='#111827' stroke='white' stroke-width='1.5'/>\n",
265        legend_x + 25.0
266    ));
267    svg.push_str(&format!(
268        "<text x='{:.1}' y='{:.1}' font-size='16' font-family='sans-serif' fill='#111827'>Obstacles</text>\n",
269        legend_x + 48.0,
270        obstacle_row_y + 1.0
271    ));
272
273    let colorbar_x = legend_x + 18.0;
274    let colorbar_y = legend_y + 160.0;
275    let colorbar_height = 250.0;
276    let colorbar_width = 26.0;
277    svg.push_str(&format!(
278        "<text x='{:.1}' y='{:.1}' font-size='16' font-family='sans-serif' font-weight='700' fill='#111827'>Probability</text>\n",
279        legend_x + 18.0,
280        colorbar_y - 14.0
281    ));
282    for step in 0..32 {
283        let t = step as f64 / 31.0;
284        let (fill, _) = gaussian_heat_style(t.max(1e-6), 1.0);
285        let y = colorbar_y + colorbar_height * (1.0 - t) - colorbar_height / 32.0;
286        svg.push_str(&format!(
287            "<rect x='{colorbar_x:.1}' y='{y:.2}' width='{colorbar_width:.1}' height='{:.2}' fill='{fill}'/>\n",
288            colorbar_height / 32.0 + 0.8
289        ));
290    }
291    svg.push_str(&format!(
292        "<rect x='{colorbar_x:.1}' y='{colorbar_y:.1}' width='{colorbar_width:.1}' height='{colorbar_height:.1}' fill='none' stroke='#9ca3af'/>\n"
293    ));
294    svg.push_str(&format!(
295        "<text x='{:.1}' y='{:.1}' font-size='13' font-family='monospace' fill='#374151'>max {:.3}</text>\n",
296        colorbar_x + colorbar_width + 14.0,
297        colorbar_y + 12.0,
298        max_probability
299    ));
300    svg.push_str(&format!(
301        "<text x='{:.1}' y='{:.1}' font-size='13' font-family='monospace' fill='#374151'>min 0.000</text>\n",
302        colorbar_x + colorbar_width + 14.0,
303        colorbar_y + colorbar_height - 2.0
304    ));
305
306    svg.push_str(&format!(
307        "<text x='{:.1}' y='{:.1}' text-anchor='middle' font-size='16' font-family='sans-serif' fill='#111827'>x [m]</text>\n",
308        plot_left + plot_width / 2.0,
309        plot_top + plot_height + 52.0
310    ));
311    svg.push_str(&format!(
312        "<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",
313        plot_left - 48.0,
314        plot_top + plot_height / 2.0,
315        plot_left - 48.0,
316        plot_top + plot_height / 2.0
317    ));
318    svg.push_str("</svg>\n");
319
320    svg
321}
322
323/// Save a Gaussian grid map SVG to a file.
324pub fn save_gaussian_grid_map_svg(
325    filename: &str,
326    grid_map: &GaussianGridMap,
327    ox: &[f64],
328    oy: &[f64],
329) -> std::io::Result<()> {
330    let svg = build_gaussian_grid_map_svg(grid_map, ox, oy);
331    let mut file = File::create(filename)?;
332    file.write_all(svg.as_bytes())?;
333    Ok(())
334}
335
336#[cfg(test)]
337mod tests {
338    use super::*;
339
340    #[test]
341    fn test_gaussian_grid_map_svg_has_white_background() {
342        let grid_map = GaussianGridMap::new(&[0.0, 1.0], &[0.0, 1.0], 1.0, 2.0);
343        let svg = build_gaussian_grid_map_svg(&grid_map, &[0.0, 1.0], &[0.0, 1.0]);
344
345        assert!(svg.contains("<rect width='100%' height='100%' fill='white'/>"));
346        assert!(svg.contains("Gaussian Grid Map"));
347        assert!(svg.contains("Probability"));
348        assert!(svg.contains("Obstacles"));
349    }
350}