1use rust_robotics_core::normalize_angle;
15use std::collections::HashMap;
16
17#[derive(Debug, Clone, Copy)]
19pub struct CorrelativeScanMatcherConfig {
20 pub linear_search_range: f64,
22 pub angular_search_range: f64,
24 pub linear_step: f64,
26 pub angular_step: f64,
28 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#[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
54pub 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}