Skip to main content

Module scan_to_map

Module scan_to_map 

Source
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§

LineSegment
A line segment used by ray_cast_ranges to synthesize range scans.
ScanRegistration
Result of register_point_to_line.
ScanToMapConfig
Tuning parameters for ScanToMapMatcher.
ScanToMapMatcher
Incremental scan-to-map LiDAR odometry.
ScanToMapUpdate
Diagnostics returned by ScanToMapMatcher::update.

Enums§

MatchStatus
Outcome of a single ScanToMapMatcher::update call.

Functions§

beam_angle
Body-frame bearing [rad] of beam beam in a full 360° scan of beam_count beams, as used by ray_cast_ranges and ranges_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 / λ_max ratio is below ratio_threshold.
estimate_scan_normals
Estimates unit normals for a beam-ordered scan by local PCA.
ranges_to_points
Converts ray_cast_ranges output to body-frame points, skipping misses.
ray_cast_ranges
Ranges of beam_count beams spread uniformly over [-π, π) from pose.
ray_cast_ranges_fov
Ranges of beam_count beams at body-frame angles angle_min + i * angle_increment from pose, e.g. a 180° front laser.
register_point_to_line
Registers body-frame source points against target points with normals, starting from seed, by point-to-line Gauss-Newton on (x, y, yaw).
relative_pose
Body-frame delta that takes from to to: from⁻¹ ⊕ to.
scan_points_with_normals
Transforms a beam-ordered body-frame scan to pose and 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.