Skip to content

Commit e41946b

Browse files
YamatoAndopre-commit-ci[bot]
YamatoAndo
and
pre-commit-ci[bot]
authored
refactor(ndt_scan_matcher): hierarchize parameters (#6208)
* refactor(ndt_scan_matcher): hierarchize parameters Signed-off-by: Yamato Ando <yamato.ando@tier4.jp> * style(pre-commit): autofix * add new lines Signed-off-by: Yamato Ando <yamato.ando@tier4.jp> * fix typo Signed-off-by: Yamato Ando <yamato.ando@tier4.jp> --------- Signed-off-by: Yamato Ando <yamato.ando@tier4.jp> Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>
1 parent 0262c68 commit e41946b

File tree

6 files changed

+334
-248
lines changed

6 files changed

+334
-248
lines changed

‎localization/ndt_scan_matcher/config/ndt_scan_matcher.param.yaml

+85-73
Original file line numberDiff line numberDiff line change
@@ -1,102 +1,114 @@
11
/**:
22
ros__parameters:
3-
# Vehicle reference frame
4-
base_frame: "base_link"
3+
frame:
4+
# Vehicle reference frame
5+
base_frame: "base_link"
56

6-
# NDT reference frame
7-
ndt_base_frame: "ndt_base_link"
7+
# NDT reference frame
8+
ndt_base_frame: "ndt_base_link"
89

9-
# map frame
10-
map_frame: "map"
10+
# Map frame
11+
map_frame: "map"
1112

12-
# Subscriber queue size
13-
input_sensor_points_queue_size: 1
1413

15-
# The maximum difference between two consecutive
16-
# transformations in order to consider convergence
17-
trans_epsilon: 0.01
14+
ndt:
15+
# The maximum difference between two consecutive
16+
# transformations in order to consider convergence
17+
trans_epsilon: 0.01
1818

19-
# The newton line search maximum step length
20-
step_size: 0.1
19+
# The newton line search maximum step length
20+
step_size: 0.1
2121

22-
# The ND voxel grid resolution
23-
resolution: 2.0
22+
# The ND voxel grid resolution
23+
resolution: 2.0
2424

25-
# The number of iterations required to calculate alignment
26-
max_iterations: 30
25+
# The number of iterations required to calculate alignment
26+
max_iterations: 30
2727

28-
# Converged param type
29-
# 0=TRANSFORM_PROBABILITY, 1=NEAREST_VOXEL_TRANSFORMATION_LIKELIHOOD
30-
converged_param_type: 1
28+
# Number of threads used for parallel computing
29+
num_threads: 4
3130

32-
# If converged_param_type is 0
33-
# Threshold for deciding whether to trust the estimation result
34-
converged_param_transform_probability: 3.0
31+
regularization:
32+
enable: false
3533

36-
# If converged_param_type is 1
37-
# Threshold for deciding whether to trust the estimation result
38-
converged_param_nearest_voxel_transformation_likelihood: 2.3
34+
# Regularization scale factor
35+
scale_factor: 0.01
3936

40-
# The number of particles to estimate initial pose
41-
initial_estimate_particles_num: 200
4237

43-
# The number of initial random trials in the TPE (Tree-Structured Parzen Estimator).
44-
# This value should be equal to or less than 'initial_estimate_particles_num' and more than 0.
45-
# If it is equal to 'initial_estimate_particles_num', the search will be the same as a full random search.
46-
n_startup_trials: 20
38+
initial_pose_estimation:
39+
# The number of particles to estimate initial pose
40+
particles_num: 200
4741

48-
# Tolerance of timestamp difference between current time and sensor pointcloud. [sec]
49-
lidar_topic_timeout_sec: 1.0
42+
# The number of initial random trials in the TPE (Tree-Structured Parzen Estimator).
43+
# This value should be equal to or less than 'initial_estimate_particles_num' and more than 0.
44+
# If it is equal to 'initial_estimate_particles_num', the search will be the same as a full random search.
45+
n_startup_trials: 20
5046

51-
# Tolerance of timestamp difference between initial_pose and sensor pointcloud. [sec]
52-
initial_pose_timeout_sec: 1.0
5347

54-
# Tolerance of distance difference between two initial poses used for linear interpolation. [m]
55-
initial_pose_distance_tolerance_m: 10.0
48+
validation:
49+
# Tolerance of timestamp difference between current time and sensor pointcloud. [sec]
50+
lidar_topic_timeout_sec: 1.0
5651

57-
# The execution time which means probably NDT cannot matches scans properly. [ms]
58-
critical_upper_bound_exe_time_ms: 100.0
52+
# Tolerance of timestamp difference between initial_pose and sensor pointcloud. [sec]
53+
initial_pose_timeout_sec: 1.0
5954

60-
# Number of threads used for parallel computing
61-
num_threads: 4
55+
# Tolerance of distance difference between two initial poses used for linear interpolation. [m]
56+
initial_pose_distance_tolerance_m: 10.0
6257

63-
# The covariance of output pose
64-
# Note that this covariance matrix is empirically derived
65-
output_pose_covariance:
66-
[
67-
0.0225, 0.0, 0.0, 0.0, 0.0, 0.0,
68-
0.0, 0.0225, 0.0, 0.0, 0.0, 0.0,
69-
0.0, 0.0, 0.0225, 0.0, 0.0, 0.0,
70-
0.0, 0.0, 0.0, 0.000625, 0.0, 0.0,
71-
0.0, 0.0, 0.0, 0.0, 0.000625, 0.0,
72-
0.0, 0.0, 0.0, 0.0, 0.0, 0.000625,
73-
]
58+
# The execution time which means probably NDT cannot matches scans properly. [ms]
59+
critical_upper_bound_exe_time_ms: 100.0
7460

75-
# 2D Real-time covariance estimation with multiple searches (output_pose_covariance is the minimum value)
76-
use_covariance_estimation: false
7761

78-
# Offset arrangement in covariance estimation [m]
79-
# initial_pose_offset_model_x & initial_pose_offset_model_y must have the same number of elements.
80-
initial_pose_offset_model_x: [0.0, 0.0, 0.5, -0.5, 1.0, -1.0]
81-
initial_pose_offset_model_y: [0.5, -0.5, 0.0, 0.0, 0.0, 0.0]
62+
score_estimation:
63+
# Converged param type
64+
# 0=TRANSFORM_PROBABILITY, 1=NEAREST_VOXEL_TRANSFORMATION_LIKELIHOOD
65+
converged_param_type: 1
8266

83-
# Regularization switch
84-
regularization_enabled: false
67+
# If converged_param_type is 0
68+
# Threshold for deciding whether to trust the estimation result
69+
converged_param_transform_probability: 3.0
8570

86-
# Regularization scale factor
87-
regularization_scale_factor: 0.01
71+
# If converged_param_type is 1
72+
# Threshold for deciding whether to trust the estimation result
73+
converged_param_nearest_voxel_transformation_likelihood: 2.3
8874

89-
# Dynamic map loading distance
90-
dynamic_map_loading_update_distance: 20.0
75+
# Scan matching score based on no ground LiDAR scan
76+
no_ground_points:
77+
enable: false
9178

92-
# Dynamic map loading loading radius
93-
dynamic_map_loading_map_radius: 150.0
79+
# If lidar_point.z - base_link.z <= this threshold , the point will be removed
80+
z_margin_for_ground_removal: 0.8
9481

95-
# Radius of input LiDAR range (used for diagnostics of dynamic map loading)
96-
lidar_radius: 100.0
9782

98-
# A flag for using scan matching score based on no ground LiDAR scan
99-
estimate_scores_by_no_ground_points: false
83+
covariance:
84+
# The covariance of output pose
85+
# Note that this covariance matrix is empirically derived
86+
output_pose_covariance:
87+
[
88+
0.0225, 0.0, 0.0, 0.0, 0.0, 0.0,
89+
0.0, 0.0225, 0.0, 0.0, 0.0, 0.0,
90+
0.0, 0.0, 0.0225, 0.0, 0.0, 0.0,
91+
0.0, 0.0, 0.0, 0.000625, 0.0, 0.0,
92+
0.0, 0.0, 0.0, 0.0, 0.000625, 0.0,
93+
0.0, 0.0, 0.0, 0.0, 0.0, 0.000625,
94+
]
10095

101-
# If lidar_point.z - base_link.z <= this threshold , the point will be removed
102-
z_margin_for_ground_removal: 0.8
96+
# 2D Real-time covariance estimation with multiple searches (output_pose_covariance is the minimum value)
97+
covariance_estimation:
98+
enable: false
99+
100+
# Offset arrangement in covariance estimation [m]
101+
# initial_pose_offset_model_x & initial_pose_offset_model_y must have the same number of elements.
102+
initial_pose_offset_model_x: [0.0, 0.0, 0.5, -0.5, 1.0, -1.0]
103+
initial_pose_offset_model_y: [0.5, -0.5, 0.0, 0.0, 0.0, 0.0]
104+
105+
106+
dynamic_map_loading:
107+
# Dynamic map loading distance
108+
update_distance: 20.0
109+
110+
# Dynamic map loading loading radius
111+
map_radius: 150.0
112+
113+
# Radius of input LiDAR range (used for diagnostics of dynamic map loading)
114+
lidar_radius: 100.0

‎localization/ndt_scan_matcher/include/ndt_scan_matcher/hyper_parameters.hpp

+173
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,173 @@
1+
// Copyright 2024 Autoware Foundation
2+
//
3+
// Licensed under the Apache License, Version 2.0 (the "License");
4+
// you may not use node file except in compliance with the License.
5+
// You may obtain a copy of the License at
6+
//
7+
// http://www.apache.org/licenses/LICENSE-2.0
8+
//
9+
// Unless required by applicable law or agreed to in writing, software
10+
// distributed under the License is distributed on an "AS IS" BASIS,
11+
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12+
// See the License for the specific language governing permissions and
13+
// limitations under the License.
14+
15+
#ifndef NDT_SCAN_MATCHER__HYPER_PARAMETERS_HPP_
16+
#define NDT_SCAN_MATCHER__HYPER_PARAMETERS_HPP_
17+
18+
#include <rclcpp/rclcpp.hpp>
19+
20+
#include <multigrid_pclomp/multigrid_ndt_omp.h>
21+
22+
#include <algorithm>
23+
#include <string>
24+
#include <vector>
25+
26+
enum class ConvergedParamType {
27+
TRANSFORM_PROBABILITY = 0,
28+
NEAREST_VOXEL_TRANSFORMATION_LIKELIHOOD = 1
29+
};
30+
31+
struct HyperParameters
32+
{
33+
struct Frame
34+
{
35+
std::string base_frame;
36+
std::string ndt_base_frame;
37+
std::string map_frame;
38+
} frame;
39+
40+
pclomp::NdtParams ndt;
41+
bool ndt_regularization_enable;
42+
43+
struct InitialPoseEstimation
44+
{
45+
int64_t particles_num;
46+
int64_t n_startup_trials;
47+
} initial_pose_estimation;
48+
49+
struct Validation
50+
{
51+
double lidar_topic_timeout_sec;
52+
double initial_pose_timeout_sec;
53+
double initial_pose_distance_tolerance_m;
54+
double critical_upper_bound_exe_time_ms;
55+
} validation;
56+
57+
struct ScoreEstimation
58+
{
59+
ConvergedParamType converged_param_type;
60+
double converged_param_transform_probability;
61+
double converged_param_nearest_voxel_transformation_likelihood;
62+
struct NoGroundPoints
63+
{
64+
bool enable;
65+
double z_margin_for_ground_removal;
66+
} no_ground_points;
67+
} score_estimation;
68+
69+
struct Covariance
70+
{
71+
std::array<double, 36> output_pose_covariance;
72+
73+
struct CovarianceEstimation
74+
{
75+
bool enable;
76+
std::vector<Eigen::Vector2d> initial_pose_offset_model;
77+
} covariance_estimation;
78+
} covariance;
79+
80+
struct DynamicMapLoading
81+
{
82+
double update_distance;
83+
double map_radius;
84+
double lidar_radius;
85+
} dynamic_map_loading;
86+
87+
public:
88+
explicit HyperParameters(rclcpp::Node * node)
89+
{
90+
frame.base_frame = node->declare_parameter<std::string>("frame.base_frame");
91+
frame.ndt_base_frame = node->declare_parameter<std::string>("frame.ndt_base_frame");
92+
frame.map_frame = node->declare_parameter<std::string>("frame.map_frame");
93+
94+
ndt.trans_epsilon = node->declare_parameter<double>("ndt.trans_epsilon");
95+
ndt.step_size = node->declare_parameter<double>("ndt.step_size");
96+
ndt.resolution = node->declare_parameter<double>("ndt.resolution");
97+
ndt.max_iterations = static_cast<int>(node->declare_parameter<int64_t>("ndt.max_iterations"));
98+
ndt.num_threads = static_cast<int>(node->declare_parameter<int64_t>("ndt.num_threads"));
99+
ndt.num_threads = std::max(ndt.num_threads, 1);
100+
ndt_regularization_enable = node->declare_parameter<bool>("ndt.regularization.enable");
101+
ndt.regularization_scale_factor =
102+
static_cast<float>(node->declare_parameter<float>("ndt.regularization.scale_factor"));
103+
104+
initial_pose_estimation.particles_num =
105+
node->declare_parameter<int64_t>("initial_pose_estimation.particles_num");
106+
initial_pose_estimation.n_startup_trials =
107+
node->declare_parameter<int64_t>("initial_pose_estimation.n_startup_trials");
108+
109+
validation.lidar_topic_timeout_sec =
110+
node->declare_parameter<double>("validation.lidar_topic_timeout_sec");
111+
validation.initial_pose_timeout_sec =
112+
node->declare_parameter<double>("validation.initial_pose_timeout_sec");
113+
validation.initial_pose_distance_tolerance_m =
114+
node->declare_parameter<double>("validation.initial_pose_distance_tolerance_m");
115+
validation.critical_upper_bound_exe_time_ms =
116+
node->declare_parameter<double>("validation.critical_upper_bound_exe_time_ms");
117+
118+
const int64_t converged_param_type_tmp =
119+
node->declare_parameter<int64_t>("score_estimation.converged_param_type");
120+
score_estimation.converged_param_type =
121+
static_cast<ConvergedParamType>(converged_param_type_tmp);
122+
score_estimation.converged_param_transform_probability =
123+
node->declare_parameter<double>("score_estimation.converged_param_transform_probability");
124+
score_estimation.converged_param_nearest_voxel_transformation_likelihood =
125+
node->declare_parameter<double>(
126+
"score_estimation.converged_param_nearest_voxel_transformation_likelihood");
127+
score_estimation.no_ground_points.enable =
128+
node->declare_parameter<bool>("score_estimation.no_ground_points.enable");
129+
score_estimation.no_ground_points.z_margin_for_ground_removal = node->declare_parameter<double>(
130+
"score_estimation.no_ground_points.z_margin_for_ground_removal");
131+
132+
std::vector<double> output_pose_covariance =
133+
node->declare_parameter<std::vector<double>>("covariance.output_pose_covariance");
134+
for (std::size_t i = 0; i < output_pose_covariance.size(); ++i) {
135+
covariance.output_pose_covariance[i] = output_pose_covariance[i];
136+
}
137+
covariance.covariance_estimation.enable =
138+
node->declare_parameter<bool>("covariance.covariance_estimation.enable");
139+
if (covariance.covariance_estimation.enable) {
140+
std::vector<double> initial_pose_offset_model_x =
141+
node->declare_parameter<std::vector<double>>(
142+
"covariance.covariance_estimation.initial_pose_offset_model_x");
143+
std::vector<double> initial_pose_offset_model_y =
144+
node->declare_parameter<std::vector<double>>(
145+
"covariance.covariance_estimation.initial_pose_offset_model_y");
146+
147+
if (initial_pose_offset_model_x.size() == initial_pose_offset_model_y.size()) {
148+
const size_t size = initial_pose_offset_model_x.size();
149+
covariance.covariance_estimation.initial_pose_offset_model.resize(size);
150+
for (size_t i = 0; i < size; i++) {
151+
covariance.covariance_estimation.initial_pose_offset_model[i].x() =
152+
initial_pose_offset_model_x[i];
153+
covariance.covariance_estimation.initial_pose_offset_model[i].y() =
154+
initial_pose_offset_model_y[i];
155+
}
156+
} else {
157+
RCLCPP_WARN(
158+
node->get_logger(),
159+
"Invalid initial pose offset model parameters. Disable covariance estimation.");
160+
covariance.covariance_estimation.enable = false;
161+
}
162+
}
163+
164+
dynamic_map_loading.update_distance =
165+
node->declare_parameter<double>("dynamic_map_loading.update_distance");
166+
dynamic_map_loading.map_radius =
167+
node->declare_parameter<double>("dynamic_map_loading.map_radius");
168+
dynamic_map_loading.lidar_radius =
169+
node->declare_parameter<double>("dynamic_map_loading.lidar_radius");
170+
}
171+
};
172+
173+
#endif // NDT_SCAN_MATCHER__HYPER_PARAMETERS_HPP_

‎localization/ndt_scan_matcher/include/ndt_scan_matcher/map_update_module.hpp

+5-4
Original file line numberDiff line numberDiff line change
@@ -16,6 +16,7 @@
1616
#define NDT_SCAN_MATCHER__MAP_UPDATE_MODULE_HPP_
1717

1818
#include "localization_util/util_func.hpp"
19+
#include "ndt_scan_matcher/hyper_parameters.hpp"
1920
#include "ndt_scan_matcher/particle.hpp"
2021

2122
#include <rclcpp/rclcpp.hpp>
@@ -47,7 +48,8 @@ class MapUpdateModule
4748
public:
4849
MapUpdateModule(
4950
rclcpp::Node * node, std::mutex * ndt_ptr_mutex,
50-
std::shared_ptr<NormalDistributionsTransform> ndt_ptr);
51+
std::shared_ptr<NormalDistributionsTransform> ndt_ptr,
52+
HyperParameters::DynamicMapLoading param);
5153

5254
private:
5355
friend class NDTScanMatcher;
@@ -70,9 +72,8 @@ class MapUpdateModule
7072
rclcpp::Clock::SharedPtr clock_;
7173

7274
std::optional<geometry_msgs::msg::Point> last_update_position_ = std::nullopt;
73-
const double dynamic_map_loading_update_distance_;
74-
const double dynamic_map_loading_map_radius_;
75-
const double lidar_radius_;
75+
76+
HyperParameters::DynamicMapLoading param_;
7677
};
7778

7879
#endif // NDT_SCAN_MATCHER__MAP_UPDATE_MODULE_HPP_

0 commit comments

Comments
 (0)