From ae30f0ba0cea64a13347a90ce4945e4452d7cf37 Mon Sep 17 00:00:00 2001 From: Abrar Rahman Protyasha Date: Fri, 11 Jun 2021 14:50:47 -0400 Subject: [PATCH] `tf2_geometry_msgs`: Fixing covariance transformation in `doTransform` (#430) Fixes #372. * Implement covariance transformation (cpp). * Fixed `TfGeometry/FrameWithCovariance` test. The updated test checks against the correctly transformed covariance matrix. Signed-off-by: Abrar Rahman Protyasha --- .../tf2_geometry_msgs/tf2_geometry_msgs.hpp | 106 +++++++++++++++++- .../test/test_tf2_geometry_msgs.cpp | 13 ++- 2 files changed, 116 insertions(+), 3 deletions(-) diff --git a/tf2_geometry_msgs/include/tf2_geometry_msgs/tf2_geometry_msgs.hpp b/tf2_geometry_msgs/include/tf2_geometry_msgs/tf2_geometry_msgs.hpp index 7e86b12a1..aa32b1ccd 100644 --- a/tf2_geometry_msgs/include/tf2_geometry_msgs/tf2_geometry_msgs.hpp +++ b/tf2_geometry_msgs/include/tf2_geometry_msgs/tf2_geometry_msgs.hpp @@ -39,12 +39,14 @@ #include #include #include +#include #include #include #include #include #include #include +#include #include #include @@ -347,6 +349,105 @@ template <> inline std::array, 6> getCovarianceMatrix(const geometry_msgs::msg::PoseWithCovarianceStamped& t) {return covarianceRowMajorToNested(t.pose.covariance);} +/** \brief Transform the covariance matrix of a PoseWithCovariance message to a new frame. + * \param cov_in The covariance matrix to transform. + * \param transform The transform to apply, as a tf2::Transform structure. + * \return The transformed covariance matrix. + */ +inline +geometry_msgs::msg::PoseWithCovariance::_covariance_type transformCovariance( + const geometry_msgs::msg::PoseWithCovariance::_covariance_type & cov_in, + const tf2::Transform & transform) +{ + /** + * To transform a covariance matrix: + * + * \verbatim[R 0] COVARIANCE [R' 0 ] + [0 R] [0 R']\endverbatim + * + * Where: + * R is the rotation matrix (3x3). + * R' is the transpose of the rotation matrix. + * COVARIANCE is the 6x6 covariance matrix to be transformed. + * + * Reference: + * A. L. Garcia, “Linear Transformations of Random Vectors,” in Probability, + * Statistics, and Random Processes For Electrical Engineering, 3rd ed., + * Pearson Prentice Hall, 2008, pp. 320–322. + */ + + // get rotation matrix (and transpose) + const tf2::Matrix3x3 R = transform.getBasis(); + const tf2::Matrix3x3 R_transpose = R.transpose(); + + // convert covariance matrix into four 3x3 blocks + const tf2::Matrix3x3 cov_11(cov_in[0], cov_in[1], cov_in[2], + cov_in[6], cov_in[7], cov_in[8], + cov_in[12], cov_in[13], cov_in[14]); + const tf2::Matrix3x3 cov_12(cov_in[3], cov_in[4], cov_in[5], + cov_in[9], cov_in[10], cov_in[11], + cov_in[15], cov_in[16], cov_in[17]); + const tf2::Matrix3x3 cov_21(cov_in[18], cov_in[19], cov_in[20], + cov_in[24], cov_in[25], cov_in[26], + cov_in[30], cov_in[31], cov_in[32]); + const tf2::Matrix3x3 cov_22(cov_in[21], cov_in[22], cov_in[23], + cov_in[27], cov_in[28], cov_in[29], + cov_in[33], cov_in[34], cov_in[35]); + + // perform blockwise matrix multiplication + const tf2::Matrix3x3 result_11 = R * cov_11 * R_transpose; + const tf2::Matrix3x3 result_12 = R * cov_12 * R_transpose; + const tf2::Matrix3x3 result_21 = R * cov_21 * R_transpose; + const tf2::Matrix3x3 result_22 = R * cov_22 * R_transpose; + + // form the output + geometry_msgs::msg::PoseWithCovariance::_covariance_type cov_out; + cov_out[0] = result_11[0][0]; + cov_out[1] = result_11[0][1]; + cov_out[2] = result_11[0][2]; + cov_out[6] = result_11[1][0]; + cov_out[7] = result_11[1][1]; + cov_out[8] = result_11[1][2]; + cov_out[12] = result_11[2][0]; + cov_out[13] = result_11[2][1]; + cov_out[14] = result_11[2][2]; + + cov_out[3] = result_12[0][0]; + cov_out[4] = result_12[0][1]; + cov_out[5] = result_12[0][2]; + cov_out[9] = result_12[1][0]; + cov_out[10] = result_12[1][1]; + cov_out[11] = result_12[1][2]; + cov_out[15] = result_12[2][0]; + cov_out[16] = result_12[2][1]; + cov_out[17] = result_12[2][2]; + + cov_out[18] = result_21[0][0]; + cov_out[19] = result_21[0][1]; + cov_out[20] = result_21[0][2]; + cov_out[24] = result_21[1][0]; + cov_out[25] = result_21[1][1]; + cov_out[26] = result_21[1][2]; + cov_out[30] = result_21[2][0]; + cov_out[31] = result_21[2][1]; + cov_out[32] = result_21[2][2]; + + cov_out[21] = result_22[0][0]; + cov_out[22] = result_22[0][1]; + cov_out[23] = result_22[0][2]; + cov_out[27] = result_22[1][0]; + cov_out[28] = result_22[1][1]; + cov_out[29] = result_22[1][2]; + cov_out[33] = result_22[2][0]; + cov_out[34] = result_22[2][1]; + cov_out[35] = result_22[2][2]; + + return cov_out; +} + +// Forward declaration +void fromMsg(const geometry_msgs::msg::Transform& in, tf2::Transform& out); + /** \brief Apply a geometry_msgs TransformStamped to an geometry_msgs Pose type. * This function is a specialization of the doTransform template defined in tf2/convert.h. * \param t_in The pose to transform, as a timestamped Pose3 message with covariance. @@ -367,7 +468,10 @@ inline v_out.M.GetQuaternion(t_out.pose.pose.orientation.x, t_out.pose.pose.orientation.y, t_out.pose.pose.orientation.z, t_out.pose.pose.orientation.w); t_out.header.stamp = transform.header.stamp; t_out.header.frame_id = transform.header.frame_id; - t_out.pose.covariance = t_in.pose.covariance; + + tf2::Transform tf_transform; + fromMsg(transform.transform, tf_transform); + t_out.pose.covariance = transformCovariance(t_in.pose.covariance, tf_transform); } /** \brief Trivial "conversion" function for Pose message type. diff --git a/tf2_geometry_msgs/test/test_tf2_geometry_msgs.cpp b/tf2_geometry_msgs/test/test_tf2_geometry_msgs.cpp index 9d6140c61..320560d2e 100644 --- a/tf2_geometry_msgs/test/test_tf2_geometry_msgs.cpp +++ b/tf2_geometry_msgs/test/test_tf2_geometry_msgs.cpp @@ -165,6 +165,15 @@ TEST(TfGeometry, FrameWithCovariance) 1.0, 2.0, 3.0, 4.0, 5.0, 6.0 }; + geometry_msgs::msg::PoseWithCovariance::_covariance_type v1_expected_covariance = { + 1.0, -2.0, -3.0, 4.0, -5.0, -6.0, + -1.0, 2.0, 3.0, -4.0, 5.0, 6.0, + -1.0, 2.0, 3.0, -4.0, 5.0, 6.0, + 1.0, -2.0, -3.0, 4.0, -5.0, -6.0, + -1.0, 2.0, 3.0, -4.0, 5.0, 6.0, + -1.0, 2.0, 3.0, -4.0, 5.0, 6.0 + }; + // simple api geometry_msgs::msg::PoseWithCovarianceStamped v_simple = tf_buffer->transform(v1, "B", tf2::durationFromSec(2.0)); EXPECT_NEAR(v_simple.pose.pose.position.x, -9, EPS); @@ -174,7 +183,7 @@ TEST(TfGeometry, FrameWithCovariance) EXPECT_NEAR(v_simple.pose.pose.orientation.y, 0.0, EPS); EXPECT_NEAR(v_simple.pose.pose.orientation.z, 0.0, EPS); EXPECT_NEAR(v_simple.pose.pose.orientation.w, 1.0, EPS); - EXPECT_EQ(v_simple.pose.covariance, v1.pose.covariance); + EXPECT_EQ(v_simple.pose.covariance, v1_expected_covariance); // advanced api @@ -187,7 +196,7 @@ TEST(TfGeometry, FrameWithCovariance) EXPECT_NEAR(v_advanced.pose.pose.orientation.y, 0.0, EPS); EXPECT_NEAR(v_advanced.pose.pose.orientation.z, 0.0, EPS); EXPECT_NEAR(v_advanced.pose.pose.orientation.w, 1.0, EPS); - EXPECT_EQ(v_advanced.pose.covariance, v1.pose.covariance); + EXPECT_EQ(v_advanced.pose.covariance, v1_expected_covariance); }