1use nalgebra::{Matrix2, Matrix3, Vector2, Vector3};
24use rust_robotics_core::Pose2D;
25
26use crate::pose_graph_optimization::{
27 optimize_pose_graph_warm, Edge2D, Pose2DNode, PoseGraphConfig,
28};
29
30const INITIAL_DAMPING: f64 = 1.0e-3;
32use crate::scan_to_map::{
33 compose_pose, register_point_to_line, relative_pose, scan_points_with_normals,
34 transform_scan_to_world, translational_observability, ScanRegistration, ScanToMapConfig,
35 ScanToMapMatcher, ScanToMapUpdate,
36};
37
38#[derive(Debug, Clone, Copy)]
40pub struct LidarGraphSlamConfig {
41 pub front_end: ScanToMapConfig,
43 pub node_translation: f64,
45 pub node_yaw: f64,
47 pub loop_search_radius: f64,
49 pub loop_min_node_gap: usize,
52 pub loop_max_candidates: usize,
54 pub loop_map_neighbors: usize,
56 pub loop_correspondence_schedule: [f64; 3],
58 pub loop_min_inlier_ratio: f64,
60 pub loop_max_mean_residual: f64,
62 pub loop_max_correction_translation: f64,
64 pub loop_max_correction_yaw: f64,
66 pub loop_ambiguity_check: bool,
71 pub loop_ambiguity_shifts: [f64; 3],
73 pub loop_ambiguity_ratio: f64,
76 pub loop_ambiguity_separation: f64,
78 pub odometry_sigma: (f64, f64),
80 pub odometry_scale_sigma: f64,
83 pub odometry_yaw_sigma: f64,
86 pub degeneracy_ratio: f64,
90 pub loop_sigma: (f64, f64),
92 pub reoptimize_threshold: (f64, f64),
96 pub deferred_optimization: bool,
101 pub pose_graph: PoseGraphConfig,
103}
104
105impl Default for LidarGraphSlamConfig {
106 fn default() -> Self {
107 Self {
108 front_end: ScanToMapConfig::default(),
109 node_translation: 1.0,
110 node_yaw: 0.5,
111 loop_search_radius: 4.0,
112 loop_min_node_gap: 15,
113 loop_max_candidates: 3,
114 loop_map_neighbors: 2,
115 loop_correspondence_schedule: [1.0, 0.5, 0.2],
116 loop_min_inlier_ratio: 0.6,
117 loop_max_mean_residual: 0.05,
118 loop_max_correction_translation: 3.0,
119 loop_max_correction_yaw: 0.6,
120 loop_ambiguity_check: true,
121 loop_ambiguity_shifts: [1.0, 2.0, 3.0],
122 loop_ambiguity_ratio: 0.9,
123 loop_ambiguity_separation: 0.3,
124 odometry_sigma: (0.005, 0.0005),
125 odometry_scale_sigma: 0.05,
126 odometry_yaw_sigma: 0.05,
127 degeneracy_ratio: 0.05,
128 loop_sigma: (0.02, 0.003),
129 reoptimize_threshold: (0.02, 0.005),
130 deferred_optimization: false,
131 pose_graph: PoseGraphConfig {
132 max_iterations: 50,
133 ..PoseGraphConfig::default()
134 },
135 }
136 }
137}
138
139#[derive(Debug, Clone, Copy, PartialEq)]
141pub struct LoopClosure {
142 pub from: usize,
144 pub to: usize,
146 pub relative: Pose2D,
148 pub inlier_ratio: f64,
150 pub mean_residual: f64,
152}
153
154#[derive(Debug, Clone, Copy, PartialEq)]
156pub struct LidarGraphSlamUpdate {
157 pub front_end: ScanToMapUpdate,
159 pub pose: Pose2D,
161 pub new_node: Option<usize>,
163 pub optimized: bool,
165 pub loop_closure: Option<LoopClosure>,
167}
168
169enum LoopVerification {
170 Accepted(LoopClosure),
171 Ambiguous,
172 Rejected,
173}
174
175const AMBIGUITY_SEEDS_PER_STEP: usize = 2;
177
178#[derive(Debug, Clone)]
180struct LoopCheck {
181 candidate: usize,
182 index: usize,
183 source: Vec<Vector2<f64>>,
184 target: Vec<Vector2<f64>>,
185 normals: Vec<Vector2<f64>>,
186 registration: ScanRegistration,
187 inlier_ratio: f64,
188 next_seed: usize,
190}
191
192#[derive(Debug, Clone)]
194struct LoopJob {
195 index: usize,
196 candidates: Vec<usize>,
197 next_candidate: usize,
198 check: Option<LoopCheck>,
199}
200
201#[derive(Debug, Clone)]
202struct Node {
203 pose: Pose2D,
205 front_end_pose: Pose2D,
207 scan: Vec<Vector2<f64>>,
208}
209
210#[derive(Debug, Clone)]
212pub struct LidarGraphSlam {
213 config: LidarGraphSlamConfig,
214 front_end: ScanToMapMatcher,
215 nodes: Vec<Node>,
216 edges: Vec<Edge2D>,
217 loop_closures: Vec<LoopClosure>,
218 ambiguous_loop_rejections: usize,
220 optimization_pending: bool,
222 damping: f64,
225 optimization_iterations: usize,
228 loop_queue: std::collections::VecDeque<LoopJob>,
230 unobserved_translation: Matrix2<f64>,
233 unobserved_yaw_distance: f64,
235}
236
237impl LidarGraphSlam {
238 pub fn new(config: LidarGraphSlamConfig, initial_pose: Pose2D) -> Self {
240 Self {
241 config,
242 front_end: ScanToMapMatcher::new(config.front_end, initial_pose),
243 nodes: Vec::new(),
244 edges: Vec::new(),
245 loop_closures: Vec::new(),
246 ambiguous_loop_rejections: 0,
247 optimization_pending: false,
248 damping: INITIAL_DAMPING,
249 optimization_iterations: 0,
250 loop_queue: std::collections::VecDeque::new(),
251 unobserved_translation: Matrix2::zeros(),
252 unobserved_yaw_distance: 0.0,
253 }
254 }
255
256 pub fn config(&self) -> &LidarGraphSlamConfig {
258 &self.config
259 }
260
261 pub fn pose(&self) -> Pose2D {
263 match self.nodes.last() {
264 Some(node) => compose_pose(
265 node.pose,
266 relative_pose(node.front_end_pose, self.front_end.pose()),
267 ),
268 None => self.front_end.pose(),
269 }
270 }
271
272 pub fn front_end_pose(&self) -> Pose2D {
274 self.front_end.pose()
275 }
276
277 pub fn node_poses(&self) -> Vec<Pose2D> {
279 self.nodes.iter().map(|node| node.pose).collect()
280 }
281
282 pub fn node_front_end_poses(&self) -> Vec<Pose2D> {
284 self.nodes.iter().map(|node| node.front_end_pose).collect()
285 }
286
287 pub fn node_scan(&self, index: usize) -> Option<&[Vector2<f64>]> {
289 self.nodes.get(index).map(|node| node.scan.as_slice())
290 }
291
292 pub fn loop_closures(&self) -> &[LoopClosure] {
294 &self.loop_closures
295 }
296
297 pub fn ambiguous_loop_rejections(&self) -> usize {
299 self.ambiguous_loop_rejections
300 }
301
302 pub fn edges(&self) -> &[Edge2D] {
304 &self.edges
305 }
306
307 pub fn map_points(&self) -> Vec<Vector2<f64>> {
309 self.nodes
310 .iter()
311 .flat_map(|node| transform_scan_to_world(&node.scan, node.pose))
312 .collect()
313 }
314
315 pub fn update(
317 &mut self,
318 odom_delta: Pose2D,
319 scan_body: &[Vector2<f64>],
320 ) -> LidarGraphSlamUpdate {
321 let front_end = self.front_end.update(odom_delta, scan_body);
322 self.accumulate_unobservable_motion(odom_delta, &front_end);
323 let mut new_node = None;
324 let mut loop_closure = None;
325 let mut optimized = false;
326
327 if self.is_new_node() {
328 let index = self.add_node(scan_body);
329 new_node = Some(index);
330 if self.config.deferred_optimization {
331 let candidates = self.loop_candidates(index);
333 if !candidates.is_empty() {
334 self.loop_queue.push_back(LoopJob {
335 index,
336 candidates,
337 next_candidate: 0,
338 check: None,
339 });
340 }
341 } else {
342 loop_closure = self.detect_loop(index);
343 if let Some(closure) = loop_closure {
344 optimized = self.accept_loop(closure);
345 }
346 }
347 }
348
349 LidarGraphSlamUpdate {
350 front_end,
351 pose: self.pose(),
352 new_node,
353 optimized,
354 loop_closure,
355 }
356 }
357
358 fn is_new_node(&self) -> bool {
359 let Some(last) = self.nodes.last() else {
360 return true;
361 };
362 let delta = relative_pose(last.front_end_pose, self.front_end.pose());
363 delta.x.hypot(delta.y) >= self.config.node_translation
364 || delta.yaw.abs() >= self.config.node_yaw
365 }
366
367 fn add_node(&mut self, scan_body: &[Vector2<f64>]) -> usize {
368 let front_end_pose = self.front_end.pose();
369 let pose = self.pose();
370 let index = self.nodes.len();
371 self.nodes.push(Node {
372 pose,
373 front_end_pose,
374 scan: scan_body.to_vec(),
375 });
376 if index > 0 {
377 let odometry = relative_pose(self.nodes[index - 1].front_end_pose, front_end_pose);
378 let information = self.odometry_information(front_end_pose.yaw);
379 self.add_edge(index - 1, index, odometry, information);
380 }
381 self.unobserved_translation = Matrix2::zeros();
382 self.unobserved_yaw_distance = 0.0;
383 index
384 }
385
386 fn accumulate_unobservable_motion(&mut self, odom_delta: Pose2D, update: &ScanToMapUpdate) {
389 let step = odom_delta.x.hypot(odom_delta.y);
390 if step == 0.0 {
391 return;
392 }
393 if !update.status.is_accepted() {
394 self.unobserved_translation += Matrix2::identity() * step;
395 self.unobserved_yaw_distance += step;
396 return;
397 }
398 let (ratio, direction) = translational_observability(&update.hessian);
399 let weakness = (1.0 - ratio / self.config.degeneracy_ratio.max(1.0e-12)).clamp(0.0, 1.0);
400 self.unobserved_translation += direction * direction.transpose() * (step * weakness);
401 }
402
403 fn odometry_information(&self, to_yaw: f64) -> Matrix3<f64> {
412 let (xy, yaw) = self.config.odometry_sigma;
413 let (sin, cos) = to_yaw.sin_cos();
414 let rotation = Matrix2::new(cos, -sin, sin, cos);
415 let spread = self.unobserved_translation * self.config.odometry_scale_sigma;
416 let covariance = Matrix2::identity() * (xy * xy)
417 + rotation.transpose() * spread * spread.transpose() * rotation;
418 let translational = covariance
419 .try_inverse()
420 .unwrap_or_else(|| Matrix2::identity() / (xy * xy));
421 let yaw_sigma = yaw + self.config.odometry_yaw_sigma * self.unobserved_yaw_distance;
422 let mut information = Matrix3::zeros();
423 information
424 .fixed_view_mut::<2, 2>(0, 0)
425 .copy_from(&translational);
426 information[(2, 2)] = 1.0 / (yaw_sigma * yaw_sigma);
427 information
428 }
429
430 fn add_edge(&mut self, from: usize, to: usize, relative: Pose2D, information: Matrix3<f64>) {
431 self.edges.push(Edge2D {
432 from,
433 to,
434 measurement: Pose2DNode::new(relative.x, relative.y, relative.yaw),
435 information,
436 });
437 }
438
439 fn detect_loop(&mut self, index: usize) -> Option<LoopClosure> {
440 for candidate in self.loop_candidates(index) {
441 match self.verify_loop(candidate, index) {
442 LoopVerification::Accepted(closure) => return Some(closure),
443 LoopVerification::Ambiguous => self.ambiguous_loop_rejections += 1,
444 LoopVerification::Rejected => {}
445 }
446 }
447 None
448 }
449
450 fn loop_candidates(&self, index: usize) -> Vec<usize> {
452 let gap = self.config.loop_min_node_gap.max(1);
455 if index < gap {
456 return Vec::new();
457 }
458 let current = self.nodes[index].pose;
459 let radius_sq = self.config.loop_search_radius * self.config.loop_search_radius;
460 let mut candidates: Vec<(f64, usize)> = self.nodes[..=index - gap]
461 .iter()
462 .enumerate()
463 .filter_map(|(candidate, node)| {
464 let distance_sq =
465 (node.pose.x - current.x).powi(2) + (node.pose.y - current.y).powi(2);
466 (distance_sq <= radius_sq).then_some((distance_sq, candidate))
467 })
468 .collect();
469 candidates.sort_by(|a, b| a.0.total_cmp(&b.0));
470 candidates
471 .into_iter()
472 .take(self.config.loop_max_candidates)
473 .map(|(_, candidate)| candidate)
474 .collect()
475 }
476
477 fn register_loop(
479 &self,
480 source: &[Vector2<f64>],
481 target: &[Vector2<f64>],
482 normals: &[Vector2<f64>],
483 seed: Pose2D,
484 ) -> ScanRegistration {
485 let front = &self.config.front_end;
486 let mut pose = seed;
487 let mut registration = None;
488 for radius in self.config.loop_correspondence_schedule {
489 let config = ScanToMapConfig {
490 max_correspondence_distance: radius,
491 max_iterations: front.max_iterations.max(30),
492 ..*front
493 };
494 let result = register_point_to_line(source, target, normals, pose, &config);
495 pose = result.pose;
496 registration = Some(result);
497 }
498 registration.expect("correspondence schedule is non-empty")
499 }
500
501 fn ambiguity_seeds(&self, best: &ScanRegistration) -> Vec<Pose2D> {
503 self.config
504 .loop_ambiguity_shifts
505 .iter()
506 .flat_map(|shift| [(*shift, 0.0), (-shift, 0.0), (0.0, *shift), (0.0, -shift)])
507 .map(|(dx, dy)| Pose2D::new(best.pose.x + dx, best.pose.y + dy, best.pose.yaw))
508 .collect()
509 }
510
511 fn is_ambiguous(&self, check: &LoopCheck, seeds: &[Pose2D]) -> bool {
514 let best = &check.registration;
515 let best_inliers = best.correspondences as f64;
516 seeds.iter().any(|seed| {
517 let alternative =
518 self.register_loop(&check.source, &check.target, &check.normals, *seed);
519 let separation =
520 (alternative.pose.x - best.pose.x).hypot(alternative.pose.y - best.pose.y);
521 separation > self.config.loop_ambiguity_separation
522 && alternative.mean_residual <= self.config.loop_max_mean_residual
523 && alternative.correspondences as f64
524 >= self.config.loop_ambiguity_ratio * best_inliers
525 })
526 }
527
528 fn verify_loop(&self, candidate: usize, index: usize) -> LoopVerification {
529 let Some(check) = self.register_candidate(candidate, index) else {
530 return LoopVerification::Rejected;
531 };
532 if self.config.loop_ambiguity_check
533 && self.is_ambiguous(&check, &self.ambiguity_seeds(&check.registration))
534 {
535 return LoopVerification::Ambiguous;
536 }
537 LoopVerification::Accepted(self.closure(&check))
538 }
539
540 fn register_candidate(&self, candidate: usize, index: usize) -> Option<LoopCheck> {
543 let front = &self.config.front_end;
544 let first = candidate.saturating_sub(self.config.loop_map_neighbors);
545 let last = (candidate + self.config.loop_map_neighbors).min(index - 1);
546 let mut target = Vec::new();
547 let mut normals = Vec::new();
548 for node in &self.nodes[first..=last] {
549 let (points, node_normals) = scan_points_with_normals(
550 &node.scan,
551 node.pose,
552 front.normal_neighbor_distance,
553 front.voxel_size,
554 );
555 target.extend(points);
556 normals.extend(node_normals);
557 }
558 let source = scan_points_with_normals(
559 &self.nodes[index].scan,
560 Pose2D::origin(),
561 front.normal_neighbor_distance,
562 front.voxel_size,
563 )
564 .0;
565 if source.len() < front.min_correspondences || target.len() < front.min_correspondences {
566 return None;
567 }
568
569 let seed = self.nodes[index].pose;
570 let registration = self.register_loop(&source, &target, &normals, seed);
571 let inlier_ratio = registration.correspondences as f64 / source.len() as f64;
572 let correction = relative_pose(seed, registration.pose);
573 let accepted = inlier_ratio >= self.config.loop_min_inlier_ratio
574 && registration.mean_residual <= self.config.loop_max_mean_residual
575 && correction.x.hypot(correction.y) <= self.config.loop_max_correction_translation
576 && correction.yaw.abs() <= self.config.loop_max_correction_yaw;
577 accepted.then_some(LoopCheck {
578 candidate,
579 index,
580 source,
581 target,
582 normals,
583 registration,
584 inlier_ratio,
585 next_seed: 0,
586 })
587 }
588
589 fn closure(&self, check: &LoopCheck) -> LoopClosure {
590 LoopClosure {
591 from: check.candidate,
592 to: check.index,
593 relative: relative_pose(self.nodes[check.candidate].pose, check.registration.pose),
594 inlier_ratio: check.inlier_ratio,
595 mean_residual: check.registration.mean_residual,
596 }
597 }
598
599 fn accept_loop(&mut self, closure: LoopClosure) -> bool {
603 let current = relative_pose(self.nodes[closure.from].pose, self.nodes[closure.to].pose);
604 let innovation = relative_pose(current, closure.relative);
605 let information = diagonal_information(self.config.loop_sigma);
606 self.add_edge(closure.from, closure.to, closure.relative, information);
607 self.loop_closures.push(closure);
608 let (xy, yaw) = self.config.reoptimize_threshold;
609 if innovation.x.hypot(innovation.y) <= xy && innovation.yaw.abs() <= yaw {
610 return false;
611 }
612 if self.config.deferred_optimization {
613 if !self.optimization_pending {
614 self.damping = INITIAL_DAMPING;
615 }
616 self.optimization_iterations = 0;
618 self.optimization_pending = true;
619 false
620 } else {
621 self.optimize();
622 true
623 }
624 }
625
626 fn loop_step(&mut self) {
629 let Some(mut job) = self.loop_queue.pop_front() else {
630 return;
631 };
632 if let Some(mut check) = job.check.take() {
633 let seeds = self.ambiguity_seeds(&check.registration);
634 let end = (check.next_seed + AMBIGUITY_SEEDS_PER_STEP).min(seeds.len());
635 if self.is_ambiguous(&check, &seeds[check.next_seed..end]) {
636 self.ambiguous_loop_rejections += 1;
637 } else if end == seeds.len() {
638 let closure = self.closure(&check);
639 self.accept_loop(closure);
640 return; } else {
642 check.next_seed = end;
643 job.check = Some(check);
644 }
645 self.loop_queue.push_front(job);
646 return;
647 }
648 let Some(&candidate) = job.candidates.get(job.next_candidate) else {
649 return; };
651 job.next_candidate += 1;
652 if let Some(check) = self.register_candidate(candidate, job.index) {
653 if self.config.loop_ambiguity_check {
654 job.check = Some(check);
655 } else {
656 let closure = self.closure(&check);
657 self.accept_loop(closure);
658 return;
659 }
660 }
661 self.loop_queue.push_front(job);
662 }
663
664 pub fn background_step(&mut self, max_iterations: usize) -> bool {
670 if self.loop_queue.is_empty() {
673 self.optimize_step(max_iterations);
674 } else {
675 self.loop_step();
676 }
677 self.background_pending()
678 }
679
680 pub fn background_pending(&self) -> bool {
683 !self.loop_queue.is_empty() || self.optimization_pending
684 }
685
686 pub fn optimize(&mut self) {
692 let config = self.config.pose_graph;
693 self.run_optimizer(&config, INITIAL_DAMPING);
694 self.optimization_pending = false;
695 }
696
697 pub fn optimization_pending(&self) -> bool {
700 self.optimization_pending
701 }
702
703 pub fn optimize_step(&mut self, max_iterations: usize) -> bool {
707 if self.optimization_pending {
708 let config = PoseGraphConfig {
709 max_iterations: max_iterations.max(1),
710 ..self.config.pose_graph
711 };
712 let (converged, damping) = self.run_optimizer(&config, self.damping);
713 self.damping = damping;
714 self.optimization_iterations += config.max_iterations;
715 self.optimization_pending =
718 !converged && self.optimization_iterations < self.config.pose_graph.max_iterations;
719 }
720 self.optimization_pending
721 }
722
723 fn run_optimizer(&mut self, config: &PoseGraphConfig, damping: f64) -> (bool, f64) {
726 let initial: Vec<Pose2DNode> = self
727 .nodes
728 .iter()
729 .map(|node| Pose2DNode::new(node.pose.x, node.pose.y, node.pose.yaw))
730 .collect();
731 let (result, damping) = optimize_pose_graph_warm(&initial, &self.edges, config, damping);
732 for (node, pose) in self.nodes.iter_mut().zip(result.poses) {
733 node.pose = Pose2D::new(pose.x, pose.y, pose.yaw);
734 }
735 (result.converged, damping)
736 }
737}
738
739fn diagonal_information((xy, yaw): (f64, f64)) -> Matrix3<f64> {
740 Matrix3::from_diagonal(&Vector3::new(
741 1.0 / (xy * xy),
742 1.0 / (xy * xy),
743 1.0 / (yaw * yaw),
744 ))
745}
746
747#[cfg(test)]
748mod tests {
749 use super::*;
750 use crate::scan_to_map::{ranges_to_points, ray_cast_ranges, LineSegment};
751
752 fn polygon(corners: &[(f64, f64)]) -> Vec<LineSegment> {
753 (0..corners.len())
754 .map(|i| {
755 let (ax, ay) = corners[i];
756 let (bx, by) = corners[(i + 1) % corners.len()];
757 LineSegment::new(Vector2::new(ax, ay), Vector2::new(bx, by))
758 })
759 .collect()
760 }
761
762 fn world() -> Vec<LineSegment> {
763 let mut walls = polygon(&[(-6.0, -4.0), (6.0, -4.0), (6.0, 4.0), (-6.0, 4.0)]);
764 walls.extend(polygon(&[(1.0, 1.0), (2.0, 1.4), (1.6, 2.4), (0.6, 2.0)]));
765 walls.extend(polygon(&[(-4.5, -3.0), (-3.7, -3.0), (-3.7, -2.2)]));
766 walls
767 }
768
769 fn scan_at(pose: Pose2D) -> Vec<Vector2<f64>> {
770 ranges_to_points(&ray_cast_ranges(pose, &world(), 360, 15.0))
771 }
772
773 #[test]
774 fn creates_nodes_and_odometry_edges() {
775 let config = LidarGraphSlamConfig {
776 node_translation: 0.5,
777 ..LidarGraphSlamConfig::default()
778 };
779 let mut slam = LidarGraphSlam::new(config, Pose2D::origin());
780 let mut truth = Pose2D::origin();
781 let step = Pose2D::new(0.1, 0.0, 0.0);
782 for _ in 0..=20 {
783 slam.update(step, &scan_at(truth));
784 truth = compose_pose(truth, step);
785 }
786 let nodes = slam.node_poses();
787 assert!(nodes.len() >= 4, "nodes: {}", nodes.len());
788 assert_eq!(slam.edges().len(), nodes.len() - 1);
789 assert!(slam.loop_closures().is_empty());
790 }
791
792 #[test]
793 fn a_zero_loop_gap_does_not_match_a_node_with_itself() {
794 let config = LidarGraphSlamConfig {
795 node_translation: 0.5,
796 loop_min_node_gap: 0,
797 ..LidarGraphSlamConfig::default()
798 };
799 let mut slam = LidarGraphSlam::new(config, Pose2D::origin());
800 let mut truth = Pose2D::origin();
801 let step = Pose2D::new(0.1, 0.0, 0.0);
802 for _ in 0..=20 {
803 slam.update(step, &scan_at(truth));
804 truth = compose_pose(truth, step);
805 }
806 assert!(slam.node_poses().len() >= 4);
807 }
808
809 fn blind_loop(
813 slam: &mut LidarGraphSlam,
814 start: Pose2D,
815 mut each_frame: impl FnMut(&mut LidarGraphSlam, &LidarGraphSlamUpdate),
816 ) -> (Pose2D, usize) {
817 let steps = 126;
819 let blind = 40..55;
820 let step = Pose2D::new(0.1, 0.0, 0.1 / 2.0);
821 let mut truth = start;
822 slam.update(Pose2D::origin(), &scan_at(truth));
823 let mut closures = 0;
824 for i in 0..steps {
825 truth = compose_pose(truth, step);
826 if blind.contains(&i) {
827 let odom = Pose2D::new(step.x * 1.15, 0.0, step.yaw + 0.003);
829 let update = slam.update(odom, &[]);
830 each_frame(slam, &update);
831 if i + 1 == blind.end {
832 slam.front_end.reset(slam.front_end.pose());
834 }
835 continue;
836 }
837 let update = slam.update(step, &scan_at(truth));
838 closures += usize::from(update.loop_closure.is_some());
839 each_frame(slam, &update);
840 }
841 (truth, closures)
842 }
843
844 fn loop_config() -> LidarGraphSlamConfig {
845 LidarGraphSlamConfig {
846 node_translation: 0.5,
847 loop_min_node_gap: 6,
848 ..LidarGraphSlamConfig::default()
849 }
850 }
851
852 #[test]
853 fn closes_a_loop_after_a_blind_odometry_stretch() {
854 let start = Pose2D::new(-1.0, -2.5, 0.0);
855 let mut slam = LidarGraphSlam::new(loop_config(), start);
856 let (truth, closures) = blind_loop(&mut slam, start, |_, _| {});
857
858 let drifted = relative_pose(truth, slam.front_end_pose());
859 let corrected = relative_pose(truth, slam.pose());
860 assert!(closures >= 1, "no loop closure accepted");
861 assert!(
862 drifted.x.hypot(drifted.y) > 0.05 && drifted.yaw.abs() > 0.03,
863 "front end did not drift: {drifted:?}"
864 );
865 assert!(
866 corrected.x.hypot(corrected.y) < 0.05,
867 "loop closure left {:.3} m error",
868 corrected.x.hypot(corrected.y)
869 );
870 assert!(corrected.yaw.abs() < 0.01, "yaw error {}", corrected.yaw);
871 }
872
873 #[test]
874 fn deferred_mode_spreads_loop_checks_and_optimization_over_frames() {
875 let config = LidarGraphSlamConfig {
876 deferred_optimization: true,
877 ..loop_config()
878 };
879 let start = Pose2D::new(-1.0, -2.5, 0.0);
880 let mut slam = LidarGraphSlam::new(config, start);
881 let (mut busy_frames, mut optimized_in_update, mut closures_in_update) = (0, false, 0);
882 let (truth, _) = blind_loop(&mut slam, start, |slam, update| {
883 optimized_in_update |= update.optimized;
884 closures_in_update += usize::from(update.loop_closure.is_some());
885 busy_frames += usize::from(slam.background_step(2));
886 });
887 while slam.background_step(2) {}
888
889 assert!(!optimized_in_update, "update optimized despite deferral");
890 assert_eq!(
891 closures_in_update, 0,
892 "update verified loops despite deferral"
893 );
894 assert!(!slam.loop_closures().is_empty(), "no loop closure accepted");
895 assert!(busy_frames >= 2, "the work did not spread over frames");
896 let corrected = relative_pose(truth, slam.pose());
897 assert!(
898 corrected.x.hypot(corrected.y) < 0.05,
899 "deferred optimization left {:.3} m error",
900 corrected.x.hypot(corrected.y)
901 );
902 assert!(corrected.yaw.abs() < 0.01, "yaw error {}", corrected.yaw);
903 }
904}