feat(ndt_scan_matcher): estimate NDT covariance in real-time with limited initial positions

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<double> | Default arrangement of initial poses (x)                         |
+| `offset_array_y`           | std::vector<double> | Default arrangement of initial poses (y)                         |

localization/ndt_scan_matcher/config/ndt_scan_matcher.param.yaml

@@ -72,6 +72,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
 

localization/ndt_scan_matcher/include/ndt_scan_matcher/ndt_scan_matcher_core.hpp

@@ -25,6 +25,7 @@
 
 #include <diagnostic_msgs/msg/diagnostic_array.hpp>
 #include <geometry_msgs/msg/pose_with_covariance_stamped.hpp>
+#include <geometry_msgs/msg/pose_array.hpp>
 #include <geometry_msgs/msg/twist_stamped.hpp>
 #include <nav_msgs/msg/odometry.hpp>
 #include <sensor_msgs/msg/point_cloud2.hpp>
@@ -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<double, 36> & 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<pcl::PointCloud<PointSource>> & 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);
 
+  void estimate_covariance(
+    const pclomp::NdtResult & ndt_result, const Eigen::Matrix4f & initial_pose_matrix,
+    std::array<double, 36> & ndt_covariance, const rclcpp::Time & sensor_ros_time);
+
   std::optional<Eigen::Matrix4f> 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<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr
     initial_pose_with_covariance_pub_;
+  rclcpp::Publisher<geometry_msgs::msg::PoseArray>::SharedPtr multi_ndt_pose_pub_;
+  rclcpp::Publisher<geometry_msgs::msg::PoseArray>::SharedPtr multi_initial_pose_pub_;
   rclcpp::Publisher<tier4_debug_msgs::msg::Float32Stamped>::SharedPtr exe_time_pub_;
   rclcpp::Publisher<tier4_debug_msgs::msg::Float32Stamped>::SharedPtr transform_probability_pub_;
   rclcpp::Publisher<tier4_debug_msgs::msg::Float32Stamped>::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<Eigen::Vector2d> offset_array_;
   std::array<double, 36> output_pose_covariance_;
 
   std::deque<geometry_msgs::msg::PoseWithCovarianceStamped::ConstSharedPtr>

localization/ndt_scan_matcher/include/ndt_scan_matcher/util_func.hpp

@@ -36,6 +36,10 @@
 #include <string>
 #include <vector>
 
+void create_offset_array(
+  const std::vector<double> & x, const std::vector<double> & y,
+  std::vector<Eigen::Vector2d> & offset_array);
+
 // ref by http://takacity.blog.fc2.com/blog-entry-69.html
 std_msgs::msg::ColorRGBA exchange_color_crc(double x);
 

localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp

@@ -1,4 +1,4 @@
 // Copyright 2015-2019 Autoware Foundation
 //
 // Licensed under the Apache License, Version 2.0 (the "License");
 // you may not use this file except in compliance with the License.
@@ -141,6 +141,15 @@
   initial_pose_distance_tolerance_m_ =
     this->declare_parameter<double>("initial_pose_distance_tolerance_m");
 
+  use_cov_estimation_ = this->declare_parameter<bool>("use_covariance_estimation");
+  if (use_cov_estimation_) {
+    std::vector<double> offset_array_x =
+      this->declare_parameter<std::vector<double>>("offset_array_x");
+    std::vector<double> offset_array_y =
+      this->declare_parameter<std::vector<double>>("offset_array_y");
+    create_offset_array(offset_array_x, offset_array_y, offset_array_);
+  }
+
   std::vector<double> output_pose_covariance =
     this->declare_parameter<std::vector<double>>("output_pose_covariance");
   for (std::size_t i = 0; i < output_pose_covariance.size(); ++i) {
@@ -191,6 +200,9 @@
   initial_pose_with_covariance_pub_ =
     this->create_publisher<geometry_msgs::msg::PoseWithCovarianceStamped>(
       "initial_pose_with_covariance", 10);
+  multi_ndt_pose_pub_ = this->create_publisher<geometry_msgs::msg::PoseArray>("multi_ndt_pose", 10);
+  multi_initial_pose_pub_ =
+    this->create_publisher<geometry_msgs::msg::PoseArray>("multi_initial_pose", 10);
   exe_time_pub_ = this->create_publisher<tier4_debug_msgs::msg::Float32Stamped>("exe_time_ms", 10);
   transform_probability_pub_ =
     this->create_publisher<tier4_debug_msgs::msg::Float32Stamped>("transform_probability", 10);
@@ -435,59 +447,66 @@
   const pclomp::NdtResult ndt_result = ndt_ptr_->getResult();
   (*state_ptr_)["state"] = "Sleeping";
 
+  // covariance estimation
+  std::array<double, 36> 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<std::chrono::microseconds>(exe_end_time - exe_start_time).count();
   const auto exe_time = static_cast<float>(duration_micro_sec) / 1000.0f;
 
   const geometry_msgs::msg::Pose result_pose_msg = matrix4f_to_pose(ndt_result.pose);
   std::vector<geometry_msgs::msg::Pose> transformation_msg_array;
   for (const auto & pose_matrix : ndt_result.transformation_array) {
     geometry_msgs::msg::Pose pose_ros = matrix4f_to_pose(pose_matrix);
     transformation_msg_array.push_back(pose_ros);
   }
 
   // perform several validations
   /*****************************************************************************
   The reason the add 2 to the ndt_ptr_->getMaximumIterations() is that there are bugs in
   implementation of ndt.
   1. gradient descent method ends when the iteration is greater than max_iteration if it does not
   converge (be careful it's 'greater than' instead of 'greater equal than'.)
      https://github.com/tier4/autoware.iv/blob/2323e5baa0b680d43a9219f5fb3b7a11dd9edc82/localization/pose_estimator/ndt_scan_matcher/ndt_omp/include/ndt_omp/ndt_omp_impl.hpp#L212
   2. iterate iteration count when end of gradient descent function.
      https://github.com/tier4/autoware.iv/blob/2323e5baa0b680d43a9219f5fb3b7a11dd9edc82/localization/pose_estimator/ndt_scan_matcher/ndt_omp/include/ndt_omp/ndt_omp_impl.hpp#L217
 
   These bugs are now resolved in original pcl implementation.
   https://github.com/PointCloudLibrary/pcl/blob/424c1c6a0ca97d94ca63e5daff4b183a4db8aae4/registration/include/pcl/registration/impl/ndt.hpp#L73-L180
   *****************************************************************************/
   bool is_ok_iteration_num =
     validate_num_iteration(ndt_result.iteration_num, ndt_ptr_->getMaximumIterations() + 2);
   bool is_local_optimal_solution_oscillation = false;
   if (!is_ok_iteration_num) {
     is_local_optimal_solution_oscillation = validate_local_optimal_solution_oscillation(
       transformation_msg_array, oscillation_threshold_, inversion_vector_threshold_);
   }
   bool is_ok_converged_param = validate_converged_param(
     ndt_result.transform_probability, ndt_result.nearest_voxel_transformation_likelihood);
   bool is_converged = is_ok_iteration_num && is_ok_converged_param;
   static size_t skipping_publish_num = 0;
   if (is_converged) {
     skipping_publish_num = 0;
   } else {
     ++skipping_publish_num;
     RCLCPP_WARN(get_logger(), "Not Converged");
   }
 
   // publish
   initial_pose_with_covariance_pub_->publish(interpolator.get_current_pose());
   exe_time_pub_->publish(make_float32_stamped(sensor_ros_time, exe_time));
   transform_probability_pub_->publish(
     make_float32_stamped(sensor_ros_time, ndt_result.transform_probability));
   nearest_voxel_transformation_likelihood_pub_->publish(
     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(),
@@ -570,7 +589,7 @@
 
 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<double, 36> & ndt_covariance, const bool is_converged)
 {
   geometry_msgs::msg::PoseStamped result_pose_stamped_msg;
   result_pose_stamped_msg.header.stamp = sensor_ros_time;
@@ -581,7 +600,7 @@
   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);
@@ -704,6 +723,71 @@
   return is_ok_converged_param;
 }
 
+void NDTScanMatcher::estimate_covariance(
+  const pclomp::NdtResult & ndt_result, const Eigen::Matrix4f & initial_pose_matrix,
+  std::array<double, 36> & 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<Eigen::Matrix2d> lap_eigensolver(hessian_inv);
+  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());
+    rot = Eigen::Rotation2Dd(th);
+  }
+
+  // first result is added to mean and covariance
+  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_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<float>(offset_2d.x());
+    sub_initial_pose_matrix(1, 3) += static_cast<float>(offset_2d.y());
+
+    // align
+    auto sub_output_cloud = std::make_shared<pcl::PointCloud<PointSource>>();
+    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<double>(sub_ndt_result(0, 3)), static_cast<double>(sub_ndt_result(1, 3)));
+    Eigen::Vector2d sub_p2d = sub_ndt_result.topRightCorner<2, 1>().cast<double>();
+    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);
+
+  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);
+
+  multi_ndt_pose_pub_->publish(multi_ndt_result_msg);
+  multi_initial_pose_pub_->publish(multi_initial_pose_msg);
+}
+
 std::optional<Eigen::Matrix4f> NDTScanMatcher::interpolate_regularization_pose(
   const rclcpp::Time & sensor_ros_time)
 {

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<double> & x, const std::vector<double> & y,
+  std::vector<Eigen::Vector2d> & 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)
 {