From 602a6ba710a0fb0082ad9eebe197cd69f7daebd8 Mon Sep 17 00:00:00 2001 From: Shintaro Sakoda Date: Thu, 11 Apr 2024 16:05:53 +0900 Subject: [PATCH 1/5] Implemented grid_search initial pose estimation Signed-off-by: Shintaro Sakoda --- .../config/ndt_scan_matcher.param.yaml | 24 ++++ .../ndt_scan_matcher/hyper_parameters.hpp | 48 +++++++ .../ndt_scan_matcher_core.hpp | 4 +- .../src/ndt_scan_matcher_core.cpp | 119 +++++++++++++++++- 4 files changed, 192 insertions(+), 3 deletions(-) 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 241892e67b66c..6b8ac6ccb6f13 100644 --- a/localization/ndt_scan_matcher/config/ndt_scan_matcher.param.yaml +++ b/localization/ndt_scan_matcher/config/ndt_scan_matcher.param.yaml @@ -42,6 +42,12 @@ initial_pose_estimation: + # method: 0=RANDOM_SEARCH(including TPE), 1=GRID_SEARCH + method: 1 + + # + # Parameters for RANDOM_SEARCH + # # The number of particles to estimate initial pose particles_num: 200 @@ -50,6 +56,24 @@ # If it is equal to 'initial_estimate_particles_num', the search will be the same as a full random search. n_startup_trials: 20 + # + # Parameters for GRID_SEARCH + # + # The number of grid search + grid_num_x: 3 + grid_num_y: 3 + grid_num_z: 1 + grid_num_roll: 1 + grid_num_pitch: 1 + grid_num_yaw: 40 + + # The range of grid search + grid_search_range_x: 1.0 # [m] (Search range: -1.0m ~ 1.0m) + grid_search_range_y: 1.0 # [m] (Search range: -1.0m ~ 1.0m) + grid_search_range_z: 0.0 # [m] + grid_search_range_roll: 0.0 # [rad] + grid_search_range_pitch: 0.0 # [rad] + grid_search_range_yaw: 3.14159265359 # [rad] (Search range: -pi ~ pi) validation: # Tolerance of timestamp difference between current time and sensor pointcloud. [sec] 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 ee1e2369c79bd..fa98c43fb935f 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 @@ -23,6 +23,11 @@ #include #include +enum class InitialPoseEstimationMethod { + RANDOM_SEARCH = 0, + GRID_SEARCH = 1, +}; + enum class ConvergedParamType { TRANSFORM_PROBABILITY = 0, NEAREST_VOXEL_TRANSFORMATION_LIKELIHOOD = 1 @@ -47,8 +52,23 @@ struct HyperParameters struct InitialPoseEstimation { + InitialPoseEstimationMethod method; + // Parameters for RANDOM_SEARCH int64_t particles_num; int64_t n_startup_trials; + // Parameters for GRID_SEARCH + int64_t grid_num_x; + int64_t grid_num_y; + int64_t grid_num_z; + int64_t grid_num_roll; + int64_t grid_num_pitch; + int64_t grid_num_yaw; + double grid_search_range_x; + double grid_search_range_y; + double grid_search_range_z; + double grid_search_range_roll; + double grid_search_range_pitch; + double grid_search_range_yaw; } initial_pose_estimation; struct Validation @@ -109,10 +129,38 @@ struct HyperParameters ndt.regularization_scale_factor = static_cast(node->declare_parameter("ndt.regularization.scale_factor")); + const int64_t initial_pose_estimation_method_tmp = + node->declare_parameter("initial_pose_estimation.method"); + initial_pose_estimation.method = + static_cast(initial_pose_estimation_method_tmp); 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.grid_num_x = + node->declare_parameter("initial_pose_estimation.grid_num_x"); + initial_pose_estimation.grid_num_y = + node->declare_parameter("initial_pose_estimation.grid_num_y"); + initial_pose_estimation.grid_num_z = + node->declare_parameter("initial_pose_estimation.grid_num_z"); + initial_pose_estimation.grid_num_roll = + node->declare_parameter("initial_pose_estimation.grid_num_roll"); + initial_pose_estimation.grid_num_pitch = + node->declare_parameter("initial_pose_estimation.grid_num_pitch"); + initial_pose_estimation.grid_num_yaw = + node->declare_parameter("initial_pose_estimation.grid_num_yaw"); + initial_pose_estimation.grid_search_range_x = + node->declare_parameter("initial_pose_estimation.grid_search_range_x"); + initial_pose_estimation.grid_search_range_y = + node->declare_parameter("initial_pose_estimation.grid_search_range_y"); + initial_pose_estimation.grid_search_range_z = + node->declare_parameter("initial_pose_estimation.grid_search_range_z"); + initial_pose_estimation.grid_search_range_roll = + node->declare_parameter("initial_pose_estimation.grid_search_range_roll"); + initial_pose_estimation.grid_search_range_pitch = + node->declare_parameter("initial_pose_estimation.grid_search_range_pitch"); + initial_pose_estimation.grid_search_range_yaw = + node->declare_parameter("initial_pose_estimation.grid_search_range_yaw"); validation.lidar_topic_timeout_sec = node->declare_parameter("validation.lidar_topic_timeout_sec"); 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 26c96ce7e3248..cc456946bc46f 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 @@ -92,7 +92,9 @@ class NDTScanMatcher : public rclcpp::Node void callback_regularization_pose( geometry_msgs::msg::PoseWithCovarianceStamped::ConstSharedPtr pose_conv_msg_ptr); - geometry_msgs::msg::PoseWithCovarianceStamped align_pose( + geometry_msgs::msg::PoseWithCovarianceStamped align_pose_by_random_search( + const geometry_msgs::msg::PoseWithCovarianceStamped & initial_pose_with_cov); + geometry_msgs::msg::PoseWithCovarianceStamped align_pose_by_grid_search( const geometry_msgs::msg::PoseWithCovarianceStamped & initial_pose_with_cov); void transform_sensor_measurement( 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 ba4190d9f66fb..58b8ef2f25954 100644 --- a/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp +++ b/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp @@ -880,12 +880,23 @@ void NDTScanMatcher::service_ndt_align( return; } - res->pose_with_covariance = align_pose(initial_pose_msg_in_map_frame); + if (param_.initial_pose_estimation.method == InitialPoseEstimationMethod::RANDOM_SEARCH) { + res->pose_with_covariance = align_pose_by_random_search(initial_pose_msg_in_map_frame); + } else if (param_.initial_pose_estimation.method == InitialPoseEstimationMethod::GRID_SEARCH) { + res->pose_with_covariance = align_pose_by_grid_search(initial_pose_msg_in_map_frame); + } else { + res->success = false; + RCLCPP_ERROR( + get_logger(), + "Unknown initial pose estimation method. " + "Please check initial_pose_estimation.method in ndt_scan_matcher.param.yaml."); + return; + } res->success = true; res->pose_with_covariance.pose.covariance = req->pose_with_covariance.pose.covariance; } -geometry_msgs::msg::PoseWithCovarianceStamped NDTScanMatcher::align_pose( +geometry_msgs::msg::PoseWithCovarianceStamped NDTScanMatcher::align_pose_by_random_search( const geometry_msgs::msg::PoseWithCovarianceStamped & initial_pose_with_cov) { output_pose_with_cov_to_log(get_logger(), "align_pose_input", initial_pose_with_cov); @@ -1011,3 +1022,107 @@ geometry_msgs::msg::PoseWithCovarianceStamped NDTScanMatcher::align_pose( return result_pose_with_cov_msg; } + +geometry_msgs::msg::PoseWithCovarianceStamped NDTScanMatcher::align_pose_by_grid_search( + const geometry_msgs::msg::PoseWithCovarianceStamped & initial_pose_with_cov) +{ + RCLCPP_INFO_STREAM(get_logger(), "start " << __func__); + + output_pose_with_cov_to_log(get_logger(), "align_pose_input", initial_pose_with_cov); + + const geometry_msgs::msg::Point base_xyz = initial_pose_with_cov.pose.pose.position; + const geometry_msgs::msg::Vector3 base_rpy = get_rpy(initial_pose_with_cov); + + std::vector particle_array; + auto output_cloud = std::make_shared>(); + + // publish the estimated poses in 20 times to see the progress and to avoid dropping data + const int64_t publish_interval = 20; + visualization_msgs::msg::MarkerArray marker_array; + + // calculate unit of grid search + const double unit_x = 2.0 * param_.initial_pose_estimation.grid_search_range_x / + std::max(param_.initial_pose_estimation.grid_num_x - 1, (int64_t)1); + const double unit_y = 2.0 * param_.initial_pose_estimation.grid_search_range_y / + std::max(param_.initial_pose_estimation.grid_num_y - 1, (int64_t)1); + const double unit_z = 2.0 * param_.initial_pose_estimation.grid_search_range_z / + std::max(param_.initial_pose_estimation.grid_num_z - 1, (int64_t)1); + const double unit_roll = 2.0 * param_.initial_pose_estimation.grid_num_roll / + std::max(param_.initial_pose_estimation.grid_num_roll - 1, (int64_t)1); + const double unit_pitch = 2.0 * param_.initial_pose_estimation.grid_num_pitch / + std::max(param_.initial_pose_estimation.grid_num_pitch - 1, (int64_t)1); + const double unit_yaw = 2.0 * param_.initial_pose_estimation.grid_num_yaw / + std::max(param_.initial_pose_estimation.grid_num_yaw - 1, (int64_t)1); + + int64_t total_i = 0; + for (int64_t ix = 0; ix < param_.initial_pose_estimation.grid_num_x; ix++) { + for (int64_t iy = 0; iy < param_.initial_pose_estimation.grid_num_y; iy++) { + for (int64_t iz = 0; iz < param_.initial_pose_estimation.grid_num_z; iz++) { + for (int64_t i_roll = 0; i_roll < param_.initial_pose_estimation.grid_num_roll; i_roll++) { + for (int64_t i_pitch = 0; i_pitch < param_.initial_pose_estimation.grid_num_pitch; + i_pitch++) { + for (int64_t i_yaw = 0; i_yaw < param_.initial_pose_estimation.grid_num_yaw; i_yaw++) { + geometry_msgs::msg::Pose initial_pose; + initial_pose.position.x = + base_xyz.x - param_.initial_pose_estimation.grid_search_range_x + ix * unit_x; + initial_pose.position.y = + base_xyz.y - param_.initial_pose_estimation.grid_search_range_y + iy * unit_y; + initial_pose.position.z = + base_xyz.z - param_.initial_pose_estimation.grid_search_range_z + iz * unit_z; + geometry_msgs::msg::Vector3 init_rpy; + init_rpy.x = base_rpy.x - param_.initial_pose_estimation.grid_search_range_roll + + i_roll * unit_roll; + init_rpy.y = base_rpy.y - param_.initial_pose_estimation.grid_search_range_pitch + + i_pitch * unit_pitch; + init_rpy.z = base_rpy.z - param_.initial_pose_estimation.grid_search_range_yaw + + i_yaw * unit_yaw; + tf2::Quaternion tf_quaternion; + tf_quaternion.setRPY(init_rpy.x, init_rpy.y, init_rpy.z); + initial_pose.orientation = tf2::toMsg(tf_quaternion); + + const Eigen::Matrix4f initial_pose_matrix = pose_to_matrix4f(initial_pose); + ndt_ptr_->align(*output_cloud, initial_pose_matrix); + const pclomp::NdtResult ndt_result = ndt_ptr_->getResult(); + + const Particle particle( + 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(), param_.frame.map_frame, particle, total_i); + if ( + (total_i + 1) % publish_interval == 0 || + (total_i + 1) == param_.initial_pose_estimation.particles_num) { + ndt_monte_carlo_initial_pose_marker_pub_->publish(marker_array); + marker_array.markers.clear(); + } + + 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); + + total_i++; + } + } + } + } + } + } + + const auto best_particle_ptr = std::max_element( + std::begin(particle_array), std::end(particle_array), + [](const Particle & lhs, const Particle & rhs) { return lhs.score < rhs.score; }); + + 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 = 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); + RCLCPP_DEBUG_STREAM(get_logger(), "best_score," << best_particle_ptr->score); + + return result_pose_with_cov_msg; +} From 51ca59da71d6e59ff97e223e3d983f9550f7d84d Mon Sep 17 00:00:00 2001 From: Shintaro Sakoda Date: Thu, 11 Apr 2024 17:03:29 +0900 Subject: [PATCH 2/5] Updated default params Signed-off-by: Shintaro Sakoda --- .../ndt_scan_matcher/config/ndt_scan_matcher.param.yaml | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) 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 6b8ac6ccb6f13..67118707b42c9 100644 --- a/localization/ndt_scan_matcher/config/ndt_scan_matcher.param.yaml +++ b/localization/ndt_scan_matcher/config/ndt_scan_matcher.param.yaml @@ -65,11 +65,11 @@ grid_num_z: 1 grid_num_roll: 1 grid_num_pitch: 1 - grid_num_yaw: 40 + grid_num_yaw: 80 # The range of grid search - grid_search_range_x: 1.0 # [m] (Search range: -1.0m ~ 1.0m) - grid_search_range_y: 1.0 # [m] (Search range: -1.0m ~ 1.0m) + grid_search_range_x: 3.0 # [m] (Search range: -3.0m ~ 3.0m) + grid_search_range_y: 3.0 # [m] (Search range: -3.0m ~ 3.0m) grid_search_range_z: 0.0 # [m] grid_search_range_roll: 0.0 # [rad] grid_search_range_pitch: 0.0 # [rad] From b52212ce3f036b2ea2b060c1bc048406a3d212ab Mon Sep 17 00:00:00 2001 From: Shintaro Sakoda Date: Thu, 11 Apr 2024 17:12:49 +0900 Subject: [PATCH 3/5] Updated initial_pose_estimation.json Signed-off-by: Shintaro Sakoda --- .../schema/sub/initial_pose_estimation.json | 99 ++++++++++++++++++- 1 file changed, 98 insertions(+), 1 deletion(-) diff --git a/localization/ndt_scan_matcher/schema/sub/initial_pose_estimation.json b/localization/ndt_scan_matcher/schema/sub/initial_pose_estimation.json index 9817f3145bbd3..bc4f4da46c75d 100644 --- a/localization/ndt_scan_matcher/schema/sub/initial_pose_estimation.json +++ b/localization/ndt_scan_matcher/schema/sub/initial_pose_estimation.json @@ -5,6 +5,15 @@ "initial_pose_estimation": { "type": "object", "properties": { + "method": { + "type": "number", + "description": "The method to estimate the initial pose. 0:RANDOM_SEARCH, 1:GRID_SEARCH", + "default": 1, + "enum": [ + 0, + 1 + ] + }, "particles_num": { "type": "number", "description": "The number of particles to estimate initial pose.", @@ -16,9 +25,97 @@ "description": "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.", "default": 20, "minimum": 1 + }, + "grid_num_x": { + "type": "number", + "description": "The number of grid search in x-axis.", + "default": 3, + "minimum": 1 + }, + "grid_num_y": { + "type": "number", + "description": "The number of grid search in y-axis.", + "default": 3, + "minimum": 1 + }, + "grid_num_z": { + "type": "number", + "description": "The number of grid search in z-axis.", + "default": 1, + "minimum": 1 + }, + "grid_num_roll": { + "type": "number", + "description": "The number of grid search in roll-axis.", + "default": 1, + "minimum": 1 + }, + "grid_num_pitch": { + "type": "number", + "description": "The number of grid search in pitch-axis.", + "default": 1, + "minimum": 1 + }, + "grid_num_yaw": { + "type": "number", + "description": "The number of grid search in yaw-axis.", + "default": 80, + "minimum": 1 + }, + "grid_search_range_x": { + "type": "number", + "description": "The range of grid search in x-axis. [m]", + "default": 3.0, + "minimum": 0.0 + }, + "grid_search_range_y": { + "type": "number", + "description": "The range of grid search in y-axis. [m]", + "default": 3.0, + "minimum": 0.0 + }, + "grid_search_range_z": { + "type": "number", + "description": "The range of grid search in z-axis. [m]", + "default": 0.0, + "minimum": 0.0 + }, + "grid_search_range_roll": { + "type": "number", + "description": "The range of grid search in roll-axis. [rad]", + "default": 0.0, + "minimum": 0.0 + }, + "grid_search_range_pitch": { + "type": "number", + "description": "The range of grid search in pitch-axis. [rad]", + "default": 0.0, + "minimum": 0.0 + }, + "grid_search_range_yaw": { + "type": "number", + "description": "The range of grid search in yaw-axis. [rad]", + "default": 3.0, + "minimum": 3.14159265359 } }, - "required": ["particles_num", "n_startup_trials"], + "required": [ + "method", + "particles_num", + "n_startup_trials", + "grid_num_x", + "grid_num_y", + "grid_num_z", + "grid_num_roll", + "grid_num_pitch", + "grid_num_yaw", + "grid_search_range_x", + "grid_search_range_y", + "grid_search_range_z", + "grid_search_range_roll", + "grid_search_range_pitch", + "grid_search_range_yaw" + ], "additionalProperties": false } } From a94ddbc67d4aa5ef3e6624a4e39d7bc562663ea6 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Thu, 11 Apr 2024 08:14:54 +0000 Subject: [PATCH 4/5] style(pre-commit): autofix --- .../ndt_scan_matcher/schema/sub/initial_pose_estimation.json | 5 +---- 1 file changed, 1 insertion(+), 4 deletions(-) diff --git a/localization/ndt_scan_matcher/schema/sub/initial_pose_estimation.json b/localization/ndt_scan_matcher/schema/sub/initial_pose_estimation.json index bc4f4da46c75d..9c16f08472b84 100644 --- a/localization/ndt_scan_matcher/schema/sub/initial_pose_estimation.json +++ b/localization/ndt_scan_matcher/schema/sub/initial_pose_estimation.json @@ -9,10 +9,7 @@ "type": "number", "description": "The method to estimate the initial pose. 0:RANDOM_SEARCH, 1:GRID_SEARCH", "default": 1, - "enum": [ - 0, - 1 - ] + "enum": [0, 1] }, "particles_num": { "type": "number", From 4c4ec09b49ce7f272f2b2a745e38e024ee905c64 Mon Sep 17 00:00:00 2001 From: Shintaro Sakoda Date: Thu, 11 Apr 2024 18:21:22 +0900 Subject: [PATCH 5/5] Removed log output Signed-off-by: Shintaro Sakoda --- localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp | 2 -- 1 file changed, 2 deletions(-) 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 58b8ef2f25954..d6f00c08f8306 100644 --- a/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp +++ b/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp @@ -1026,8 +1026,6 @@ geometry_msgs::msg::PoseWithCovarianceStamped NDTScanMatcher::align_pose_by_rand geometry_msgs::msg::PoseWithCovarianceStamped NDTScanMatcher::align_pose_by_grid_search( const geometry_msgs::msg::PoseWithCovarianceStamped & initial_pose_with_cov) { - RCLCPP_INFO_STREAM(get_logger(), "start " << __func__); - output_pose_with_cov_to_log(get_logger(), "align_pose_input", initial_pose_with_cov); const geometry_msgs::msg::Point base_xyz = initial_pose_with_cov.pose.pose.position;