ndt scan matcher#
Purpose#
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
Inner-workings / Algorithms#
Flowcharts#
Overview#
callbackInitialPose#
callbackMapPoints#
callbackSensorPoints#
serviceNDTAlign#
timerDiagnostic#
Sequence Diagrams#
callbackSensorPoints and callbackInitialPose#
The execution time of callbackSensorPoints occasionally exceeds 100ms with Intel Core i7-8700, which will block callbackInitialPose if the two callbacks are executed sequentially. The callbacks callbackSensorPoints and callbackInitialPose should be executed in parallel so that callbackSensorPoints does not block callbackInitialPose.
Inputs / Outputs#
Input#
| Name | Type | Description |
|---|---|---|
ekf_pose_with_covariance |
geometry_msgs::msg::PoseWithCovarianceStamped |
initial pose |
pointcloud_map |
sensor_msgs::msg::PointCloud2 |
map pointcloud |
points_raw |
sensor_msgs::msg::PointCloud2 |
sensor pointcloud |
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 |
initial_pose_with_covariance |
geometry_msgs::msg::PoseWithCovarianceStamped |
[debug topic] initial pose used in scan matching |
exe_time_ms |
autoware_debug_msgs::msg::Float32Stamped |
[debug topic] execution time for scan matching [ms] |
transform_probability |
autoware_debug_msgs::msg::Float32Stamped |
[debug topic] score of scan matching |
iteration_num |
autoware_debug_msgs::msg::Int32Stamped |
[debug topic] number of scan matching iterations |
initial_to_result_distance |
autoware_debug_msgs::msg::Float32Stamped |
[debug topic] distance difference between the initial point and the convergence point [m] |
initial_to_result_distance_old |
autoware_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 |
autoware_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#
| Name | Type | Description |
|---|---|---|
base_frame |
string | Vehicle reference frame |
input_sensor_points_queue_size |
int | Subscriber queue size |
ndt_implement_type |
int | NDT implementation type (0=PCL_GENERIC, 1=PCL_MODIFIED, 2=OMP) |
trans_epsilon |
double | The maximum difference between two consecutive transformations in order to consider convergence |
step_size |
double | The newton line search maximum step length |
resolution |
double | The ND voxel grid resolution [m] |
max_iterations |
int | The number of iterations required to calculate alignment |
converged_param_transform_probability |
double | Threshold for deciding whether to trust the estimation result |
omp_neighborhood_search_method |
int | The Neighborhood search method in OMP (0=KDTREE, 1=DIRECT26, 2=DIRECT7, 3=DIRECT1) |
omp_num_threads |
int | Number of threads used for parallel computing |
Assumptions / Known limits#
(Optional) Error detection and handling#
(Optional) Performance characterization#
(Optional) References/External links#
(Optional) Future extensions / Unimplemented parts#
Implementation Specification in ROS2#
callbackInitialPose is executed in parallel with callbackMapPoints, callbackSensorPoints, and serviceNDTAlign so that callbackSensorPoints does not block callbackInitialPose. The other callbacks belong to the same callback group to avoid a bug segmentation fault.
| CallbackGroup | CallbackGroupType | CallbackName |
|---|---|---|
| initial_pose_callback_group | MutuallyExclusive | callbackInitialPose |
| main_callback_group | MutuallyExclusive | callbackMapPoints |
| main_callback_group | MutuallyExclusive | callbackSensorPoints |
| main_callback_group | MutuallyExclusive | serviceNDTAlign |