Package Summary
Tags | No category tags. |
Version | 0.1.0 |
License | Apache License 2.0 |
Build type | AMENT_CMAKE |
Use | RECOMMENDED |
Repository Summary
Description | |
Checkout URI | https://github.com/qin-yuan/autoware_car.git |
VCS Type | git |
VCS Version | master |
Last Updated | 2024-12-19 |
Dev Status | UNKNOWN |
Released | UNRELEASED |
Tags | No category tags. |
Contributing |
Help Wanted (-)
Good First Issues (-) Pull Requests to Review (-) |
Package Description
Additional Links
Maintainers
- Yamato Ando
- Kento Yabuuchi
- Koji Minoda
Authors
- Yamato Ando
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
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 |
pointcloud_map |
sensor_msgs::msg::PointCloud2 |
map pointcloud |
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] de-grounded pointcloud aligned by scan matching |
initial_pose_with_covariance |
geometry_msgs::msg::PoseWithCovarianceStamped |
[debug topic] initial pose used in scan matching |
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 de-grounded LiDAR scan |
iteration_num |
tier4_debug_msgs::msg::Int32Stamped |
[debug topic] number of scan matching iterations |
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
Name | Type | Description |
---|---|---|
base_frame |
string | Vehicle reference frame |
input_sensor_points_queue_size |
int | Subscriber queue size |
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_type |
int | The type of indicators for scan matching score (0: TP, 1: NVTL) |
converged_param_transform_probability |
double | Threshold for deciding whether to trust the estimation result |
num_threads |
int | Number of threads used for parallel computing |
(TP: Transform Probability, NVTL: Nearest Voxel Transform Probability)
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.
File truncated at 100 lines see the full file
Package Dependencies
System Dependencies
Name |
---|
fmt |
libpcl-all-dev |
Dependant Packages
Launch files
- launch/autoware_pose_estimator.launch.xml
-
- ndt_scan_matcher_param_path [default: $(find-pkg-share autoware_ndt_scan_matcher)/config/autoware_ndt_scan_matcher.param.yaml]
- launch/ndt_scan_matcher.launch.xml
-
- param_file [default: $(find-pkg-share ndt_scan_matcher)/config/ndt_scan_matcher.param.yaml]
- input/pointcloud [default: /points_raw]
- input_initial_pose_topic [default: /ekf_pose_with_covariance]
- input_map_points_topic [default: /pointcloud_map]
- input_regularization_pose_topic [default: /sensing/gnss/pose_with_covariance]
- input_service_trigger_node [default: trigger_node]
- input_ekf_odom [default: /localization/kinematic_state]
- output_pose_topic [default: ndt_pose]
- output_pose_with_covariance_topic [default: ndt_pose_with_covariance]
- client_map_loader [default: /map/get_differential_pointcloud_map]
- node_name [default: ndt_scan_matcher]
- launch/random_downsample_filter.launch.xml
-
- input_topic_name [default: /sensing/lidar/top/pointcloud_raw]
- output_topic_name [default: /localization/util/downsample/pointcloud]
- sample_num [default: 1500]
- input_frame [default: ]
- output_frame [default: base_link]
Messages
Services
Plugins
Recent questions tagged ndt_scan_matcher at Robotics Stack Exchange
Package Summary
Tags | No category tags. |
Version | 0.1.0 |
License | Apache License 2.0 |
Build type | AMENT_CMAKE |
Use | RECOMMENDED |
Repository Summary
Description | |
Checkout URI | https://github.com/qin-yuan/autoware_car.git |
VCS Type | git |
VCS Version | master |
Last Updated | 2024-12-19 |
Dev Status | UNKNOWN |
Released | UNRELEASED |
Tags | No category tags. |
Contributing |
Help Wanted (-)
Good First Issues (-) Pull Requests to Review (-) |
Package Description
Additional Links
Maintainers
- Yamato Ando
- Kento Yabuuchi
- Koji Minoda
Authors
- Yamato Ando
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
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 |
pointcloud_map |
sensor_msgs::msg::PointCloud2 |
map pointcloud |
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] de-grounded pointcloud aligned by scan matching |
initial_pose_with_covariance |
geometry_msgs::msg::PoseWithCovarianceStamped |
[debug topic] initial pose used in scan matching |
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 de-grounded LiDAR scan |
iteration_num |
tier4_debug_msgs::msg::Int32Stamped |
[debug topic] number of scan matching iterations |
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
Name | Type | Description |
---|---|---|
base_frame |
string | Vehicle reference frame |
input_sensor_points_queue_size |
int | Subscriber queue size |
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_type |
int | The type of indicators for scan matching score (0: TP, 1: NVTL) |
converged_param_transform_probability |
double | Threshold for deciding whether to trust the estimation result |
num_threads |
int | Number of threads used for parallel computing |
(TP: Transform Probability, NVTL: Nearest Voxel Transform Probability)
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.
File truncated at 100 lines see the full file
Package Dependencies
System Dependencies
Name |
---|
fmt |
libpcl-all-dev |
Dependant Packages
Launch files
- launch/autoware_pose_estimator.launch.xml
-
- ndt_scan_matcher_param_path [default: $(find-pkg-share autoware_ndt_scan_matcher)/config/autoware_ndt_scan_matcher.param.yaml]
- launch/ndt_scan_matcher.launch.xml
-
- param_file [default: $(find-pkg-share ndt_scan_matcher)/config/ndt_scan_matcher.param.yaml]
- input/pointcloud [default: /points_raw]
- input_initial_pose_topic [default: /ekf_pose_with_covariance]
- input_map_points_topic [default: /pointcloud_map]
- input_regularization_pose_topic [default: /sensing/gnss/pose_with_covariance]
- input_service_trigger_node [default: trigger_node]
- input_ekf_odom [default: /localization/kinematic_state]
- output_pose_topic [default: ndt_pose]
- output_pose_with_covariance_topic [default: ndt_pose_with_covariance]
- client_map_loader [default: /map/get_differential_pointcloud_map]
- node_name [default: ndt_scan_matcher]
- launch/random_downsample_filter.launch.xml
-
- input_topic_name [default: /sensing/lidar/top/pointcloud_raw]
- output_topic_name [default: /localization/util/downsample/pointcloud]
- sample_num [default: 1500]
- input_frame [default: ]
- output_frame [default: base_link]
Messages
Services
Plugins
Recent questions tagged ndt_scan_matcher at Robotics Stack Exchange
Package Summary
Tags | No category tags. |
Version | 0.1.0 |
License | Apache License 2.0 |
Build type | AMENT_CMAKE |
Use | RECOMMENDED |
Repository Summary
Description | |
Checkout URI | https://github.com/qin-yuan/autoware_car.git |
VCS Type | git |
VCS Version | master |
Last Updated | 2024-12-19 |
Dev Status | UNKNOWN |
Released | UNRELEASED |
Tags | No category tags. |
Contributing |
Help Wanted (-)
Good First Issues (-) Pull Requests to Review (-) |
Package Description
Additional Links
Maintainers
- Yamato Ando
- Kento Yabuuchi
- Koji Minoda
Authors
- Yamato Ando
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
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 |
pointcloud_map |
sensor_msgs::msg::PointCloud2 |
map pointcloud |
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] de-grounded pointcloud aligned by scan matching |
initial_pose_with_covariance |
geometry_msgs::msg::PoseWithCovarianceStamped |
[debug topic] initial pose used in scan matching |
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 de-grounded LiDAR scan |
iteration_num |
tier4_debug_msgs::msg::Int32Stamped |
[debug topic] number of scan matching iterations |
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
Name | Type | Description |
---|---|---|
base_frame |
string | Vehicle reference frame |
input_sensor_points_queue_size |
int | Subscriber queue size |
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_type |
int | The type of indicators for scan matching score (0: TP, 1: NVTL) |
converged_param_transform_probability |
double | Threshold for deciding whether to trust the estimation result |
num_threads |
int | Number of threads used for parallel computing |
(TP: Transform Probability, NVTL: Nearest Voxel Transform Probability)
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.
File truncated at 100 lines see the full file
Package Dependencies
System Dependencies
Name |
---|
fmt |
libpcl-all-dev |
Dependant Packages
Launch files
- launch/autoware_pose_estimator.launch.xml
-
- ndt_scan_matcher_param_path [default: $(find-pkg-share autoware_ndt_scan_matcher)/config/autoware_ndt_scan_matcher.param.yaml]
- launch/ndt_scan_matcher.launch.xml
-
- param_file [default: $(find-pkg-share ndt_scan_matcher)/config/ndt_scan_matcher.param.yaml]
- input/pointcloud [default: /points_raw]
- input_initial_pose_topic [default: /ekf_pose_with_covariance]
- input_map_points_topic [default: /pointcloud_map]
- input_regularization_pose_topic [default: /sensing/gnss/pose_with_covariance]
- input_service_trigger_node [default: trigger_node]
- input_ekf_odom [default: /localization/kinematic_state]
- output_pose_topic [default: ndt_pose]
- output_pose_with_covariance_topic [default: ndt_pose_with_covariance]
- client_map_loader [default: /map/get_differential_pointcloud_map]
- node_name [default: ndt_scan_matcher]
- launch/random_downsample_filter.launch.xml
-
- input_topic_name [default: /sensing/lidar/top/pointcloud_raw]
- output_topic_name [default: /localization/util/downsample/pointcloud]
- sample_num [default: 1500]
- input_frame [default: ]
- output_frame [default: base_link]
Messages
Services
Plugins
Recent questions tagged ndt_scan_matcher at Robotics Stack Exchange
Package Summary
Tags | No category tags. |
Version | 0.1.0 |
License | Apache License 2.0 |
Build type | AMENT_CMAKE |
Use | RECOMMENDED |
Repository Summary
Description | |
Checkout URI | https://github.com/qin-yuan/autoware_car.git |
VCS Type | git |
VCS Version | master |
Last Updated | 2024-12-19 |
Dev Status | UNKNOWN |
Released | UNRELEASED |
Tags | No category tags. |
Contributing |
Help Wanted (-)
Good First Issues (-) Pull Requests to Review (-) |
Package Description
Additional Links
Maintainers
- Yamato Ando
- Kento Yabuuchi
- Koji Minoda
Authors
- Yamato Ando
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
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 |
pointcloud_map |
sensor_msgs::msg::PointCloud2 |
map pointcloud |
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] de-grounded pointcloud aligned by scan matching |
initial_pose_with_covariance |
geometry_msgs::msg::PoseWithCovarianceStamped |
[debug topic] initial pose used in scan matching |
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 de-grounded LiDAR scan |
iteration_num |
tier4_debug_msgs::msg::Int32Stamped |
[debug topic] number of scan matching iterations |
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
Name | Type | Description |
---|---|---|
base_frame |
string | Vehicle reference frame |
input_sensor_points_queue_size |
int | Subscriber queue size |
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_type |
int | The type of indicators for scan matching score (0: TP, 1: NVTL) |
converged_param_transform_probability |
double | Threshold for deciding whether to trust the estimation result |
num_threads |
int | Number of threads used for parallel computing |
(TP: Transform Probability, NVTL: Nearest Voxel Transform Probability)
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.
File truncated at 100 lines see the full file
Package Dependencies
System Dependencies
Name |
---|
fmt |
libpcl-all-dev |
Dependant Packages
Launch files
- launch/autoware_pose_estimator.launch.xml
-
- ndt_scan_matcher_param_path [default: $(find-pkg-share autoware_ndt_scan_matcher)/config/autoware_ndt_scan_matcher.param.yaml]
- launch/ndt_scan_matcher.launch.xml
-
- param_file [default: $(find-pkg-share ndt_scan_matcher)/config/ndt_scan_matcher.param.yaml]
- input/pointcloud [default: /points_raw]
- input_initial_pose_topic [default: /ekf_pose_with_covariance]
- input_map_points_topic [default: /pointcloud_map]
- input_regularization_pose_topic [default: /sensing/gnss/pose_with_covariance]
- input_service_trigger_node [default: trigger_node]
- input_ekf_odom [default: /localization/kinematic_state]
- output_pose_topic [default: ndt_pose]
- output_pose_with_covariance_topic [default: ndt_pose_with_covariance]
- client_map_loader [default: /map/get_differential_pointcloud_map]
- node_name [default: ndt_scan_matcher]
- launch/random_downsample_filter.launch.xml
-
- input_topic_name [default: /sensing/lidar/top/pointcloud_raw]
- output_topic_name [default: /localization/util/downsample/pointcloud]
- sample_num [default: 1500]
- input_frame [default: ]
- output_frame [default: base_link]
Messages
Services
Plugins
Recent questions tagged ndt_scan_matcher at Robotics Stack Exchange
Package Summary
Tags | No category tags. |
Version | 0.1.0 |
License | Apache License 2.0 |
Build type | AMENT_CMAKE |
Use | RECOMMENDED |
Repository Summary
Description | |
Checkout URI | https://github.com/qin-yuan/autoware_car.git |
VCS Type | git |
VCS Version | master |
Last Updated | 2024-12-19 |
Dev Status | UNKNOWN |
Released | UNRELEASED |
Tags | No category tags. |
Contributing |
Help Wanted (-)
Good First Issues (-) Pull Requests to Review (-) |
Package Description
Additional Links
Maintainers
- Yamato Ando
- Kento Yabuuchi
- Koji Minoda
Authors
- Yamato Ando
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
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 |
pointcloud_map |
sensor_msgs::msg::PointCloud2 |
map pointcloud |
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] de-grounded pointcloud aligned by scan matching |
initial_pose_with_covariance |
geometry_msgs::msg::PoseWithCovarianceStamped |
[debug topic] initial pose used in scan matching |
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 de-grounded LiDAR scan |
iteration_num |
tier4_debug_msgs::msg::Int32Stamped |
[debug topic] number of scan matching iterations |
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
Name | Type | Description |
---|---|---|
base_frame |
string | Vehicle reference frame |
input_sensor_points_queue_size |
int | Subscriber queue size |
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_type |
int | The type of indicators for scan matching score (0: TP, 1: NVTL) |
converged_param_transform_probability |
double | Threshold for deciding whether to trust the estimation result |
num_threads |
int | Number of threads used for parallel computing |
(TP: Transform Probability, NVTL: Nearest Voxel Transform Probability)
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.
File truncated at 100 lines see the full file
Package Dependencies
System Dependencies
Name |
---|
fmt |
libpcl-all-dev |
Dependant Packages
Launch files
- launch/autoware_pose_estimator.launch.xml
-
- ndt_scan_matcher_param_path [default: $(find-pkg-share autoware_ndt_scan_matcher)/config/autoware_ndt_scan_matcher.param.yaml]
- launch/ndt_scan_matcher.launch.xml
-
- param_file [default: $(find-pkg-share ndt_scan_matcher)/config/ndt_scan_matcher.param.yaml]
- input/pointcloud [default: /points_raw]
- input_initial_pose_topic [default: /ekf_pose_with_covariance]
- input_map_points_topic [default: /pointcloud_map]
- input_regularization_pose_topic [default: /sensing/gnss/pose_with_covariance]
- input_service_trigger_node [default: trigger_node]
- input_ekf_odom [default: /localization/kinematic_state]
- output_pose_topic [default: ndt_pose]
- output_pose_with_covariance_topic [default: ndt_pose_with_covariance]
- client_map_loader [default: /map/get_differential_pointcloud_map]
- node_name [default: ndt_scan_matcher]
- launch/random_downsample_filter.launch.xml
-
- input_topic_name [default: /sensing/lidar/top/pointcloud_raw]
- output_topic_name [default: /localization/util/downsample/pointcloud]
- sample_num [default: 1500]
- input_frame [default: ]
- output_frame [default: base_link]
Messages
Services
Plugins
Recent questions tagged ndt_scan_matcher at Robotics Stack Exchange
Package Summary
Tags | No category tags. |
Version | 0.1.0 |
License | Apache License 2.0 |
Build type | AMENT_CMAKE |
Use | RECOMMENDED |
Repository Summary
Description | |
Checkout URI | https://github.com/qin-yuan/autoware_car.git |
VCS Type | git |
VCS Version | master |
Last Updated | 2024-12-19 |
Dev Status | UNKNOWN |
Released | UNRELEASED |
Tags | No category tags. |
Contributing |
Help Wanted (-)
Good First Issues (-) Pull Requests to Review (-) |
Package Description
Additional Links
Maintainers
- Yamato Ando
- Kento Yabuuchi
- Koji Minoda
Authors
- Yamato Ando
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
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 |
pointcloud_map |
sensor_msgs::msg::PointCloud2 |
map pointcloud |
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] de-grounded pointcloud aligned by scan matching |
initial_pose_with_covariance |
geometry_msgs::msg::PoseWithCovarianceStamped |
[debug topic] initial pose used in scan matching |
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 de-grounded LiDAR scan |
iteration_num |
tier4_debug_msgs::msg::Int32Stamped |
[debug topic] number of scan matching iterations |
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
Name | Type | Description |
---|---|---|
base_frame |
string | Vehicle reference frame |
input_sensor_points_queue_size |
int | Subscriber queue size |
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_type |
int | The type of indicators for scan matching score (0: TP, 1: NVTL) |
converged_param_transform_probability |
double | Threshold for deciding whether to trust the estimation result |
num_threads |
int | Number of threads used for parallel computing |
(TP: Transform Probability, NVTL: Nearest Voxel Transform Probability)
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.
File truncated at 100 lines see the full file
Package Dependencies
System Dependencies
Name |
---|
fmt |
libpcl-all-dev |
Dependant Packages
Launch files
- launch/autoware_pose_estimator.launch.xml
-
- ndt_scan_matcher_param_path [default: $(find-pkg-share autoware_ndt_scan_matcher)/config/autoware_ndt_scan_matcher.param.yaml]
- launch/ndt_scan_matcher.launch.xml
-
- param_file [default: $(find-pkg-share ndt_scan_matcher)/config/ndt_scan_matcher.param.yaml]
- input/pointcloud [default: /points_raw]
- input_initial_pose_topic [default: /ekf_pose_with_covariance]
- input_map_points_topic [default: /pointcloud_map]
- input_regularization_pose_topic [default: /sensing/gnss/pose_with_covariance]
- input_service_trigger_node [default: trigger_node]
- input_ekf_odom [default: /localization/kinematic_state]
- output_pose_topic [default: ndt_pose]
- output_pose_with_covariance_topic [default: ndt_pose_with_covariance]
- client_map_loader [default: /map/get_differential_pointcloud_map]
- node_name [default: ndt_scan_matcher]
- launch/random_downsample_filter.launch.xml
-
- input_topic_name [default: /sensing/lidar/top/pointcloud_raw]
- output_topic_name [default: /localization/util/downsample/pointcloud]
- sample_num [default: 1500]
- input_frame [default: ]
- output_frame [default: base_link]
Messages
Services
Plugins
Recent questions tagged ndt_scan_matcher at Robotics Stack Exchange
Package Summary
Tags | No category tags. |
Version | 0.1.0 |
License | Apache License 2.0 |
Build type | AMENT_CMAKE |
Use | RECOMMENDED |
Repository Summary
Description | |
Checkout URI | https://github.com/qin-yuan/autoware_car.git |
VCS Type | git |
VCS Version | master |
Last Updated | 2024-12-19 |
Dev Status | UNKNOWN |
Released | UNRELEASED |
Tags | No category tags. |
Contributing |
Help Wanted (-)
Good First Issues (-) Pull Requests to Review (-) |
Package Description
Additional Links
Maintainers
- Yamato Ando
- Kento Yabuuchi
- Koji Minoda
Authors
- Yamato Ando
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
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 |
pointcloud_map |
sensor_msgs::msg::PointCloud2 |
map pointcloud |
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] de-grounded pointcloud aligned by scan matching |
initial_pose_with_covariance |
geometry_msgs::msg::PoseWithCovarianceStamped |
[debug topic] initial pose used in scan matching |
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 de-grounded LiDAR scan |
iteration_num |
tier4_debug_msgs::msg::Int32Stamped |
[debug topic] number of scan matching iterations |
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
Name | Type | Description |
---|---|---|
base_frame |
string | Vehicle reference frame |
input_sensor_points_queue_size |
int | Subscriber queue size |
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_type |
int | The type of indicators for scan matching score (0: TP, 1: NVTL) |
converged_param_transform_probability |
double | Threshold for deciding whether to trust the estimation result |
num_threads |
int | Number of threads used for parallel computing |
(TP: Transform Probability, NVTL: Nearest Voxel Transform Probability)
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.
File truncated at 100 lines see the full file
Package Dependencies
System Dependencies
Name |
---|
fmt |
libpcl-all-dev |
Dependant Packages
Launch files
- launch/autoware_pose_estimator.launch.xml
-
- ndt_scan_matcher_param_path [default: $(find-pkg-share autoware_ndt_scan_matcher)/config/autoware_ndt_scan_matcher.param.yaml]
- launch/ndt_scan_matcher.launch.xml
-
- param_file [default: $(find-pkg-share ndt_scan_matcher)/config/ndt_scan_matcher.param.yaml]
- input/pointcloud [default: /points_raw]
- input_initial_pose_topic [default: /ekf_pose_with_covariance]
- input_map_points_topic [default: /pointcloud_map]
- input_regularization_pose_topic [default: /sensing/gnss/pose_with_covariance]
- input_service_trigger_node [default: trigger_node]
- input_ekf_odom [default: /localization/kinematic_state]
- output_pose_topic [default: ndt_pose]
- output_pose_with_covariance_topic [default: ndt_pose_with_covariance]
- client_map_loader [default: /map/get_differential_pointcloud_map]
- node_name [default: ndt_scan_matcher]
- launch/random_downsample_filter.launch.xml
-
- input_topic_name [default: /sensing/lidar/top/pointcloud_raw]
- output_topic_name [default: /localization/util/downsample/pointcloud]
- sample_num [default: 1500]
- input_frame [default: ]
- output_frame [default: base_link]
Messages
Services
Plugins
Recent questions tagged ndt_scan_matcher at Robotics Stack Exchange
Package Summary
Tags | No category tags. |
Version | 0.1.0 |
License | Apache License 2.0 |
Build type | AMENT_CMAKE |
Use | RECOMMENDED |
Repository Summary
Description | |
Checkout URI | https://github.com/qin-yuan/autoware_car.git |
VCS Type | git |
VCS Version | master |
Last Updated | 2024-12-19 |
Dev Status | UNKNOWN |
Released | UNRELEASED |
Tags | No category tags. |
Contributing |
Help Wanted (-)
Good First Issues (-) Pull Requests to Review (-) |
Package Description
Additional Links
Maintainers
- Yamato Ando
- Kento Yabuuchi
- Koji Minoda
Authors
- Yamato Ando
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
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 |
pointcloud_map |
sensor_msgs::msg::PointCloud2 |
map pointcloud |
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] de-grounded pointcloud aligned by scan matching |
initial_pose_with_covariance |
geometry_msgs::msg::PoseWithCovarianceStamped |
[debug topic] initial pose used in scan matching |
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 de-grounded LiDAR scan |
iteration_num |
tier4_debug_msgs::msg::Int32Stamped |
[debug topic] number of scan matching iterations |
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
Name | Type | Description |
---|---|---|
base_frame |
string | Vehicle reference frame |
input_sensor_points_queue_size |
int | Subscriber queue size |
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_type |
int | The type of indicators for scan matching score (0: TP, 1: NVTL) |
converged_param_transform_probability |
double | Threshold for deciding whether to trust the estimation result |
num_threads |
int | Number of threads used for parallel computing |
(TP: Transform Probability, NVTL: Nearest Voxel Transform Probability)
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.
File truncated at 100 lines see the full file
Package Dependencies
System Dependencies
Name |
---|
fmt |
libpcl-all-dev |
Dependant Packages
Launch files
- launch/autoware_pose_estimator.launch.xml
-
- ndt_scan_matcher_param_path [default: $(find-pkg-share autoware_ndt_scan_matcher)/config/autoware_ndt_scan_matcher.param.yaml]
- launch/ndt_scan_matcher.launch.xml
-
- param_file [default: $(find-pkg-share ndt_scan_matcher)/config/ndt_scan_matcher.param.yaml]
- input/pointcloud [default: /points_raw]
- input_initial_pose_topic [default: /ekf_pose_with_covariance]
- input_map_points_topic [default: /pointcloud_map]
- input_regularization_pose_topic [default: /sensing/gnss/pose_with_covariance]
- input_service_trigger_node [default: trigger_node]
- input_ekf_odom [default: /localization/kinematic_state]
- output_pose_topic [default: ndt_pose]
- output_pose_with_covariance_topic [default: ndt_pose_with_covariance]
- client_map_loader [default: /map/get_differential_pointcloud_map]
- node_name [default: ndt_scan_matcher]
- launch/random_downsample_filter.launch.xml
-
- input_topic_name [default: /sensing/lidar/top/pointcloud_raw]
- output_topic_name [default: /localization/util/downsample/pointcloud]
- sample_num [default: 1500]
- input_frame [default: ]
- output_frame [default: base_link]
Messages
Services
Plugins
Recent questions tagged ndt_scan_matcher at Robotics Stack Exchange
Package Summary
Tags | No category tags. |
Version | 0.1.0 |
License | Apache License 2.0 |
Build type | AMENT_CMAKE |
Use | RECOMMENDED |
Repository Summary
Description | |
Checkout URI | https://github.com/qin-yuan/autoware_car.git |
VCS Type | git |
VCS Version | master |
Last Updated | 2024-12-19 |
Dev Status | UNKNOWN |
Released | UNRELEASED |
Tags | No category tags. |
Contributing |
Help Wanted (-)
Good First Issues (-) Pull Requests to Review (-) |
Package Description
Additional Links
Maintainers
- Yamato Ando
- Kento Yabuuchi
- Koji Minoda
Authors
- Yamato Ando
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
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 |
pointcloud_map |
sensor_msgs::msg::PointCloud2 |
map pointcloud |
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] de-grounded pointcloud aligned by scan matching |
initial_pose_with_covariance |
geometry_msgs::msg::PoseWithCovarianceStamped |
[debug topic] initial pose used in scan matching |
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 de-grounded LiDAR scan |
iteration_num |
tier4_debug_msgs::msg::Int32Stamped |
[debug topic] number of scan matching iterations |
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
Name | Type | Description |
---|---|---|
base_frame |
string | Vehicle reference frame |
input_sensor_points_queue_size |
int | Subscriber queue size |
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_type |
int | The type of indicators for scan matching score (0: TP, 1: NVTL) |
converged_param_transform_probability |
double | Threshold for deciding whether to trust the estimation result |
num_threads |
int | Number of threads used for parallel computing |
(TP: Transform Probability, NVTL: Nearest Voxel Transform Probability)
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.
File truncated at 100 lines see the full file
Package Dependencies
System Dependencies
Name |
---|
fmt |
libpcl-all-dev |
Dependant Packages
Launch files
- launch/autoware_pose_estimator.launch.xml
-
- ndt_scan_matcher_param_path [default: $(find-pkg-share autoware_ndt_scan_matcher)/config/autoware_ndt_scan_matcher.param.yaml]
- launch/ndt_scan_matcher.launch.xml
-
- param_file [default: $(find-pkg-share ndt_scan_matcher)/config/ndt_scan_matcher.param.yaml]
- input/pointcloud [default: /points_raw]
- input_initial_pose_topic [default: /ekf_pose_with_covariance]
- input_map_points_topic [default: /pointcloud_map]
- input_regularization_pose_topic [default: /sensing/gnss/pose_with_covariance]
- input_service_trigger_node [default: trigger_node]
- input_ekf_odom [default: /localization/kinematic_state]
- output_pose_topic [default: ndt_pose]
- output_pose_with_covariance_topic [default: ndt_pose_with_covariance]
- client_map_loader [default: /map/get_differential_pointcloud_map]
- node_name [default: ndt_scan_matcher]
- launch/random_downsample_filter.launch.xml
-
- input_topic_name [default: /sensing/lidar/top/pointcloud_raw]
- output_topic_name [default: /localization/util/downsample/pointcloud]
- sample_num [default: 1500]
- input_frame [default: ]
- output_frame [default: base_link]