feat(ndt_scan_matcher): estimate the covariance of ndt scan matching …

…in real time instead of using a predefined value (#7596) * enable to calculate 2D covariance in real time Signed-off-by: yuhei <[email protected]> * enable to display offset_pose Signed-off-by: yuhei <[email protected]> * enable to publish estimated ndt_dev Signed-off-by: yuhei <[email protected]> * Addition of explanation of 2D real-time covariance estimation Signed-off-by: yuhei <[email protected]> * fix name of picture Signed-off-by: yuhei <[email protected]> * style(pre-commit): autofix * fix covariance_covariance_estimation.json Signed-off-by: yuhei <[email protected]> * fix covariance_covariance_estimation.json Signed-off-by: yuhei <[email protected]> * style(pre-commit): autofix * Commonality using estimated_cov_2d and removal of unnecessary publishers Signed-off-by: yuhei <[email protected]> * Add a specific explanation for 2D real-time covariance estimation Signed-off-by: yuhei <[email protected]> * style(pre-commit): autofix * Remove initial_pose_offset_model_x and initial_pose_offset_model_y from the structure in hyper_parameters.hpp Signed-off-by: yuhei <[email protected]> * Generate initial_pose_offset_model_x and initial_pose_offset_model_y from initial_pose_offset_model Signed-off-by: yuhei <[email protected]> * Addition of covariance_estimation_type Signed-off-by: yuhei <[email protected]> * style(pre-commit): autofix * Add temperature to parameters Signed-off-by: yuhei <[email protected]> * Remove covariance_estimation.enable from the parameters and change the return value of estimate_covariance() Signed-off-by: yuhei <[email protected]> * style(pre-commit): autofix * Add a temperature description Signed-off-by: yuhei <[email protected]> * Add processing when the inverse matrix cannot be calculated in estimate_covariance() Signed-off-by: yuhei <[email protected]> * set default value and range of temperature Signed-off-by: yuhei <[email protected]> * Simplify the code Signed-off-by: yuhei <[email protected]> * style(pre-commit): autofix * Fix explanation of covariance_estimation_type Signed-off-by: yuhei <[email protected]> * Define initial_pose_offset_model_x and initial_pose_offset_model_y in hyper_parameters Signed-off-by: yuhei <[email protected]> * style(pre-commit): autofix * Add explanation of 2D real-time covariance estimation Signed-off-by: yuhei <[email protected]> --------- Signed-off-by: yuhei <[email protected]> Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>
autowarefoundation · Jul 5, 2024 · 71fc4c3 · 71fc4c3
1 parent cc1ab05
commit 71fc4c3
Show file tree

Hide file tree

Showing 7 changed files with 117 additions and 93 deletions.
diff --git a/localization/ndt_scan_matcher/README.md b/localization/ndt_scan_matcher/README.md
@@ -241,18 +241,26 @@ This is a function that uses no ground LiDAR scan to estimate the scan matching
 
 ### 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.
+Initially, the covariance of NDT scan matching is a fixed value(FIXED_VALUE mode).
+So, three modes are provided for 2D covariance (xx, xy, yx, yy) estimation in real time: LAPLACE_APPROXIMATION, MULTI_NDT, and MULTI_NDT_SCORE.
+LAPLACE_APPROXIMATION calculates the inverse matrix of the XY (2x2) part of the Hessian obtained by NDT scan matching and uses it as the covariance matrix.
+On the other hand, MULTI_NDT, and MULTI_NDT_SCORE use NDT convergence from multiple initial poses to obtain 2D covariance.
+Ideally, 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.
+To obtain the covariance, MULTI_NDT computes until convergence at each initial position, while MULTI_NDT_SCORE uses the nearest voxel transformation likelihood.
 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/).
 
+<img src="./media/calculation_of_ndt_covariance.png" alt="drawing" width="600"/>
+
 Note that this function may spoil healthy system behavior if it consumes much calculation resources.
 
 ### Parameters
 
+There are three types in the calculation of 2D covariance in real time.You can select the method by changing covariance_estimation_type.
 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.
+In MULTI_NDT_SCORE mode, the scale of the output 2D covariance can be adjusted according to the temperature.
 
 {{ json_to_markdown("localization/ndt_scan_matcher/schema/sub/covariance_covariance_estimation.json") }}
 

diff --git a/localization/ndt_scan_matcher/config/ndt_scan_matcher.param.yaml b/localization/ndt_scan_matcher/config/ndt_scan_matcher.param.yaml
@@ -107,13 +107,18 @@
 
       # 2D Real-time covariance estimation with multiple searches (output_pose_covariance is the minimum value)
       covariance_estimation:
-        enable: false
+        # Covariance estimation type
+        # 0=FIXED_VALUE, 1=LAPLACE_APPROXIMATION, 2=MULTI_NDT, 3=MULTI_NDT_SCORE
+        covariance_estimation_type: 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]
 
+        # In MULTI_NDT_SCORE, the parameter that adjusts the estimated 2D covariance
+        temperature: 0.1
+
 
     dynamic_map_loading:
       # Dynamic map loading distance

diff --git a/localization/ndt_scan_matcher/include/ndt_scan_matcher/hyper_parameters.hpp b/localization/ndt_scan_matcher/include/ndt_scan_matcher/hyper_parameters.hpp
@@ -29,6 +29,13 @@ enum class ConvergedParamType {
   NEAREST_VOXEL_TRANSFORMATION_LIKELIHOOD = 1
 };
 
+enum class CovarianceEstimationType {
+  FIXED_VALUE = 0,
+  LAPLACE_APPROXIMATION = 1,
+  MULTI_NDT = 2,
+  MULTI_NDT_SCORE = 3,
+};
+
 struct HyperParameters
 {
   struct Frame
@@ -80,8 +87,10 @@ struct HyperParameters
 
     struct CovarianceEstimation
     {
-      bool enable{};
-      std::vector<Eigen::Vector2d> initial_pose_offset_model{};
+      CovarianceEstimationType covariance_estimation_type{};
+      std::vector<double> initial_pose_offset_model_x{};
+      std::vector<double> initial_pose_offset_model_y{};
+      double temperature{};
     } covariance_estimation{};
   } covariance{};
 
@@ -148,33 +157,27 @@ struct HyperParameters
     for (std::size_t i = 0; i < output_pose_covariance.size(); ++i) {
       covariance.output_pose_covariance[i] = output_pose_covariance[i];
     }
-    covariance.covariance_estimation.enable =
-      node->declare_parameter<bool>("covariance.covariance_estimation.enable");
-    if (covariance.covariance_estimation.enable) {
-      std::vector<double> initial_pose_offset_model_x =
-        node->declare_parameter<std::vector<double>>(
-          "covariance.covariance_estimation.initial_pose_offset_model_x");
-      std::vector<double> initial_pose_offset_model_y =
-        node->declare_parameter<std::vector<double>>(
-          "covariance.covariance_estimation.initial_pose_offset_model_y");
-
-      if (initial_pose_offset_model_x.size() == initial_pose_offset_model_y.size()) {
-        const size_t size = initial_pose_offset_model_x.size();
-        covariance.covariance_estimation.initial_pose_offset_model.resize(size);
-        for (size_t i = 0; i < size; i++) {
-          covariance.covariance_estimation.initial_pose_offset_model[i].x() =
-            initial_pose_offset_model_x[i];
-          covariance.covariance_estimation.initial_pose_offset_model[i].y() =
-            initial_pose_offset_model_y[i];
-        }
-      } else {
-        std::stringstream message;
-        message << "Invalid initial pose offset model parameters."
-                << "Please make sure that the number of elements in "
-                << "initial_pose_offset_model_x and initial_pose_offset_model_y are the same.";
-        throw std::runtime_error(message.str());
-      }
+    const int64_t covariance_estimation_type_tmp = node->declare_parameter<int64_t>(
+      "covariance.covariance_estimation.covariance_estimation_type");
+    covariance.covariance_estimation.covariance_estimation_type =
+      static_cast<CovarianceEstimationType>(covariance_estimation_type_tmp);
+    covariance.covariance_estimation.initial_pose_offset_model_x =
+      node->declare_parameter<std::vector<double>>(
+        "covariance.covariance_estimation.initial_pose_offset_model_x");
+    covariance.covariance_estimation.initial_pose_offset_model_y =
+      node->declare_parameter<std::vector<double>>(
+        "covariance.covariance_estimation.initial_pose_offset_model_y");
+    if (
+      covariance.covariance_estimation.initial_pose_offset_model_x.size() !=
+      covariance.covariance_estimation.initial_pose_offset_model_y.size()) {
+      std::stringstream message;
+      message << "Invalid initial pose offset model parameters."
+              << "Please make sure that the number of elements in "
+              << "initial_pose_offset_model_x and initial_pose_offset_model_y are the same.";
+      throw std::runtime_error(message.str());
     }
+    covariance.covariance_estimation.temperature =
+      node->declare_parameter<double>("covariance.covariance_estimation.temperature");
 
     dynamic_map_loading.update_distance =
       node->declare_parameter<double>("dynamic_map_loading.update_distance");

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
@@ -131,7 +131,7 @@ class NDTScanMatcher : public rclcpp::Node
 
   static int count_oscillation(const std::vector<geometry_msgs::msg::Pose> & result_pose_msg_array);
 
-  std::array<double, 36> estimate_covariance(
+  Eigen::Matrix2d estimate_covariance(
     const pclomp::NdtResult & ndt_result, const Eigen::Matrix4f & initial_pose_matrix,
     const rclcpp::Time & sensor_ros_time);
 

diff --git a/localization/ndt_scan_matcher/media/calculation_of_ndt_covariance.png b/localization/ndt_scan_matcher/media/calculation_of_ndt_covariance.png
diff --git a/localization/ndt_scan_matcher/schema/sub/covariance_covariance_estimation.json b/localization/ndt_scan_matcher/schema/sub/covariance_covariance_estimation.json
@@ -5,10 +5,12 @@
     "covariance_estimation": {
       "type": "object",
       "properties": {
-        "enable": {
-          "type": "boolean",
-          "description": "2D Real-time covariance estimation with multiple searches (output_pose_covariance is the minimum value).",
-          "default": false
+        "covariance_estimation_type": {
+          "type": "number",
+          "description": "2D Real-time Converged estimation type. 0=FIXED_VALUE, 1=LAPLACE_APPROXIMATION, 2=MULTI_NDT, 3=MULTI_NDT_SCORE (FIXED_VALUE mode does not perform 2D Real-time Converged estimation)",
+          "default": 0,
+          "minimum": 0,
+          "maximum": 3
         },
         "initial_pose_offset_model_x": {
           "type": "array",
@@ -19,9 +21,20 @@
           "type": "array",
           "description": "Offset arrangement in covariance estimation [m]. initial_pose_offset_model_x & initial_pose_offset_model_y must have the same number of elements.",
           "default": [0.5, -0.5, 0.0, 0.0, 0.0, 0.0]
+        },
+        "temperature": {
+          "type": "number",
+          "description": "In MULTI_NDT_SCORE, the parameter that adjusts the estimated 2D covariance",
+          "default": 0.1,
+          "exclusiveMinimum": 0
         }
       },
-      "required": ["enable", "initial_pose_offset_model_x", "initial_pose_offset_model_y"],
+
+      "required": [
+        "covariance_estimation_type",
+        "initial_pose_offset_model_x",
+        "initial_pose_offset_model_y"
+      ],
       "additionalProperties": false
     }
   }

diff --git a/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp b/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp
@@ -21,6 +21,7 @@
 
 #include <autoware/universe_utils/geometry/geometry.hpp>
 #include <autoware/universe_utils/transform/transforms.hpp>
+#include <estimate_covariance/estimate_covariance.hpp>
 
 #include <pcl_conversions/pcl_conversions.h>
 
@@ -566,11 +567,17 @@ bool NDTScanMatcher::callback_sensor_points_main(
 
   std::array<double, 36> ndt_covariance =
     rotate_covariance(param_.covariance.output_pose_covariance, map_to_base_link_rotation);
-
-  if (is_converged && param_.covariance.covariance_estimation.enable) {
-    const auto estimated_covariance =
+  if (
+    param_.covariance.covariance_estimation.covariance_estimation_type !=
+    CovarianceEstimationType::FIXED_VALUE) {
+    const Eigen::Matrix2d estimated_covariance_2d =
       estimate_covariance(ndt_result, initial_pose_matrix, sensor_ros_time);
-    ndt_covariance = estimated_covariance;
+    const Eigen::Matrix2d estimated_covariance_2d_adj =
+      pclomp::adjust_diagonal_covariance(estimated_covariance_2d, ndt_result.pose, 0.0225, 0.0225);
+    ndt_covariance[0 + 6 * 0] = estimated_covariance_2d_adj(0, 0);
+    ndt_covariance[1 + 6 * 1] = estimated_covariance_2d_adj(1, 1);
+    ndt_covariance[1 + 6 * 0] = estimated_covariance_2d_adj(1, 0);
+    ndt_covariance[0 + 6 * 1] = estimated_covariance_2d_adj(0, 1);
   }
 
   // check distance_initial_to_result
@@ -807,7 +814,7 @@ int NDTScanMatcher::count_oscillation(
   return max_oscillation_cnt;
 }
 
-std::array<double, 36> NDTScanMatcher::estimate_covariance(
+Eigen::Matrix2d NDTScanMatcher::estimate_covariance(
   const pclomp::NdtResult & ndt_result, const Eigen::Matrix4f & initial_pose_matrix,
   const rclcpp::Time & sensor_ros_time)
 {
@@ -819,19 +826,9 @@ std::array<double, 36> NDTScanMatcher::estimate_covariance(
     std::stringstream message;
     message << "Error in Eigen solver: " << e.what();
     RCLCPP_WARN_STREAM_THROTTLE(this->get_logger(), *this->get_clock(), 1000, message.str());
-
-    return param_.covariance.output_pose_covariance;
+    return Eigen::Matrix2d::Identity() * param_.covariance.output_pose_covariance[0 + 6 * 0];
   }
 
-  // first result is added to mean
-  const int n =
-    static_cast<int>(param_.covariance.covariance_estimation.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;
@@ -841,47 +838,45 @@ std::array<double, 36> NDTScanMatcher::estimate_covariance(
   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 :
-       param_.covariance.covariance_estimation.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();
+  if (
+    param_.covariance.covariance_estimation.covariance_estimation_type ==
+    CovarianceEstimationType::LAPLACE_APPROXIMATION) {
+    return pclomp::estimate_xy_covariance_by_Laplace_approximation(ndt_result.hessian);
+  } else if (
+    param_.covariance.covariance_estimation.covariance_estimation_type ==
+    CovarianceEstimationType::MULTI_NDT) {
+    const std::vector<Eigen::Matrix4f> poses_to_search = pclomp::propose_poses_to_search(
+      ndt_result, param_.covariance.covariance_estimation.initial_pose_offset_model_x,
+      param_.covariance.covariance_estimation.initial_pose_offset_model_y);
+    const pclomp::ResultOfMultiNdtCovarianceEstimation result_of_multi_ndt_covariance_estimation =
+      estimate_xy_covariance_by_multi_ndt(ndt_result, ndt_ptr_, poses_to_search);
+    for (size_t i = 0; i < result_of_multi_ndt_covariance_estimation.ndt_initial_poses.size();
+         i++) {
+      multi_ndt_result_msg.poses.push_back(
+        matrix4f_to_pose(result_of_multi_ndt_covariance_estimation.ndt_results[i].pose));
+      multi_initial_pose_msg.poses.push_back(
+        matrix4f_to_pose(result_of_multi_ndt_covariance_estimation.ndt_initial_poses[i]));
+    }
+    multi_ndt_pose_pub_->publish(multi_ndt_result_msg);
+    multi_initial_pose_pub_->publish(multi_initial_pose_msg);
+    return result_of_multi_ndt_covariance_estimation.covariance;
+  } else if (
+    param_.covariance.covariance_estimation.covariance_estimation_type ==
+    CovarianceEstimationType::MULTI_NDT_SCORE) {
+    const std::vector<Eigen::Matrix4f> poses_to_search = pclomp::propose_poses_to_search(
+      ndt_result, param_.covariance.covariance_estimation.initial_pose_offset_model_x,
+      param_.covariance.covariance_estimation.initial_pose_offset_model_y);
+    const pclomp::ResultOfMultiNdtCovarianceEstimation
+      result_of_multi_ndt_score_covariance_estimation = estimate_xy_covariance_by_multi_ndt_score(
+        ndt_result, ndt_ptr_, poses_to_search, param_.covariance.covariance_estimation.temperature);
+    for (const auto & sub_initial_pose_matrix : poses_to_search) {
+      multi_initial_pose_msg.poses.push_back(matrix4f_to_pose(sub_initial_pose_matrix));
+    }
+    multi_initial_pose_pub_->publish(multi_initial_pose_msg);
+    return result_of_multi_ndt_score_covariance_estimation.covariance;
+  } else {
+    return Eigen::Matrix2d::Identity() * param_.covariance.output_pose_covariance[0 + 6 * 0];
   }
-  pca_covariance /= (n - 1);  // unbiased covariance
-
-  std::array<double, 36> ndt_covariance = param_.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;
 }
 
 void NDTScanMatcher::add_regularization_pose(const rclcpp::Time & sensor_ros_time)