-
Notifications
You must be signed in to change notification settings - Fork 661
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
refactor(ndt_scan_matcher): hierarchize parameters (#6208)
* refactor(ndt_scan_matcher): hierarchize parameters Signed-off-by: Yamato Ando <[email protected]> * style(pre-commit): autofix * add new lines Signed-off-by: Yamato Ando <[email protected]> * fix typo Signed-off-by: Yamato Ando <[email protected]> --------- Signed-off-by: Yamato Ando <[email protected]> Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>
- Loading branch information
1 parent
0262c68
commit e41946b
Showing
6 changed files
with
334 additions
and
248 deletions.
There are no files selected for viewing
158 changes: 85 additions & 73 deletions
158
localization/ndt_scan_matcher/config/ndt_scan_matcher.param.yaml
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -1,102 +1,114 @@ | ||
/**: | ||
ros__parameters: | ||
# Vehicle reference frame | ||
base_frame: "base_link" | ||
frame: | ||
# Vehicle reference frame | ||
base_frame: "base_link" | ||
|
||
# NDT reference frame | ||
ndt_base_frame: "ndt_base_link" | ||
# NDT reference frame | ||
ndt_base_frame: "ndt_base_link" | ||
|
||
# map frame | ||
map_frame: "map" | ||
# Map frame | ||
map_frame: "map" | ||
|
||
# Subscriber queue size | ||
input_sensor_points_queue_size: 1 | ||
|
||
# The maximum difference between two consecutive | ||
# transformations in order to consider convergence | ||
trans_epsilon: 0.01 | ||
ndt: | ||
# The maximum difference between two consecutive | ||
# transformations in order to consider convergence | ||
trans_epsilon: 0.01 | ||
|
||
# The newton line search maximum step length | ||
step_size: 0.1 | ||
# The newton line search maximum step length | ||
step_size: 0.1 | ||
|
||
# The ND voxel grid resolution | ||
resolution: 2.0 | ||
# The ND voxel grid resolution | ||
resolution: 2.0 | ||
|
||
# The number of iterations required to calculate alignment | ||
max_iterations: 30 | ||
# The number of iterations required to calculate alignment | ||
max_iterations: 30 | ||
|
||
# Converged param type | ||
# 0=TRANSFORM_PROBABILITY, 1=NEAREST_VOXEL_TRANSFORMATION_LIKELIHOOD | ||
converged_param_type: 1 | ||
# Number of threads used for parallel computing | ||
num_threads: 4 | ||
|
||
# If converged_param_type is 0 | ||
# Threshold for deciding whether to trust the estimation result | ||
converged_param_transform_probability: 3.0 | ||
regularization: | ||
enable: false | ||
|
||
# If converged_param_type is 1 | ||
# Threshold for deciding whether to trust the estimation result | ||
converged_param_nearest_voxel_transformation_likelihood: 2.3 | ||
# Regularization scale factor | ||
scale_factor: 0.01 | ||
|
||
# The number of particles to estimate initial pose | ||
initial_estimate_particles_num: 200 | ||
|
||
# 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. | ||
n_startup_trials: 20 | ||
initial_pose_estimation: | ||
# The number of particles to estimate initial pose | ||
particles_num: 200 | ||
|
||
# Tolerance of timestamp difference between current time and sensor pointcloud. [sec] | ||
lidar_topic_timeout_sec: 1.0 | ||
# 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. | ||
n_startup_trials: 20 | ||
|
||
# Tolerance of timestamp difference between initial_pose and sensor pointcloud. [sec] | ||
initial_pose_timeout_sec: 1.0 | ||
|
||
# Tolerance of distance difference between two initial poses used for linear interpolation. [m] | ||
initial_pose_distance_tolerance_m: 10.0 | ||
validation: | ||
# Tolerance of timestamp difference between current time and sensor pointcloud. [sec] | ||
lidar_topic_timeout_sec: 1.0 | ||
|
||
# The execution time which means probably NDT cannot matches scans properly. [ms] | ||
critical_upper_bound_exe_time_ms: 100.0 | ||
# Tolerance of timestamp difference between initial_pose and sensor pointcloud. [sec] | ||
initial_pose_timeout_sec: 1.0 | ||
|
||
# Number of threads used for parallel computing | ||
num_threads: 4 | ||
# Tolerance of distance difference between two initial poses used for linear interpolation. [m] | ||
initial_pose_distance_tolerance_m: 10.0 | ||
|
||
# The covariance of output pose | ||
# Note that this covariance matrix is empirically derived | ||
output_pose_covariance: | ||
[ | ||
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, | ||
] | ||
# The execution time which means probably NDT cannot matches scans properly. [ms] | ||
critical_upper_bound_exe_time_ms: 100.0 | ||
|
||
# 2D Real-time covariance estimation with multiple searches (output_pose_covariance is the minimum value) | ||
use_covariance_estimation: false | ||
|
||
# Offset arrangement in covariance estimation [m] | ||
# initial_pose_offset_model_x & initial_pose_offset_model_y must have the same number of elements. | ||
initial_pose_offset_model_x: [0.0, 0.0, 0.5, -0.5, 1.0, -1.0] | ||
initial_pose_offset_model_y: [0.5, -0.5, 0.0, 0.0, 0.0, 0.0] | ||
score_estimation: | ||
# Converged param type | ||
# 0=TRANSFORM_PROBABILITY, 1=NEAREST_VOXEL_TRANSFORMATION_LIKELIHOOD | ||
converged_param_type: 1 | ||
|
||
# Regularization switch | ||
regularization_enabled: false | ||
# If converged_param_type is 0 | ||
# Threshold for deciding whether to trust the estimation result | ||
converged_param_transform_probability: 3.0 | ||
|
||
# Regularization scale factor | ||
regularization_scale_factor: 0.01 | ||
# If converged_param_type is 1 | ||
# Threshold for deciding whether to trust the estimation result | ||
converged_param_nearest_voxel_transformation_likelihood: 2.3 | ||
|
||
# Dynamic map loading distance | ||
dynamic_map_loading_update_distance: 20.0 | ||
# Scan matching score based on no ground LiDAR scan | ||
no_ground_points: | ||
enable: false | ||
|
||
# Dynamic map loading loading radius | ||
dynamic_map_loading_map_radius: 150.0 | ||
# If lidar_point.z - base_link.z <= this threshold , the point will be removed | ||
z_margin_for_ground_removal: 0.8 | ||
|
||
# Radius of input LiDAR range (used for diagnostics of dynamic map loading) | ||
lidar_radius: 100.0 | ||
|
||
# A flag for using scan matching score based on no ground LiDAR scan | ||
estimate_scores_by_no_ground_points: false | ||
covariance: | ||
# The covariance of output pose | ||
# Note that this covariance matrix is empirically derived | ||
output_pose_covariance: | ||
[ | ||
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, | ||
] | ||
|
||
# If lidar_point.z - base_link.z <= this threshold , the point will be removed | ||
z_margin_for_ground_removal: 0.8 | ||
# 2D Real-time covariance estimation with multiple searches (output_pose_covariance is the minimum value) | ||
covariance_estimation: | ||
enable: false | ||
|
||
# Offset arrangement in covariance estimation [m] | ||
# initial_pose_offset_model_x & initial_pose_offset_model_y must have the same number of elements. | ||
initial_pose_offset_model_x: [0.0, 0.0, 0.5, -0.5, 1.0, -1.0] | ||
initial_pose_offset_model_y: [0.5, -0.5, 0.0, 0.0, 0.0, 0.0] | ||
|
||
|
||
dynamic_map_loading: | ||
# Dynamic map loading distance | ||
update_distance: 20.0 | ||
|
||
# Dynamic map loading loading radius | ||
map_radius: 150.0 | ||
|
||
# Radius of input LiDAR range (used for diagnostics of dynamic map loading) | ||
lidar_radius: 100.0 |
173 changes: 173 additions & 0 deletions
173
localization/ndt_scan_matcher/include/ndt_scan_matcher/hyper_parameters.hpp
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -0,0 +1,173 @@ | ||
// Copyright 2024 Autoware Foundation | ||
// | ||
// Licensed under the Apache License, Version 2.0 (the "License"); | ||
// you may not use node file except in compliance with the License. | ||
// You may obtain a copy of the License at | ||
// | ||
// http://www.apache.org/licenses/LICENSE-2.0 | ||
// | ||
// Unless required by applicable law or agreed to in writing, software | ||
// distributed under the License is distributed on an "AS IS" BASIS, | ||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. | ||
// See the License for the specific language governing permissions and | ||
// limitations under the License. | ||
|
||
#ifndef NDT_SCAN_MATCHER__HYPER_PARAMETERS_HPP_ | ||
#define NDT_SCAN_MATCHER__HYPER_PARAMETERS_HPP_ | ||
|
||
#include <rclcpp/rclcpp.hpp> | ||
|
||
#include <multigrid_pclomp/multigrid_ndt_omp.h> | ||
|
||
#include <algorithm> | ||
#include <string> | ||
#include <vector> | ||
|
||
enum class ConvergedParamType { | ||
TRANSFORM_PROBABILITY = 0, | ||
NEAREST_VOXEL_TRANSFORMATION_LIKELIHOOD = 1 | ||
}; | ||
|
||
struct HyperParameters | ||
{ | ||
struct Frame | ||
{ | ||
std::string base_frame; | ||
std::string ndt_base_frame; | ||
std::string map_frame; | ||
} frame; | ||
|
||
pclomp::NdtParams ndt; | ||
bool ndt_regularization_enable; | ||
|
||
struct InitialPoseEstimation | ||
{ | ||
int64_t particles_num; | ||
int64_t n_startup_trials; | ||
} initial_pose_estimation; | ||
|
||
struct Validation | ||
{ | ||
double lidar_topic_timeout_sec; | ||
double initial_pose_timeout_sec; | ||
double initial_pose_distance_tolerance_m; | ||
double critical_upper_bound_exe_time_ms; | ||
} validation; | ||
|
||
struct ScoreEstimation | ||
{ | ||
ConvergedParamType converged_param_type; | ||
double converged_param_transform_probability; | ||
double converged_param_nearest_voxel_transformation_likelihood; | ||
struct NoGroundPoints | ||
{ | ||
bool enable; | ||
double z_margin_for_ground_removal; | ||
} no_ground_points; | ||
} score_estimation; | ||
|
||
struct Covariance | ||
{ | ||
std::array<double, 36> output_pose_covariance; | ||
|
||
struct CovarianceEstimation | ||
{ | ||
bool enable; | ||
std::vector<Eigen::Vector2d> initial_pose_offset_model; | ||
} covariance_estimation; | ||
} covariance; | ||
|
||
struct DynamicMapLoading | ||
{ | ||
double update_distance; | ||
double map_radius; | ||
double lidar_radius; | ||
} dynamic_map_loading; | ||
|
||
public: | ||
explicit HyperParameters(rclcpp::Node * node) | ||
{ | ||
frame.base_frame = node->declare_parameter<std::string>("frame.base_frame"); | ||
frame.ndt_base_frame = node->declare_parameter<std::string>("frame.ndt_base_frame"); | ||
frame.map_frame = node->declare_parameter<std::string>("frame.map_frame"); | ||
|
||
ndt.trans_epsilon = node->declare_parameter<double>("ndt.trans_epsilon"); | ||
ndt.step_size = node->declare_parameter<double>("ndt.step_size"); | ||
ndt.resolution = node->declare_parameter<double>("ndt.resolution"); | ||
ndt.max_iterations = static_cast<int>(node->declare_parameter<int64_t>("ndt.max_iterations")); | ||
ndt.num_threads = static_cast<int>(node->declare_parameter<int64_t>("ndt.num_threads")); | ||
ndt.num_threads = std::max(ndt.num_threads, 1); | ||
ndt_regularization_enable = node->declare_parameter<bool>("ndt.regularization.enable"); | ||
ndt.regularization_scale_factor = | ||
static_cast<float>(node->declare_parameter<float>("ndt.regularization.scale_factor")); | ||
|
||
initial_pose_estimation.particles_num = | ||
node->declare_parameter<int64_t>("initial_pose_estimation.particles_num"); | ||
initial_pose_estimation.n_startup_trials = | ||
node->declare_parameter<int64_t>("initial_pose_estimation.n_startup_trials"); | ||
|
||
validation.lidar_topic_timeout_sec = | ||
node->declare_parameter<double>("validation.lidar_topic_timeout_sec"); | ||
validation.initial_pose_timeout_sec = | ||
node->declare_parameter<double>("validation.initial_pose_timeout_sec"); | ||
validation.initial_pose_distance_tolerance_m = | ||
node->declare_parameter<double>("validation.initial_pose_distance_tolerance_m"); | ||
validation.critical_upper_bound_exe_time_ms = | ||
node->declare_parameter<double>("validation.critical_upper_bound_exe_time_ms"); | ||
|
||
const int64_t converged_param_type_tmp = | ||
node->declare_parameter<int64_t>("score_estimation.converged_param_type"); | ||
score_estimation.converged_param_type = | ||
static_cast<ConvergedParamType>(converged_param_type_tmp); | ||
score_estimation.converged_param_transform_probability = | ||
node->declare_parameter<double>("score_estimation.converged_param_transform_probability"); | ||
score_estimation.converged_param_nearest_voxel_transformation_likelihood = | ||
node->declare_parameter<double>( | ||
"score_estimation.converged_param_nearest_voxel_transformation_likelihood"); | ||
score_estimation.no_ground_points.enable = | ||
node->declare_parameter<bool>("score_estimation.no_ground_points.enable"); | ||
score_estimation.no_ground_points.z_margin_for_ground_removal = node->declare_parameter<double>( | ||
"score_estimation.no_ground_points.z_margin_for_ground_removal"); | ||
|
||
std::vector<double> output_pose_covariance = | ||
node->declare_parameter<std::vector<double>>("covariance.output_pose_covariance"); | ||
for (std::size_t i = 0; i < output_pose_covariance.size(); ++i) { | ||
covariance.output_pose_covariance[i] = output_pose_covariance[i]; | ||
} | ||
covariance.covariance_estimation.enable = | ||
node->declare_parameter<bool>("covariance.covariance_estimation.enable"); | ||
if (covariance.covariance_estimation.enable) { | ||
std::vector<double> initial_pose_offset_model_x = | ||
node->declare_parameter<std::vector<double>>( | ||
"covariance.covariance_estimation.initial_pose_offset_model_x"); | ||
std::vector<double> initial_pose_offset_model_y = | ||
node->declare_parameter<std::vector<double>>( | ||
"covariance.covariance_estimation.initial_pose_offset_model_y"); | ||
|
||
if (initial_pose_offset_model_x.size() == initial_pose_offset_model_y.size()) { | ||
const size_t size = initial_pose_offset_model_x.size(); | ||
covariance.covariance_estimation.initial_pose_offset_model.resize(size); | ||
for (size_t i = 0; i < size; i++) { | ||
covariance.covariance_estimation.initial_pose_offset_model[i].x() = | ||
initial_pose_offset_model_x[i]; | ||
covariance.covariance_estimation.initial_pose_offset_model[i].y() = | ||
initial_pose_offset_model_y[i]; | ||
} | ||
} else { | ||
RCLCPP_WARN( | ||
node->get_logger(), | ||
"Invalid initial pose offset model parameters. Disable covariance estimation."); | ||
covariance.covariance_estimation.enable = false; | ||
} | ||
} | ||
|
||
dynamic_map_loading.update_distance = | ||
node->declare_parameter<double>("dynamic_map_loading.update_distance"); | ||
dynamic_map_loading.map_radius = | ||
node->declare_parameter<double>("dynamic_map_loading.map_radius"); | ||
dynamic_map_loading.lidar_radius = | ||
node->declare_parameter<double>("dynamic_map_loading.lidar_radius"); | ||
} | ||
}; | ||
|
||
#endif // NDT_SCAN_MATCHER__HYPER_PARAMETERS_HPP_ |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Oops, something went wrong.