Expand description
Scan-to-map 2D LiDAR odometry with a bounded local submap.
Scan-to-scan matching registers each new scan against the previous one, so per-scan matching noise accumulates as a random walk. This module instead keeps a local submap of recent keyframe scans stored in the corrected world frame and registers every new scan against it:
predicted_pose = corrected_pose ⊕ odom_delta
corrected_pose = ICP(submap_world, scan_body, seed = predicted_pose)
submap ← submap ∪ (corrected_pose * scan_body) (keyframes only)Because the submap is anchored to the running corrected pose, each accepted correction “sticks”: the next scan is matched against geometry that already absorbed previous corrections, instead of against a single neighbor.
Registration is point-to-line Gauss-Newton: submap points carry a normal
estimated from their neighbors in beam order, and correspondences farther
than ScanToMapConfig::max_correspondence_distance are rejected so newly
observed geometry does not drag the estimate. Corrections larger than the
configured gates are rejected and the odometry prediction is kept.
Setting max_scans = 1 and a zero keyframe threshold reduces the matcher to
seeded scan-to-scan matching (ScanToMapConfig::scan_to_scan), so both
modes can be compared under identical inputs.
Design notes: docs/scan_to_map_icp_design.md.
Structs§
- Line
Segment - A line segment used by
ray_cast_rangesto synthesize range scans. - Scan
Registration - Result of
register_point_to_line. - Scan
ToMap Config - Tuning parameters for
ScanToMapMatcher. - Scan
ToMap Matcher - Incremental scan-to-map LiDAR odometry.
- Scan
ToMap Update - Diagnostics returned by
ScanToMapMatcher::update.
Enums§
- Match
Status - Outcome of a single
ScanToMapMatcher::updatecall.
Functions§
- beam_
angle - Body-frame bearing [rad] of beam
beamin a full 360° scan ofbeam_countbeams, as used byray_cast_rangesandranges_to_points. - compose_
pose - Composes a body-frame delta onto
pose:pose ⊕ delta. - degenerate_
direction - Weakest translation direction of a registration Hessian, if its
λ_min / λ_maxratio is belowratio_threshold. - estimate_
scan_ normals - Estimates unit normals for a beam-ordered scan by local PCA.
- ranges_
to_ points - Converts
ray_cast_rangesoutput to body-frame points, skipping misses. - ray_
cast_ ranges - Ranges of
beam_countbeams spread uniformly over[-π, π)frompose. - ray_
cast_ ranges_ fov - Ranges of
beam_countbeams at body-frame anglesangle_min + i * angle_incrementfrompose, e.g. a 180° front laser. - register_
point_ to_ line - Registers body-frame
sourcepoints againsttargetpoints with normals, starting fromseed, by point-to-line Gauss-Newton on(x, y, yaw). - relative_
pose - Body-frame delta that takes
fromtoto:from⁻¹ ⊕ to. - scan_
points_ with_ normals - Transforms a beam-ordered body-frame scan to
poseand attaches normals. - transform_
scan_ to_ world - Transforms body-frame scan points into the world frame at
pose. - translational_
observability (λ_min / λ_max, weakest unit direction)of the translational 2×2 block of a registration Hessian. A ratio near 0 means translation along the direction is unobservable; an all-zero Hessian returns ratio 0.