Skip to main content

rust_robotics_slam/
lidar_graph_slam.rs

1//! 2D LiDAR graph SLAM: scan-to-map odometry plus loop closure.
2//!
3//! [`ScanToMapMatcher`] keeps local drift small but cannot remove it: once the
4//! robot leaves the submap radius, errors are frozen into the trajectory. This
5//! module adds the global layer on top of it:
6//!
7//! 1. **Pose-graph nodes** are created every `node_translation` / `node_yaw`
8//!    of front-end motion and keep their body-frame scan. Consecutive nodes are
9//!    linked by odometry edges measured by the front end.
10//! 2. **Loop detection**: each new node searches older nodes (at least
11//!    `loop_min_node_gap` back) within `loop_search_radius` of its current
12//!    estimate, and registers its scan against the candidate's neighborhood
13//!    map with coarse-to-fine point-to-line ICP.
14//! 3. **Verification**: a closure is accepted only if the inlier ratio, mean
15//!    residual, and correction size pass the gates; it then becomes a loop
16//!    edge and the whole graph is re-optimized with
17//!    [`optimize_pose_graph`](crate::pose_graph_optimization::optimize_pose_graph).
18//!
19//! The front end keeps running in its own (drifting) frame; only its relative
20//! motion between nodes is used, so optimized node poses never disturb the
21//! local matcher.
22
23use 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
30/// Levenberg-Marquardt damping a fresh optimization starts from.
31const 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/// Tuning parameters for [`LidarGraphSlam`].
39#[derive(Debug, Clone, Copy)]
40pub struct LidarGraphSlamConfig {
41    /// Local scan-to-map front end.
42    pub front_end: ScanToMapConfig,
43    /// Front-end translation between pose-graph nodes \[m\].
44    pub node_translation: f64,
45    /// Front-end rotation between pose-graph nodes \[rad\].
46    pub node_yaw: f64,
47    /// Older nodes within this distance of the new node are loop candidates \[m\].
48    pub loop_search_radius: f64,
49    /// Candidates must be at least this many nodes older than the new node
50    /// (at least 1).
51    pub loop_min_node_gap: usize,
52    /// At most this many nearest candidates are verified per new node.
53    pub loop_max_candidates: usize,
54    /// The candidate's map uses nodes `candidate ± loop_map_neighbors`.
55    pub loop_map_neighbors: usize,
56    /// Correspondence radii for the coarse-to-fine loop registration \[m\].
57    pub loop_correspondence_schedule: [f64; 3],
58    /// Minimum fraction of scan points matched at the finest radius.
59    pub loop_min_inlier_ratio: f64,
60    /// Maximum mean point-to-line residual at the finest radius \[m\].
61    pub loop_max_mean_residual: f64,
62    /// Maximum loop correction translation relative to the estimate \[m\].
63    pub loop_max_correction_translation: f64,
64    /// Maximum loop correction rotation relative to the estimate \[rad\].
65    pub loop_max_correction_yaw: f64,
66    /// Reject loop matches that are not unique (perceptual aliasing): the
67    /// accepted solution is re-registered from seeds shifted by
68    /// ±`loop_ambiguity_shifts` along x and y, and any distinct alternative
69    /// that scores nearly as well makes the match ambiguous.
70    pub loop_ambiguity_check: bool,
71    /// Seed shifts \[m\] used by the ambiguity check.
72    pub loop_ambiguity_shifts: [f64; 3],
73    /// An alternative is "nearly as good" when its inlier ratio is at least
74    /// this fraction of the best match's.
75    pub loop_ambiguity_ratio: f64,
76    /// Alternatives closer than this to the best match are the same solution \[m\].
77    pub loop_ambiguity_separation: f64,
78    /// Base standard deviations `(xy [m], yaw [rad])` of an odometry edge.
79    pub odometry_sigma: (f64, f64),
80    /// Odometry translation uncertainty per meter travelled in a direction
81    /// the front end cannot observe (no match, or a degenerate match).
82    pub odometry_scale_sigma: f64,
83    /// Odometry heading uncertainty per meter travelled without any accepted
84    /// scan match \[rad/m\].
85    pub odometry_yaw_sigma: f64,
86    /// A match is treated as fully degenerate along its weakest translation
87    /// direction when `λ_min / λ_max` of the translational Hessian is 0, and
88    /// fully constrained at or above this ratio (linear in between).
89    pub degeneracy_ratio: f64,
90    /// Standard deviations `(xy [m], yaw [rad])` of a loop edge.
91    pub loop_sigma: (f64, f64),
92    /// A new loop edge triggers re-optimization only when it disagrees with
93    /// the current graph by more than `(xy [m], yaw [rad])`; consistent
94    /// edges are kept and folded in by the next optimization.
95    pub reoptimize_threshold: (f64, f64),
96    /// When `true`, [`LidarGraphSlam::update`] only queues loop detection and
97    /// marks the graph as needing re-optimization; the caller spreads that
98    /// work over frames with [`LidarGraphSlam::background_step`] (for
99    /// interactive applications).
100    pub deferred_optimization: bool,
101    /// Back-end optimizer settings.
102    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/// An accepted loop closure edge.
140#[derive(Debug, Clone, Copy, PartialEq)]
141pub struct LoopClosure {
142    /// Older node the new node was matched against.
143    pub from: usize,
144    /// New node.
145    pub to: usize,
146    /// Measured relative pose `from⁻¹ ⊕ to`.
147    pub relative: Pose2D,
148    /// Fraction of scan points matched at the finest correspondence radius.
149    pub inlier_ratio: f64,
150    /// Mean point-to-line residual at the finest radius \[m\].
151    pub mean_residual: f64,
152}
153
154/// Diagnostics returned by [`LidarGraphSlam::update`].
155#[derive(Debug, Clone, Copy, PartialEq)]
156pub struct LidarGraphSlamUpdate {
157    /// Front-end matcher diagnostics for this scan.
158    pub front_end: ScanToMapUpdate,
159    /// Best current pose estimate in the graph frame.
160    pub pose: Pose2D,
161    /// Index of the node created by this scan, if any.
162    pub new_node: Option<usize>,
163    /// Whether the pose graph was re-optimized during this update.
164    pub optimized: bool,
165    /// Loop closure accepted for the new node, if any.
166    pub loop_closure: Option<LoopClosure>,
167}
168
169enum LoopVerification {
170    Accepted(LoopClosure),
171    Ambiguous,
172    Rejected,
173}
174
175/// Ambiguity re-registrations per [`LidarGraphSlam::background_step`].
176const AMBIGUITY_SEEDS_PER_STEP: usize = 2;
177
178/// A loop match that passed the gates, with what the ambiguity check needs.
179#[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    /// Ambiguity seeds already re-registered.
189    next_seed: usize,
190}
191
192/// Deferred loop detection for one node.
193#[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    /// Optimized pose in the graph frame.
204    pose: Pose2D,
205    /// Front-end pose when the node was created.
206    front_end_pose: Pose2D,
207    scan: Vec<Vector2<f64>>,
208}
209
210/// Scan-to-map odometry with pose-graph loop closure.
211#[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    /// Loop matches rejected by the ambiguity check.
219    ambiguous_loop_rejections: usize,
220    /// A loop edge disagreed with the graph and optimization has not converged.
221    optimization_pending: bool,
222    /// Levenberg-Marquardt damping carried between [`Self::optimize_step`]
223    /// calls.
224    damping: f64,
225    /// Iterations spent on the pending optimization so far; it ends after
226    /// `pose_graph.max_iterations` like a one-shot [`Self::optimize`].
227    optimization_iterations: usize,
228    /// Nodes whose loop candidates still await verification (deferred mode).
229    loop_queue: std::collections::VecDeque<LoopJob>,
230    /// Distance-weighted weak directions `Σ dₖ wₖ vₖ vₖᵀ` (front-end world
231    /// frame) travelled since the last node without scan constraints.
232    unobserved_translation: Matrix2<f64>,
233    /// Distance travelled since the last node without any accepted match.
234    unobserved_yaw_distance: f64,
235}
236
237impl LidarGraphSlam {
238    /// Creates a SLAM instance whose graph frame starts at `initial_pose`.
239    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    /// Configuration.
257    pub fn config(&self) -> &LidarGraphSlamConfig {
258        &self.config
259    }
260
261    /// Best current pose estimate: last optimized node plus front-end motion since.
262    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    /// Front-end (odometry-only) pose, without loop closure.
273    pub fn front_end_pose(&self) -> Pose2D {
274        self.front_end.pose()
275    }
276
277    /// Optimized pose-graph node poses.
278    pub fn node_poses(&self) -> Vec<Pose2D> {
279        self.nodes.iter().map(|node| node.pose).collect()
280    }
281
282    /// Front-end poses recorded when each node was created.
283    pub fn node_front_end_poses(&self) -> Vec<Pose2D> {
284        self.nodes.iter().map(|node| node.front_end_pose).collect()
285    }
286
287    /// Body-frame scan stored with node `index`, if it exists.
288    pub fn node_scan(&self, index: usize) -> Option<&[Vector2<f64>]> {
289        self.nodes.get(index).map(|node| node.scan.as_slice())
290    }
291
292    /// Accepted loop closures, oldest first.
293    pub fn loop_closures(&self) -> &[LoopClosure] {
294        &self.loop_closures
295    }
296
297    /// Number of loop matches rejected as ambiguous (perceptual aliasing).
298    pub fn ambiguous_loop_rejections(&self) -> usize {
299        self.ambiguous_loop_rejections
300    }
301
302    /// Pose-graph edges (odometry and loop) in insertion order.
303    pub fn edges(&self) -> &[Edge2D] {
304        &self.edges
305    }
306
307    /// All node scans rendered at their optimized poses.
308    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    /// Integrates one odometry step and the beam-ordered body-frame scan at its end.
316    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                // Verified a little per frame by `background_step`.
332                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    /// Inflates the pending edge covariance along directions the front end
387    /// could not observe during this step.
388    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    /// Information of the odometry edge ending at a node whose front-end yaw
404    /// is `to_yaw`.
405    ///
406    /// Odometry scale and heading errors are systematic, so unobserved motion
407    /// grows the standard deviation linearly with distance:
408    /// `Σ_xy = σ_xy² I + κ² M Mᵀ` with `M = Σ dₖ wₖ vₖ vₖᵀ` over the weak
409    /// directions `vₖ`. [`optimize_pose_graph`] expresses the translational
410    /// error in the `to` frame, so `Σ_xy` is rotated into it.
411    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    /// Older nodes near node `index`, closest first.
451    fn loop_candidates(&self, index: usize) -> Vec<usize> {
452        // A node is never its own loop candidate (gap 0 would also break
453        // `register_candidate`'s `index - 1` on the first node).
454        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    /// Coarse-to-fine point-to-line registration for loop verification.
478    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    /// Seeds around the accepted alignment for the ambiguity check.
502    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    /// Whether re-registering from any of `seeds` finds a distinct
512    /// alternative alignment that scores nearly as well as the accepted one.
513    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    /// Registers node `index` against the submap around `candidate`;
541    /// `None` when there is too little data or the match fails the gates.
542    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    /// Adds an accepted loop edge and re-optimizes (now, or later when
600    /// deferred) if it disagrees with the graph. Returns whether it
601    /// optimized now.
602    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            // A new edge restarts the iteration budget.
617            self.optimization_iterations = 0;
618            self.optimization_pending = true;
619            false
620        } else {
621            self.optimize();
622            true
623        }
624    }
625
626    /// Runs one bounded unit of queued loop-detection work: one candidate
627    /// registration, or one batch of ambiguity re-registrations.
628    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; // This node is done: one closure per node.
641            } 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; // No candidate left for this node.
650        };
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    /// Spreads deferred work over frames: each call either takes one small
665    /// step of queued loop verification (one registration, or two ambiguity
666    /// re-registrations) or, once no loop check is queued, runs at most
667    /// `max_iterations` optimizer iterations if a loop edge left the graph
668    /// inconsistent. Call once per frame; returns whether work remains.
669    pub fn background_step(&mut self, max_iterations: usize) -> bool {
670        // One kind of work per call keeps every call short; loop checks go
671        // first so new edges join the next optimization.
672        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    /// Whether [`Self::background_step`] still has queued loop checks or a
681    /// pending optimization.
682    pub fn background_pending(&self) -> bool {
683        !self.loop_queue.is_empty() || self.optimization_pending
684    }
685
686    /// Re-optimizes the pose graph over all odometry and loop edges.
687    ///
688    /// [`Self::update`] calls this when a loop closure disagrees with the
689    /// graph; call it explicitly to fold in consistent loop edges, e.g.
690    /// before reading the final trajectory.
691    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    /// Whether a disagreeing loop edge is waiting for (more) optimization.
698    /// Only set when [`LidarGraphSlamConfig::deferred_optimization`] is on.
699    pub fn optimization_pending(&self) -> bool {
700        self.optimization_pending
701    }
702
703    /// Runs at most `max_iterations` optimizer iterations if optimization is
704    /// pending, warm-started from the current node poses, and returns whether
705    /// it is still pending. Call once per frame to bound the work per frame.
706    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            // Inexact inner solves may never meet the convergence test;
716            // stop after the same budget a one-shot optimization gets.
717            self.optimization_pending =
718                !converged && self.optimization_iterations < self.config.pose_graph.max_iterations;
719        }
720        self.optimization_pending
721    }
722
723    /// Optimizes the node poses in place from `damping`; returns whether it
724    /// converged and the damping it ended with.
725    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    /// Drives a closed circle with a LiDAR-blind stretch of biased odometry,
810    /// calling `each_frame` after every update. Returns the truth and the
811    /// number of accepted loop closures.
812    fn blind_loop(
813        slam: &mut LidarGraphSlam,
814        start: Pose2D,
815        mut each_frame: impl FnMut(&mut LidarGraphSlam, &LidarGraphSlamUpdate),
816    ) -> (Pose2D, usize) {
817        // Drive a closed circle of radius 2 m back to the start.
818        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                // LiDAR blind: biased odometry alone carries the front end.
828                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                    // The front end lost its map, so it cannot snap back.
833                    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}