Skip to content

autoware_ndt_scan_matcher#

Purpose#

autoware_ndt_scan_matcher is a package for position estimation using the NDT scan matching method.

There are two main functions in this package:

  • estimate position by scan matching
  • estimate initial position via the ROS service using the Monte Carlo method

One optional function is regularization. Please see the regularization chapter in the back for details. It is disabled by default.

Inputs / Outputs#

Input#

Name Type Description
ekf_pose_with_covariance geometry_msgs::msg::PoseWithCovarianceStamped initial pose
points_raw sensor_msgs::msg::PointCloud2 sensor pointcloud
sensing/gnss/pose_with_covariance sensor_msgs::msg::PoseWithCovarianceStamped base position for regularization term

sensing/gnss/pose_with_covariance is required only when regularization is enabled.

Output#

Name Type Description
ndt_pose geometry_msgs::msg::PoseStamped estimated pose
ndt_pose_with_covariance geometry_msgs::msg::PoseWithCovarianceStamped estimated pose with covariance
/diagnostics diagnostic_msgs::msg::DiagnosticArray diagnostics
points_aligned sensor_msgs::msg::PointCloud2 [debug topic] pointcloud aligned by scan matching
points_aligned_no_ground sensor_msgs::msg::PointCloud2 [debug topic] no ground pointcloud aligned by scan matching
initial_pose_with_covariance geometry_msgs::msg::PoseWithCovarianceStamped [debug topic] initial pose used in scan matching
multi_ndt_pose geometry_msgs::msg::PoseArray [debug topic] estimated poses from multiple initial poses in real-time covariance estimation
multi_initial_pose geometry_msgs::msg::PoseArray [debug topic] initial poses for real-time covariance estimation
exe_time_ms tier4_debug_msgs::msg::Float32Stamped [debug topic] execution time for scan matching [ms]
transform_probability tier4_debug_msgs::msg::Float32Stamped [debug topic] score of scan matching
no_ground_transform_probability tier4_debug_msgs::msg::Float32Stamped [debug topic] score of scan matching based on no ground LiDAR scan
iteration_num tier4_debug_msgs::msg::Int32Stamped [debug topic] number of scan matching iterations
initial_to_result_relative_pose geometry_msgs::msg::PoseStamped [debug topic] relative pose between the initial point and the convergence point
initial_to_result_distance tier4_debug_msgs::msg::Float32Stamped [debug topic] distance difference between the initial point and the convergence point [m]
initial_to_result_distance_old tier4_debug_msgs::msg::Float32Stamped [debug topic] distance difference between the older of the two initial points used in linear interpolation and the convergence point [m]
initial_to_result_distance_new tier4_debug_msgs::msg::Float32Stamped [debug topic] distance difference between the newer of the two initial points used in linear interpolation and the convergence point [m]
ndt_marker visualization_msgs::msg::MarkerArray [debug topic] markers for debugging
monte_carlo_initial_pose_marker visualization_msgs::msg::MarkerArray [debug topic] particles used in initial position estimation

Service#

Name Type Description
ndt_align_srv autoware_localization_srvs::srv::PoseWithCovarianceStamped service to estimate initial pose

Parameters#

Core Parameters#

Frame#

Name Type Description Default Range
base_frame string Vehicle reference frame. base_link N/A
ndt_base_frame string NDT reference frame. ndt_base_link N/A
map_frame string Map frame. map N/A

Sensor Points#

Name Type Description Default Range
timeout_sec float Tolerance of timestamp difference between current time and sensor pointcloud. [sec] 1 ≥0.0
required_distance float Required distance of input sensor points [m]. If the max distance of input sensor points is lower than this value, the scan matching will not be performed. 10 N/A

Ndt#

Name Type Description Default Range
trans_epsilon float The maximum difference between two consecutive transformations in order to consider convergence. 0.01 ≥0.0
step_size float The newton line search maximum step length. 0.1 ≥0.0
resolution float The ND voxel grid resolution. 2 ≥0.0
max_iterations float The number of iterations required to calculate alignment. 30 ≥1
num_threads float Number of threads used for parallel computing. 4 ≥1

Initial Pose Estimation#

Name Type Description Default Range
particles_num float The number of particles to estimate initial pose. 200 ≥1
n_startup_trials float The number of initial random trials in the TPE (Tree-Structured Parzen Estimator). This value should be equal to or less than 'initial_estimate_particles_num' and more than 0. If it is equal to 'initial_estimate_particles_num', the search will be the same as a full random search. 100 ≥1

Validation#

Name Type Description Default Range
initial_pose_timeout_sec float Tolerance of timestamp difference between initial_pose and sensor pointcloud. [sec] 1 ≥0.0
initial_pose_distance_tolerance_m float Tolerance of distance difference between two initial poses used for linear interpolation. [m] 10 ≥0.0
initial_to_result_distance_tolerance_m float Tolerance of distance difference from initial pose to result pose. [m] 3 ≥0.0
critical_upper_bound_exe_time_ms float The execution time which means probably NDT cannot matches scans properly. [ms] 100 ≥0.0
skipping_publish_num float Tolerance for the number for times rejected estimation results consecutively. 5 ≥1

Score Estimation#

Name Type Description Default Range
converged_param_type float Converged param type. 0=TRANSFORM_PROBABILITY, 1=NEAREST_VOXEL_TRANSFORMATION_LIKELIHOOD 1 ≥0
≤1
converged_param_transform_probability float If converged_param_type is 0, threshold for deciding whether to trust the estimation result. 3 ≥0.0
converged_param_nearest_voxel_transformation_likelihood float If converged_param_type is 1, threshold for deciding whether to trust the estimation result. 2.3 ≥0.0

Covariance#

Name Type Description Default Range
output_pose_covariance array The covariance of output pose. Note that this covariance matrix is empirically derived. [0.0225, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0225, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0225, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.000625, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.000625, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.000625] N/A

Regularization#

Abstract#

This is a function that adds the regularization term to the NDT optimization problem as follows.

\[ \begin{align} \min_{\mathbf{R},\mathbf{t}} \mathrm{NDT}(\mathbf{R},\mathbf{t}) +\mathrm{scale\ factor}\cdot \left| \mathbf{R}^\top (\mathbf{t_{base}-\mathbf{t}}) \cdot \begin{pmatrix} 1\\ 0\\ 0 \end{pmatrix} \right|^2 \end{align} \]

, where t_base is base position measured by GNSS or other means. NDT(R,t) stands for the pure NDT cost function. The regularization term shifts the optimal solution to the base position in the longitudinal direction of the vehicle. Only errors along the longitudinal direction with respect to the base position are considered; errors along Z-axis and lateral-axis error are not considered.

Although the regularization term has rotation as a parameter, the gradient and hessian associated with it is not computed to stabilize the optimization. Specifically, the gradients are computed as follows.

\[ \begin{align} &g_x=\nabla_x \mathrm{NDT}(\mathbf{R},\mathbf{t}) + 2 \mathrm{scale\ factor} \cos\theta_z\cdot e_{\mathrm{longitudinal}} \\ &g_y=\nabla_y \mathrm{NDT}(\mathbf{R},\mathbf{t}) + 2 \mathrm{scale\ factor} \sin\theta_z\cdot e_{\mathrm{longitudinal}} \\ &g_z=\nabla_z \mathrm{NDT}(\mathbf{R},\mathbf{t}) \\ &g_\mathbf{R}=\nabla_\mathbf{R} \mathrm{NDT}(\mathbf{R},\mathbf{t}) \end{align} \]

Regularization is disabled by default. If you wish to use it, please edit the following parameters to enable it.

Where is regularization available#

This feature is effective on feature-less roads where GNSS is available, such as

  • bridges
  • highways
  • farm roads

By remapping the base position topic to something other than GNSS, as described below, it can be valid outside of these.

Using other base position#

Other than GNSS, you can give other global position topics obtained from magnetic markers, visual markers or etc. if they are available in your environment. (Currently Autoware does not provide a node that gives such pose.) To use your topic for regularization, you need to remap the input_regularization_pose_topic with your topic in ndt_scan_matcher.launch.xml. By default, it is remapped with /sensing/gnss/pose_with_covariance.

Limitations#

Since this function determines the base position by linear interpolation from the recently subscribed poses, topics that are published at a low frequency relative to the driving speed cannot be used. Inappropriate linear interpolation may result in bad optimization results.

When using GNSS for base location, the regularization can have negative effects in tunnels, indoors, and near skyscrapers. This is because if the base position is far off from the true value, NDT scan matching may converge to inappropriate optimal position.

Parameters#

Name Type Description Default Range
enable boolean Regularization switch. 0 N/A
scale_factor float Regularization scale factor. 0.01 ≥0.0

Regularization is disabled by default because GNSS is not always accurate enough to serve the appropriate base position in any scenes.

If the scale_factor is too large, the NDT will be drawn to the base position and scan matching may fail. Conversely, if it is too small, the regularization benefit will be lost.

Note that setting scale_factor to 0 is equivalent to disabling regularization.

Example#

The following figures show tested maps.

  • The left is a map with enough features that NDT can successfully localize.
  • The right is a map with so few features that the NDT cannot localize well.

drawing drawing

The following figures show the trajectories estimated on the feature-less map with standard NDT and regularization-enabled NDT, respectively. The color of the trajectory indicates the error (meter) from the reference trajectory, which is computed with the feature-rich map.

  • The left figure shows that the pure NDT causes a longitudinal error in the bridge and is not able to recover.
  • The right figure shows that the regularization suppresses the longitudinal error.

drawing drawing

Dynamic map loading#

Autoware supports dynamic map loading feature for ndt_scan_matcher. Using this feature, NDT dynamically requests for the surrounding pointcloud map to pointcloud_map_loader, and then receive and preprocess the map in an online fashion.

Using the feature, ndt_scan_matcher can theoretically handle any large size maps in terms of memory usage. (Note that it is still possible that there exists a limitation due to other factors, e.g. floating-point error)

drawing

Additional interfaces#

Additional outputs#

Name Type Description
debug/loaded_pointcloud_map sensor_msgs::msg::PointCloud2 pointcloud maps used for localization (for debug)

Additional client#

Name Type Description
client_map_loader autoware_map_msgs::srv::GetDifferentialPointCloudMap map loading client

Parameters#

Name Type Description Default Range
update_distance float Dynamic map loading distance. 20 ≥0.0
map_radius float Dynamic map loading loading radius. 150 ≥0.0
lidar_radius float Radius of input LiDAR range (used for diagnostics of dynamic map loading). 100 ≥0.0

Notes for dynamic map loading#

To use dynamic map loading feature for ndt_scan_matcher, you also need to split the PCD files into grids (recommended size: 20[m] x 20[m])

Note that the dynamic map loading may FAIL if the map is split into two or more large size map (e.g. 1000[m] x 1000[m]). Please provide either of

  • one PCD map file
  • multiple PCD map files divided into small size (~20[m])

Here is a split PCD map for sample-map-rosbag from Autoware tutorial: sample-map-rosbag_split.zip

PCD files How NDT loads map(s)
single file at once (standard)
multiple files dynamically

Scan matching score based on no ground LiDAR scan#

Abstract#

This is a function that uses no ground LiDAR scan to estimate the scan matching score. This score can reflect the current localization performance more accurately. related issue.

Parameters#

Name Type Description Default Range
enable boolean A flag for using scan matching score based on de-grounded LiDAR scan. 0 N/A
z_margin_for_ground_removal float If lidar_point.z - base_link.z <= this threshold , the point will be removed. 0.8 ≥0.0

2D real-time covariance estimation#

Abstract#

Initially, the covariance of NDT scan matching is a fixed value(FIXED_VALUE mode). So, three modes are provided for 2D covariance (xx, xy, yx, yy) estimation in real time: LAPLACE_APPROXIMATION, MULTI_NDT, and MULTI_NDT_SCORE. LAPLACE_APPROXIMATION calculates the inverse matrix of the XY (2x2) part of the Hessian obtained by NDT scan matching and uses it as the covariance matrix. On the other hand, MULTI_NDT, and MULTI_NDT_SCORE use NDT convergence from multiple initial poses to obtain 2D covariance. Ideally, the arrangement of multiple initial poses is efficiently limited by the Hessian matrix of the NDT score function. In this implementation, the number of initial positions is fixed to simplify the code. To obtain the covariance, MULTI_NDT computes until convergence at each initial position, while MULTI_NDT_SCORE uses the nearest voxel transformation likelihood. The covariance can be seen as error ellipse from ndt_pose_with_covariance setting on rviz2. original paper.

drawing

Note that this function may spoil healthy system behavior if it consumes much calculation resources.

Parameters#

There are three types in the calculation of 2D covariance in real time.You can select the method by changing covariance_estimation_type. initial_pose_offset_model is rotated around (x,y) = (0,0) in the direction of the first principal component of the Hessian matrix. initial_pose_offset_model_x & initial_pose_offset_model_y must have the same number of elements. In MULTI_NDT_SCORE mode, the scale of the output 2D covariance can be adjusted according to the temperature.

Name Type Description Default Range
covariance_estimation_type float 2D Real-time Converged estimation type. 0=FIXED_VALUE, 1=LAPLACE_APPROXIMATION, 2=MULTI_NDT, 3=MULTI_NDT_SCORE (FIXED_VALUE mode does not perform 2D Real-time Converged estimation) 0 ≥0
≤3
initial_pose_offset_model_x array Offset arrangement in covariance estimation [m]. initial_pose_offset_model_x & initial_pose_offset_model_y must have the same number of elements. [0.0, 0.0, 0.5, -0.5, 1.0, -1.0] N/A
initial_pose_offset_model_y array Offset arrangement in covariance estimation [m]. initial_pose_offset_model_x & initial_pose_offset_model_y must have the same number of elements. [0.5, -0.5, 0.0, 0.0, 0.0, 0.0] N/A
temperature float In MULTI_NDT_SCORE, the parameter that adjusts the estimated 2D covariance 0.05 >0
scale_factor float Scale value for adjusting the estimated covariance by a constant multiplication 1.0 >0

Diagnostics#

scan_matching_status#

drawing

Name Description Transition condition to Warning Transition condition to Error Whether to reject the estimation result (affects skipping_publish_num)
topic_time_stamp the time stamp of input topic none none no
sensor_points_size the size of sensor points the size is 0 none yes
sensor_points_delay_time_sec the delay time of sensor points the time is longer than sensor_points.timeout_sec none yes
is_succeed_transform_sensor_points whether transform sensor points is succeed or not none failed yes
sensor_points_max_distance the max distance of sensor points the max distance is shorter than sensor_points.required_distance none yes
is_activated whether the node is in the "activate" state or not not "activate" state none if is_activated is false, then estimation is not executed and skipping_publish_num is set to 0.
is_succeed_interpolate_initial_pose whether the interpolate of initial pose is succeed or not failed.
(1) the size of initial_pose_buffer_ is smaller than 2.
(2) the timestamp difference between initial_pose and sensor pointcloud is longer than validation.initial_pose_timeout_sec.
(3) distance difference between two initial poses used for linear interpolation is longer than validation.initial_pose_distance_tolerance_m
none yes
is_set_map_points whether the map points is set or not not set none yes
iteration_num the number of times calculate alignment the number of times is larger than ndt.max_iterations none yes
local_optimal_solution_oscillation_num the number of times the solution is judged to be oscillating the number of times is larger than 10 none yes
transform_probability the score of how well the map aligns with the sensor points the score is smaller thanscore_estimation.converged_param_transform_probability (only in the case of score_estimation.converged_param_type is 0=TRANSFORM_PROBABILITY) none yes
transform_probability_diff the tp score difference for the current ndt optimization none none no
transform_probability_before the tp score before the current ndt optimization none none no
nearest_voxel_transformation_likelihood the score of how well the map aligns with the sensor points the score is smaller than score_estimation.converged_param_nearest_voxel_transformation_likelihood (only in the case of score_estimation.converged_param_type is 1=NEAREST_VOXEL_TRANSFORMATION_LIKELIHOOD) none yes
nearest_voxel_transformation_likelihood_diff the nvtl score difference for the current ndt optimization none none no
nearest_voxel_transformation_likelihood_before the nvtl score before the current ndt optimization none none no
distance_initial_to_result the distance between the position before convergence processing and the position after the distance is longer than validation.initial_to_result_distance_tolerance_m none no
execution_time the time for convergence processing the time is longer than validation.critical_upper_bound_exe_time_ms none no
skipping_publish_num the number of times rejected estimation results consecutively the number of times is validation.skipping_publish_num or more none -

※The sensor_points_callback shares the same callback group as the trigger_node_service and ndt_align_service. Consequently, if the initial pose estimation takes too long, this diagnostic may become stale.

initial_pose_subscriber_status#

drawing

Name Description Transition condition to Warning Transition condition to Error
topic_time_stamp the time stamp of input topic none none
is_activated whether the node is in the "activate" state or not not "activate" state none
is_expected_frame_id whether the input frame_id is the same as frame.map_frame or not none not the same

regularization_pose_subscriber_status#

drawing

Name Description Transition condition to Warning Transition condition to Error
topic_time_stamp the time stamp of input topic none none

trigger_node_service_status#

drawing

Name Description Transition condition to Warning Transition condition to Error
service_call_time_stamp the time stamp of service calling none none
is_activated whether the node is in the "activate" state or not none none
is_succeed_service whether the process of service is succeed or not none none

※ This diagnostic is only published when the service is called, so it becomes stale after the initial pose estimation is completed.

ndt_align_service_status#

drawing

Name Description Transition condition to Warning Transition condition to Error
service_call_time_stamp the time stamp of service calling none none
is_succeed_transform_initial_pose whether transform initial pose is succeed or not none failed
is_need_rebuild whether it need to rebuild the map. If the map has not been loaded yet or if distance_last_update_position_to_current_position encounters is an Error state, it is considered necessary to reconstruct the map, and is_need_rebuild becomes True. none none
maps_size_before the number of maps before update map none none
is_succeed_call_pcd_loader whether call pcd_loader service is succeed or not failed none
maps_to_add_size the number of maps to be added none none
maps_to_remove_size the number of maps to be removed none none
map_update_execution_time the time for map updating none none
maps_size_after the number of maps after update map none none
is_updated_map whether map is updated. If the map update couldn't be performed or there was no need to update the map, it becomes False none is_updated_map is False but is_need_rebuild is True
is_set_map_points whether the map points is set or not not set none
is_set_sensor_points whether the sensor points is set or not not set none
best_particle_score the best score of particle none none
is_succeed_service whether the process of service is succeed or not failed none

※ This diagnostic is only published when the service is called, so it becomes stale after the initial pose estimation is completed.

map_update_status#

drawing

Name Description Transition condition to Warning Transition condition to Error
timer_callback_time_stamp the time stamp of timer_callback calling none none
is_activated whether the node is in the "activate" state or not not "activate" state none
is_set_last_update_position whether the last_update_position is set or not not set none
distance_last_update_position_to_current_position the distance of last_update_position to current position none (the distance + dynamic_map_loading.lidar_radius) is larger than dynamic_map_loading.map_radius
is_need_rebuild whether it need to rebuild the map. If the map has not been loaded yet or if distance_last_update_position_to_current_position encounters is an Error state, it is considered necessary to reconstruct the map, and is_need_rebuild becomes True. none none
maps_size_before the number of maps before update map none none
is_succeed_call_pcd_loader whether call pcd_loader service is succeed or not failed none
maps_to_add_size the number of maps to be added none none
maps_to_remove_size the number of maps to be removed none none
map_update_execution_time the time for map updating none none
maps_size_after the number of maps after update map none none
is_updated_map whether map is updated. If the map update couldn't be performed or there was no need to update the map, it becomes False none is_updated_map is False but is_need_rebuild is True