From 55c615c410b22ce2276f2603a2b2c1c493cdc860 Mon Sep 17 00:00:00 2001 From: KokiAoki <81670028+KOKIAOKI@users.noreply.github.com> Date: Tue, 7 Nov 2023 16:43:15 +0900 Subject: [PATCH] feat(ndt_scan_matcher): estimate NDT covariance in real-time with limited initial positions (#5338) * add covariance estiamtion * fix: msg * fix: typo * fix: yaml * fix * Update localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp Initialize Eigen::Vector2d Co-authored-by: Kento Yabuuchi * fix: vector initilization, add: explanation in README Signed-off-by: Koki Aoki * fix: initialization and some build errors * refactor: variable and function names, changed covariance calculation process Signed-off-by: Koki Aoki * fix: README explanation Signed-off-by: Koki Aoki * fix: functionalization of eigenvector calculation, README empty line Signed-off-by: Koki Aoki * add warning outputs, rename function, move two functions in util.cpp to ndt_scan_matcher.cpp, fix README line Signed-off-by: Koki Aoki * fix: format of markdown * fix: add space in readme --------- Signed-off-by: Koki Aoki Co-authored-by: Kento Yabuuchi --- localization/ndt_scan_matcher/README.md | 25 +++ .../config/ndt_scan_matcher.param.yaml | 8 + .../ndt_scan_matcher_core.hpp | 11 +- .../src/ndt_scan_matcher_core.cpp | 143 +++++++++++++++++- 4 files changed, 178 insertions(+), 9 deletions(-) diff --git a/localization/ndt_scan_matcher/README.md b/localization/ndt_scan_matcher/README.md index e210d3a28796f..d7a7b1c5c37f3 100644 --- a/localization/ndt_scan_matcher/README.md +++ b/localization/ndt_scan_matcher/README.md @@ -34,6 +34,8 @@ One optional function is regularization. Please see the regularization chapter i | `points_aligned` | `sensor_msgs::msg::PointCloud2` | [debug topic] pointcloud aligned by scan matching | | `points_aligned_no_ground` | `sensor_msgs::msg::PointCloud2` | [debug topic] de-grounded pointcloud aligned by scan matching | | `initial_pose_with_covariance` | `geometry_msgs::msg::PoseWithCovarianceStamped` | [debug topic] initial pose used in scan matching | +| `multi_ndt_pose` | `geometry_msgs::msg::PoseArray` | [debug topic] estimated poses from multiple initial poses in real-time covariance estimation | +| `multi_initial_pose` | `geometry_msgs::msg::PoseArray` | [debug topic] initial poses for real-time covariance estimation | | `exe_time_ms` | `tier4_debug_msgs::msg::Float32Stamped` | [debug topic] execution time for scan matching [ms] | | `transform_probability` | `tier4_debug_msgs::msg::Float32Stamped` | [debug topic] score of scan matching | | `no_ground_transform_probability` | `tier4_debug_msgs::msg::Float32Stamped` | [debug topic] score of scan matching based on de-grounded LiDAR scan | @@ -257,3 +259,26 @@ This is a function that uses de-grounded LiDAR scan to estimate the scan matchin | ------------------------------------- | ------ | ------------------------------------------------------------------------------------- | | `estimate_scores_for_degrounded_scan` | bool | Flag for using scan matching score based on de-grounded LiDAR scan (FALSE by default) | | `z_margin_for_ground_removal` | double | Z-value margin for removal ground points | + +## 2D real-time covariance estimation + +### Abstract + +Calculate 2D covariance (xx, xy, yx, yy) in real time using the NDT convergence from multiple initial poses. +The arrangement of multiple initial poses is efficiently limited by the Hessian matrix of the NDT score function. +In this implementation, the number of initial positions is fixed to simplify the code. +The covariance can be seen as error ellipse from ndt_pose_with_covariance setting on rviz2. +[original paper](https://www.fujipress.jp/jrm/rb/robot003500020435/). + +Note that this function may spoil healthy system behavior if it consumes much calculation resources. + +### Parameters + +initial_pose_offset_model is rotated around (x,y) = (0,0) in the direction of the first principal component of the Hessian matrix. +initial_pose_offset_model_x & initial_pose_offset_model_y must have the same number of elements. + +| Name | Type | Description | +| ----------------------------- | ------------------- | ----------------------------------------------------------------- | +| `use_covariance_estimation` | bool | Flag for using real-time covariance estimation (FALSE by default) | +| `initial_pose_offset_model_x` | std::vector | X-axis offset [m] | +| `initial_pose_offset_model_y` | std::vector | Y-axis offset [m] | 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 0ba1b1a2e15f4..1a6c26cd9c351 100644 --- a/localization/ndt_scan_matcher/config/ndt_scan_matcher.param.yaml +++ b/localization/ndt_scan_matcher/config/ndt_scan_matcher.param.yaml @@ -72,6 +72,14 @@ 0.0, 0.0, 0.0, 0.0, 0.0, 0.000625, ] + # 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] + # Regularization switch regularization_enabled: false 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 09fc08a97a3ac..9eb62230dad9e 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 @@ -25,6 +25,7 @@ #include #include +#include #include #include #include @@ -105,7 +106,7 @@ class NDTScanMatcher : public rclcpp::Node const rclcpp::Time & sensor_ros_time, const geometry_msgs::msg::Pose & result_pose_msg); void publish_pose( const rclcpp::Time & sensor_ros_time, const geometry_msgs::msg::Pose & result_pose_msg, - const bool is_converged); + const std::array & ndt_covariance, const bool is_converged); void publish_point_cloud( const rclcpp::Time & sensor_ros_time, const std::string & frame_id, const pcl::shared_ptr> & sensor_points_in_map_ptr); @@ -123,6 +124,10 @@ class NDTScanMatcher : public rclcpp::Node bool validate_converged_param( const double & transform_probability, const double & nearest_voxel_transformation_likelihood); + std::array estimate_covariance( + const pclomp::NdtResult & ndt_result, const Eigen::Matrix4f & initial_pose_matrix, + const rclcpp::Time & sensor_ros_time); + std::optional interpolate_regularization_pose( const rclcpp::Time & sensor_ros_time); void add_regularization_pose(const rclcpp::Time & sensor_ros_time); @@ -141,6 +146,8 @@ class NDTScanMatcher : public rclcpp::Node ndt_pose_with_covariance_pub_; rclcpp::Publisher::SharedPtr initial_pose_with_covariance_pub_; + rclcpp::Publisher::SharedPtr multi_ndt_pose_pub_; + rclcpp::Publisher::SharedPtr multi_initial_pose_pub_; rclcpp::Publisher::SharedPtr exe_time_pub_; rclcpp::Publisher::SharedPtr transform_probability_pub_; rclcpp::Publisher::SharedPtr @@ -187,6 +194,8 @@ class NDTScanMatcher : public rclcpp::Node double initial_pose_distance_tolerance_m_; float inversion_vector_threshold_; float oscillation_threshold_; + bool use_cov_estimation_; + std::vector initial_pose_offset_model_; std::array output_pose_covariance_; std::deque 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 ac30b09df1c8e..cb66998147220 100644 --- a/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp +++ b/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp @@ -53,6 +53,32 @@ tier4_debug_msgs::msg::Int32Stamped make_int32_stamped( return tier4_debug_msgs::build().stamp(stamp).data(data); } +std::vector create_initial_pose_offset_model( + const std::vector & x, const std::vector & y) +{ + int size = x.size(); + std::vector initial_pose_offset_model(size); + for (int i = 0; i < size; i++) { + initial_pose_offset_model[i].x() = x[i]; + initial_pose_offset_model[i].y() = y[i]; + } + + return initial_pose_offset_model; +} + +Eigen::Matrix2d find_rotation_matrix_aligning_covariance_to_principal_axes( + const Eigen::Matrix2d & matrix) +{ + const Eigen::SelfAdjointEigenSolver eigensolver(matrix); + if (eigensolver.info() == Eigen::Success) { + const Eigen::Vector2d eigen_vec = eigensolver.eigenvectors().col(0); + const double th = std::atan2(eigen_vec.y(), eigen_vec.x()); + return Eigen::Rotation2Dd(th).toRotationMatrix(); + } else { + throw std::runtime_error("Eigen solver failed. Return output_pose_covariance value."); + } +} + bool validate_local_optimal_solution_oscillation( const std::vector & result_pose_msg_array, const float oscillation_threshold, const float inversion_vector_threshold) @@ -141,6 +167,24 @@ NDTScanMatcher::NDTScanMatcher() initial_pose_distance_tolerance_m_ = this->declare_parameter("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()) { + initial_pose_offset_model_ = + create_initial_pose_offset_model(initial_pose_offset_model_x, initial_pose_offset_model_y); + } 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) { @@ -191,6 +235,9 @@ NDTScanMatcher::NDTScanMatcher() initial_pose_with_covariance_pub_ = this->create_publisher( "initial_pose_with_covariance", 10); + multi_ndt_pose_pub_ = this->create_publisher("multi_ndt_pose", 10); + multi_initial_pose_pub_ = + this->create_publisher("multi_initial_pose", 10); exe_time_pub_ = this->create_publisher("exe_time_ms", 10); transform_probability_pub_ = this->create_publisher("transform_probability", 10); @@ -437,11 +484,6 @@ void NDTScanMatcher::callback_sensor_points( const pclomp::NdtResult ndt_result = ndt_ptr_->getResult(); (*state_ptr_)["state"] = "Sleeping"; - const auto exe_end_time = std::chrono::system_clock::now(); - const auto duration_micro_sec = - std::chrono::duration_cast(exe_end_time - exe_start_time).count(); - const auto exe_time = static_cast(duration_micro_sec) / 1000.0f; - const geometry_msgs::msg::Pose result_pose_msg = matrix4f_to_pose(ndt_result.pose); std::vector transformation_msg_array; for (const auto & pose_matrix : ndt_result.transformation_array) { @@ -468,6 +510,19 @@ void NDTScanMatcher::callback_sensor_points( RCLCPP_WARN(get_logger(), "Not Converged"); } + // covariance estimation + std::array ndt_covariance = output_pose_covariance_; + if (is_converged && use_cov_estimation_) { + const auto estimated_covariance = + estimate_covariance(ndt_result, initial_pose_matrix, sensor_ros_time); + ndt_covariance = estimated_covariance; + } + + const auto exe_end_time = std::chrono::system_clock::now(); + const auto duration_micro_sec = + std::chrono::duration_cast(exe_end_time - exe_start_time).count(); + const auto exe_time = static_cast(duration_micro_sec) / 1000.0f; + // publish initial_pose_with_covariance_pub_->publish(interpolator.get_current_pose()); exe_time_pub_->publish(make_float32_stamped(sensor_ros_time, exe_time)); @@ -477,7 +532,7 @@ void NDTScanMatcher::callback_sensor_points( make_float32_stamped(sensor_ros_time, ndt_result.nearest_voxel_transformation_likelihood)); iteration_num_pub_->publish(make_int32_stamped(sensor_ros_time, ndt_result.iteration_num)); publish_tf(sensor_ros_time, result_pose_msg); - publish_pose(sensor_ros_time, result_pose_msg, is_converged); + publish_pose(sensor_ros_time, result_pose_msg, ndt_covariance, is_converged); publish_marker(sensor_ros_time, transformation_msg_array); publish_initial_to_result( sensor_ros_time, result_pose_msg, interpolator.get_current_pose(), interpolator.get_old_pose(), @@ -560,7 +615,7 @@ void NDTScanMatcher::publish_tf( void NDTScanMatcher::publish_pose( const rclcpp::Time & sensor_ros_time, const geometry_msgs::msg::Pose & result_pose_msg, - const bool is_converged) + const std::array & ndt_covariance, const bool is_converged) { geometry_msgs::msg::PoseStamped result_pose_stamped_msg; result_pose_stamped_msg.header.stamp = sensor_ros_time; @@ -571,7 +626,7 @@ void NDTScanMatcher::publish_pose( 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.pose.pose = result_pose_msg; - result_pose_with_cov_msg.pose.covariance = output_pose_covariance_; + result_pose_with_cov_msg.pose.covariance = ndt_covariance; if (is_converged) { ndt_pose_pub_->publish(result_pose_stamped_msg); @@ -694,6 +749,78 @@ bool NDTScanMatcher::validate_converged_param( return is_ok_converged_param; } +std::array NDTScanMatcher::estimate_covariance( + const pclomp::NdtResult & ndt_result, const Eigen::Matrix4f & initial_pose_matrix, + const rclcpp::Time & sensor_ros_time) +{ + Eigen::Matrix2d rot = Eigen::Matrix2d::Identity(); + try { + rot = find_rotation_matrix_aligning_covariance_to_principal_axes( + ndt_result.hessian.inverse().block(0, 0, 2, 2)); + } catch (const std::exception & e) { + RCLCPP_WARN(get_logger(), e.what()); + return output_pose_covariance_; + } + + // first result is added to mean + const int n = 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; + ndt_pose_2d_vec.reserve(n); + ndt_pose_2d_vec.emplace_back(ndt_pose_2d); + + 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_initial_pose_msg.header.stamp = sensor_ros_time; + multi_initial_pose_msg.header.frame_id = 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_) { + const Eigen::Vector2d rotated_pose_offset_2d = rot * pose_offset; + + Eigen::Matrix4f sub_initial_pose_matrix(Eigen::Matrix4f::Identity()); + sub_initial_pose_matrix = ndt_result.pose; + sub_initial_pose_matrix(0, 3) += static_cast(rotated_pose_offset_2d.x()); + sub_initial_pose_matrix(1, 3) += static_cast(rotated_pose_offset_2d.y()); + + auto sub_output_cloud = std::make_shared>(); + ndt_ptr_->align(*sub_output_cloud, sub_initial_pose_matrix); + const Eigen::Matrix4f sub_ndt_result = ndt_ptr_->getResult().pose; + + const Eigen::Vector2d sub_ndt_pose_2d = sub_ndt_result.topRightCorner<2, 1>().cast(); + mean += sub_ndt_pose_2d; + ndt_pose_2d_vec.emplace_back(sub_ndt_pose_2d); + + multi_ndt_result_msg.poses.push_back(matrix4f_to_pose(sub_ndt_result)); + multi_initial_pose_msg.poses.push_back(matrix4f_to_pose(sub_initial_pose_matrix)); + } + + // calculate the covariance matrix + mean /= n; + Eigen::Matrix2d pca_covariance = Eigen::Matrix2d::Zero(); + for (const auto & temp_ndt_pose_2d : ndt_pose_2d_vec) { + const Eigen::Vector2d diff_2d = temp_ndt_pose_2d - mean; + pca_covariance += diff_2d * diff_2d.transpose(); + } + pca_covariance /= (n - 1); // unbiased covariance + + std::array ndt_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); + ndt_covariance[1 + 6 * 1] += pca_covariance(1, 1); + + multi_ndt_pose_pub_->publish(multi_ndt_result_msg); + multi_initial_pose_pub_->publish(multi_initial_pose_msg); + + return ndt_covariance; +} + std::optional NDTScanMatcher::interpolate_regularization_pose( const rclcpp::Time & sensor_ros_time) {