Feat estimate-NDT-covariance-in-real-time-with-limited-initial-positi…

…ons-autowarefoundation#533

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
 

localization/ndt_scan_matcher/include/ndt_scan_matcher/ndt_scan_matcher_core.hpp

@@ -24,6 +24,7 @@
 #include <rclcpp/rclcpp.hpp>
 
 #include <diagnostic_msgs/msg/diagnostic_array.hpp>
+#include <geometry_msgs/msg/pose_array.hpp>
 #include <geometry_msgs/msg/pose_with_covariance_stamped.hpp>
 #include <geometry_msgs/msg/twist_stamped.hpp>
 #include <nav_msgs/msg/odometry.hpp>
@@ -104,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<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);
@@ -122,6 +123,10 @@ class NDTScanMatcher : public rclcpp::Node
   bool validate_converged_param(
     const double & transform_probability, const double & nearest_voxel_transformation_likelihood);
 
+  std::array<double, 36> estimate_covariance(
+    const pclomp::NdtResult & ndt_result, const Eigen::Matrix4f & initial_pose_matrix,
+    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);
@@ -140,6 +145,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
@@ -186,6 +193,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> initial_pose_offset_model_;
   std::array<double, 36> output_pose_covariance_;
 
   std::deque<geometry_msgs::msg::PoseWithCovarianceStamped::ConstSharedPtr>

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<T>().stamp(stamp).data(data);
 }
 
+std::vector<Eigen::Vector2d> create_initial_pose_offset_model(
+  const std::vector<double> & x, const std::vector<double> & y)
+{
+  int size = x.size();
+  std::vector<Eigen::Vector2d> 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<Eigen::Matrix2d> 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<geometry_msgs::msg::Pose> & result_pose_msg_array,
   const float oscillation_threshold, const float inversion_vector_threshold)
@@ -141,6 +167,25 @@ NDTScanMatcher::NDTScanMatcher()
   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> initial_pose_offset_model_x =
+      this->declare_parameter<std::vector<double>>("initial_pose_offset_model_x");
+    std::vector<double> initial_pose_offset_model_y =
+      this->declare_parameter<std::vector<double>>("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<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 +236,9 @@ NDTScanMatcher::NDTScanMatcher()
   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,11 +483,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<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) {
@@ -466,6 +509,19 @@ void NDTScanMatcher::callback_sensor_points(
     RCLCPP_WARN(get_logger(), "Not Converged");
   }
 
+  // covariance estimation
+  std::array<double, 36> 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<std::chrono::microseconds>(exe_end_time - exe_start_time).count();
+  const auto exe_time = static_cast<float>(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));
@@ -475,7 +531,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(),
@@ -558,7 +614,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<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;
@@ -569,7 +625,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);
@@ -692,6 +748,78 @@ bool NDTScanMatcher::validate_converged_param(
   return is_ok_converged_param;
 }
 
+std::array<double, 36> 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<Eigen::Vector2d> 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<float>(rotated_pose_offset_2d.x());
+    sub_initial_pose_matrix(1, 3) += static_cast<float>(rotated_pose_offset_2d.y());
+
+    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;
+
+    const Eigen::Vector2d sub_ndt_pose_2d = sub_ndt_result.topRightCorner<2, 1>().cast<double>();
+    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<double, 36> 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<Eigen::Matrix4f> NDTScanMatcher::interpolate_regularization_pose(
   const rclcpp::Time & sensor_ros_time)
 {