Skip to main content

rust_robotics_slam/
scan_to_map.rs

1//! Scan-to-map 2D LiDAR odometry with a bounded local submap.
2//!
3//! Scan-to-scan matching registers each new scan against the previous one, so
4//! per-scan matching noise accumulates as a random walk. This module instead
5//! keeps a **local submap** of recent keyframe scans stored in the corrected
6//! world frame and registers every new scan against it:
7//!
8//! ```text
9//! predicted_pose = corrected_pose ⊕ odom_delta
10//! corrected_pose = ICP(submap_world, scan_body, seed = predicted_pose)
11//! submap        ← submap ∪ (corrected_pose * scan_body)   (keyframes only)
12//! ```
13//!
14//! Because the submap is anchored to the running corrected pose, each accepted
15//! correction "sticks": the next scan is matched against geometry that already
16//! absorbed previous corrections, instead of against a single neighbor.
17//!
18//! Registration is point-to-line Gauss-Newton: submap points carry a normal
19//! estimated from their neighbors in beam order, and correspondences farther
20//! than [`ScanToMapConfig::max_correspondence_distance`] are rejected so newly
21//! observed geometry does not drag the estimate. Corrections larger than the
22//! configured gates are rejected and the odometry prediction is kept.
23//!
24//! Setting `max_scans = 1` and a zero keyframe threshold reduces the matcher to
25//! seeded scan-to-scan matching ([`ScanToMapConfig::scan_to_scan`]), so both
26//! modes can be compared under identical inputs.
27//!
28//! Design notes: `docs/scan_to_map_icp_design.md`.
29
30use 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/// Tuning parameters for [`ScanToMapMatcher`].
38#[derive(Debug, Clone, Copy, PartialEq)]
39pub struct ScanToMapConfig {
40    /// Maximum number of keyframe scans kept in the submap.
41    pub max_scans: usize,
42    /// Submap points farther than this from the predicted pose are ignored \[m\].
43    pub max_radius: f64,
44    /// Minimum translation since the last keyframe before a scan is inserted \[m\].
45    pub keyframe_translation: f64,
46    /// Minimum rotation since the last keyframe before a scan is inserted \[rad\].
47    pub keyframe_yaw: f64,
48    /// Voxel edge used to decimate scans before matching and insertion \[m\].
49    /// `0.0` disables decimation.
50    pub voxel_size: f64,
51    /// Gauss-Newton iterations (each one re-associates correspondences).
52    pub max_iterations: usize,
53    /// Correspondences farther than this are rejected \[m\].
54    pub max_correspondence_distance: f64,
55    /// Huber threshold on the point-to-line residual \[m\].
56    pub huber_delta: f64,
57    /// Minimum number of accepted correspondences for a valid match.
58    pub min_correspondences: usize,
59    /// Reject matches whose correction exceeds this translation \[m\].
60    pub max_correction_translation: f64,
61    /// Reject matches whose correction exceeds this rotation \[rad\].
62    pub max_correction_yaw: f64,
63    /// Reject matches whose mean absolute point-to-line residual exceeds this \[m\].
64    pub max_mean_residual: f64,
65    /// Neighbor search radius used for normal estimation in beam order \[m\].
66    pub normal_neighbor_distance: f64,
67    /// When `λ_min / λ_max` of the translational Hessian falls below this
68    /// ratio (e.g. in a featureless corridor), the Gauss-Newton step along the
69    /// weak direction is discarded and the odometry prediction is kept there.
70    /// `0.0` disables the projection.
71    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    /// Seeded scan-to-scan preset: the submap holds only the previous scan.
97    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/// Outcome of a single [`ScanToMapMatcher::update`] call.
108#[derive(Debug, Clone, Copy, PartialEq, Eq)]
109pub enum MatchStatus {
110    /// The submap was too sparse to match against; the prediction was kept.
111    Bootstrap,
112    /// The match passed all gates and its correction was applied.
113    Accepted,
114    /// Too few correspondences survived the distance gate.
115    RejectedTooFewCorrespondences,
116    /// The correction exceeded the translation or rotation gate.
117    RejectedLargeCorrection,
118    /// The final mean residual exceeded [`ScanToMapConfig::max_mean_residual`].
119    RejectedHighResidual,
120}
121
122impl MatchStatus {
123    /// Whether the match correction was applied.
124    pub fn is_accepted(self) -> bool {
125        self == Self::Accepted
126    }
127}
128
129/// Diagnostics returned by [`ScanToMapMatcher::update`].
130#[derive(Debug, Clone, Copy, PartialEq)]
131pub struct ScanToMapUpdate {
132    /// Pose predicted from the previous corrected pose and the odometry delta.
133    pub predicted_pose: Pose2D,
134    /// Pose after registration (equal to `predicted_pose` unless accepted).
135    pub corrected_pose: Pose2D,
136    /// Match status and gate decision.
137    pub status: MatchStatus,
138    /// Gauss-Newton iterations run.
139    pub iterations: usize,
140    /// Correspondences used in the final iteration.
141    pub correspondences: usize,
142    /// Mean absolute point-to-line residual after registration \[m\].
143    pub mean_residual: f64,
144    /// Registration normal matrix (see [`ScanRegistration::hessian`]);
145    /// zero when no registration ran.
146    pub hessian: Matrix3<f64>,
147    /// Submap points inside the matching radius.
148    pub submap_points: usize,
149    /// Whether this scan was inserted into the submap as a keyframe.
150    pub inserted_keyframe: bool,
151}
152
153/// A line segment used by [`ray_cast_ranges`] to synthesize range scans.
154#[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/// Incremental scan-to-map LiDAR odometry.
173#[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    /// Creates a matcher whose corrected trajectory starts at `initial_pose`.
183    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    /// Current corrected pose.
193    pub fn pose(&self) -> Pose2D {
194        self.pose
195    }
196
197    /// Matcher configuration.
198    pub fn config(&self) -> &ScanToMapConfig {
199        &self.config
200    }
201
202    /// Number of keyframe scans currently held in the submap.
203    pub fn keyframe_count(&self) -> usize {
204        self.keyframes.len()
205    }
206
207    /// All submap points in the corrected world frame.
208    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    /// Drops the submap and restarts the trajectory at `pose`.
216    pub fn reset(&mut self, pose: Pose2D) {
217        self.pose = pose;
218        self.keyframes.clear();
219        self.last_keyframe_pose = None;
220    }
221
222    /// Integrates one odometry step and registers the scan taken at its end.
223    ///
224    /// * `odom_delta` - body-frame motion since the previous call
225    ///   (`x` forward, `y` left, `yaw` counter-clockwise).
226    /// * `scan_body` - scan points in the body frame, **ordered by beam angle**
227    ///   (the order is used to estimate normals).
228    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        // Newest keyframes first so overlapping voxels keep the freshest point.
317        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/// Result of [`register_point_to_line`].
338#[derive(Debug, Clone, Copy, PartialEq)]
339pub struct ScanRegistration {
340    /// Estimated pose mapping source (body) points onto the target frame.
341    pub pose: Pose2D,
342    /// Gauss-Newton iterations run.
343    pub iterations: usize,
344    /// Correspondences inside `max_correspondence_distance` in the last iteration.
345    pub correspondences: usize,
346    /// Mean absolute point-to-line residual of those correspondences \[m\].
347    pub mean_residual: f64,
348    /// Gauss-Newton normal matrix `Σ w J Jᵀ` of the last iteration over the
349    /// world-frame `(x, y, yaw)`. Its small eigenvalues reveal directions the
350    /// geometry cannot observe (e.g. along a featureless corridor).
351    pub hessian: Matrix3<f64>,
352}
353
354/// Transforms a beam-ordered body-frame scan to `pose` and attaches normals.
355///
356/// Points without a reliable normal are dropped; `voxel_size > 0` keeps one
357/// point per voxel. Returns `(points, normals)` in the frame of `pose`.
358pub 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
384/// Registers body-frame `source` points against `target` points with normals,
385/// starting from `seed`, by point-to-line Gauss-Newton on `(x, y, yaw)`.
386///
387/// Uses `max_iterations`, `max_correspondence_distance`, and `huber_delta`
388/// from `config`; the acceptance gates are left to the caller.
389pub 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    // Whether `terms` were measured at `pose` (false right after a step).
405    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        // Light Levenberg damping keeps degenerate geometry (corridors) bounded.
416        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    // The iteration cap stopped it mid-descent: report statistics for the
437    // pose returned, which is what callers gate on, not the one before it.
438    // (A converged step is too small to change them.)
439    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/// Point-to-line Gauss-Newton terms of one registration iterate.
457#[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
497/// Weakest translation direction of a registration Hessian, if its
498/// `λ_min / λ_max` ratio is below `ratio_threshold`.
499pub 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
504/// `(λ_min / λ_max, weakest unit direction)` of the translational 2×2 block of
505/// a registration Hessian. A ratio near 0 means translation along the
506/// direction is unobservable; an all-zero Hessian returns ratio 0.
507pub 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    // Eigenvector of the larger eigenvalue has angle 0.5·atan2(2b, a − c).
513    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
521/// Dense uniform grid answering "nearest point within `radius`" queries.
522///
523/// The cell edge equals the radius, so the 3×3 block around the query cell
524/// contains every candidate. Cells are stored CSR-style (offsets + indices).
525struct 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
616/// Estimates unit normals for a beam-ordered scan by local PCA.
617///
618/// A point gets `None` when fewer than two neighbors within `max_distance`
619/// exist in its ±2 beam window, or when the neighborhood is not line-like.
620pub 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            // Closed-form eigen decomposition of the 2×2 scatter matrix.
649            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            // Principal (line) direction angle; the normal is perpendicular.
656            let angle = 0.5 * (2.0 * sxy).atan2(sxx - syy);
657            Some(Vector2::new(-angle.sin(), angle.cos()))
658        })
659        .collect()
660}
661
662/// Composes a body-frame delta onto `pose`: `pose ⊕ delta`.
663pub 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
672/// Body-frame delta that takes `from` to `to`: `from⁻¹ ⊕ to`.
673pub 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
684/// Transforms body-frame scan points into the world frame at `pose`.
685pub 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
694/// Ranges of `beam_count` beams spread uniformly over `[-π, π)` from `pose`.
695///
696/// Beams that hit nothing within `max_range` return `f64::INFINITY`.
697pub 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
713/// Ranges of `beam_count` beams at body-frame angles
714/// `angle_min + i * angle_increment` from `pose`, e.g. a 180° front laser.
715///
716/// Beams that hit nothing within `max_range` return `f64::INFINITY`.
717pub 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
739/// Converts [`ray_cast_ranges`] output to body-frame points, skipping misses.
740pub 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
752/// Body-frame bearing \[rad\] of beam `beam` in a full 360° scan of
753/// `beam_count` beams, as used by [`ray_cast_ranges`] and [`ranges_to_points`].
754pub 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        // An off-axis box and a stub wall break the room's symmetry.
813        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        // Beams at -pi, -pi/2, 0, pi/2.
893        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        // A beam toward the box is occluded before the +y wall.
898        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        // Odometry overestimates the motion and misses part of the turn.
929        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}