1use nalgebra::DMatrix;
6use std::fs::File;
7use std::io::Write;
8
9pub const EXTEND_AREA: f64 = 10.0;
11pub const XY_RESOLUTION: f64 = 0.5;
13pub const STD: f64 = 5.0;
15
16pub 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 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 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 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 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
94pub 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
100pub 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
146pub 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
323pub 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}