Skip to main content

rust_robotics_slam/
correlative_scan_matching.rs

1//! Brute-force correlative scan matching.
2//!
3//! The matcher scores candidate poses by transforming a query scan into the
4//! reference frame and counting occupancy hits in a discretized grid.
5//!
6//! Reference:
7//! - Edwin Olson, "Real-Time Correlative Scan Matching":
8//!   <https://april.eecs.umich.edu/pdfs/olson2009icra.pdf>
9//!
10//! The lookup table follows the rasterization idea in Olson's paper: each
11//! reference point contributes a radially symmetric score field and the grid
12//! stores the maximum value contributed at each cell.
13
14use rust_robotics_core::normalize_angle;
15use std::collections::HashMap;
16
17/// Configuration for correlative scan matching.
18#[derive(Debug, Clone, Copy)]
19pub struct CorrelativeScanMatcherConfig {
20    /// Search window for x and y offsets \[m\].
21    pub linear_search_range: f64,
22    /// Search window for yaw offsets \[rad\].
23    pub angular_search_range: f64,
24    /// Search step for x and y offsets \[m\].
25    pub linear_step: f64,
26    /// Search step for yaw offsets \[rad\].
27    pub angular_step: f64,
28    /// Occupancy lookup grid resolution \[m\].
29    pub grid_resolution: f64,
30}
31
32impl Default for CorrelativeScanMatcherConfig {
33    fn default() -> Self {
34        Self {
35            linear_search_range: 1.0,
36            angular_search_range: 0.2,
37            linear_step: 0.1,
38            angular_step: 0.02,
39            grid_resolution: 0.05,
40        }
41    }
42}
43
44/// Best scan matching result found in the search window.
45#[derive(Debug, Clone, Copy)]
46pub struct ScanMatchResult {
47    pub x: f64,
48    pub y: f64,
49    pub yaw: f64,
50    pub score: f64,
51    pub converged: bool,
52}
53
54/// Performs brute-force correlative scan matching.
55pub fn correlative_scan_match(
56    reference_x: &[f64],
57    reference_y: &[f64],
58    query_x: &[f64],
59    query_y: &[f64],
60    initial_pose: (f64, f64, f64),
61    config: &CorrelativeScanMatcherConfig,
62) -> ScanMatchResult {
63    if reference_x.len() != reference_y.len()
64        || query_x.len() != query_y.len()
65        || reference_x.is_empty()
66        || query_x.is_empty()
67        || config.linear_step <= 0.0
68        || config.angular_step <= 0.0
69        || config.grid_resolution <= 0.0
70    {
71        return ScanMatchResult {
72            x: initial_pose.0,
73            y: initial_pose.1,
74            yaw: initial_pose.2,
75            score: 0.0,
76            converged: false,
77        };
78    }
79
80    let grid = build_lookup_table(reference_x, reference_y, config.grid_resolution);
81    let linear_offsets = enumerate_offsets(config.linear_search_range, config.linear_step);
82    let angular_offsets = enumerate_offsets(config.angular_search_range, config.angular_step);
83
84    let mut best = ScanMatchResult {
85        x: initial_pose.0,
86        y: initial_pose.1,
87        yaw: normalize_angle(initial_pose.2),
88        score: -1.0,
89        converged: false,
90    };
91    let mut best_penalty = f64::INFINITY;
92
93    for dx in &linear_offsets {
94        for dy in &linear_offsets {
95            for dyaw in &angular_offsets {
96                let candidate = (
97                    initial_pose.0 + dx,
98                    initial_pose.1 + dy,
99                    normalize_angle(initial_pose.2 + dyaw),
100                );
101                let score =
102                    score_candidate(&grid, query_x, query_y, candidate, config.grid_resolution);
103                let penalty = dx * dx + dy * dy + dyaw * dyaw;
104
105                if score > best.score || (score == best.score && penalty < best_penalty) {
106                    best = ScanMatchResult {
107                        x: candidate.0,
108                        y: candidate.1,
109                        yaw: candidate.2,
110                        score,
111                        converged: score > 0.0,
112                    };
113                    best_penalty = penalty;
114                }
115            }
116        }
117    }
118
119    best
120}
121
122fn enumerate_offsets(range: f64, step: f64) -> Vec<f64> {
123    let n_steps = (range / step).round() as i32;
124    (-n_steps..=n_steps)
125        .map(|index| index as f64 * step)
126        .collect()
127}
128
129fn build_lookup_table(
130    reference_x: &[f64],
131    reference_y: &[f64],
132    resolution: f64,
133) -> HashMap<(i32, i32), f64> {
134    let mut grid: HashMap<(i32, i32), f64> = HashMap::new();
135    let sigma = resolution;
136    let cutoff_radius = (3.0 * sigma / resolution).ceil() as i32;
137    let inv_two_sigma_sq = 0.5 / (sigma * sigma);
138
139    for (&x, &y) in reference_x.iter().zip(reference_y.iter()) {
140        let (cx, cy) = cell_index(x, y, resolution);
141        for ix in (cx - cutoff_radius)..=(cx + cutoff_radius) {
142            for iy in (cy - cutoff_radius)..=(cy + cutoff_radius) {
143                let gx = ix as f64 * resolution;
144                let gy = iy as f64 * resolution;
145                let squared_distance = (gx - x).powi(2) + (gy - y).powi(2);
146                let weight = (-squared_distance * inv_two_sigma_sq).exp();
147                if weight < 1.0e-6 {
148                    continue;
149                }
150
151                grid.entry((ix, iy))
152                    .and_modify(|score| *score = (*score).max(weight))
153                    .or_insert(weight);
154            }
155        }
156    }
157
158    grid
159}
160
161fn score_candidate(
162    grid: &HashMap<(i32, i32), f64>,
163    query_x: &[f64],
164    query_y: &[f64],
165    pose: (f64, f64, f64),
166    resolution: f64,
167) -> f64 {
168    let cos_yaw = pose.2.cos();
169    let sin_yaw = pose.2.sin();
170    let mut score = 0.0;
171
172    for (&x, &y) in query_x.iter().zip(query_y.iter()) {
173        let world_x = cos_yaw * x - sin_yaw * y + pose.0;
174        let world_y = sin_yaw * x + cos_yaw * y + pose.1;
175        let cell = cell_index(world_x, world_y, resolution);
176        score += grid.get(&cell).copied().unwrap_or(0.0);
177    }
178
179    score
180}
181
182fn cell_index(x: f64, y: f64, resolution: f64) -> (i32, i32) {
183    (
184        (x / resolution).round() as i32,
185        (y / resolution).round() as i32,
186    )
187}
188
189#[cfg(test)]
190mod tests {
191    use super::*;
192
193    fn inverse_transform_points(
194        points: &[(f64, f64)],
195        pose: (f64, f64, f64),
196    ) -> (Vec<f64>, Vec<f64>) {
197        let cos_yaw = pose.2.cos();
198        let sin_yaw = pose.2.sin();
199        let mut xs = Vec::with_capacity(points.len());
200        let mut ys = Vec::with_capacity(points.len());
201
202        for &(x, y) in points {
203            let dx = x - pose.0;
204            let dy = y - pose.1;
205            xs.push(cos_yaw * dx + sin_yaw * dy);
206            ys.push(-sin_yaw * dx + cos_yaw * dy);
207        }
208
209        (xs, ys)
210    }
211
212    fn fixture_points() -> Vec<(f64, f64)> {
213        vec![
214            (0.0, 0.0),
215            (1.0, 0.0),
216            (2.0, 0.0),
217            (0.0, 1.0),
218            (0.0, 2.0),
219            (1.0, 1.0),
220            (1.5, 2.0),
221        ]
222    }
223
224    #[test]
225    fn test_identity_match() {
226        let points = fixture_points();
227        let (reference_x, reference_y): (Vec<_>, Vec<_>) = points.iter().copied().unzip();
228        let config = CorrelativeScanMatcherConfig::default();
229
230        let result = correlative_scan_match(
231            &reference_x,
232            &reference_y,
233            &reference_x,
234            &reference_y,
235            (0.0, 0.0, 0.0),
236            &config,
237        );
238
239        assert!(result.converged);
240        assert_eq!(result.x, 0.0);
241        assert_eq!(result.y, 0.0);
242        assert_eq!(result.yaw, 0.0);
243        assert_eq!(result.score, points.len() as f64);
244    }
245
246    #[test]
247    fn test_known_translation_recovery() {
248        let points = fixture_points();
249        let (reference_x, reference_y): (Vec<_>, Vec<_>) = points.iter().copied().unzip();
250        let true_pose = (0.4, -0.3, 0.0);
251        let (query_x, query_y) = inverse_transform_points(&points, true_pose);
252        let config = CorrelativeScanMatcherConfig {
253            linear_search_range: 0.6,
254            angular_search_range: 0.1,
255            linear_step: 0.1,
256            angular_step: 0.05,
257            grid_resolution: 0.05,
258        };
259
260        let result = correlative_scan_match(
261            &reference_x,
262            &reference_y,
263            &query_x,
264            &query_y,
265            (0.0, 0.0, 0.0),
266            &config,
267        );
268
269        assert!(result.converged);
270        assert!((result.x - true_pose.0).abs() < 1.0e-9);
271        assert!((result.y - true_pose.1).abs() < 1.0e-9);
272        assert!(result.yaw.abs() < 1.0e-9);
273    }
274
275    #[test]
276    fn test_known_rotation_recovery() {
277        let points = fixture_points();
278        let (reference_x, reference_y): (Vec<_>, Vec<_>) = points.iter().copied().unzip();
279        let true_pose = (0.0, 0.0, 0.16);
280        let (query_x, query_y) = inverse_transform_points(&points, true_pose);
281        let config = CorrelativeScanMatcherConfig {
282            linear_search_range: 0.2,
283            angular_search_range: 0.3,
284            linear_step: 0.1,
285            angular_step: 0.02,
286            grid_resolution: 0.05,
287        };
288
289        let result = correlative_scan_match(
290            &reference_x,
291            &reference_y,
292            &query_x,
293            &query_y,
294            (0.0, 0.0, 0.0),
295            &config,
296        );
297
298        assert!(result.converged);
299        assert!(result.x.abs() < 1.0e-9);
300        assert!(result.y.abs() < 1.0e-9);
301        assert!((result.yaw - true_pose.2).abs() < 1.0e-9);
302    }
303
304    #[test]
305    fn test_lookup_table_prefers_exact_alignment() {
306        let points = fixture_points();
307        let (reference_x, reference_y): (Vec<_>, Vec<_>) = points.iter().copied().unzip();
308        let resolution = 0.05;
309        let grid = build_lookup_table(&reference_x, &reference_y, resolution);
310
311        let aligned = score_candidate(
312            &grid,
313            &reference_x,
314            &reference_y,
315            (0.0, 0.0, 0.0),
316            resolution,
317        );
318        let translated = score_candidate(
319            &grid,
320            &reference_x,
321            &reference_y,
322            (0.15, 0.0, 0.0),
323            resolution,
324        );
325        let rotated = score_candidate(
326            &grid,
327            &reference_x,
328            &reference_y,
329            (0.0, 0.0, 0.15),
330            resolution,
331        );
332
333        assert!(aligned > translated);
334        assert!(aligned > rotated);
335    }
336}