feat(ndt_scan_matcher): change coordinate of output_pose_covariance (a…

…utowarefoundation#6065)

* feat(ndt_scan_matcher): change coordinate of output_pose_covariance

Signed-off-by: yamato-ando <yamato.ando@tier4.jp>

* style(pre-commit): autofix

* Update localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp

Co-authored-by: Kento Yabuuchi <moc.liamg.8y8@gmail.com>

* style(pre-commit): autofix

---------

Signed-off-by: yamato-ando <yamato.ando@tier4.jp>
Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>
Co-authored-by: Kento Yabuuchi <moc.liamg.8y8@gmail.com>

localization/ndt_scan_matcher/include/ndt_scan_matcher/ndt_scan_matcher_core.hpp

@@ -127,6 +127,8 @@ class NDTScanMatcher : public rclcpp::Node
     const double & transform_probability, const double & nearest_voxel_transformation_likelihood);
   static int count_oscillation(const std::vector<geometry_msgs::msg::Pose> & result_pose_msg_array);
 
+  std::array<double, 36> rotate_covariance(
+    const std::array<double, 36> & src_covariance, const Eigen::Matrix3d & rotation) const;
   std::array<double, 36> estimate_covariance(
     const pclomp::NdtResult & ndt_result, const Eigen::Matrix4f & initial_pose_matrix,
     const rclcpp::Time & sensor_ros_time);

localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp

@@ -486,7 +486,15 @@ void NDTScanMatcher::callback_sensor_points(
   }
 
   // covariance estimation
-  std::array<double, 36> ndt_covariance = output_pose_covariance_;
+  const Eigen::Quaterniond map_to_base_link_quat = Eigen::Quaterniond(
+    result_pose_msg.orientation.w, result_pose_msg.orientation.x, result_pose_msg.orientation.y,
+    result_pose_msg.orientation.z);
+  const Eigen::Matrix3d map_to_base_link_rotation =
+    map_to_base_link_quat.normalized().toRotationMatrix();
+
+  std::array<double, 36> ndt_covariance =
+    rotate_covariance(output_pose_covariance_, map_to_base_link_rotation);
+
   if (is_converged && use_cov_estimation_) {
     const auto estimated_covariance =
       estimate_covariance(ndt_result, initial_pose_matrix, sensor_ros_time);
@@ -766,6 +774,28 @@ int NDTScanMatcher::count_oscillation(
   return max_oscillation_cnt;
 }
 
+std::array<double, 36> NDTScanMatcher::rotate_covariance(
+  const std::array<double, 36> & src_covariance, const Eigen::Matrix3d & rotation) const
+{
+  std::array<double, 36> ret_covariance = src_covariance;
+
+  Eigen::Matrix3d src_cov;
+  src_cov << src_covariance[0], src_covariance[1], src_covariance[2], src_covariance[6],
+    src_covariance[7], src_covariance[8], src_covariance[12], src_covariance[13],
+    src_covariance[14];
+
+  Eigen::Matrix3d ret_cov;
+  ret_cov = rotation * src_cov * rotation.transpose();
+
+  for (Eigen::Index i = 0; i < 3; ++i) {
+    ret_covariance[i] = ret_cov(0, i);
+    ret_covariance[i + 6] = ret_cov(1, i);
+    ret_covariance[i + 12] = ret_cov(2, i);
+  }
+
+  return ret_covariance;
+}
+
 std::array<double, 36> NDTScanMatcher::estimate_covariance(
   const pclomp::NdtResult & ndt_result, const Eigen::Matrix4f & initial_pose_matrix,
   const rclcpp::Time & sensor_ros_time)