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 e9aa265c78f34..71895c59ec37d 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 @@ -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 & result_pose_msg_array); + std::array rotate_covariance( + const std::array & src_covariance, const Eigen::Matrix3d & rotation) const; std::array 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/src/ndt_scan_matcher_core.cpp b/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp index 0f6fdbc15db26..942df03410165 100644 --- a/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp +++ b/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp @@ -486,7 +486,15 @@ void NDTScanMatcher::callback_sensor_points( } // covariance estimation - std::array 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 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 NDTScanMatcher::rotate_covariance( + const std::array & src_covariance, const Eigen::Matrix3d & rotation) const +{ + std::array 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 NDTScanMatcher::estimate_covariance( const pclomp::NdtResult & ndt_result, const Eigen::Matrix4f & initial_pose_matrix, const rclcpp::Time & sensor_ros_time)