Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

refactor(ndt_scan_matcher): hierarchize parameters #6208

Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
158 changes: 85 additions & 73 deletions localization/ndt_scan_matcher/config/ndt_scan_matcher.param.yaml
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
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

Check warning on line 69 in localization/ndt_scan_matcher/include/ndt_scan_matcher/hyper_parameters.hpp

View check run for this annotation

Codecov / codecov/patch

localization/ndt_scan_matcher/include/ndt_scan_matcher/hyper_parameters.hpp#L69

Added line #L69 was not covered by tests
{
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() =

Check warning on line 151 in localization/ndt_scan_matcher/include/ndt_scan_matcher/hyper_parameters.hpp

View check run for this annotation

Codecov / codecov/patch

localization/ndt_scan_matcher/include/ndt_scan_matcher/hyper_parameters.hpp#L151

Added line #L151 was not covered by tests
initial_pose_offset_model_x[i];
covariance.covariance_estimation.initial_pose_offset_model[i].y() =

Check warning on line 153 in localization/ndt_scan_matcher/include/ndt_scan_matcher/hyper_parameters.hpp

View check run for this annotation

Codecov / codecov/patch

localization/ndt_scan_matcher/include/ndt_scan_matcher/hyper_parameters.hpp#L153

Added line #L153 was not covered by tests
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;

Check warning on line 160 in localization/ndt_scan_matcher/include/ndt_scan_matcher/hyper_parameters.hpp

View check run for this annotation

Codecov / codecov/patch

localization/ndt_scan_matcher/include/ndt_scan_matcher/hyper_parameters.hpp#L160

Added line #L160 was not covered by tests
}
}

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_
Original file line number Diff line number Diff line change
Expand Up @@ -16,6 +16,7 @@
#define NDT_SCAN_MATCHER__MAP_UPDATE_MODULE_HPP_

#include "localization_util/util_func.hpp"
#include "ndt_scan_matcher/hyper_parameters.hpp"
#include "ndt_scan_matcher/particle.hpp"

#include <rclcpp/rclcpp.hpp>
Expand Down Expand Up @@ -47,7 +48,8 @@ class MapUpdateModule
public:
MapUpdateModule(
rclcpp::Node * node, std::mutex * ndt_ptr_mutex,
std::shared_ptr<NormalDistributionsTransform> ndt_ptr);
std::shared_ptr<NormalDistributionsTransform> ndt_ptr,
HyperParameters::DynamicMapLoading param);

private:
friend class NDTScanMatcher;
Expand All @@ -70,9 +72,8 @@ class MapUpdateModule
rclcpp::Clock::SharedPtr clock_;

std::optional<geometry_msgs::msg::Point> last_update_position_ = std::nullopt;
const double dynamic_map_loading_update_distance_;
const double dynamic_map_loading_map_radius_;
const double lidar_radius_;

HyperParameters::DynamicMapLoading param_;
};

#endif // NDT_SCAN_MATCHER__MAP_UPDATE_MODULE_HPP_
Loading
Loading