NDT Scan Matcher
NDT Scan Matcher
NDT (Normal Distributions Transform) Scan Matcher is the primary absolute localization algorithm in Autoware. It divides the pre-built 3D point cloud map (PCD Map) into voxel grids and computes the normal distribution (mean vector and covariance matrix) for points within each voxel. At runtime, the algorithm finds a rigid body transformation that maximizes the probability of current LiDAR scan points falling within the map voxel distributions.
NDT Voxel Grid and Gaussian Distribution Alignment Visualization
Algorithm Flow
NDT Scan Matcher Complete Runtime Flow
NDT Mathematical Principles
Score = Σᵢ exp(-½ (xᵢ - μⱼ)ᵀ Σⱼ⁻¹ (xᵢ - μⱼ)) — where xᵢ is the transformed scan point, μⱼ and Σⱼ are the mean and covariance of the containing voxel. Optimization uses Newton's method for iterative pose updates.
ROS 2 Interfaces
Subscribers
| Topic | Message Type | Description |
|---|---|---|
ekf_pose_with_covariance | PoseWithCovarianceStamped | EKF predicted pose, used as NDT optimization initial value |
points_raw | PointCloud2 | LiDAR raw point cloud |
regularization_pose_with_covariance | PoseWithCovarianceStamped | Optional GNSS regularization pose |
Publishers
| Topic | Message Type | Description |
|---|---|---|
ndt_pose | PoseStamped | NDT estimated pose |
ndt_pose_with_covariance | PoseWithCovarianceStamped | NDT estimated pose (with covariance) |
points_aligned | PointCloud2 | Aligned point cloud (for debugging) |
transform_probability | Float32Stamped | Matching score |
iteration_num | Int32Stamped | Number of iterations |
initial_to_result_distance | Float32Stamped | Distance from initial to result |
exe_time_ms | Float32Stamped | Execution time (milliseconds) |
Services
| Service | Type | Description |
|---|---|---|
ndt_align_srv | PoseWithCovarianceStamped | Monte Carlo initial pose estimation |
trigger_node_srv | SetBool | Activate/deactivate node |
Key Parameters
| Parameter | Default | Description | Port Tuning Suggestion |
|---|---|---|---|
resolution | 2.0 m | NDT voxel grid resolution | Open areas 3.0m, dense areas 1.5m |
max_iterations | 30 | Maximum iterations | Frequent ceiling hits indicate poor initial pose |
trans_epsilon | 0.01 | Convergence threshold | Usually no modification needed |
num_threads | 4 | Parallel thread count | Adjust based on CPU core count |
converged_param_type | 1 (NVTL) | Score evaluation method | Keep NVTL unchanged |
converged_param_nearest_voxel_transformation_likelihood | 2.3 | NVTL convergence threshold | Adjust based on field test data |
initial_to_result_distance_tolerance_m | 3.0 m | Max distance from initial to result | Tighten for repetitive scenarios |
particles_num | 200 | Monte Carlo particle count | Increase to 500 for repetitive scenarios |
dynamic_map_loading.update_distance | 20.0 m | Travel distance to trigger map update | Adjust based on site size |
dynamic_map_loading.map_radius | 150.0 m | Map loading radius | Keep larger than effective LiDAR range |
Source Code Reading Order
- config/ndt_scan_matcher.param.yaml — Understand all configurable parameters
- src/ndt_scan_matcher_core.cpp constructor — Node initialization
- callback_sensor_points_main — Main runtime path
- service_ndt_align_main + align_pose — Initialization service
- estimate_covariance — Covariance estimation strategy
- map_update_module.cpp — Dynamic map loading
- particle.cpp + ndt_omp/* — Underlying optimization implementation