From 729b3fe96c43eda41f847a78f084db6309919a0c Mon Sep 17 00:00:00 2001 From: Yamato Ando Date: Mon, 29 Jan 2024 17:10:03 +0900 Subject: [PATCH 1/4] refactor(ndt_scan_matcher): hierarchize parameters Signed-off-by: Yamato Ando --- .../config/ndt_scan_matcher.param.yaml | 156 ++++++++------- .../ndt_scan_matcher/hyper_parameters.hpp | 154 +++++++++++++++ .../ndt_scan_matcher/map_update_module.hpp | 9 +- .../ndt_scan_matcher_core.hpp | 30 +-- .../src/map_update_module.cpp | 15 +- .../src/ndt_scan_matcher_core.cpp | 182 +++++------------- 6 files changed, 300 insertions(+), 246 deletions(-) create mode 100644 localization/ndt_scan_matcher/include/ndt_scan_matcher/hyper_parameters.hpp diff --git a/localization/ndt_scan_matcher/config/ndt_scan_matcher.param.yaml b/localization/ndt_scan_matcher/config/ndt_scan_matcher.param.yaml index d4c49b7e8eec5..dcef00a2318c6 100644 --- a/localization/ndt_scan_matcher/config/ndt_scan_matcher.param.yaml +++ b/localization/ndt_scan_matcher/config/ndt_scan_matcher.param.yaml @@ -1,102 +1,112 @@ /**: 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 + score_estimation: + # Converged param type + # 0=TRANSFORM_PROBABILITY, 1=NEAREST_VOXEL_TRANSFORMATION_LIKELIHOOD + converged_param_type: 1 - # 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] + # If converged_param_type is 0 + # Threshold for deciding whether to trust the estimation result + converged_param_transform_probability: 3.0 - # Regularization switch - regularization_enabled: 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 - regularization_scale_factor: 0.01 + # Scan matching score based on no ground LiDAR scan + no_ground_points: + enable: false - # Dynamic map loading distance - dynamic_map_loading_update_distance: 20.0 + # If lidar_point.z - base_link.z <= this threshold , the point will be removed + z_margin_for_ground_removal: 0.8 - # Dynamic map loading loading radius - dynamic_map_loading_map_radius: 150.0 - # Radius of input LiDAR range (used for diagnostics of dynamic map loading) - lidar_radius: 100.0 + 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, + ] - # A flag for using scan matching score based on no ground LiDAR scan - estimate_scores_by_no_ground_points: false + # 2D Real-time covariance estimation with multiple searches (output_pose_covariance is the minimum value) + covariance_estimation: + enable: false - # If lidar_point.z - base_link.z <= this threshold , the point will be removed - z_margin_for_ground_removal: 0.8 + # 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 diff --git a/localization/ndt_scan_matcher/include/ndt_scan_matcher/hyper_parameters.hpp b/localization/ndt_scan_matcher/include/ndt_scan_matcher/hyper_parameters.hpp new file mode 100644 index 0000000000000..d7a42ea61ee9c --- /dev/null +++ b/localization/ndt_scan_matcher/include/ndt_scan_matcher/hyper_parameters.hpp @@ -0,0 +1,154 @@ +// 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_SCNA_MACHER__HYPER_PARAMETERS_HPP_ +#define NDT_SCNA_MACHER__HYPER_PARAMETERS_HPP_ + +#include + +#include +#include +#include + +#include + + +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 Covarinace { + std::array output_pose_covariance; + + struct CovarianceEstimation { + bool enable; + std::vector 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("frame.base_frame"); + frame.ndt_base_frame = node->declare_parameter("frame.ndt_base_frame"); + frame.map_frame = node->declare_parameter("frame.map_frame"); + + ndt.trans_epsilon = node->declare_parameter("ndt.trans_epsilon"); + ndt.step_size = node->declare_parameter("ndt.step_size"); + ndt.resolution = node->declare_parameter("ndt.resolution"); + ndt.max_iterations = static_cast(node->declare_parameter("ndt.max_iterations")); + ndt.num_threads = static_cast(node->declare_parameter("ndt.num_threads")); + ndt.num_threads = std::max(ndt.num_threads, 1); + ndt_regularization_enable = node->declare_parameter("ndt.regularization.enable"); + ndt.regularization_scale_factor = + static_cast(node->declare_parameter("ndt.regularization.scale_factor")); + + initial_pose_estimation.particles_num = + node->declare_parameter("initial_pose_estimation.particles_num"); + initial_pose_estimation.n_startup_trials = node->declare_parameter("initial_pose_estimation.n_startup_trials"); + + validation.lidar_topic_timeout_sec = node->declare_parameter("validation.lidar_topic_timeout_sec"); + validation.initial_pose_timeout_sec = node->declare_parameter("validation.initial_pose_timeout_sec"); + validation.initial_pose_distance_tolerance_m = + node->declare_parameter("validation.initial_pose_distance_tolerance_m"); + validation.critical_upper_bound_exe_time_ms = + node->declare_parameter("validation.critical_upper_bound_exe_time_ms"); + + const int64_t converged_param_type_tmp = node->declare_parameter("score_estimation.converged_param_type"); + score_estimation.converged_param_type = static_cast(converged_param_type_tmp); + score_estimation.converged_param_transform_probability = + node->declare_parameter("score_estimation.converged_param_transform_probability"); + score_estimation.converged_param_nearest_voxel_transformation_likelihood = + node->declare_parameter("score_estimation.converged_param_nearest_voxel_transformation_likelihood"); + score_estimation.no_ground_points.enable = + node->declare_parameter("score_estimation.no_ground_points.enable"); + score_estimation.no_ground_points.z_margin_for_ground_removal = node->declare_parameter("score_estimation.no_ground_points.z_margin_for_ground_removal"); + + std::vector output_pose_covariance = + node->declare_parameter>("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("covariance.covariance_estimation.enable"); + if (covariance.covariance_estimation.enable) { + std::vector initial_pose_offset_model_x = + node->declare_parameter>("covariance.covariance_estimation.initial_pose_offset_model_x"); + std::vector initial_pose_offset_model_y = + node->declare_parameter>("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("dynamic_map_loading.update_distance"); + dynamic_map_loading.map_radius = node->declare_parameter("dynamic_map_loading.map_radius"); + dynamic_map_loading.lidar_radius = node->declare_parameter("dynamic_map_loading.lidar_radius"); + } +}; + +#endif // NDT_SCNA_MACHER__HYPER_PARAMETERS_HPP_ diff --git a/localization/ndt_scan_matcher/include/ndt_scan_matcher/map_update_module.hpp b/localization/ndt_scan_matcher/include/ndt_scan_matcher/map_update_module.hpp index 8b192b0186b42..7b7d5d6993fc2 100644 --- a/localization/ndt_scan_matcher/include/ndt_scan_matcher/map_update_module.hpp +++ b/localization/ndt_scan_matcher/include/ndt_scan_matcher/map_update_module.hpp @@ -17,6 +17,7 @@ #include "localization_util/util_func.hpp" #include "ndt_scan_matcher/particle.hpp" +#include "ndt_scan_matcher/hyper_parameters.hpp" #include #include @@ -47,7 +48,8 @@ class MapUpdateModule public: MapUpdateModule( rclcpp::Node * node, std::mutex * ndt_ptr_mutex, - std::shared_ptr ndt_ptr); + std::shared_ptr ndt_ptr, + HyperParameters::DynamicMapLoading param); private: friend class NDTScanMatcher; @@ -70,9 +72,8 @@ class MapUpdateModule rclcpp::Clock::SharedPtr clock_; std::optional 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_ diff --git a/localization/ndt_scan_matcher/include/ndt_scan_matcher/ndt_scan_matcher_core.hpp b/localization/ndt_scan_matcher/include/ndt_scan_matcher/ndt_scan_matcher_core.hpp index 2883aa761444d..7d65d44e8e759 100644 --- a/localization/ndt_scan_matcher/include/ndt_scan_matcher/ndt_scan_matcher_core.hpp +++ b/localization/ndt_scan_matcher/include/ndt_scan_matcher/ndt_scan_matcher_core.hpp @@ -19,6 +19,7 @@ #include "localization_util/smart_pose_buffer.hpp" #include "ndt_scan_matcher/map_update_module.hpp" +#include "ndt_scan_matcher/hyper_parameters.hpp" #include #include @@ -65,11 +66,6 @@ #include #include -enum class ConvergedParamType { - TRANSFORM_PROBABILITY = 0, - NEAREST_VOXEL_TRANSFORMATION_LIKELIHOOD = 1 -}; - class NDTScanMatcher : public rclcpp::Node { using PointSource = pcl::PointXYZ; @@ -187,44 +183,22 @@ class NDTScanMatcher : public rclcpp::Node std::shared_ptr> state_ptr_; Eigen::Matrix4f base_to_sensor_matrix_; - std::string base_frame_; - std::string ndt_base_frame_; - std::string map_frame_; - - ConvergedParamType converged_param_type_; - double converged_param_transform_probability_; - double converged_param_nearest_voxel_transformation_likelihood_; - - int64_t initial_estimate_particles_num_; - int64_t n_startup_trials_; - double lidar_topic_timeout_sec_; - double initial_pose_timeout_sec_; - double initial_pose_distance_tolerance_m_; - bool use_cov_estimation_; - std::vector initial_pose_offset_model_; - std::array output_pose_covariance_; std::mutex ndt_ptr_mtx_; std::unique_ptr initial_pose_buffer_; // Keep latest position for dynamic map loading - // This variable is only used when use_dynamic_map_loading is true std::mutex latest_ekf_position_mtx_; std::optional latest_ekf_position_ = std::nullopt; - // variables for regularization - const bool regularization_enabled_; // whether to use longitudinal regularization std::unique_ptr regularization_pose_buffer_; std::atomic is_activated_; std::unique_ptr map_update_module_; std::unique_ptr logger_configure_; - bool estimate_scores_by_no_ground_points_; - double z_margin_for_ground_removal_; + HyperParameters param_; - // The execution time which means probably NDT cannot matches scans properly - double critical_upper_bound_exe_time_ms_; }; #endif // NDT_SCAN_MATCHER__NDT_SCAN_MATCHER_CORE_HPP_ diff --git a/localization/ndt_scan_matcher/src/map_update_module.cpp b/localization/ndt_scan_matcher/src/map_update_module.cpp index 39b92fe248660..b75946eaa0907 100644 --- a/localization/ndt_scan_matcher/src/map_update_module.cpp +++ b/localization/ndt_scan_matcher/src/map_update_module.cpp @@ -16,16 +16,13 @@ MapUpdateModule::MapUpdateModule( rclcpp::Node * node, std::mutex * ndt_ptr_mutex, - std::shared_ptr ndt_ptr) + std::shared_ptr ndt_ptr, + HyperParameters::DynamicMapLoading param) : ndt_ptr_(std::move(ndt_ptr)), ndt_ptr_mutex_(ndt_ptr_mutex), logger_(node->get_logger()), clock_(node->get_clock()), - dynamic_map_loading_update_distance_( - node->declare_parameter("dynamic_map_loading_update_distance")), - dynamic_map_loading_map_radius_( - node->declare_parameter("dynamic_map_loading_map_radius")), - lidar_radius_(node->declare_parameter("lidar_radius")) + param_(param) { loaded_pcd_pub_ = node->create_publisher( "debug/loaded_pointcloud_map", rclcpp::QoS{1}.transient_local()); @@ -42,10 +39,10 @@ bool MapUpdateModule::should_update_map(const geometry_msgs::msg::Point & positi const double dx = position.x - last_update_position_.value().x; const double dy = position.y - last_update_position_.value().y; const double distance = std::hypot(dx, dy); - if (distance + lidar_radius_ > dynamic_map_loading_map_radius_) { + if (distance + param_.lidar_radius > param_.map_radius) { RCLCPP_ERROR_STREAM_THROTTLE(logger_, *clock_, 1000, "Dynamic map loading is not keeping up."); } - return distance > dynamic_map_loading_update_distance_; + return distance > param_.update_distance; } void MapUpdateModule::update_map(const geometry_msgs::msg::Point & position) @@ -53,7 +50,7 @@ void MapUpdateModule::update_map(const geometry_msgs::msg::Point & position) auto request = std::make_shared(); request->area.center_x = static_cast(position.x); request->area.center_y = static_cast(position.y); - request->area.radius = static_cast(dynamic_map_loading_map_radius_); + request->area.radius = static_cast(param_.map_radius); request->cached_ids = ndt_ptr_->getCurrentMapIDs(); while (!pcd_loader_client_->wait_for_service(std::chrono::seconds(1)) && rclcpp::ok()) { diff --git a/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp b/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp index 0278a62341981..e9143f18c2019 100644 --- a/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp +++ b/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp @@ -71,99 +71,11 @@ NDTScanMatcher::NDTScanMatcher() tf2_listener_(tf2_buffer_), ndt_ptr_(new NormalDistributionsTransform), state_ptr_(new std::map), - output_pose_covariance_({}), - regularization_enabled_(declare_parameter("regularization_enabled")), - is_activated_(false) + is_activated_(false), + param_(this) { (*state_ptr_)["state"] = "Initializing"; - int64_t points_queue_size = this->declare_parameter("input_sensor_points_queue_size"); - points_queue_size = std::max(points_queue_size, (int64_t)0); - RCLCPP_INFO(get_logger(), "points_queue_size: %ld", points_queue_size); - - base_frame_ = this->declare_parameter("base_frame"); - RCLCPP_INFO(get_logger(), "base_frame_id: %s", base_frame_.c_str()); - - ndt_base_frame_ = this->declare_parameter("ndt_base_frame"); - RCLCPP_INFO(get_logger(), "ndt_base_frame_id: %s", ndt_base_frame_.c_str()); - - map_frame_ = this->declare_parameter("map_frame"); - RCLCPP_INFO(get_logger(), "map_frame_id: %s", map_frame_.c_str()); - - pclomp::NdtParams ndt_params{}; - ndt_params.trans_epsilon = this->declare_parameter("trans_epsilon"); - ndt_params.step_size = this->declare_parameter("step_size"); - ndt_params.resolution = this->declare_parameter("resolution"); - ndt_params.max_iterations = static_cast(this->declare_parameter("max_iterations")); - ndt_params.num_threads = static_cast(this->declare_parameter("num_threads")); - ndt_params.num_threads = std::max(ndt_params.num_threads, 1); - ndt_params.regularization_scale_factor = - static_cast(this->declare_parameter("regularization_scale_factor")); - ndt_ptr_->setParams(ndt_params); - - RCLCPP_INFO( - get_logger(), "trans_epsilon: %lf, step_size: %lf, resolution: %lf, max_iterations: %d", - ndt_params.trans_epsilon, ndt_params.step_size, ndt_params.resolution, - ndt_params.max_iterations); - - const int64_t converged_param_type_tmp = this->declare_parameter("converged_param_type"); - converged_param_type_ = static_cast(converged_param_type_tmp); - - converged_param_transform_probability_ = - this->declare_parameter("converged_param_transform_probability"); - converged_param_nearest_voxel_transformation_likelihood_ = - this->declare_parameter("converged_param_nearest_voxel_transformation_likelihood"); - - lidar_topic_timeout_sec_ = this->declare_parameter("lidar_topic_timeout_sec"); - - critical_upper_bound_exe_time_ms_ = - this->declare_parameter("critical_upper_bound_exe_time_ms"); - - initial_pose_timeout_sec_ = this->declare_parameter("initial_pose_timeout_sec"); - - initial_pose_distance_tolerance_m_ = - this->declare_parameter("initial_pose_distance_tolerance_m"); - - initial_pose_buffer_ = std::make_unique( - this->get_logger(), initial_pose_timeout_sec_, initial_pose_distance_tolerance_m_); - - use_cov_estimation_ = this->declare_parameter("use_covariance_estimation"); - if (use_cov_estimation_) { - std::vector initial_pose_offset_model_x = - this->declare_parameter>("initial_pose_offset_model_x"); - std::vector initial_pose_offset_model_y = - this->declare_parameter>("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(); - initial_pose_offset_model_.resize(size); - for (size_t i = 0; i < size; i++) { - initial_pose_offset_model_[i].x() = initial_pose_offset_model_x[i]; - initial_pose_offset_model_[i].y() = initial_pose_offset_model_y[i]; - } - } else { - RCLCPP_WARN( - get_logger(), - "Invalid initial pose offset model parameters. Disable covariance estimation."); - use_cov_estimation_ = false; - } - } - - std::vector output_pose_covariance = - this->declare_parameter>("output_pose_covariance"); - for (std::size_t i = 0; i < output_pose_covariance.size(); ++i) { - output_pose_covariance_[i] = output_pose_covariance[i]; - } - - initial_estimate_particles_num_ = - this->declare_parameter("initial_estimate_particles_num"); - n_startup_trials_ = this->declare_parameter("n_startup_trials"); - - estimate_scores_by_no_ground_points_ = - this->declare_parameter("estimate_scores_by_no_ground_points"); - - z_margin_for_ground_removal_ = this->declare_parameter("z_margin_for_ground_removal"); - timer_callback_group_ = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); rclcpp::CallbackGroup::SharedPtr initial_pose_callback_group = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); @@ -186,12 +98,12 @@ NDTScanMatcher::NDTScanMatcher() std::bind(&NDTScanMatcher::callback_initial_pose, this, std::placeholders::_1), initial_pose_sub_opt); sensor_points_sub_ = this->create_subscription( - "points_raw", rclcpp::SensorDataQoS().keep_last(points_queue_size), + "points_raw", rclcpp::SensorDataQoS().keep_last(1), std::bind(&NDTScanMatcher::callback_sensor_points, this, std::placeholders::_1), sensor_sub_opt); // Only if regularization is enabled, subscribe to the regularization base pose - if (regularization_enabled_) { + if (param_.ndt_regularization_enable) { // NOTE: The reason that the regularization subscriber does not belong to the // sensor_callback_group is to ensure that the regularization callback is called even if // sensor_callback takes long time to process. @@ -263,7 +175,13 @@ NDTScanMatcher::NDTScanMatcher() &NDTScanMatcher::service_trigger_node, this, std::placeholders::_1, std::placeholders::_2), rclcpp::ServicesQoS().get_rmw_qos_profile(), sensor_callback_group); - map_update_module_ = std::make_unique(this, &ndt_ptr_mtx_, ndt_ptr_); + + ndt_ptr_->setParams(param_.ndt); + + initial_pose_buffer_ = std::make_unique( + this->get_logger(), param_.validation.initial_pose_timeout_sec, param_.validation.initial_pose_distance_tolerance_m); + + map_update_module_ = std::make_unique(this, &ndt_ptr_mtx_, ndt_ptr_, param_.dynamic_map_loading); logger_configure_ = std::make_unique(this); } @@ -289,7 +207,7 @@ void NDTScanMatcher::publish_diagnostic() } if ( state_ptr_->count("lidar_topic_delay_time_sec") && - std::stod((*state_ptr_)["lidar_topic_delay_time_sec"]) > lidar_topic_timeout_sec_) { + std::stod((*state_ptr_)["lidar_topic_delay_time_sec"]) > param_.validation.lidar_topic_timeout_sec) { diag_status_msg.level = diagnostic_msgs::msg::DiagnosticStatus::WARN; diag_status_msg.message += "lidar_topic_delay_time_sec exceed limit. "; } @@ -309,13 +227,13 @@ void NDTScanMatcher::publish_diagnostic() if ( state_ptr_->count("nearest_voxel_transformation_likelihood") && std::stod((*state_ptr_)["nearest_voxel_transformation_likelihood"]) < - converged_param_nearest_voxel_transformation_likelihood_) { + param_.score_estimation.converged_param_nearest_voxel_transformation_likelihood) { diag_status_msg.level = diagnostic_msgs::msg::DiagnosticStatus::WARN; diag_status_msg.message += "NDT score is unreliably low. "; } if ( state_ptr_->count("execution_time") && - std::stod((*state_ptr_)["execution_time"]) >= critical_upper_bound_exe_time_ms_) { + std::stod((*state_ptr_)["execution_time"]) >= param_.validation.critical_upper_bound_exe_time_ms) { diag_status_msg.level = diagnostic_msgs::msg::DiagnosticStatus::WARN; diag_status_msg.message += "NDT exe time is too long. (took " + (*state_ptr_)["execution_time"] + " [ms])"; @@ -360,13 +278,13 @@ void NDTScanMatcher::callback_initial_pose( { if (!is_activated_) return; - if (initial_pose_msg_ptr->header.frame_id == map_frame_) { + if (initial_pose_msg_ptr->header.frame_id == param_.frame.map_frame) { initial_pose_buffer_->push_back(initial_pose_msg_ptr); } else { RCLCPP_ERROR_STREAM_THROTTLE( get_logger(), *this->get_clock(), 1000, "Received initial pose message with frame_id " - << initial_pose_msg_ptr->header.frame_id << ", but expected " << map_frame_ + << initial_pose_msg_ptr->header.frame_id << ", but expected " << param_.frame.map_frame << ". Please check the frame_id in the input topic and ensure it is correct."); } @@ -395,12 +313,12 @@ void NDTScanMatcher::callback_sensor_points( const double lidar_topic_delay_time_sec = (this->now() - sensor_ros_time).seconds(); (*state_ptr_)["lidar_topic_delay_time_sec"] = std::to_string(lidar_topic_delay_time_sec); - if (lidar_topic_delay_time_sec > lidar_topic_timeout_sec_) { + if (lidar_topic_delay_time_sec > param_.validation.lidar_topic_timeout_sec) { RCLCPP_WARN( this->get_logger(), "The LiDAR topic is experiencing latency. The delay time is %lf[sec] (the tolerance is " "%lf[sec])", - lidar_topic_delay_time_sec, lidar_topic_timeout_sec_); + lidar_topic_delay_time_sec, param_.validation.lidar_topic_timeout_sec); // If the delay time of the LiDAR topic exceeds the delay compensation time of ekf_localizer, // even if further processing continues, the estimated result will be rejected by ekf_localizer. @@ -424,7 +342,7 @@ void NDTScanMatcher::callback_sensor_points( pcl::fromROSMsg(*sensor_points_msg_in_sensor_frame, *sensor_points_in_sensor_frame); transform_sensor_measurement( - sensor_frame, base_frame_, sensor_points_in_sensor_frame, sensor_points_in_baselink_frame); + sensor_frame, param_.frame.base_frame, sensor_points_in_sensor_frame, sensor_points_in_baselink_frame); ndt_ptr_->setInputSource(sensor_points_in_baselink_frame); if (!is_activated_) return; @@ -440,7 +358,7 @@ void NDTScanMatcher::callback_sensor_points( interpolation_result_opt.value(); // if regularization is enabled and available, set pose to NDT for regularization - if (regularization_enabled_) { + if (param_.ndt_regularization_enable) { add_regularization_pose(sensor_ros_time); } @@ -491,9 +409,9 @@ void NDTScanMatcher::callback_sensor_points( map_to_base_link_quat.normalized().toRotationMatrix(); std::array ndt_covariance = - rotate_covariance(output_pose_covariance_, map_to_base_link_rotation); + rotate_covariance(param_.covariance.output_pose_covariance, map_to_base_link_rotation); - if (is_converged && use_cov_estimation_) { + if (is_converged && param_.covariance.covariance_estimation.enable) { const auto estimated_covariance = estimate_covariance(ndt_result, initial_pose_matrix, sensor_ros_time); ndt_covariance = estimated_covariance; @@ -523,16 +441,16 @@ void NDTScanMatcher::callback_sensor_points( new pcl::PointCloud); tier4_autoware_utils::transformPointCloud( *sensor_points_in_baselink_frame, *sensor_points_in_map_ptr, ndt_result.pose); - publish_point_cloud(sensor_ros_time, map_frame_, sensor_points_in_map_ptr); + publish_point_cloud(sensor_ros_time, param_.frame.map_frame, sensor_points_in_map_ptr); // whether use no ground points to calculate score - if (estimate_scores_by_no_ground_points_) { + if (param_.score_estimation.no_ground_points.enable) { // remove ground pcl::shared_ptr> no_ground_points_in_map_ptr( new pcl::PointCloud); for (std::size_t i = 0; i < sensor_points_in_map_ptr->size(); i++) { const float point_z = sensor_points_in_map_ptr->points[i].z; // NOLINT - if (point_z - matrix4f_to_pose(ndt_result.pose).position.z > z_margin_for_ground_removal_) { + if (point_z - matrix4f_to_pose(ndt_result.pose).position.z > param_.score_estimation.no_ground_points.z_margin_for_ground_removal) { no_ground_points_in_map_ptr->points.push_back(sensor_points_in_map_ptr->points[i]); } } @@ -540,7 +458,7 @@ void NDTScanMatcher::callback_sensor_points( sensor_msgs::msg::PointCloud2 no_ground_points_msg_in_map; pcl::toROSMsg(*no_ground_points_in_map_ptr, no_ground_points_msg_in_map); no_ground_points_msg_in_map.header.stamp = sensor_ros_time; - no_ground_points_msg_in_map.header.frame_id = map_frame_; + no_ground_points_msg_in_map.header.frame_id = param_.frame.map_frame; no_ground_points_aligned_pose_pub_->publish(no_ground_points_msg_in_map); // calculate score const auto no_ground_transform_probability = static_cast( @@ -603,10 +521,10 @@ void NDTScanMatcher::publish_tf( { geometry_msgs::msg::PoseStamped result_pose_stamped_msg; result_pose_stamped_msg.header.stamp = sensor_ros_time; - result_pose_stamped_msg.header.frame_id = map_frame_; + result_pose_stamped_msg.header.frame_id = param_.frame.map_frame; result_pose_stamped_msg.pose = result_pose_msg; tf2_broadcaster_.sendTransform( - tier4_autoware_utils::pose2transform(result_pose_stamped_msg, ndt_base_frame_)); + tier4_autoware_utils::pose2transform(result_pose_stamped_msg, param_.frame.ndt_base_frame)); } void NDTScanMatcher::publish_pose( @@ -615,12 +533,12 @@ void NDTScanMatcher::publish_pose( { geometry_msgs::msg::PoseStamped result_pose_stamped_msg; result_pose_stamped_msg.header.stamp = sensor_ros_time; - result_pose_stamped_msg.header.frame_id = map_frame_; + result_pose_stamped_msg.header.frame_id = param_.frame.map_frame; result_pose_stamped_msg.pose = result_pose_msg; geometry_msgs::msg::PoseWithCovarianceStamped result_pose_with_cov_msg; result_pose_with_cov_msg.header.stamp = sensor_ros_time; - result_pose_with_cov_msg.header.frame_id = map_frame_; + result_pose_with_cov_msg.header.frame_id = param_.frame.map_frame; result_pose_with_cov_msg.pose.pose = result_pose_msg; result_pose_with_cov_msg.pose.covariance = ndt_covariance; @@ -647,7 +565,7 @@ void NDTScanMatcher::publish_marker( visualization_msgs::msg::MarkerArray marker_array; visualization_msgs::msg::Marker marker; marker.header.stamp = sensor_ros_time; - marker.header.frame_id = map_frame_; + marker.header.frame_id = param_.frame.map_frame; marker.type = visualization_msgs::msg::Marker::ARROW; marker.action = visualization_msgs::msg::Marker::ADD; marker.scale = tier4_autoware_utils::createMarkerScale(0.3, 0.1, 0.1); @@ -681,7 +599,7 @@ void NDTScanMatcher::publish_initial_to_result( initial_to_result_relative_pose_stamped.pose = tier4_autoware_utils::inverseTransformPose(result_pose_msg, initial_pose_cov_msg.pose.pose); initial_to_result_relative_pose_stamped.header.stamp = sensor_ros_time; - initial_to_result_relative_pose_stamped.header.frame_id = map_frame_; + initial_to_result_relative_pose_stamped.header.frame_id = param_.frame.map_frame; initial_to_result_relative_pose_pub_->publish(initial_to_result_relative_pose_stamped); const auto initial_to_result_distance = @@ -729,13 +647,13 @@ bool NDTScanMatcher::validate_converged_param( const double & transform_probability, const double & nearest_voxel_transformation_likelihood) { bool is_ok_converged_param = false; - if (converged_param_type_ == ConvergedParamType::TRANSFORM_PROBABILITY) { + if (param_.score_estimation.converged_param_type == ConvergedParamType::TRANSFORM_PROBABILITY) { is_ok_converged_param = validate_score( - transform_probability, converged_param_transform_probability_, "Transform Probability"); - } else if (converged_param_type_ == ConvergedParamType::NEAREST_VOXEL_TRANSFORMATION_LIKELIHOOD) { + transform_probability, param_.score_estimation.converged_param_transform_probability, "Transform Probability"); + } else if (param_.score_estimation.converged_param_type == ConvergedParamType::NEAREST_VOXEL_TRANSFORMATION_LIKELIHOOD) { is_ok_converged_param = validate_score( nearest_voxel_transformation_likelihood, - converged_param_nearest_voxel_transformation_likelihood_, + param_.score_estimation.converged_param_nearest_voxel_transformation_likelihood, "Nearest Voxel Transformation Likelihood"); } else { is_ok_converged_param = false; @@ -804,11 +722,11 @@ std::array NDTScanMatcher::estimate_covariance( ndt_result.hessian.inverse().block(0, 0, 2, 2)); } catch (const std::exception & e) { RCLCPP_WARN(get_logger(), "Error in Eigen solver: %s", e.what()); - return output_pose_covariance_; + return param_.covariance.output_pose_covariance; } // first result is added to mean - const int n = static_cast(initial_pose_offset_model_.size()) + 1; + const int n = static_cast(param_.covariance.covariance_estimation.initial_pose_offset_model.size()) + 1; const Eigen::Vector2d ndt_pose_2d(ndt_result.pose(0, 3), ndt_result.pose(1, 3)); Eigen::Vector2d mean = ndt_pose_2d; std::vector ndt_pose_2d_vec; @@ -818,14 +736,14 @@ std::array NDTScanMatcher::estimate_covariance( geometry_msgs::msg::PoseArray multi_ndt_result_msg; geometry_msgs::msg::PoseArray multi_initial_pose_msg; multi_ndt_result_msg.header.stamp = sensor_ros_time; - multi_ndt_result_msg.header.frame_id = map_frame_; + multi_ndt_result_msg.header.frame_id = param_.frame.map_frame; multi_initial_pose_msg.header.stamp = sensor_ros_time; - multi_initial_pose_msg.header.frame_id = map_frame_; + multi_initial_pose_msg.header.frame_id = param_.frame.map_frame; multi_ndt_result_msg.poses.push_back(matrix4f_to_pose(ndt_result.pose)); multi_initial_pose_msg.poses.push_back(matrix4f_to_pose(initial_pose_matrix)); // multiple searches - for (const auto & pose_offset : initial_pose_offset_model_) { + for (const auto & pose_offset : param_.covariance.covariance_estimation.initial_pose_offset_model) { const Eigen::Vector2d rotated_pose_offset_2d = rot * pose_offset; Eigen::Matrix4f sub_initial_pose_matrix(Eigen::Matrix4f::Identity()); @@ -854,7 +772,7 @@ std::array NDTScanMatcher::estimate_covariance( } pca_covariance /= (n - 1); // unbiased covariance - std::array ndt_covariance = output_pose_covariance_; + std::array ndt_covariance = param_.covariance.output_pose_covariance; ndt_covariance[0 + 6 * 0] += pca_covariance(0, 0); ndt_covariance[1 + 6 * 0] += pca_covariance(1, 0); ndt_covariance[0 + 6 * 1] += pca_covariance(0, 1); @@ -898,7 +816,7 @@ void NDTScanMatcher::service_ndt_align( tier4_localization_msgs::srv::PoseWithCovarianceStamped::Response::SharedPtr res) { // get TF from pose_frame to map_frame - const std::string & target_frame = map_frame_; + const std::string & target_frame = param_.frame.map_frame; const std::string & source_frame = req->pose_with_covariance.header.frame_id; geometry_msgs::msg::TransformStamped transform_s2t; @@ -982,7 +900,7 @@ geometry_msgs::msg::PoseWithCovarianceStamped NDTScanMatcher::align_pose( // the ego vehicle is aligned with the ground to some extent about roll and pitch. const std::vector is_loop_variable = {false, false, false, false, false, true}; TreeStructuredParzenEstimator tpe( - TreeStructuredParzenEstimator::Direction::MAXIMIZE, n_startup_trials_, is_loop_variable); + TreeStructuredParzenEstimator::Direction::MAXIMIZE, param_.initial_pose_estimation.n_startup_trials, is_loop_variable); std::vector particle_array; auto output_cloud = std::make_shared>(); @@ -990,9 +908,9 @@ geometry_msgs::msg::PoseWithCovarianceStamped NDTScanMatcher::align_pose( // publish the estimated poses in 20 times to see the progress and to avoid dropping data visualization_msgs::msg::MarkerArray marker_array; constexpr int64_t publish_num = 20; - const int64_t publish_interval = initial_estimate_particles_num_ / publish_num; + const int64_t publish_interval = param_.initial_pose_estimation.particles_num / publish_num; - for (int64_t i = 0; i < initial_estimate_particles_num_; i++) { + for (int64_t i = 0; i < param_.initial_pose_estimation.particles_num; i++) { const TreeStructuredParzenEstimator::Input input = tpe.get_next_input(); geometry_msgs::msg::Pose initial_pose; @@ -1018,8 +936,8 @@ geometry_msgs::msg::PoseWithCovarianceStamped NDTScanMatcher::align_pose( initial_pose, matrix4f_to_pose(ndt_result.pose), ndt_result.transform_probability, ndt_result.iteration_num); particle_array.push_back(particle); - push_debug_markers(marker_array, get_clock()->now(), map_frame_, particle, i); - if ((i + 1) % publish_interval == 0 || (i + 1) == initial_estimate_particles_num_) { + push_debug_markers(marker_array, get_clock()->now(), param_.frame.map_frame, particle, i); + if ((i + 1) % publish_interval == 0 || (i + 1) == param_.initial_pose_estimation.particles_num) { ndt_monte_carlo_initial_pose_marker_pub_->publish(marker_array); marker_array.markers.clear(); } @@ -1048,7 +966,7 @@ geometry_msgs::msg::PoseWithCovarianceStamped NDTScanMatcher::align_pose( auto sensor_points_in_map_ptr = std::make_shared>(); tier4_autoware_utils::transformPointCloud( *ndt_ptr_->getInputSource(), *sensor_points_in_map_ptr, ndt_result.pose); - publish_point_cloud(initial_pose_with_cov.header.stamp, map_frame_, sensor_points_in_map_ptr); + publish_point_cloud(initial_pose_with_cov.header.stamp, param_.frame.map_frame, sensor_points_in_map_ptr); } auto best_particle_ptr = std::max_element( @@ -1057,7 +975,7 @@ geometry_msgs::msg::PoseWithCovarianceStamped NDTScanMatcher::align_pose( geometry_msgs::msg::PoseWithCovarianceStamped result_pose_with_cov_msg; result_pose_with_cov_msg.header.stamp = initial_pose_with_cov.header.stamp; - result_pose_with_cov_msg.header.frame_id = map_frame_; + result_pose_with_cov_msg.header.frame_id = param_.frame.map_frame; result_pose_with_cov_msg.pose.pose = best_particle_ptr->result_pose; output_pose_with_cov_to_log(get_logger(), "align_pose_output", result_pose_with_cov_msg); From 63d7933c582c56091d5c2c262842b3e1c16a5c2f Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Mon, 29 Jan 2024 08:32:40 +0000 Subject: [PATCH 2/4] style(pre-commit): autofix --- .../ndt_scan_matcher/hyper_parameters.hpp | 87 +++++++++++-------- .../ndt_scan_matcher/map_update_module.hpp | 2 +- .../ndt_scan_matcher_core.hpp | 3 +- .../src/map_update_module.cpp | 3 +- .../src/ndt_scan_matcher_core.cpp | 43 +++++---- 5 files changed, 84 insertions(+), 54 deletions(-) diff --git a/localization/ndt_scan_matcher/include/ndt_scan_matcher/hyper_parameters.hpp b/localization/ndt_scan_matcher/include/ndt_scan_matcher/hyper_parameters.hpp index d7a42ea61ee9c..ee9fcff37630b 100644 --- a/localization/ndt_scan_matcher/include/ndt_scan_matcher/hyper_parameters.hpp +++ b/localization/ndt_scan_matcher/include/ndt_scan_matcher/hyper_parameters.hpp @@ -12,18 +12,17 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef NDT_SCNA_MACHER__HYPER_PARAMETERS_HPP_ -#define NDT_SCNA_MACHER__HYPER_PARAMETERS_HPP_ +#ifndef NDT_SCAN_MATCHER__HYPER_PARAMETERS_HPP_ +#define NDT_SCAN_MATCHER__HYPER_PARAMETERS_HPP_ #include +#include + #include #include #include -#include - - enum class ConvergedParamType { TRANSFORM_PROBABILITY = 0, NEAREST_VOXEL_TRANSFORMATION_LIKELIHOOD = 1 @@ -31,8 +30,8 @@ enum class ConvergedParamType { struct HyperParameters { - - struct Frame { + struct Frame + { std::string base_frame; std::string ndt_base_frame; std::string map_frame; @@ -41,46 +40,51 @@ struct HyperParameters pclomp::NdtParams ndt; bool ndt_regularization_enable; - struct InitialPoseEstimation { + struct InitialPoseEstimation + { int64_t particles_num; int64_t n_startup_trials; } initial_pose_estimation; - struct Validation { + 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 { + struct ScoreEstimation + { ConvergedParamType converged_param_type; double converged_param_transform_probability; double converged_param_nearest_voxel_transformation_likelihood; - struct NoGroundPoints { + struct NoGroundPoints + { bool enable; double z_margin_for_ground_removal; } no_ground_points; } score_estimation; - - struct Covarinace { + + struct Covarinace + { std::array output_pose_covariance; - struct CovarianceEstimation { + struct CovarianceEstimation + { bool enable; std::vector initial_pose_offset_model; } covariance_estimation; } covariance; - struct DynamicMapLoading { + 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("frame.base_frame"); @@ -95,47 +99,59 @@ struct HyperParameters ndt.num_threads = std::max(ndt.num_threads, 1); ndt_regularization_enable = node->declare_parameter("ndt.regularization.enable"); ndt.regularization_scale_factor = - static_cast(node->declare_parameter("ndt.regularization.scale_factor")); + static_cast(node->declare_parameter("ndt.regularization.scale_factor")); initial_pose_estimation.particles_num = node->declare_parameter("initial_pose_estimation.particles_num"); - initial_pose_estimation.n_startup_trials = node->declare_parameter("initial_pose_estimation.n_startup_trials"); + initial_pose_estimation.n_startup_trials = + node->declare_parameter("initial_pose_estimation.n_startup_trials"); - validation.lidar_topic_timeout_sec = node->declare_parameter("validation.lidar_topic_timeout_sec"); - validation.initial_pose_timeout_sec = node->declare_parameter("validation.initial_pose_timeout_sec"); + validation.lidar_topic_timeout_sec = + node->declare_parameter("validation.lidar_topic_timeout_sec"); + validation.initial_pose_timeout_sec = + node->declare_parameter("validation.initial_pose_timeout_sec"); validation.initial_pose_distance_tolerance_m = node->declare_parameter("validation.initial_pose_distance_tolerance_m"); validation.critical_upper_bound_exe_time_ms = node->declare_parameter("validation.critical_upper_bound_exe_time_ms"); - const int64_t converged_param_type_tmp = node->declare_parameter("score_estimation.converged_param_type"); - score_estimation.converged_param_type = static_cast(converged_param_type_tmp); + const int64_t converged_param_type_tmp = + node->declare_parameter("score_estimation.converged_param_type"); + score_estimation.converged_param_type = + static_cast(converged_param_type_tmp); score_estimation.converged_param_transform_probability = node->declare_parameter("score_estimation.converged_param_transform_probability"); score_estimation.converged_param_nearest_voxel_transformation_likelihood = - node->declare_parameter("score_estimation.converged_param_nearest_voxel_transformation_likelihood"); + node->declare_parameter( + "score_estimation.converged_param_nearest_voxel_transformation_likelihood"); score_estimation.no_ground_points.enable = node->declare_parameter("score_estimation.no_ground_points.enable"); - score_estimation.no_ground_points.z_margin_for_ground_removal = node->declare_parameter("score_estimation.no_ground_points.z_margin_for_ground_removal"); + score_estimation.no_ground_points.z_margin_for_ground_removal = node->declare_parameter( + "score_estimation.no_ground_points.z_margin_for_ground_removal"); std::vector output_pose_covariance = node->declare_parameter>("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("covariance.covariance_estimation.enable"); + covariance.covariance_estimation.enable = + node->declare_parameter("covariance.covariance_estimation.enable"); if (covariance.covariance_estimation.enable) { std::vector initial_pose_offset_model_x = - node->declare_parameter>("covariance.covariance_estimation.initial_pose_offset_model_x"); + node->declare_parameter>( + "covariance.covariance_estimation.initial_pose_offset_model_x"); std::vector initial_pose_offset_model_y = - node->declare_parameter>("covariance.covariance_estimation.initial_pose_offset_model_y"); + node->declare_parameter>( + "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]; + 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( @@ -145,10 +161,13 @@ struct HyperParameters } } - dynamic_map_loading.update_distance = node->declare_parameter("dynamic_map_loading.update_distance"); - dynamic_map_loading.map_radius = node->declare_parameter("dynamic_map_loading.map_radius"); - dynamic_map_loading.lidar_radius = node->declare_parameter("dynamic_map_loading.lidar_radius"); + dynamic_map_loading.update_distance = + node->declare_parameter("dynamic_map_loading.update_distance"); + dynamic_map_loading.map_radius = + node->declare_parameter("dynamic_map_loading.map_radius"); + dynamic_map_loading.lidar_radius = + node->declare_parameter("dynamic_map_loading.lidar_radius"); } }; -#endif // NDT_SCNA_MACHER__HYPER_PARAMETERS_HPP_ +#endif // NDT_SCAN_MATCHER__HYPER_PARAMETERS_HPP_ diff --git a/localization/ndt_scan_matcher/include/ndt_scan_matcher/map_update_module.hpp b/localization/ndt_scan_matcher/include/ndt_scan_matcher/map_update_module.hpp index 7b7d5d6993fc2..157421fc3ccb1 100644 --- a/localization/ndt_scan_matcher/include/ndt_scan_matcher/map_update_module.hpp +++ b/localization/ndt_scan_matcher/include/ndt_scan_matcher/map_update_module.hpp @@ -16,8 +16,8 @@ #define NDT_SCAN_MATCHER__MAP_UPDATE_MODULE_HPP_ #include "localization_util/util_func.hpp" -#include "ndt_scan_matcher/particle.hpp" #include "ndt_scan_matcher/hyper_parameters.hpp" +#include "ndt_scan_matcher/particle.hpp" #include #include diff --git a/localization/ndt_scan_matcher/include/ndt_scan_matcher/ndt_scan_matcher_core.hpp b/localization/ndt_scan_matcher/include/ndt_scan_matcher/ndt_scan_matcher_core.hpp index 7d65d44e8e759..ca69576061e21 100644 --- a/localization/ndt_scan_matcher/include/ndt_scan_matcher/ndt_scan_matcher_core.hpp +++ b/localization/ndt_scan_matcher/include/ndt_scan_matcher/ndt_scan_matcher_core.hpp @@ -18,8 +18,8 @@ #define FMT_HEADER_ONLY #include "localization_util/smart_pose_buffer.hpp" -#include "ndt_scan_matcher/map_update_module.hpp" #include "ndt_scan_matcher/hyper_parameters.hpp" +#include "ndt_scan_matcher/map_update_module.hpp" #include #include @@ -198,7 +198,6 @@ class NDTScanMatcher : public rclcpp::Node std::unique_ptr logger_configure_; HyperParameters param_; - }; #endif // NDT_SCAN_MATCHER__NDT_SCAN_MATCHER_CORE_HPP_ diff --git a/localization/ndt_scan_matcher/src/map_update_module.cpp b/localization/ndt_scan_matcher/src/map_update_module.cpp index b75946eaa0907..402ec9da32782 100644 --- a/localization/ndt_scan_matcher/src/map_update_module.cpp +++ b/localization/ndt_scan_matcher/src/map_update_module.cpp @@ -16,8 +16,7 @@ MapUpdateModule::MapUpdateModule( rclcpp::Node * node, std::mutex * ndt_ptr_mutex, - std::shared_ptr ndt_ptr, - HyperParameters::DynamicMapLoading param) + std::shared_ptr ndt_ptr, HyperParameters::DynamicMapLoading param) : ndt_ptr_(std::move(ndt_ptr)), ndt_ptr_mutex_(ndt_ptr_mutex), logger_(node->get_logger()), diff --git a/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp b/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp index e9143f18c2019..cd637791f04b6 100644 --- a/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp +++ b/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp @@ -175,13 +175,14 @@ NDTScanMatcher::NDTScanMatcher() &NDTScanMatcher::service_trigger_node, this, std::placeholders::_1, std::placeholders::_2), rclcpp::ServicesQoS().get_rmw_qos_profile(), sensor_callback_group); - ndt_ptr_->setParams(param_.ndt); initial_pose_buffer_ = std::make_unique( - this->get_logger(), param_.validation.initial_pose_timeout_sec, param_.validation.initial_pose_distance_tolerance_m); + this->get_logger(), param_.validation.initial_pose_timeout_sec, + param_.validation.initial_pose_distance_tolerance_m); - map_update_module_ = std::make_unique(this, &ndt_ptr_mtx_, ndt_ptr_, param_.dynamic_map_loading); + map_update_module_ = + std::make_unique(this, &ndt_ptr_mtx_, ndt_ptr_, param_.dynamic_map_loading); logger_configure_ = std::make_unique(this); } @@ -207,7 +208,8 @@ void NDTScanMatcher::publish_diagnostic() } if ( state_ptr_->count("lidar_topic_delay_time_sec") && - std::stod((*state_ptr_)["lidar_topic_delay_time_sec"]) > param_.validation.lidar_topic_timeout_sec) { + std::stod((*state_ptr_)["lidar_topic_delay_time_sec"]) > + param_.validation.lidar_topic_timeout_sec) { diag_status_msg.level = diagnostic_msgs::msg::DiagnosticStatus::WARN; diag_status_msg.message += "lidar_topic_delay_time_sec exceed limit. "; } @@ -232,8 +234,8 @@ void NDTScanMatcher::publish_diagnostic() diag_status_msg.message += "NDT score is unreliably low. "; } if ( - state_ptr_->count("execution_time") && - std::stod((*state_ptr_)["execution_time"]) >= param_.validation.critical_upper_bound_exe_time_ms) { + state_ptr_->count("execution_time") && std::stod((*state_ptr_)["execution_time"]) >= + param_.validation.critical_upper_bound_exe_time_ms) { diag_status_msg.level = diagnostic_msgs::msg::DiagnosticStatus::WARN; diag_status_msg.message += "NDT exe time is too long. (took " + (*state_ptr_)["execution_time"] + " [ms])"; @@ -342,7 +344,8 @@ void NDTScanMatcher::callback_sensor_points( pcl::fromROSMsg(*sensor_points_msg_in_sensor_frame, *sensor_points_in_sensor_frame); transform_sensor_measurement( - sensor_frame, param_.frame.base_frame, sensor_points_in_sensor_frame, sensor_points_in_baselink_frame); + sensor_frame, param_.frame.base_frame, sensor_points_in_sensor_frame, + sensor_points_in_baselink_frame); ndt_ptr_->setInputSource(sensor_points_in_baselink_frame); if (!is_activated_) return; @@ -450,7 +453,9 @@ void NDTScanMatcher::callback_sensor_points( new pcl::PointCloud); for (std::size_t i = 0; i < sensor_points_in_map_ptr->size(); i++) { const float point_z = sensor_points_in_map_ptr->points[i].z; // NOLINT - if (point_z - matrix4f_to_pose(ndt_result.pose).position.z > param_.score_estimation.no_ground_points.z_margin_for_ground_removal) { + if ( + point_z - matrix4f_to_pose(ndt_result.pose).position.z > + param_.score_estimation.no_ground_points.z_margin_for_ground_removal) { no_ground_points_in_map_ptr->points.push_back(sensor_points_in_map_ptr->points[i]); } } @@ -649,8 +654,11 @@ bool NDTScanMatcher::validate_converged_param( bool is_ok_converged_param = false; if (param_.score_estimation.converged_param_type == ConvergedParamType::TRANSFORM_PROBABILITY) { is_ok_converged_param = validate_score( - transform_probability, param_.score_estimation.converged_param_transform_probability, "Transform Probability"); - } else if (param_.score_estimation.converged_param_type == ConvergedParamType::NEAREST_VOXEL_TRANSFORMATION_LIKELIHOOD) { + transform_probability, param_.score_estimation.converged_param_transform_probability, + "Transform Probability"); + } else if ( + param_.score_estimation.converged_param_type == + ConvergedParamType::NEAREST_VOXEL_TRANSFORMATION_LIKELIHOOD) { is_ok_converged_param = validate_score( nearest_voxel_transformation_likelihood, param_.score_estimation.converged_param_nearest_voxel_transformation_likelihood, @@ -726,7 +734,8 @@ std::array NDTScanMatcher::estimate_covariance( } // first result is added to mean - const int n = static_cast(param_.covariance.covariance_estimation.initial_pose_offset_model.size()) + 1; + const int n = + static_cast(param_.covariance.covariance_estimation.initial_pose_offset_model.size()) + 1; const Eigen::Vector2d ndt_pose_2d(ndt_result.pose(0, 3), ndt_result.pose(1, 3)); Eigen::Vector2d mean = ndt_pose_2d; std::vector ndt_pose_2d_vec; @@ -743,7 +752,8 @@ std::array NDTScanMatcher::estimate_covariance( multi_initial_pose_msg.poses.push_back(matrix4f_to_pose(initial_pose_matrix)); // multiple searches - for (const auto & pose_offset : param_.covariance.covariance_estimation.initial_pose_offset_model) { + for (const auto & pose_offset : + param_.covariance.covariance_estimation.initial_pose_offset_model) { const Eigen::Vector2d rotated_pose_offset_2d = rot * pose_offset; Eigen::Matrix4f sub_initial_pose_matrix(Eigen::Matrix4f::Identity()); @@ -900,7 +910,8 @@ geometry_msgs::msg::PoseWithCovarianceStamped NDTScanMatcher::align_pose( // the ego vehicle is aligned with the ground to some extent about roll and pitch. const std::vector is_loop_variable = {false, false, false, false, false, true}; TreeStructuredParzenEstimator tpe( - TreeStructuredParzenEstimator::Direction::MAXIMIZE, param_.initial_pose_estimation.n_startup_trials, is_loop_variable); + TreeStructuredParzenEstimator::Direction::MAXIMIZE, + param_.initial_pose_estimation.n_startup_trials, is_loop_variable); std::vector particle_array; auto output_cloud = std::make_shared>(); @@ -937,7 +948,8 @@ geometry_msgs::msg::PoseWithCovarianceStamped NDTScanMatcher::align_pose( ndt_result.iteration_num); particle_array.push_back(particle); push_debug_markers(marker_array, get_clock()->now(), param_.frame.map_frame, particle, i); - if ((i + 1) % publish_interval == 0 || (i + 1) == param_.initial_pose_estimation.particles_num) { + if ( + (i + 1) % publish_interval == 0 || (i + 1) == param_.initial_pose_estimation.particles_num) { ndt_monte_carlo_initial_pose_marker_pub_->publish(marker_array); marker_array.markers.clear(); } @@ -966,7 +978,8 @@ geometry_msgs::msg::PoseWithCovarianceStamped NDTScanMatcher::align_pose( auto sensor_points_in_map_ptr = std::make_shared>(); tier4_autoware_utils::transformPointCloud( *ndt_ptr_->getInputSource(), *sensor_points_in_map_ptr, ndt_result.pose); - publish_point_cloud(initial_pose_with_cov.header.stamp, param_.frame.map_frame, sensor_points_in_map_ptr); + publish_point_cloud( + initial_pose_with_cov.header.stamp, param_.frame.map_frame, sensor_points_in_map_ptr); } auto best_particle_ptr = std::max_element( From be638608a9982982ac8c2423596b27395950aa92 Mon Sep 17 00:00:00 2001 From: Yamato Ando Date: Mon, 29 Jan 2024 17:40:36 +0900 Subject: [PATCH 3/4] add new lines Signed-off-by: Yamato Ando --- .../ndt_scan_matcher/config/ndt_scan_matcher.param.yaml | 2 ++ 1 file changed, 2 insertions(+) diff --git a/localization/ndt_scan_matcher/config/ndt_scan_matcher.param.yaml b/localization/ndt_scan_matcher/config/ndt_scan_matcher.param.yaml index dcef00a2318c6..144449ce75084 100644 --- a/localization/ndt_scan_matcher/config/ndt_scan_matcher.param.yaml +++ b/localization/ndt_scan_matcher/config/ndt_scan_matcher.param.yaml @@ -58,6 +58,7 @@ # The execution time which means probably NDT cannot matches scans properly. [ms] critical_upper_bound_exe_time_ms: 100.0 + score_estimation: # Converged param type # 0=TRANSFORM_PROBABILITY, 1=NEAREST_VOXEL_TRANSFORMATION_LIKELIHOOD @@ -101,6 +102,7 @@ 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 From 1004efe4429259a2040868739d9ba17906d00807 Mon Sep 17 00:00:00 2001 From: Yamato Ando Date: Mon, 29 Jan 2024 17:43:50 +0900 Subject: [PATCH 4/4] fix typo Signed-off-by: Yamato Ando --- .../include/ndt_scan_matcher/hyper_parameters.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/localization/ndt_scan_matcher/include/ndt_scan_matcher/hyper_parameters.hpp b/localization/ndt_scan_matcher/include/ndt_scan_matcher/hyper_parameters.hpp index ee9fcff37630b..2955e3cb9a5f7 100644 --- a/localization/ndt_scan_matcher/include/ndt_scan_matcher/hyper_parameters.hpp +++ b/localization/ndt_scan_matcher/include/ndt_scan_matcher/hyper_parameters.hpp @@ -66,7 +66,7 @@ struct HyperParameters } no_ground_points; } score_estimation; - struct Covarinace + struct Covariance { std::array output_pose_covariance;