1use std::collections::{HashMap, VecDeque};
31use std::f64::consts::PI;
32
33use nalgebra::{Matrix2, Matrix3, Vector2, Vector3};
34use rust_robotics_core::normalize_angle;
35use rust_robotics_core::Pose2D;
36
37#[derive(Debug, Clone, Copy, PartialEq)]
39pub struct ScanToMapConfig {
40 pub max_scans: usize,
42 pub max_radius: f64,
44 pub keyframe_translation: f64,
46 pub keyframe_yaw: f64,
48 pub voxel_size: f64,
51 pub max_iterations: usize,
53 pub max_correspondence_distance: f64,
55 pub huber_delta: f64,
57 pub min_correspondences: usize,
59 pub max_correction_translation: f64,
61 pub max_correction_yaw: f64,
63 pub max_mean_residual: f64,
65 pub normal_neighbor_distance: f64,
67 pub degeneracy_ratio: f64,
72}
73
74impl Default for ScanToMapConfig {
75 fn default() -> Self {
76 Self {
77 max_scans: 20,
78 max_radius: 12.0,
79 keyframe_translation: 0.25,
80 keyframe_yaw: 0.1,
81 voxel_size: 0.05,
82 max_iterations: 20,
83 max_correspondence_distance: 0.3,
84 huber_delta: 0.05,
85 min_correspondences: 20,
86 max_correction_translation: 0.5,
87 max_correction_yaw: 0.2,
88 max_mean_residual: 0.1,
89 normal_neighbor_distance: 0.3,
90 degeneracy_ratio: 0.03,
91 }
92 }
93}
94
95impl ScanToMapConfig {
96 pub fn scan_to_scan() -> Self {
98 Self {
99 max_scans: 1,
100 keyframe_translation: 0.0,
101 keyframe_yaw: 0.0,
102 ..Self::default()
103 }
104 }
105}
106
107#[derive(Debug, Clone, Copy, PartialEq, Eq)]
109pub enum MatchStatus {
110 Bootstrap,
112 Accepted,
114 RejectedTooFewCorrespondences,
116 RejectedLargeCorrection,
118 RejectedHighResidual,
120}
121
122impl MatchStatus {
123 pub fn is_accepted(self) -> bool {
125 self == Self::Accepted
126 }
127}
128
129#[derive(Debug, Clone, Copy, PartialEq)]
131pub struct ScanToMapUpdate {
132 pub predicted_pose: Pose2D,
134 pub corrected_pose: Pose2D,
136 pub status: MatchStatus,
138 pub iterations: usize,
140 pub correspondences: usize,
142 pub mean_residual: f64,
144 pub hessian: Matrix3<f64>,
147 pub submap_points: usize,
149 pub inserted_keyframe: bool,
151}
152
153#[derive(Debug, Clone, Copy, PartialEq)]
155pub struct LineSegment {
156 pub start: Vector2<f64>,
157 pub end: Vector2<f64>,
158}
159
160impl LineSegment {
161 pub fn new(start: Vector2<f64>, end: Vector2<f64>) -> Self {
162 Self { start, end }
163 }
164}
165
166#[derive(Debug, Clone)]
167struct Keyframe {
168 points: Vec<Vector2<f64>>,
169 normals: Vec<Vector2<f64>>,
170}
171
172#[derive(Debug, Clone)]
174pub struct ScanToMapMatcher {
175 config: ScanToMapConfig,
176 pose: Pose2D,
177 keyframes: VecDeque<Keyframe>,
178 last_keyframe_pose: Option<Pose2D>,
179}
180
181impl ScanToMapMatcher {
182 pub fn new(config: ScanToMapConfig, initial_pose: Pose2D) -> Self {
184 Self {
185 config,
186 pose: initial_pose,
187 keyframes: VecDeque::new(),
188 last_keyframe_pose: None,
189 }
190 }
191
192 pub fn pose(&self) -> Pose2D {
194 self.pose
195 }
196
197 pub fn config(&self) -> &ScanToMapConfig {
199 &self.config
200 }
201
202 pub fn keyframe_count(&self) -> usize {
204 self.keyframes.len()
205 }
206
207 pub fn submap_points(&self) -> Vec<Vector2<f64>> {
209 self.keyframes
210 .iter()
211 .flat_map(|keyframe| keyframe.points.iter().copied())
212 .collect()
213 }
214
215 pub fn reset(&mut self, pose: Pose2D) {
217 self.pose = pose;
218 self.keyframes.clear();
219 self.last_keyframe_pose = None;
220 }
221
222 pub fn update(&mut self, odom_delta: Pose2D, scan_body: &[Vector2<f64>]) -> ScanToMapUpdate {
229 let predicted_pose = compose_pose(self.pose, odom_delta);
230 let (target_points, target_normals) = self.local_target(predicted_pose);
231 let source = voxel_downsample(scan_body, self.config.voxel_size);
232
233 let mut update = ScanToMapUpdate {
234 predicted_pose,
235 corrected_pose: predicted_pose,
236 status: MatchStatus::Bootstrap,
237 iterations: 0,
238 correspondences: 0,
239 mean_residual: 0.0,
240 hessian: Matrix3::zeros(),
241 submap_points: target_points.len(),
242 inserted_keyframe: false,
243 };
244
245 if target_points.len() >= self.config.min_correspondences {
246 let registration = register_point_to_line(
247 &source,
248 &target_points,
249 &target_normals,
250 predicted_pose,
251 &self.config,
252 );
253 update.iterations = registration.iterations;
254 update.correspondences = registration.correspondences;
255 update.mean_residual = registration.mean_residual;
256 update.hessian = registration.hessian;
257 let correction = relative_pose(predicted_pose, registration.pose);
258 update.status = if registration.correspondences < self.config.min_correspondences {
259 MatchStatus::RejectedTooFewCorrespondences
260 } else if correction.x.hypot(correction.y) > self.config.max_correction_translation
261 || correction.yaw.abs() > self.config.max_correction_yaw
262 {
263 MatchStatus::RejectedLargeCorrection
264 } else if registration.mean_residual > self.config.max_mean_residual {
265 MatchStatus::RejectedHighResidual
266 } else {
267 update.corrected_pose = registration.pose;
268 MatchStatus::Accepted
269 };
270 }
271
272 self.pose = update.corrected_pose;
273 let may_insert = matches!(
274 update.status,
275 MatchStatus::Bootstrap | MatchStatus::Accepted
276 );
277 if may_insert && self.is_keyframe(self.pose) {
278 self.insert_keyframe(scan_body, self.pose);
279 update.inserted_keyframe = true;
280 }
281 update
282 }
283
284 fn is_keyframe(&self, pose: Pose2D) -> bool {
285 let Some(last) = self.last_keyframe_pose else {
286 return true;
287 };
288 let delta = relative_pose(last, pose);
289 delta.x.hypot(delta.y) >= self.config.keyframe_translation
290 || delta.yaw.abs() >= self.config.keyframe_yaw
291 }
292
293 fn insert_keyframe(&mut self, scan_body: &[Vector2<f64>], pose: Pose2D) {
294 let (points, world_normals) = scan_points_with_normals(
295 scan_body,
296 pose,
297 self.config.normal_neighbor_distance,
298 self.config.voxel_size,
299 );
300 self.keyframes.push_back(Keyframe {
301 points,
302 normals: world_normals,
303 });
304 while self.keyframes.len() > self.config.max_scans.max(1) {
305 self.keyframes.pop_front();
306 }
307 self.last_keyframe_pose = Some(pose);
308 }
309
310 fn local_target(&self, center: Pose2D) -> (Vec<Vector2<f64>>, Vec<Vector2<f64>>) {
311 let center = Vector2::new(center.x, center.y);
312 let radius_sq = self.config.max_radius * self.config.max_radius;
313 let mut points = Vec::new();
314 let mut normals = Vec::new();
315 let mut occupied = HashMap::new();
316 for keyframe in self.keyframes.iter().rev() {
318 for (point, normal) in keyframe.points.iter().zip(&keyframe.normals) {
319 if (point - center).norm_squared() > radius_sq {
320 continue;
321 }
322 if self.config.voxel_size > 0.0
323 && occupied
324 .insert(voxel_key(point, self.config.voxel_size), ())
325 .is_some()
326 {
327 continue;
328 }
329 points.push(*point);
330 normals.push(*normal);
331 }
332 }
333 (points, normals)
334 }
335}
336
337#[derive(Debug, Clone, Copy, PartialEq)]
339pub struct ScanRegistration {
340 pub pose: Pose2D,
342 pub iterations: usize,
344 pub correspondences: usize,
346 pub mean_residual: f64,
348 pub hessian: Matrix3<f64>,
352}
353
354pub fn scan_points_with_normals(
359 scan_body: &[Vector2<f64>],
360 pose: Pose2D,
361 normal_neighbor_distance: f64,
362 voxel_size: f64,
363) -> (Vec<Vector2<f64>>, Vec<Vector2<f64>>) {
364 let normals = estimate_scan_normals(scan_body, normal_neighbor_distance);
365 let rotation = rotation(pose.yaw);
366 let translation = Vector2::new(pose.x, pose.y);
367 let mut points = Vec::new();
368 let mut world_normals = Vec::new();
369 let mut occupied = HashMap::new();
370 for (point, normal) in scan_body.iter().zip(normals) {
371 let Some(normal) = normal else {
372 continue;
373 };
374 let world = rotation * point + translation;
375 if voxel_size > 0.0 && occupied.insert(voxel_key(&world, voxel_size), ()).is_some() {
376 continue;
377 }
378 points.push(world);
379 world_normals.push(rotation * normal);
380 }
381 (points, world_normals)
382}
383
384pub fn register_point_to_line(
390 source: &[Vector2<f64>],
391 target: &[Vector2<f64>],
392 target_normals: &[Vector2<f64>],
393 seed: Pose2D,
394 config: &ScanToMapConfig,
395) -> ScanRegistration {
396 let grid = NeighborGrid::new(target, config.max_correspondence_distance);
397 let evaluate = |pose: Pose2D| {
398 PointToLineTerms::evaluate(source, target, target_normals, &grid, pose, config)
399 };
400
401 let mut pose = seed;
402 let mut iterations = 0;
403 let mut terms = PointToLineTerms::default();
404 let mut terms_at_pose = false;
406
407 for _ in 0..config.max_iterations.max(1) {
408 iterations += 1;
409 terms = evaluate(pose);
410 terms_at_pose = true;
411 if terms.correspondences < 3 {
412 break;
413 }
414 let hessian = terms.hessian;
415 let damping = 1.0e-6 * hessian.trace().max(1.0e-9);
417 let Some(inverse) = (hessian + Matrix3::identity() * damping).try_inverse() else {
418 break;
419 };
420 let mut step = -inverse * terms.gradient;
421 if let Some(weak) = degenerate_direction(&hessian, config.degeneracy_ratio) {
422 let along = weak.dot(&step.xy());
423 step.x -= along * weak.x;
424 step.y -= along * weak.y;
425 }
426 pose = Pose2D::new(
427 pose.x + step.x,
428 pose.y + step.y,
429 normalize_angle(pose.yaw + step.z),
430 );
431 terms_at_pose = false;
432 if step.x.hypot(step.y) < 1.0e-5 && step.z.abs() < 1.0e-5 {
433 break;
434 }
435 }
436 if !terms_at_pose && iterations == config.max_iterations.max(1) {
440 terms = evaluate(pose);
441 }
442
443 ScanRegistration {
444 pose,
445 iterations,
446 correspondences: terms.correspondences,
447 mean_residual: if terms.correspondences == 0 {
448 f64::INFINITY
449 } else {
450 terms.residual_sum / terms.correspondences as f64
451 },
452 hessian: terms.hessian,
453 }
454}
455
456#[derive(Default)]
458struct PointToLineTerms {
459 hessian: Matrix3<f64>,
460 gradient: Vector3<f64>,
461 residual_sum: f64,
462 correspondences: usize,
463}
464
465impl PointToLineTerms {
466 fn evaluate(
467 source: &[Vector2<f64>],
468 target: &[Vector2<f64>],
469 target_normals: &[Vector2<f64>],
470 grid: &NeighborGrid,
471 pose: Pose2D,
472 config: &ScanToMapConfig,
473 ) -> Self {
474 let (sin, cos) = pose.yaw.sin_cos();
475 let rot = Matrix2::new(cos, -sin, sin, cos);
476 let rot_derivative = Matrix2::new(-sin, -cos, cos, -sin);
477 let translation = Vector2::new(pose.x, pose.y);
478 let mut terms = Self::default();
479 for point in source {
480 let world = rot * point + translation;
481 let Some(index) = grid.nearest_within(&world) else {
482 continue;
483 };
484 let normal = target_normals[index];
485 let residual = normal.dot(&(world - target[index]));
486 let jacobian = Vector3::new(normal.x, normal.y, normal.dot(&(rot_derivative * point)));
487 let weight = huber_weight(residual, config.huber_delta);
488 terms.hessian += weight * jacobian * jacobian.transpose();
489 terms.gradient += weight * jacobian * residual;
490 terms.residual_sum += residual.abs();
491 terms.correspondences += 1;
492 }
493 terms
494 }
495}
496
497pub fn degenerate_direction(hessian: &Matrix3<f64>, ratio_threshold: f64) -> Option<Vector2<f64>> {
500 let (ratio, direction) = translational_observability(hessian);
501 (ratio < ratio_threshold).then_some(direction)
502}
503
504pub fn translational_observability(hessian: &Matrix3<f64>) -> (f64, Vector2<f64>) {
508 let (a, b, c) = (hessian[(0, 0)], hessian[(0, 1)], hessian[(1, 1)]);
509 let half_trace = 0.5 * (a + c);
510 let spread = (0.25 * (a - c) * (a - c) + b * b).sqrt();
511 let (large, small) = (half_trace + spread, half_trace - spread);
512 let angle = 0.5 * (2.0 * b).atan2(a - c);
514 let weak = Vector2::new(-angle.sin(), angle.cos());
515 if large <= 0.0 {
516 return (0.0, weak);
517 }
518 ((small / large).max(0.0), weak)
519}
520
521struct NeighborGrid<'a> {
526 points: &'a [Vector2<f64>],
527 radius: f64,
528 min_x: f64,
529 min_y: f64,
530 columns: i64,
531 rows: i64,
532 offsets: Vec<usize>,
533 indices: Vec<usize>,
534}
535
536impl<'a> NeighborGrid<'a> {
537 fn new(points: &'a [Vector2<f64>], radius: f64) -> Self {
538 let radius = radius.max(1.0e-6);
539 let (mut min_x, mut min_y) = (f64::INFINITY, f64::INFINITY);
540 let (mut max_x, mut max_y) = (f64::NEG_INFINITY, f64::NEG_INFINITY);
541 for point in points {
542 min_x = min_x.min(point.x);
543 min_y = min_y.min(point.y);
544 max_x = max_x.max(point.x);
545 max_y = max_y.max(point.y);
546 }
547 if points.is_empty() {
548 (min_x, min_y, max_x, max_y) = (0.0, 0.0, 0.0, 0.0);
549 }
550 let columns = ((max_x - min_x) / radius).floor() as i64 + 1;
551 let rows = ((max_y - min_y) / radius).floor() as i64 + 1;
552 let cell_of = |point: &Vector2<f64>| -> usize {
553 let column = ((point.x - min_x) / radius).floor() as i64;
554 let row = ((point.y - min_y) / radius).floor() as i64;
555 (row * columns + column) as usize
556 };
557 let mut offsets = vec![0usize; (columns * rows) as usize + 1];
558 for point in points {
559 offsets[cell_of(point) + 1] += 1;
560 }
561 for cell in 1..offsets.len() {
562 offsets[cell] += offsets[cell - 1];
563 }
564 let mut cursor = offsets.clone();
565 let mut indices = vec![0usize; points.len()];
566 for (index, point) in points.iter().enumerate() {
567 let cell = cell_of(point);
568 indices[cursor[cell]] = index;
569 cursor[cell] += 1;
570 }
571 Self {
572 points,
573 radius,
574 min_x,
575 min_y,
576 columns,
577 rows,
578 offsets,
579 indices,
580 }
581 }
582
583 fn nearest_within(&self, query: &Vector2<f64>) -> Option<usize> {
584 let (qx, qy) = (query.x, query.y);
585 let column = ((qx - self.min_x) / self.radius).floor() as i64;
586 let row = ((qy - self.min_y) / self.radius).floor() as i64;
587 let mut best = None;
588 let mut best_distance_sq = self.radius * self.radius;
589 for r in (row - 1).max(0)..=(row + 1).min(self.rows - 1) {
590 for c in (column - 1).max(0)..=(column + 1).min(self.columns - 1) {
591 let cell = (r * self.columns + c) as usize;
592 for &index in &self.indices[self.offsets[cell]..self.offsets[cell + 1]] {
593 let point = &self.points[index];
594 let (dx, dy) = (point.x - qx, point.y - qy);
595 let distance_sq = dx * dx + dy * dy;
596 if distance_sq <= best_distance_sq {
597 best_distance_sq = distance_sq;
598 best = Some(index);
599 }
600 }
601 }
602 }
603 best
604 }
605}
606
607fn huber_weight(residual: f64, delta: f64) -> f64 {
608 let magnitude = residual.abs();
609 if delta <= 0.0 || magnitude <= delta {
610 1.0
611 } else {
612 delta / magnitude
613 }
614}
615
616pub fn estimate_scan_normals(
621 points: &[Vector2<f64>],
622 max_distance: f64,
623) -> Vec<Option<Vector2<f64>>> {
624 const WINDOW: usize = 2;
625 let max_distance_sq = max_distance * max_distance;
626 (0..points.len())
627 .map(|i| {
628 let start = i.saturating_sub(WINDOW);
629 let end = (i + WINDOW + 1).min(points.len());
630 let neighborhood: Vec<Vector2<f64>> = points[start..end]
631 .iter()
632 .filter(|p| (*p - points[i]).norm_squared() <= max_distance_sq)
633 .copied()
634 .collect();
635 if neighborhood.len() < 3 {
636 return None;
637 }
638 let count = neighborhood.len() as f64;
639 let mean_x = neighborhood.iter().map(|p| p.x).sum::<f64>() / count;
640 let mean_y = neighborhood.iter().map(|p| p.y).sum::<f64>() / count;
641 let (mut sxx, mut sxy, mut syy) = (0.0, 0.0, 0.0);
642 for p in &neighborhood {
643 let (dx, dy) = (p.x - mean_x, p.y - mean_y);
644 sxx += dx * dx;
645 sxy += dx * dy;
646 syy += dy * dy;
647 }
648 let half_trace = 0.5 * (sxx + syy);
650 let spread = (0.25 * (sxx - syy) * (sxx - syy) + sxy * sxy).sqrt();
651 let (large, small) = (half_trace + spread, half_trace - spread);
652 if large <= 0.0 || small > 0.1 * large {
653 return None;
654 }
655 let angle = 0.5 * (2.0 * sxy).atan2(sxx - syy);
657 Some(Vector2::new(-angle.sin(), angle.cos()))
658 })
659 .collect()
660}
661
662pub fn compose_pose(pose: Pose2D, delta: Pose2D) -> Pose2D {
664 let (sin, cos) = pose.yaw.sin_cos();
665 Pose2D::new(
666 pose.x + cos * delta.x - sin * delta.y,
667 pose.y + sin * delta.x + cos * delta.y,
668 normalize_angle(pose.yaw + delta.yaw),
669 )
670}
671
672pub fn relative_pose(from: Pose2D, to: Pose2D) -> Pose2D {
674 let (sin, cos) = from.yaw.sin_cos();
675 let dx = to.x - from.x;
676 let dy = to.y - from.y;
677 Pose2D::new(
678 cos * dx + sin * dy,
679 -sin * dx + cos * dy,
680 normalize_angle(to.yaw - from.yaw),
681 )
682}
683
684pub fn transform_scan_to_world(scan_body: &[Vector2<f64>], pose: Pose2D) -> Vec<Vector2<f64>> {
686 let rotation = rotation(pose.yaw);
687 let translation = Vector2::new(pose.x, pose.y);
688 scan_body
689 .iter()
690 .map(|point| rotation * point + translation)
691 .collect()
692}
693
694pub fn ray_cast_ranges(
698 pose: Pose2D,
699 segments: &[LineSegment],
700 beam_count: usize,
701 max_range: f64,
702) -> Vec<f64> {
703 ray_cast_ranges_fov(
704 pose,
705 segments,
706 -PI,
707 2.0 * PI / beam_count.max(1) as f64,
708 beam_count,
709 max_range,
710 )
711}
712
713pub fn ray_cast_ranges_fov(
718 pose: Pose2D,
719 segments: &[LineSegment],
720 angle_min: f64,
721 angle_increment: f64,
722 beam_count: usize,
723 max_range: f64,
724) -> Vec<f64> {
725 let origin = Vector2::new(pose.x, pose.y);
726 (0..beam_count)
727 .map(|beam| {
728 let angle = pose.yaw + angle_min + angle_increment * beam as f64;
729 let direction = Vector2::new(angle.cos(), angle.sin());
730 segments
731 .iter()
732 .filter_map(|segment| ray_segment_distance(origin, direction, segment))
733 .filter(|range| *range <= max_range)
734 .fold(f64::INFINITY, f64::min)
735 })
736 .collect()
737}
738
739pub fn ranges_to_points(ranges: &[f64]) -> Vec<Vector2<f64>> {
741 ranges
742 .iter()
743 .enumerate()
744 .filter(|(_, range)| range.is_finite())
745 .map(|(beam, range)| {
746 let angle = beam_angle(beam, ranges.len());
747 Vector2::new(range * angle.cos(), range * angle.sin())
748 })
749 .collect()
750}
751
752pub fn beam_angle(beam: usize, beam_count: usize) -> f64 {
755 -PI + 2.0 * PI * beam as f64 / beam_count.max(1) as f64
756}
757
758fn ray_segment_distance(
759 origin: Vector2<f64>,
760 direction: Vector2<f64>,
761 segment: &LineSegment,
762) -> Option<f64> {
763 let edge = segment.end - segment.start;
764 let denominator = direction.perp(&edge);
765 if denominator.abs() < 1.0e-12 {
766 return None;
767 }
768 let offset = segment.start - origin;
769 let range = offset.perp(&edge) / denominator;
770 let along = offset.perp(&direction) / denominator;
771 (range > 0.0 && (0.0..=1.0).contains(&along)).then_some(range)
772}
773
774fn voxel_downsample(points: &[Vector2<f64>], voxel_size: f64) -> Vec<Vector2<f64>> {
775 if voxel_size <= 0.0 {
776 return points.to_vec();
777 }
778 let mut occupied = HashMap::new();
779 points
780 .iter()
781 .filter(|point| occupied.insert(voxel_key(point, voxel_size), ()).is_none())
782 .copied()
783 .collect()
784}
785
786fn voxel_key(point: &Vector2<f64>, voxel_size: f64) -> (i64, i64) {
787 (
788 (point.x / voxel_size).floor() as i64,
789 (point.y / voxel_size).floor() as i64,
790 )
791}
792
793fn rotation(yaw: f64) -> Matrix2<f64> {
794 let (sin, cos) = yaw.sin_cos();
795 Matrix2::new(cos, -sin, sin, cos)
796}
797
798#[cfg(test)]
799mod tests {
800 use super::*;
801
802 fn room() -> Vec<LineSegment> {
803 let corners = [
804 Vector2::new(-6.0, -4.0),
805 Vector2::new(6.0, -4.0),
806 Vector2::new(6.0, 4.0),
807 Vector2::new(-6.0, 4.0),
808 ];
809 let mut walls: Vec<LineSegment> = (0..4)
810 .map(|i| LineSegment::new(corners[i], corners[(i + 1) % 4]))
811 .collect();
812 let box_corners = [
814 Vector2::new(1.0, 1.0),
815 Vector2::new(2.0, 1.4),
816 Vector2::new(1.6, 2.4),
817 Vector2::new(0.6, 2.0),
818 ];
819 walls.extend((0..4).map(|i| LineSegment::new(box_corners[i], box_corners[(i + 1) % 4])));
820 walls.push(LineSegment::new(
821 Vector2::new(-3.0, -4.0),
822 Vector2::new(-3.0, -1.5),
823 ));
824 walls
825 }
826
827 fn scan_at(pose: Pose2D) -> Vec<Vector2<f64>> {
828 ranges_to_points(&ray_cast_ranges(pose, &room(), 360, 15.0))
829 }
830
831 fn pose_error(a: Pose2D, b: Pose2D) -> (f64, f64) {
832 let delta = relative_pose(a, b);
833 (delta.x.hypot(delta.y), delta.yaw.abs())
834 }
835
836 #[test]
837 fn a_capped_registration_reports_statistics_for_the_pose_it_returns() {
838 let config = ScanToMapConfig {
839 max_iterations: 1,
840 ..ScanToMapConfig::default()
841 };
842 let (target, normals) = scan_points_with_normals(
843 &scan_at(Pose2D::origin()),
844 Pose2D::origin(),
845 config.normal_neighbor_distance,
846 config.voxel_size,
847 );
848 let truth = Pose2D::new(0.3, 0.05, 0.04);
849 let seed = Pose2D::new(0.42, -0.03, 0.0);
850 let result = register_point_to_line(&scan_at(truth), &target, &normals, seed, &config);
851 assert!(
852 pose_error(result.pose, seed).0 > 0.01,
853 "the step moved the pose"
854 );
855
856 let grid = NeighborGrid::new(&target, config.max_correspondence_distance);
857 let at_result = PointToLineTerms::evaluate(
858 &scan_at(truth),
859 &target,
860 &normals,
861 &grid,
862 result.pose,
863 &config,
864 );
865 assert_eq!(result.correspondences, at_result.correspondences);
866 let mean = at_result.residual_sum / at_result.correspondences as f64;
867 assert!((result.mean_residual - mean).abs() < 1e-12);
868 assert_eq!(result.hessian, at_result.hessian);
869 }
870
871 #[test]
872 fn compose_and_relative_are_inverse() {
873 let a = Pose2D::new(1.0, -2.0, 0.7);
874 let delta = Pose2D::new(0.3, 0.1, -0.2);
875 let b = compose_pose(a, delta);
876 let recovered = relative_pose(a, b);
877 assert!((recovered.x - delta.x).abs() < 1e-12);
878 assert!((recovered.y - delta.y).abs() < 1e-12);
879 assert!((recovered.yaw - delta.yaw).abs() < 1e-12);
880 }
881
882 #[test]
883 fn transform_scan_to_world_rotates_then_translates() {
884 let world =
885 transform_scan_to_world(&[Vector2::new(1.0, 0.0)], Pose2D::new(2.0, 3.0, PI / 2.0));
886 assert!((world[0] - Vector2::new(2.0, 4.0)).norm() < 1e-12);
887 }
888
889 #[test]
890 fn ray_cast_hits_room_walls() {
891 let ranges = ray_cast_ranges(Pose2D::origin(), &room(), 4, 15.0);
892 assert!((ranges[0] - 6.0).abs() < 1e-9);
894 assert!((ranges[1] - 4.0).abs() < 1e-9);
895 assert!((ranges[2] - 6.0).abs() < 1e-9);
896 assert!((ranges[3] - 4.0).abs() < 1e-9);
897 let toward_box = ray_cast_ranges(Pose2D::new(1.3, 0.0, PI / 2.0), &room(), 4, 15.0);
899 assert!(toward_box[2] < 1.5);
900 }
901
902 #[test]
903 fn normals_are_perpendicular_to_a_straight_wall() {
904 let points: Vec<_> = (0..10).map(|i| Vector2::new(i as f64 * 0.1, 2.0)).collect();
905 let normals = estimate_scan_normals(&points, 0.3);
906 for normal in normals.iter().flatten() {
907 assert!(normal.x.abs() < 1e-9 && (normal.y.abs() - 1.0).abs() < 1e-9);
908 }
909 assert!(normals.iter().all(Option::is_some));
910 }
911
912 #[test]
913 fn first_update_bootstraps_submap() {
914 let mut matcher = ScanToMapMatcher::new(ScanToMapConfig::default(), Pose2D::origin());
915 let update = matcher.update(Pose2D::origin(), &scan_at(Pose2D::origin()));
916 assert_eq!(update.status, MatchStatus::Bootstrap);
917 assert!(update.inserted_keyframe);
918 assert_eq!(matcher.keyframe_count(), 1);
919 assert!(!matcher.submap_points().is_empty());
920 }
921
922 #[test]
923 fn corrects_a_biased_odometry_step() {
924 let mut matcher = ScanToMapMatcher::new(ScanToMapConfig::default(), Pose2D::origin());
925 matcher.update(Pose2D::origin(), &scan_at(Pose2D::origin()));
926
927 let truth = Pose2D::new(0.3, 0.05, 0.04);
928 let biased_delta = Pose2D::new(0.42, -0.03, 0.0);
930 let update = matcher.update(biased_delta, &scan_at(truth));
931 assert_eq!(update.status, MatchStatus::Accepted);
932 let (predicted_xy, predicted_yaw) = pose_error(update.predicted_pose, truth);
933 let (xy, yaw) = pose_error(update.corrected_pose, truth);
934 assert!(predicted_xy > 0.1 && predicted_yaw > 0.03);
935 assert!(xy < 5e-3, "translation error {xy}");
936 assert!(yaw < 1e-3, "yaw error {yaw}");
937 }
938
939 #[test]
940 fn rejects_corrections_beyond_the_gate() {
941 let config = ScanToMapConfig {
942 max_correction_translation: 0.05,
943 ..ScanToMapConfig::default()
944 };
945 let mut matcher = ScanToMapMatcher::new(config, Pose2D::origin());
946 matcher.update(Pose2D::origin(), &scan_at(Pose2D::origin()));
947 let update = matcher.update(
948 Pose2D::new(0.5, 0.0, 0.0),
949 &scan_at(Pose2D::new(0.3, 0.0, 0.0)),
950 );
951 assert_eq!(update.status, MatchStatus::RejectedLargeCorrection);
952 assert_eq!(update.corrected_pose, update.predicted_pose);
953 assert!(!update.inserted_keyframe);
954 }
955
956 #[test]
957 fn stationary_scans_do_not_grow_the_submap() {
958 let mut matcher = ScanToMapMatcher::new(ScanToMapConfig::default(), Pose2D::origin());
959 let scan = scan_at(Pose2D::origin());
960 for _ in 0..5 {
961 matcher.update(Pose2D::origin(), &scan);
962 }
963 assert_eq!(matcher.keyframe_count(), 1);
964 }
965
966 #[test]
967 fn scan_to_scan_preset_keeps_one_scan() {
968 let mut matcher = ScanToMapMatcher::new(ScanToMapConfig::scan_to_scan(), Pose2D::origin());
969 let mut truth = Pose2D::origin();
970 for _ in 0..4 {
971 let delta = Pose2D::new(0.2, 0.0, 0.02);
972 truth = compose_pose(truth, delta);
973 matcher.update(delta, &scan_at(truth));
974 }
975 assert_eq!(matcher.keyframe_count(), 1);
976 let (xy, yaw) = pose_error(matcher.pose(), truth);
977 assert!(xy < 1e-2 && yaw < 1e-3);
978 }
979
980 #[test]
981 fn submap_is_bounded_by_max_scans() {
982 let config = ScanToMapConfig {
983 max_scans: 3,
984 ..ScanToMapConfig::default()
985 };
986 let mut matcher = ScanToMapMatcher::new(config, Pose2D::origin());
987 let mut truth = Pose2D::origin();
988 for _ in 0..8 {
989 let delta = Pose2D::new(0.3, 0.0, 0.0);
990 truth = compose_pose(truth, delta);
991 matcher.update(delta, &scan_at(truth));
992 }
993 assert_eq!(matcher.keyframe_count(), 3);
994 }
995}