From 711b8218e6d33e566815b9592d896dbb1830e7d6 Mon Sep 17 00:00:00 2001 From: koki Date: Thu, 3 Aug 2023 14:10:56 +0900 Subject: [PATCH 01/14] add covariance estiamtion --- .../config/ndt_scan_matcher.param.yaml | 4 + .../ndt_scan_matcher_core.hpp | 10 +- .../include/ndt_scan_matcher/util_func.hpp | 4 + .../src/ndt_scan_matcher_core.cpp | 93 ++++++++++++++++++- .../ndt_scan_matcher/src/util_func.cpp | 13 +++ 5 files changed, 120 insertions(+), 4 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 a5d8142b6616e..f8232303f6b59 100644 --- a/localization/ndt_scan_matcher/config/ndt_scan_matcher.param.yaml +++ b/localization/ndt_scan_matcher/config/ndt_scan_matcher.param.yaml @@ -47,6 +47,10 @@ num_threads: 4 # The covariance of output pose + use_covariance_estimation: false + offset_array_x: [0.0,0.0,0.5,-0.5,1.0,-1.0] + offset_array_y: [0.5,-0.5,0.0,0.0,0.0,0.0] + # Do note that this covariance matrix is empirically derived output_pose_covariance: [ 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 8657e354c728a..de980596957a9 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 @@ -105,7 +105,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_covarianve, 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 +123,10 @@ class NDTScanMatcher : public rclcpp::Node bool validate_converged_param( const double & transform_probability, const double & nearest_voxel_transformation_likelihood); + void estimate_covariance( + const pclomp::NdtResult & ndt_result, const Eigen::Matrix4f & initial_pose_matrix, + std::array & ndt_covariance, 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 +145,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 @@ -183,6 +189,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 offset_array_; std::array output_pose_covariance_; std::deque diff --git a/localization/ndt_scan_matcher/include/ndt_scan_matcher/util_func.hpp b/localization/ndt_scan_matcher/include/ndt_scan_matcher/util_func.hpp index 933375d5796f2..f28df89221c26 100644 --- a/localization/ndt_scan_matcher/include/ndt_scan_matcher/util_func.hpp +++ b/localization/ndt_scan_matcher/include/ndt_scan_matcher/util_func.hpp @@ -35,6 +35,10 @@ #include #include +void create_offset_array( + const std::vector & x, const std::vector & y, + std::vector & offset_array); + // ref by http://takacity.blog.fc2.com/blog-entry-69.html std_msgs::msg::ColorRGBA exchange_color_crc(double x); 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 667fd59196df3..943c71608fb1d 100644 --- a/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp +++ b/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp @@ -147,6 +147,15 @@ NDTScanMatcher::NDTScanMatcher() initial_pose_distance_tolerance_m_ = this->declare_parameter( "initial_pose_distance_tolerance_m", initial_pose_distance_tolerance_m_); + use_cov_estimation_ = this->declare_parameter("use_covariance_estimation", use_cov_estimation_); + if (use_cov_estimation_) { + std::vector offset_array_x = + this->declare_parameter>("offset_array_x"); + std::vector offset_array_y = + this->declare_parameter>("offset_array_y"); + create_offset_array(offset_array_x, offset_array_y, offset_array_); + } + std::vector output_pose_covariance = this->declare_parameter>("output_pose_covariance"); for (std::size_t i = 0; i < output_pose_covariance.size(); ++i) { @@ -192,6 +201,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); @@ -438,6 +450,13 @@ void NDTScanMatcher::callback_sensor_points( RCLCPP_WARN(get_logger(), "Not Converged"); } + // covariance estimation + std::array ndt_covariance; + ndt_covariance = output_pose_covariance_; + if (is_converged && use_cov_estimation_) { + estimate_covariance(ndt_result, initial_pose_matrix, ndt_covariance, sensor_ros_time); + } + // publish initial_pose_with_covariance_pub_->publish(interpolator.get_current_pose()); exe_time_pub_->publish(make_float32_stamped(sensor_ros_time, exe_time)); @@ -447,7 +466,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_covarianve, is_converged); publish_marker(sensor_ros_time, transformation_msg_array); publish_initial_to_result_distances( sensor_ros_time, result_pose_msg, interpolator.get_current_pose(), interpolator.get_old_pose(), @@ -529,7 +548,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_covarianve, const bool is_converged) { geometry_msgs::msg::PoseStamped result_pose_stamped_msg; result_pose_stamped_msg.header.stamp = sensor_ros_time; @@ -540,7 +559,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_covarianve; if (is_converged) { ndt_pose_pub_->publish(result_pose_stamped_msg); @@ -656,6 +675,74 @@ bool NDTScanMatcher::validate_converged_param( return is_ok_converged_param; } +void NDTScanMatcher::estimate_covariance( + const pclomp::NdtResult & ndt_result, const Eigen::Matrix4f & initial_pose_matrix, + std::array & ndt_covariance, const rclcpp::Time & sensor_ros_time) +{ + // Laplace approximation + const Eigen::Matrix2d hessian_inv = -ndt_result.hessian.inverse().block(0, 0, 2, 2); + const Eigen::SelfAdjointEigenSolver lap_eigensolver(hessian_inv); + Eigen::Matrix2d rot; + rot = Eigen::Rotation2Dd(0.0); + if (lap_eigensolver.info() == Eigen::Success) { + const Eigen::Vector2d lap_eigen_vec = lap_eigensolver.eigenvectors().col(1); + const double th = std::atan2(lap_eigen_vec.y(), lap_eigen_vec.x()); + rot = Eigen::Rotation2Dd(th); + } + + // first result is added to mean and covariance + Eigen::Vector2d tmp_centroid; + Eigen::Matrix2d tmp_cov; + Eigen::Vector2d p2d(ndt_result.pose(0, 3), ndt_result.pose(1, 3)); + tmp_centroid += p2d; + tmp_cov += p2d * p2d.transpose(); + + // prepare msg + 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_initial_pose_msg.header.stamp = sensor_ros_time; + 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 search + for (const auto & offset_vec : offset_array_) { + Eigen::Vector2d offset_2d; + offset_2d = rot * offset_vec; + + Eigen::Matrix4f sub_initial_pose_matrix(Eigen::Matrix4f::Identity()); + sub_initial_pose_matrix = ndt_result.pose; + sub_initial_pose_matrix(0, 3) += static_cast(offset_2d.x()); + sub_initial_pose_matrix(1, 3) += static_cast(offset_2d.y()); + + // align + 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; + + // Eigen::Vector2d sub_p2d( + // static_cast(sub_ndt_result(0, 3)), static_cast(sub_ndt_result(1, 3))); + Eigen::Vector2d sub_p2d = sub_ndt_result.topRightCorner<2, 1>().cast(); + tmp_centroid += sub_p2d; + tmp_cov += sub_p2d * sub_p2d.transpose(); + + // push back msg + 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)); + } + Eigen::Vector2d centroid = tmp_centroid / (offset_array_.size() + 1); + // TODO unbiased covariance: R * ((n-1)/n)) + Eigen::Matrix2d pca_covariance; + pca_covariance = (tmp_cov / (offset_array_.size() + 1) - (centroid * centroid.transpose())); + ndt_covariance[0] = output_pose_covariance_[0] + pca_covariance(0, 0); + ndt_covariance[1] = pca_covariance(1, 0); + ndt_covariance[6] = pca_covariance(0, 1); + ndt_covariance[7] = output_pose_covariance_[7] + pca_covariance(1, 1); + + multi_ndt_pose_pub_->publish(multi_ndt_result_msg); + multi_initial_pose_pub_->publish(multi_initial_pose_msg); +} + std::optional NDTScanMatcher::interpolate_regularization_pose( const rclcpp::Time & sensor_ros_time) { diff --git a/localization/ndt_scan_matcher/src/util_func.cpp b/localization/ndt_scan_matcher/src/util_func.cpp index d947c56dfe183..e4ecdadb3d0d7 100644 --- a/localization/ndt_scan_matcher/src/util_func.cpp +++ b/localization/ndt_scan_matcher/src/util_func.cpp @@ -18,6 +18,19 @@ static std::random_device seed_gen; +void create_offset_array( + const std::vector & x, const std::vector & y, + std::vector & offset_array) +{ + int size = x.size(); + offset_array.resize(size); + if (x.size() == y.size()) + for (int i = 0; i < size; i++) { + offset_array[i].x() = x[i]; + offset_array[i].y() = y[i]; + } +} + // ref by http://takacity.blog.fc2.com/blog-entry-69.html std_msgs::msg::ColorRGBA exchange_color_crc(double x) { From 7025f9e3127f87b0938914b1d8c74dcef73065d9 Mon Sep 17 00:00:00 2001 From: koki Date: Thu, 3 Aug 2023 14:41:19 +0900 Subject: [PATCH 02/14] fix: msg --- .../include/ndt_scan_matcher/ndt_scan_matcher_core.hpp | 1 + 1 file changed, 1 insertion(+) 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 de980596957a9..9fd393ebb3c0c 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 From 50180f36fcd5735c77b66c93e7b2b6aed31969c2 Mon Sep 17 00:00:00 2001 From: koki Date: Thu, 3 Aug 2023 14:45:24 +0900 Subject: [PATCH 03/14] fix: typo --- .../include/ndt_scan_matcher/ndt_scan_matcher_core.hpp | 2 +- localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp | 6 +++--- 2 files changed, 4 insertions(+), 4 deletions(-) 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 9fd393ebb3c0c..50b6e8a45bcfb 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 @@ -106,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 std::array & ndt_covarianve, 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); 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 943c71608fb1d..f83d198f8b525 100644 --- a/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp +++ b/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp @@ -466,7 +466,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, ndt_covarianve, 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_distances( sensor_ros_time, result_pose_msg, interpolator.get_current_pose(), interpolator.get_old_pose(), @@ -548,7 +548,7 @@ void NDTScanMatcher::publish_tf( void NDTScanMatcher::publish_pose( const rclcpp::Time & sensor_ros_time, const geometry_msgs::msg::Pose & result_pose_msg, - const std::array & ndt_covarianve, 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; @@ -559,7 +559,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 = ndt_covarianve; + result_pose_with_cov_msg.pose.covariance = ndt_covariance; if (is_converged) { ndt_pose_pub_->publish(result_pose_stamped_msg); From 6444d2f65e5a37e1d184a6a0a3f1637e42bc846b Mon Sep 17 00:00:00 2001 From: koki Date: Tue, 17 Oct 2023 20:49:03 +0900 Subject: [PATCH 04/14] fix: yaml --- .../ndt_scan_matcher/config/ndt_scan_matcher.param.yaml | 5 +++++ 1 file changed, 5 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 f8232303f6b59..5d0aea7ebc234 100644 --- a/localization/ndt_scan_matcher/config/ndt_scan_matcher.param.yaml +++ b/localization/ndt_scan_matcher/config/ndt_scan_matcher.param.yaml @@ -62,6 +62,11 @@ 0.0, 0.0, 0.0, 0.0, 0.0, 0.000625, ] + # 2D Real-time covariance estimation (output_pose_covariance is the minimum value) + use_covariance_estimation: false + offset_array_x: [0.0,0.0,0.5,-0.5,1.0,-1.0] + offset_array_y: [0.5,-0.5,0.0,0.0,0.0,0.0] + # Regularization switch regularization_enabled: false From 130a008273c3099a2a4117d5895a893171d22721 Mon Sep 17 00:00:00 2001 From: koki Date: Tue, 17 Oct 2023 20:59:52 +0900 Subject: [PATCH 05/14] fix --- .../config/ndt_scan_matcher.param.yaml | 27 ++++++++++++++----- 1 file changed, 20 insertions(+), 7 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 5d0aea7ebc234..bebbfa26098af 100644 --- a/localization/ndt_scan_matcher/config/ndt_scan_matcher.param.yaml +++ b/localization/ndt_scan_matcher/config/ndt_scan_matcher.param.yaml @@ -6,6 +6,12 @@ # Vehicle reference frame base_frame: "base_link" + # NDT reference frame + ndt_base_frame: "ndt_base_link" + + # map frame + map_frame: "map" + # Subscriber queue size input_sensor_points_queue_size: 1 @@ -35,7 +41,15 @@ converged_param_nearest_voxel_transformation_likelihood: 2.3 # The number of particles to estimate initial pose - initial_estimate_particles_num: 100 + 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 + + # Tolerance of timestamp difference between current time and sensor pointcloud. [sec] + lidar_topic_timeout_sec: 1.0 # Tolerance of timestamp difference between initial_pose and sensor pointcloud. [sec] initial_pose_timeout_sec: 1.0 @@ -47,11 +61,7 @@ num_threads: 4 # The covariance of output pose - use_covariance_estimation: false - offset_array_x: [0.0,0.0,0.5,-0.5,1.0,-1.0] - offset_array_y: [0.5,-0.5,0.0,0.0,0.0,0.0] - - # Do note that this covariance matrix is empirically derived + # Note that this covariance matrix is empirically derived output_pose_covariance: [ 0.0225, 0.0, 0.0, 0.0, 0.0, 0.0, @@ -70,7 +80,7 @@ # Regularization switch regularization_enabled: false - # regularization scale factor + # Regularization scale factor regularization_scale_factor: 0.01 # Dynamic map loading distance @@ -88,3 +98,6 @@ # If lidar_point.z - base_link.z <= this threshold , the point will be removed z_margin_for_ground_removal: 0.8 + + # The execution time which means probably NDT cannot matches scans properly. [ms] + critical_upper_bound_exe_time_ms: 100 From f20bee4d18e30317c76bcddb66515323cbe86f42 Mon Sep 17 00:00:00 2001 From: KokiAoki <81670028+KOKIAOKI@users.noreply.github.com> Date: Wed, 18 Oct 2023 11:31:38 +0900 Subject: [PATCH 06/14] Update localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp Initialize Eigen::Vector2d Co-authored-by: Kento Yabuuchi --- localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp | 6 ++---- 1 file changed, 2 insertions(+), 4 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 888411935695c..b9b826f35cecd 100644 --- a/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp +++ b/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp @@ -739,11 +739,9 @@ void NDTScanMatcher::estimate_covariance( } // first result is added to mean and covariance - Eigen::Vector2d tmp_centroid; - Eigen::Matrix2d tmp_cov; Eigen::Vector2d p2d(ndt_result.pose(0, 3), ndt_result.pose(1, 3)); - tmp_centroid += p2d; - tmp_cov += p2d * p2d.transpose(); + Eigen::Vector2d tmp_centroid = p2d; + Eigen::Matrix2d tmp_cov = p2d * p2d.transpose(); // prepare msg geometry_msgs::msg::PoseArray multi_ndt_result_msg; From 692625fb7e35117a9a51159e513dd2bcc397c8c5 Mon Sep 17 00:00:00 2001 From: Koki Aoki Date: Wed, 18 Oct 2023 17:32:00 +0900 Subject: [PATCH 07/14] fix: vector initilization, add: explanation in README Signed-off-by: Koki Aoki --- localization/ndt_scan_matcher/README.md | 20 +++++++++++++++ .../src/ndt_scan_matcher_core.cpp | 25 +++++++++---------- 2 files changed, 32 insertions(+), 13 deletions(-) diff --git a/localization/ndt_scan_matcher/README.md b/localization/ndt_scan_matcher/README.md index e210d3a28796f..c67e7989bd8d4 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,21 @@ 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 rea-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/). + +### Parameters + +| Name | Type | Description | +| ---------------------------| ------------------- | -----------------------------------------------------------------| +| `use_covariance_estimation`| bool | Flag for using real-time covariance estimation (FALSE by default)| +| `offset_array_x` | std::vector | Default arrangement of initial poses (x) | +| `offset_array_y` | std::vector | Default arrangement of initial poses (y) | \ No newline at end of file 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 b9b826f35cecd..dce9e4d59c6f8 100644 --- a/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp +++ b/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp @@ -141,7 +141,7 @@ NDTScanMatcher::NDTScanMatcher() initial_pose_distance_tolerance_m_ = this->declare_parameter("initial_pose_distance_tolerance_m"); - use_cov_estimation_ = this->declare_parameter("use_covariance_estimation", use_cov_estimation_); + use_cov_estimation_ = this->declare_parameter("use_covariance_estimation"); if (use_cov_estimation_) { std::vector offset_array_x = this->declare_parameter>("offset_array_x"); @@ -447,6 +447,13 @@ void NDTScanMatcher::callback_sensor_points( const pclomp::NdtResult ndt_result = ndt_ptr_->getResult(); (*state_ptr_)["state"] = "Sleeping"; + // covariance estimation + std::array ndt_covariance; + ndt_covariance = output_pose_covariance_; + if (is_converged && use_cov_estimation_) { + estimate_covariance(ndt_result, initial_pose_matrix, ndt_covariance, sensor_ros_time); + } + 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(); @@ -490,13 +497,6 @@ void NDTScanMatcher::callback_sensor_points( RCLCPP_WARN(get_logger(), "Not Converged"); } - // covariance estimation - std::array ndt_covariance; - ndt_covariance = output_pose_covariance_; - if (is_converged && use_cov_estimation_) { - estimate_covariance(ndt_result, initial_pose_matrix, ndt_covariance, sensor_ros_time); - } - // publish initial_pose_with_covariance_pub_->publish(interpolator.get_current_pose()); exe_time_pub_->publish(make_float32_stamped(sensor_ros_time, exe_time)); @@ -730,8 +730,7 @@ void NDTScanMatcher::estimate_covariance( // Laplace approximation const Eigen::Matrix2d hessian_inv = -ndt_result.hessian.inverse().block(0, 0, 2, 2); const Eigen::SelfAdjointEigenSolver lap_eigensolver(hessian_inv); - Eigen::Matrix2d rot; - rot = Eigen::Rotation2Dd(0.0); + Eigen::Matrix2d rot = Eigen::Rotation2Dd(0.0); if (lap_eigensolver.info() == Eigen::Success) { const Eigen::Vector2d lap_eigen_vec = lap_eigensolver.eigenvectors().col(1); const double th = std::atan2(lap_eigen_vec.y(), lap_eigen_vec.x()); @@ -777,9 +776,9 @@ void NDTScanMatcher::estimate_covariance( multi_initial_pose_msg.poses.push_back(matrix4f_to_pose(sub_initial_pose_matrix)); } Eigen::Vector2d centroid = tmp_centroid / (offset_array_.size() + 1); - // TODO unbiased covariance: R * ((n-1)/n)) - Eigen::Matrix2d pca_covariance; - pca_covariance = (tmp_cov / (offset_array_.size() + 1) - (centroid * centroid.transpose())); + + Eigen::Matrix2d pca_covariance = + (tmp_cov / (offset_array_.size() + 1) - (centroid * centroid.transpose())); ndt_covariance[0] = output_pose_covariance_[0] + pca_covariance(0, 0); ndt_covariance[1] = pca_covariance(1, 0); ndt_covariance[6] = pca_covariance(0, 1); From 69f0e2cd39c262ced060f5ca9ef1870bd944d02c Mon Sep 17 00:00:00 2001 From: Koki Aoki Date: Tue, 24 Oct 2023 17:56:23 +0900 Subject: [PATCH 08/14] fix: initialization and some build errors --- .../src/ndt_scan_matcher_core.cpp | 46 +++++++++---------- 1 file changed, 21 insertions(+), 25 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 a3f017d65c31b..1e9151892d162 100644 --- a/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp +++ b/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp @@ -447,18 +447,6 @@ void NDTScanMatcher::callback_sensor_points( const pclomp::NdtResult ndt_result = ndt_ptr_->getResult(); (*state_ptr_)["state"] = "Sleeping"; - // covariance estimation - std::array ndt_covariance; - ndt_covariance = output_pose_covariance_; - if (is_converged && use_cov_estimation_) { - estimate_covariance(ndt_result, initial_pose_matrix, ndt_covariance, sensor_ros_time); - } - - 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) { @@ -485,6 +473,18 @@ void NDTScanMatcher::callback_sensor_points( RCLCPP_WARN(get_logger(), "Not Converged"); } + // covariance estimation + std::array ndt_covariance; + ndt_covariance = output_pose_covariance_; + if (is_converged && use_cov_estimation_) { + estimate_covariance(ndt_result, initial_pose_matrix, ndt_covariance, sensor_ros_time); + } + + 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)); @@ -718,7 +718,7 @@ void NDTScanMatcher::estimate_covariance( // Laplace approximation const Eigen::Matrix2d hessian_inv = -ndt_result.hessian.inverse().block(0, 0, 2, 2); const Eigen::SelfAdjointEigenSolver lap_eigensolver(hessian_inv); - Eigen::Matrix2d rot = Eigen::Rotation2Dd(0.0); + Eigen::Matrix2d rot = Eigen::Matrix2d::Identity(); if (lap_eigensolver.info() == Eigen::Success) { const Eigen::Vector2d lap_eigen_vec = lap_eigensolver.eigenvectors().col(1); const double th = std::atan2(lap_eigen_vec.y(), lap_eigen_vec.x()); @@ -726,46 +726,42 @@ void NDTScanMatcher::estimate_covariance( } // first result is added to mean and covariance - Eigen::Vector2d p2d(ndt_result.pose(0, 3), ndt_result.pose(1, 3)); + const Eigen::Vector2d p2d(ndt_result.pose(0, 3), ndt_result.pose(1, 3)); Eigen::Vector2d tmp_centroid = p2d; Eigen::Matrix2d tmp_cov = p2d * p2d.transpose(); - // prepare msg 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 search + // multiple searches for (const auto & offset_vec : offset_array_) { - Eigen::Vector2d offset_2d; - offset_2d = rot * offset_vec; + const Eigen::Vector2d offset_2d = rot * offset_vec; Eigen::Matrix4f sub_initial_pose_matrix(Eigen::Matrix4f::Identity()); sub_initial_pose_matrix = ndt_result.pose; sub_initial_pose_matrix(0, 3) += static_cast(offset_2d.x()); sub_initial_pose_matrix(1, 3) += static_cast(offset_2d.y()); - // align 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; - // Eigen::Vector2d sub_p2d( - // static_cast(sub_ndt_result(0, 3)), static_cast(sub_ndt_result(1, 3))); - Eigen::Vector2d sub_p2d = sub_ndt_result.topRightCorner<2, 1>().cast(); + const Eigen::Vector2d sub_p2d = sub_ndt_result.topRightCorner<2, 1>().cast(); tmp_centroid += sub_p2d; tmp_cov += sub_p2d * sub_p2d.transpose(); - // push back msg 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)); } - Eigen::Vector2d centroid = tmp_centroid / (offset_array_.size() + 1); + const Eigen::Vector2d centroid = tmp_centroid / (offset_array_.size() + 1); - Eigen::Matrix2d pca_covariance = + const Eigen::Matrix2d pca_covariance = (tmp_cov / (offset_array_.size() + 1) - (centroid * centroid.transpose())); ndt_covariance[0] = output_pose_covariance_[0] + pca_covariance(0, 0); ndt_covariance[1] = pca_covariance(1, 0); From 2cf6018c437461004b5a33e12a8b9ff6685036b9 Mon Sep 17 00:00:00 2001 From: Koki Aoki Date: Wed, 25 Oct 2023 16:26:01 +0900 Subject: [PATCH 09/14] refactor: variable and function names, changed covariance calculation process Signed-off-by: Koki Aoki --- .../include/localization_util/util_func.hpp | 5 +- .../localization_util/src/util_func.cpp | 18 ++--- localization/ndt_scan_matcher/README.md | 17 +++-- .../config/ndt_scan_matcher.param.yaml | 9 ++- .../ndt_scan_matcher_core.hpp | 6 +- .../src/ndt_scan_matcher_core.cpp | 75 ++++++++++++------- 6 files changed, 77 insertions(+), 53 deletions(-) diff --git a/localization/localization_util/include/localization_util/util_func.hpp b/localization/localization_util/include/localization_util/util_func.hpp index 37affa9073983..9dd5a8257419f 100644 --- a/localization/localization_util/include/localization_util/util_func.hpp +++ b/localization/localization_util/include/localization_util/util_func.hpp @@ -36,9 +36,8 @@ #include #include -void create_offset_array( - const std::vector & x, const std::vector & y, - std::vector & offset_array); +std::vector create_initial_pose_offset_model( + const std::vector & x, const std::vector & y); // ref by http://takacity.blog.fc2.com/blog-entry-69.html std_msgs::msg::ColorRGBA exchange_color_crc(double x); diff --git a/localization/localization_util/src/util_func.cpp b/localization/localization_util/src/util_func.cpp index 3741219075cdc..7d3759b150082 100644 --- a/localization/localization_util/src/util_func.cpp +++ b/localization/localization_util/src/util_func.cpp @@ -18,17 +18,17 @@ static std::random_device seed_gen; -void create_offset_array( - const std::vector & x, const std::vector & y, - std::vector & offset_array) +std::vector create_initial_pose_offset_model( + const std::vector & x, const std::vector & y) { int size = x.size(); - offset_array.resize(size); - if (x.size() == y.size()) - for (int i = 0; i < size; i++) { - offset_array[i].x() = x[i]; - offset_array[i].y() = y[i]; - } + 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; } // ref by http://takacity.blog.fc2.com/blog-entry-69.html diff --git a/localization/ndt_scan_matcher/README.md b/localization/ndt_scan_matcher/README.md index c67e7989bd8d4..bb8573fa5c3bf 100644 --- a/localization/ndt_scan_matcher/README.md +++ b/localization/ndt_scan_matcher/README.md @@ -260,7 +260,7 @@ 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 rea-time covariance estimation +## 2D real-time covariance estimation ### Abstract @@ -270,10 +270,13 @@ In this implementation, the number of initial positions is fixed to simplify the 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/). -### Parameters +Note that this function may spoil healthy system behavior if it consumes much calculation resource. -| Name | Type | Description | -| ---------------------------| ------------------- | -----------------------------------------------------------------| -| `use_covariance_estimation`| bool | Flag for using real-time covariance estimation (FALSE by default)| -| `offset_array_x` | std::vector | Default arrangement of initial poses (x) | -| `offset_array_y` | std::vector | Default arrangement of initial poses (y) | \ No newline at end of file +### Parameters +initial_pose_offset_model is rotated around (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] | \ No newline at end of file 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 bebbfa26098af..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,10 +72,13 @@ 0.0, 0.0, 0.0, 0.0, 0.0, 0.000625, ] - # 2D Real-time covariance estimation (output_pose_covariance is the minimum value) + # 2D Real-time covariance estimation with multiple searches (output_pose_covariance is the minimum value) use_covariance_estimation: false - offset_array_x: [0.0,0.0,0.5,-0.5,1.0,-1.0] - offset_array_y: [0.5,-0.5,0.0,0.0,0.0,0.0] + + # 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 5cb723e936dec..774193958bbc3 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 @@ -123,9 +123,9 @@ class NDTScanMatcher : public rclcpp::Node bool validate_converged_param( const double & transform_probability, const double & nearest_voxel_transformation_likelihood); - void estimate_covariance( + std::array estimate_covariance( const pclomp::NdtResult & ndt_result, const Eigen::Matrix4f & initial_pose_matrix, - std::array & ndt_covariance, const rclcpp::Time & sensor_ros_time); + const rclcpp::Time & sensor_ros_time); std::optional interpolate_regularization_pose( const rclcpp::Time & sensor_ros_time); @@ -194,7 +194,7 @@ class NDTScanMatcher : public rclcpp::Node float inversion_vector_threshold_; float oscillation_threshold_; bool use_cov_estimation_; - std::vector offset_array_; + 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 79f5882d5f68c..7c35e9eb6db19 100644 --- a/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp +++ b/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp @@ -143,11 +143,17 @@ NDTScanMatcher::NDTScanMatcher() use_cov_estimation_ = this->declare_parameter("use_covariance_estimation"); if (use_cov_estimation_) { - std::vector offset_array_x = - this->declare_parameter>("offset_array_x"); - std::vector offset_array_y = - this->declare_parameter>("offset_array_y"); - create_offset_array(offset_array_x, offset_array_y, offset_array_); + 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 { + use_cov_estimation_ = false; + } } std::vector output_pose_covariance = @@ -474,10 +480,11 @@ void NDTScanMatcher::callback_sensor_points( } // covariance estimation - std::array ndt_covariance; - ndt_covariance = output_pose_covariance_; + std::array ndt_covariance = output_pose_covariance_; if (is_converged && use_cov_estimation_) { - estimate_covariance(ndt_result, initial_pose_matrix, ndt_covariance, sensor_ros_time); + 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(); @@ -711,9 +718,9 @@ bool NDTScanMatcher::validate_converged_param( return is_ok_converged_param; } -void NDTScanMatcher::estimate_covariance( +std::array NDTScanMatcher::estimate_covariance( const pclomp::NdtResult & ndt_result, const Eigen::Matrix4f & initial_pose_matrix, - std::array & ndt_covariance, const rclcpp::Time & sensor_ros_time) + const rclcpp::Time & sensor_ros_time) { // Laplace approximation const Eigen::Matrix2d hessian_inv = -ndt_result.hessian.inverse().block(0, 0, 2, 2); @@ -725,10 +732,13 @@ void NDTScanMatcher::estimate_covariance( rot = Eigen::Rotation2Dd(th); } - // first result is added to mean and covariance - const Eigen::Vector2d p2d(ndt_result.pose(0, 3), ndt_result.pose(1, 3)); - Eigen::Vector2d tmp_centroid = p2d; - Eigen::Matrix2d tmp_cov = p2d * p2d.transpose(); + // 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; @@ -740,36 +750,45 @@ void NDTScanMatcher::estimate_covariance( multi_initial_pose_msg.poses.push_back(matrix4f_to_pose(initial_pose_matrix)); // multiple searches - for (const auto & offset_vec : offset_array_) { - const Eigen::Vector2d offset_2d = rot * offset_vec; + 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(offset_2d.x()); - sub_initial_pose_matrix(1, 3) += static_cast(offset_2d.y()); + 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_p2d = sub_ndt_result.topRightCorner<2, 1>().cast(); - tmp_centroid += sub_p2d; - tmp_cov += sub_p2d * sub_p2d.transpose(); + 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)); } - const Eigen::Vector2d centroid = tmp_centroid / (offset_array_.size() + 1); - const Eigen::Matrix2d pca_covariance = - (tmp_cov / (offset_array_.size() + 1) - (centroid * centroid.transpose())); - ndt_covariance[0] = output_pose_covariance_[0] + pca_covariance(0, 0); - ndt_covariance[1] = pca_covariance(1, 0); - ndt_covariance[6] = pca_covariance(0, 1); - ndt_covariance[7] = output_pose_covariance_[7] + pca_covariance(1, 1); + // 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( From 9f6098b7538c1287164410b2d4f86c0b769b49d8 Mon Sep 17 00:00:00 2001 From: Koki Aoki Date: Wed, 25 Oct 2023 20:35:39 +0900 Subject: [PATCH 10/14] fix: README explanation Signed-off-by: Koki Aoki --- localization/ndt_scan_matcher/README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/localization/ndt_scan_matcher/README.md b/localization/ndt_scan_matcher/README.md index bb8573fa5c3bf..c79645eb6d6f1 100644 --- a/localization/ndt_scan_matcher/README.md +++ b/localization/ndt_scan_matcher/README.md @@ -273,7 +273,7 @@ The covariance can be seen as error ellipse from ndt_pose_with_covariance settin Note that this function may spoil healthy system behavior if it consumes much calculation resource. ### Parameters -initial_pose_offset_model is rotated around (0,0) in the direction of the first principal component of the Hessian matrix. +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 | | -----------------------------| ------------------- | -----------------------------------------------------------------| From 41442ad02b8682ab916db7c25e3b6093ddbb6657 Mon Sep 17 00:00:00 2001 From: Koki Aoki Date: Tue, 31 Oct 2023 16:01:58 +0900 Subject: [PATCH 11/14] fix: functionalization of eigenvector calculation, README empty line Signed-off-by: Koki Aoki --- .../include/localization_util/util_func.hpp | 4 ++++ localization/localization_util/src/util_func.cpp | 13 +++++++++++++ localization/ndt_scan_matcher/README.md | 4 ++-- .../ndt_scan_matcher/src/ndt_scan_matcher_core.cpp | 11 +++-------- 4 files changed, 22 insertions(+), 10 deletions(-) diff --git a/localization/localization_util/include/localization_util/util_func.hpp b/localization/localization_util/include/localization_util/util_func.hpp index 9dd5a8257419f..92cf17b9da06d 100644 --- a/localization/localization_util/include/localization_util/util_func.hpp +++ b/localization/localization_util/include/localization_util/util_func.hpp @@ -29,6 +29,8 @@ #include #endif +#include + #include #include #include @@ -39,6 +41,8 @@ std::vector create_initial_pose_offset_model( const std::vector & x, const std::vector & y); +Eigen::Matrix2d calc_eigenvector_angle(const Eigen::Matrix2d & matrix); + // ref by http://takacity.blog.fc2.com/blog-entry-69.html std_msgs::msg::ColorRGBA exchange_color_crc(double x); diff --git a/localization/localization_util/src/util_func.cpp b/localization/localization_util/src/util_func.cpp index 7d3759b150082..9239056c08537 100644 --- a/localization/localization_util/src/util_func.cpp +++ b/localization/localization_util/src/util_func.cpp @@ -31,6 +31,19 @@ std::vector create_initial_pose_offset_model( return initial_pose_offset_model; } +Eigen::Matrix2d calc_eigenvector_angle(const Eigen::Matrix2d & matrix) +{ + const Eigen::SelfAdjointEigenSolver eigensolver(matrix); + Eigen::Matrix2d rot = Eigen::Matrix2d::Zero(); + 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()); + rot = Eigen::Rotation2Dd(th); + } + + return rot; +} + // ref by http://takacity.blog.fc2.com/blog-entry-69.html std_msgs::msg::ColorRGBA exchange_color_crc(double x) { diff --git a/localization/ndt_scan_matcher/README.md b/localization/ndt_scan_matcher/README.md index c79645eb6d6f1..9c07bb91d697d 100644 --- a/localization/ndt_scan_matcher/README.md +++ b/localization/ndt_scan_matcher/README.md @@ -270,7 +270,7 @@ In this implementation, the number of initial positions is fixed to simplify the 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 resource. +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. @@ -279,4 +279,4 @@ initial_pose_offset_model_x & initial_pose_offset_model_y must have the same num | -----------------------------| ------------------- | -----------------------------------------------------------------| | `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] | \ No newline at end of file +| `initial_pose_offset_model_y`| std::vector | Y-axis offset [m] | 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 7c35e9eb6db19..a5f5e7c4b41bd 100644 --- a/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp +++ b/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp @@ -722,14 +722,9 @@ std::array NDTScanMatcher::estimate_covariance( const pclomp::NdtResult & ndt_result, const Eigen::Matrix4f & initial_pose_matrix, const rclcpp::Time & sensor_ros_time) { - // Laplace approximation - const Eigen::Matrix2d hessian_inv = -ndt_result.hessian.inverse().block(0, 0, 2, 2); - const Eigen::SelfAdjointEigenSolver lap_eigensolver(hessian_inv); - Eigen::Matrix2d rot = Eigen::Matrix2d::Identity(); - if (lap_eigensolver.info() == Eigen::Success) { - const Eigen::Vector2d lap_eigen_vec = lap_eigensolver.eigenvectors().col(1); - const double th = std::atan2(lap_eigen_vec.y(), lap_eigen_vec.x()); - rot = Eigen::Rotation2Dd(th); + Eigen::Matrix2d rot = calc_eigenvector_angle(ndt_result.hessian.inverse().block(0, 0, 2, 2)); + if (rot == Eigen::Matrix2d::Zero()) { + return output_pose_covariance_; } // first result is added to mean From 468d3ce8f0229eb5c2e004b8d0af94661e0c9864 Mon Sep 17 00:00:00 2001 From: Koki Aoki Date: Wed, 1 Nov 2023 14:32:55 +0900 Subject: [PATCH 12/14] add warning outputs, rename function, move two functions in util.cpp to ndt_scan_matcher.cpp, fix README line Signed-off-by: Koki Aoki --- .../include/localization_util/util_func.hpp | 7 ---- .../localization_util/src/util_func.cpp | 26 ------------- localization/ndt_scan_matcher/README.md | 1 + .../ndt_scan_matcher_core.hpp | 2 +- .../src/ndt_scan_matcher_core.cpp | 37 ++++++++++++++++++- 5 files changed, 37 insertions(+), 36 deletions(-) diff --git a/localization/localization_util/include/localization_util/util_func.hpp b/localization/localization_util/include/localization_util/util_func.hpp index 92cf17b9da06d..bd9a2b9305e25 100644 --- a/localization/localization_util/include/localization_util/util_func.hpp +++ b/localization/localization_util/include/localization_util/util_func.hpp @@ -29,8 +29,6 @@ #include #endif -#include - #include #include #include @@ -38,11 +36,6 @@ #include #include -std::vector create_initial_pose_offset_model( - const std::vector & x, const std::vector & y); - -Eigen::Matrix2d calc_eigenvector_angle(const Eigen::Matrix2d & matrix); - // ref by http://takacity.blog.fc2.com/blog-entry-69.html std_msgs::msg::ColorRGBA exchange_color_crc(double x); diff --git a/localization/localization_util/src/util_func.cpp b/localization/localization_util/src/util_func.cpp index 9239056c08537..865cc02cff293 100644 --- a/localization/localization_util/src/util_func.cpp +++ b/localization/localization_util/src/util_func.cpp @@ -18,32 +18,6 @@ static std::random_device seed_gen; -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 calc_eigenvector_angle(const Eigen::Matrix2d & matrix) -{ - const Eigen::SelfAdjointEigenSolver eigensolver(matrix); - Eigen::Matrix2d rot = Eigen::Matrix2d::Zero(); - 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()); - rot = Eigen::Rotation2Dd(th); - } - - return rot; -} - // ref by http://takacity.blog.fc2.com/blog-entry-69.html std_msgs::msg::ColorRGBA exchange_color_crc(double x) { diff --git a/localization/ndt_scan_matcher/README.md b/localization/ndt_scan_matcher/README.md index 9c07bb91d697d..5bbea444040d4 100644 --- a/localization/ndt_scan_matcher/README.md +++ b/localization/ndt_scan_matcher/README.md @@ -273,6 +273,7 @@ The covariance can be seen as error ellipse from ndt_pose_with_covariance settin 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 | 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 774193958bbc3..48498ec3bb024 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 @@ -24,8 +24,8 @@ #include #include -#include #include +#include #include #include #include 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 a5f5e7c4b41bd..697dd87fd0b7a 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) @@ -152,6 +178,9 @@ NDTScanMatcher::NDTScanMatcher() 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; } } @@ -722,8 +751,12 @@ 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 = calc_eigenvector_angle(ndt_result.hessian.inverse().block(0, 0, 2, 2)); - if (rot == Eigen::Matrix2d::Zero()) { + 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_; } From 5eda6c364a542b1d87614500c7a1c1c6451484c1 Mon Sep 17 00:00:00 2001 From: Koki Aoki Date: Thu, 2 Nov 2023 14:01:05 +0900 Subject: [PATCH 13/14] fix: format of markdown --- localization/ndt_scan_matcher/README.md | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/localization/ndt_scan_matcher/README.md b/localization/ndt_scan_matcher/README.md index 5bbea444040d4..5b798a09baca6 100644 --- a/localization/ndt_scan_matcher/README.md +++ b/localization/ndt_scan_matcher/README.md @@ -34,8 +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 | +| `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 | @@ -276,6 +276,7 @@ Note that this function may spoil healthy system behavior if it consumes much ca 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)| From 3e31b932356fe0228f96fbb6fe48ccdbec85ee20 Mon Sep 17 00:00:00 2001 From: Koki Aoki Date: Thu, 2 Nov 2023 17:39:44 +0900 Subject: [PATCH 14/14] fix: add space in readme --- localization/ndt_scan_matcher/README.md | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/localization/ndt_scan_matcher/README.md b/localization/ndt_scan_matcher/README.md index 5b798a09baca6..d7a7b1c5c37f3 100644 --- a/localization/ndt_scan_matcher/README.md +++ b/localization/ndt_scan_matcher/README.md @@ -277,8 +277,8 @@ Note that this function may spoil healthy system behavior if it consumes much ca 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] | +| 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] |