Skip to main content

rust_robotics_mapping/
distance_map.rs

1//! Distance Map computation using the Euclidean Distance Transform (EDT).
2//!
3//! Computes unsigned distance fields (UDF) and signed distance fields (SDF)
4//! from a 2D boolean obstacle grid.
5//!
6//! Reference: <https://cs.brown.edu/people/pfelzens/papers/dt-final.pdf>
7
8use nalgebra::DMatrix;
9
10const INF: f64 = 1e20;
11
12/// Compute 1D distance transform in-place under squared Euclidean distance.
13///
14/// Uses the algorithm from Felzenszwalb & Huttenlocher (2012).
15fn dt_1d(d: &mut [f64]) {
16    let n = d.len();
17    if n == 0 {
18        return;
19    }
20    let mut v = vec![0usize; n + 1];
21    let mut z = vec![0.0f64; n + 1];
22    let mut k: usize = 0;
23    v[0] = 0;
24    z[0] = -INF;
25    z[1] = INF;
26
27    for q in 1..n {
28        let vk = v[k];
29        let mut s = ((d[q] + (q * q) as f64) - (d[vk] + (vk * vk) as f64))
30            / (2.0 * q as f64 - 2.0 * vk as f64);
31        while s <= z[k] {
32            k -= 1;
33            let vk2 = v[k];
34            s = ((d[q] + (q * q) as f64) - (d[vk2] + (vk2 * vk2) as f64))
35                / (2.0 * q as f64 - 2.0 * vk2 as f64);
36        }
37        k += 1;
38        v[k] = q;
39        z[k] = s;
40        if k + 1 < z.len() {
41            z[k + 1] = INF;
42        }
43    }
44
45    k = 0;
46    for q in 0..n {
47        while k + 1 < z.len() && z[k + 1] < q as f64 {
48            k += 1;
49        }
50        let dx = q as f64 - v[k] as f64;
51        d[q] = dx * dx + d[v[k]];
52    }
53}
54
55/// Compute the unsigned distance field (UDF) from a boolean obstacle grid.
56///
57/// # Arguments
58/// * `obstacles` - A 2D matrix where `true` represents an obstacle cell and
59///   `false` represents free space.
60///
61/// # Returns
62/// A matrix of Euclidean distances from each cell to the nearest obstacle.
63pub fn compute_udf(obstacles: &DMatrix<bool>) -> DMatrix<f64> {
64    let nrows = obstacles.nrows();
65    let ncols = obstacles.ncols();
66
67    // Initialize: obstacle cells = 0, free cells = INF
68    let mut edt = DMatrix::from_fn(
69        nrows,
70        ncols,
71        |r, c| {
72            if obstacles[(r, c)] {
73                0.0
74            } else {
75                INF
76            }
77        },
78    );
79
80    // Transform rows
81    for r in 0..nrows {
82        let mut row: Vec<f64> = (0..ncols).map(|c| edt[(r, c)]).collect();
83        dt_1d(&mut row);
84        for c in 0..ncols {
85            edt[(r, c)] = row[c];
86        }
87    }
88
89    // Transform columns
90    for c in 0..ncols {
91        let mut col: Vec<f64> = (0..nrows).map(|r| edt[(r, c)]).collect();
92        dt_1d(&mut col);
93        for r in 0..nrows {
94            edt[(r, c)] = col[r];
95        }
96    }
97
98    // Take square root to get Euclidean distance
99    edt.map(|v| v.sqrt())
100}
101
102/// Compute the signed distance field (SDF) from a boolean obstacle grid.
103///
104/// Positive values indicate distance to the nearest obstacle (free space),
105/// negative values indicate distance to the nearest free space (inside obstacle).
106///
107/// # Arguments
108/// * `obstacles` - A 2D matrix where `true` represents an obstacle cell and
109///   `false` represents free space.
110///
111/// # Returns
112/// A matrix of signed Euclidean distances.
113pub fn compute_sdf(obstacles: &DMatrix<bool>) -> DMatrix<f64> {
114    let udf_obstacle = compute_udf(obstacles);
115    let inverted = obstacles.map(|v| !v);
116    let udf_free = compute_udf(&inverted);
117    udf_obstacle - udf_free
118}
119
120#[cfg(test)]
121mod tests {
122    use super::*;
123    use nalgebra::DMatrix;
124
125    fn make_obstacles() -> DMatrix<bool> {
126        // 5x5 grid with obstacles at specific cells
127        #[rustfmt::skip]
128        let data: Vec<bool> = vec![
129            true,  false, false, false, false,
130            false, true,  true,  true,  false,
131            false, true,  true,  true,  false,
132            false, false, true,  true,  false,
133            false, false, true,  false, false,
134        ];
135        DMatrix::from_row_slice(5, 5, &data)
136    }
137
138    #[test]
139    fn test_udf_obstacle_cells_are_zero() {
140        let obs = make_obstacles();
141        let udf = compute_udf(&obs);
142
143        // All obstacle cells should have distance 0
144        for r in 0..obs.nrows() {
145            for c in 0..obs.ncols() {
146                if obs[(r, c)] {
147                    assert!(
148                        udf[(r, c)].abs() < 1e-10,
149                        "Obstacle cell ({}, {}) should be 0, got {}",
150                        r,
151                        c,
152                        udf[(r, c)]
153                    );
154                }
155            }
156        }
157    }
158
159    #[test]
160    fn test_udf_non_negative() {
161        let obs = make_obstacles();
162        let udf = compute_udf(&obs);
163
164        for r in 0..udf.nrows() {
165            for c in 0..udf.ncols() {
166                assert!(
167                    udf[(r, c)] >= 0.0,
168                    "UDF should be non-negative at ({}, {}), got {}",
169                    r,
170                    c,
171                    udf[(r, c)]
172                );
173            }
174        }
175    }
176
177    #[test]
178    fn test_udf_free_cells_positive() {
179        let obs = make_obstacles();
180        let udf = compute_udf(&obs);
181
182        for r in 0..obs.nrows() {
183            for c in 0..obs.ncols() {
184                if !obs[(r, c)] {
185                    assert!(
186                        udf[(r, c)] > 0.0,
187                        "Free cell ({}, {}) should have positive distance",
188                        r,
189                        c
190                    );
191                }
192            }
193        }
194    }
195
196    #[test]
197    fn test_sdf_sign_convention() {
198        let obs = make_obstacles();
199        let sdf = compute_sdf(&obs);
200
201        // Obstacle cells: UDF=0, so SDF = 0 - udf_free <= 0
202        for r in 0..obs.nrows() {
203            for c in 0..obs.ncols() {
204                if obs[(r, c)] {
205                    assert!(
206                        sdf[(r, c)] <= 0.0,
207                        "Obstacle cell ({}, {}) should have non-positive SDF, got {}",
208                        r,
209                        c,
210                        sdf[(r, c)]
211                    );
212                }
213            }
214        }
215    }
216
217    #[test]
218    fn test_udf_known_distance() {
219        // Single obstacle at (0,0) in a 3x3 grid
220        #[rustfmt::skip]
221        let data = vec![
222            true, false, false,
223            false, false, false,
224            false, false, false,
225        ];
226        let obs = DMatrix::from_row_slice(3, 3, &data);
227        let udf = compute_udf(&obs);
228
229        assert!((udf[(0, 0)] - 0.0).abs() < 1e-10);
230        assert!((udf[(0, 1)] - 1.0).abs() < 1e-10);
231        assert!((udf[(1, 0)] - 1.0).abs() < 1e-10);
232        // Diagonal: sqrt(2)
233        assert!((udf[(1, 1)] - std::f64::consts::SQRT_2).abs() < 1e-10);
234    }
235
236    #[test]
237    fn test_all_obstacles() {
238        let obs = DMatrix::from_element(4, 4, true);
239        let udf = compute_udf(&obs);
240        for r in 0..4 {
241            for c in 0..4 {
242                assert!((udf[(r, c)] - 0.0).abs() < 1e-10);
243            }
244        }
245    }
246
247    #[test]
248    fn test_dt_1d_basic() {
249        let mut d = vec![0.0, INF, INF, INF];
250        dt_1d(&mut d);
251        assert!((d[0] - 0.0).abs() < 1e-10);
252        assert!((d[1] - 1.0).abs() < 1e-10);
253        assert!((d[2] - 4.0).abs() < 1e-10);
254        assert!((d[3] - 9.0).abs() < 1e-10);
255    }
256}