From 182a7eeaa392a5a60f86cf92d2b63de0be7dc137 Mon Sep 17 00:00:00 2001 From: Bjar Ne Date: Thu, 28 Jan 2021 15:34:19 +0000 Subject: [PATCH 1/4] Add Eigen buffer transform tests --- tf2_eigen/CMakeLists.txt | 1 + tf2_eigen/package.xml | 1 + tf2_eigen/test/tf2_eigen-test.cpp | 134 +++++++++++++++++++++++++++++- 3 files changed, 134 insertions(+), 2 deletions(-) diff --git a/tf2_eigen/CMakeLists.txt b/tf2_eigen/CMakeLists.txt index c7b192ef8..8fcc34718 100644 --- a/tf2_eigen/CMakeLists.txt +++ b/tf2_eigen/CMakeLists.txt @@ -5,6 +5,7 @@ find_package(catkin REQUIRED COMPONENTS cmake_modules geometry_msgs tf2 + tf2_ros ) # Finding Eigen is somewhat complicated because of our need to support Ubuntu diff --git a/tf2_eigen/package.xml b/tf2_eigen/package.xml index 8a3edcc62..4b804ffb8 100644 --- a/tf2_eigen/package.xml +++ b/tf2_eigen/package.xml @@ -11,6 +11,7 @@ geometry_msgs tf2 + tf2_ros cmake_modules eigen diff --git a/tf2_eigen/test/tf2_eigen-test.cpp b/tf2_eigen/test/tf2_eigen-test.cpp index f175e6c0a..0ae78e1c1 100644 --- a/tf2_eigen/test/tf2_eigen-test.cpp +++ b/tf2_eigen/test/tf2_eigen-test.cpp @@ -26,10 +26,25 @@ /** \author Wim Meeussen */ +// To get M_PI, especially on Windows. + +#ifdef _MSC_VER +#ifndef _USE_MATH_DEFINES +#define _USE_MATH_DEFINES +#endif +#endif + +#include + -#include #include #include +#include +#include +#include + + +#include TEST(TfEigen, ConvertVector3dStamped) { @@ -204,9 +219,124 @@ TEST(TfEigen, ConvertTransform) EXPECT_TRUE(tm.isApprox(Iback.matrix())); } +struct EigenTransform : public ::testing::Test +{ + static void SetUpTestSuite() + { + geometry_msgs::TransformStamped t; + transform.transform.translation.x = 10; + transform.transform.translation.y = 20; + transform.transform.translation.z = 30; + transform.transform.rotation.w = 0; + transform.transform.rotation.x = 1; + transform.transform.rotation.y = 0; + transform.transform.rotation.z = 0; + transform.header.stamp = ros::Time(2.0); + transform.header.frame_id = "A"; + transform.child_frame_id = "B"; + } + + template + void testEigenTransform(); + + ::testing::AssertionResult doTestEigenQuaternion( + const Eigen::Quaterniond & parameter, const Eigen::Quaterniond & expected); + + static geometry_msgs::TransformStamped transform; + static constexpr double EPS = 1e-3; +}; + +geometry_msgs::TransformStamped EigenTransform::transform; + +template +void EigenTransform::testEigenTransform() +{ + using T = Eigen::Transform; + using stampedT = tf2::Stamped; + + const stampedT i1{ + T{Eigen::Translation3d{1, 2, 3} *Eigen::Quaterniond{0, 1, 0, 0}}, ros::Time(2), "A"}; + + stampedT i_simple; + tf2::doTransform(i1, i_simple, transform); + + EXPECT_NEAR(i_simple.translation().x(), 11, EPS); + EXPECT_NEAR(i_simple.translation().y(), 18, EPS); + EXPECT_NEAR(i_simple.translation().z(), 27, EPS); + const auto q1 = Eigen::Quaterniond(i_simple.linear()); + EXPECT_NEAR(q1.x(), 0.0, EPS); + EXPECT_NEAR(q1.y(), 0.0, EPS); + EXPECT_NEAR(q1.z(), 0.0, EPS); + EXPECT_NEAR(q1.w(), 1.0, EPS); +} + +TEST_F(EigenTransform, Affine3d) { + testEigenTransform(); +} + +TEST_F(EigenTransform, Isometry3d) { + testEigenTransform(); +} + +TEST_F(EigenTransform, Vector) +{ + const tf2::Stamped v1{{1, 2, 3}, ros::Time(2), "A"}; + // simple api + tf2::Stamped v_simple; + tf2::doTransform(v1, v_simple, transform); -int main(int argc, char **argv){ + EXPECT_NEAR(v_simple.x(), 11, EPS); + EXPECT_NEAR(v_simple.y(), 18, EPS); + EXPECT_NEAR(v_simple.z(), 27, EPS); +} + +// helper method for Quaternion tests +::testing::AssertionResult EigenTransform::doTestEigenQuaternion( + const Eigen::Quaterniond & parameter, const Eigen::Quaterniond & expected) +{ + const tf2::Stamped q1{parameter, ros::Time(2), "A"}; + // avoid linking error + const double eps = EPS; + + // simple api + tf2::Stamped q_simple; + tf2::doTransform(q1, q_simple, transform); + // compare rotation matrices, as the quaternions can be ambigous + EXPECT_TRUE(q_simple.toRotationMatrix().isApprox(expected.toRotationMatrix(), eps)); + + return ::testing::AssertionSuccess(); +} + +TEST_F(EigenTransform, QuaternionRotY) +{ + // rotated by -90° around y + // 0, 0, -1 + // 0, 1, 0, + // 1, 0, 0 + const Eigen::Quaterniond param{Eigen::AngleAxisd(-1 * M_PI_2, Eigen::Vector3d::UnitY())}; + const Eigen::Quaterniond expected{0, M_SQRT1_2, 0, -1 * M_SQRT1_2}; + EXPECT_TRUE(doTestEigenQuaternion(param, expected)); +} + +TEST_F(EigenTransform, QuaternionRotX) +{ + // rotated by 90° around y + const Eigen::Quaterniond param{Eigen::AngleAxisd(M_PI_2, Eigen::Vector3d::UnitX())}; + const Eigen::Quaterniond expected{Eigen::AngleAxisd(-1 * M_PI_2, Eigen::Vector3d::UnitX())}; + EXPECT_TRUE(doTestEigenQuaternion(param, expected)); +} + +TEST_F(EigenTransform, QuaternionRotZ) +{ + // rotated by 180° around z + const Eigen::Quaterniond param{Eigen::AngleAxisd(M_PI, Eigen::Vector3d::UnitZ())}; + const Eigen::Quaterniond expected{Eigen::AngleAxisd(M_PI, Eigen::Vector3d::UnitY())}; + EXPECT_TRUE(doTestEigenQuaternion(param, expected)); +} + +int main(int argc, char ** argv) +{ testing::InitGoogleTest(&argc, argv); return RUN_ALL_TESTS(); From 28fd107e83ab67536eb3382fb36f840a226415e2 Mon Sep 17 00:00:00 2001 From: Bjar Ne Date: Thu, 28 Jan 2021 16:20:32 +0000 Subject: [PATCH 2/4] Fix Eigen Quaternion transform --- tf2_eigen/include/tf2_eigen/tf2_eigen.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/tf2_eigen/include/tf2_eigen/tf2_eigen.h b/tf2_eigen/include/tf2_eigen/tf2_eigen.h index 00c789ba7..0d60200f7 100644 --- a/tf2_eigen/include/tf2_eigen/tf2_eigen.h +++ b/tf2_eigen/include/tf2_eigen/tf2_eigen.h @@ -281,7 +281,7 @@ void doTransform(const Eigen::Quaterniond& t_in, const geometry_msgs::TransformStamped& transform) { Eigen::Quaterniond t; fromMsg(transform.transform.rotation, t); - t_out = t.inverse() * t_in * t; + t_out = t * t_in; } /** \brief Convert a stamped Eigen Quaterniond type to a QuaternionStamped message. From a26f09ef7a81adf04e8c34be12ccdc80471e51ab Mon Sep 17 00:00:00 2001 From: Bjar Ne Date: Thu, 28 Jan 2021 17:38:17 +0000 Subject: [PATCH 3/4] Unit test for doTransform(QuaternionStamped) --- .../test/test_tf2_geometry_msgs.cpp | 75 ++++++++++++++----- 1 file changed, 57 insertions(+), 18 deletions(-) diff --git a/tf2_geometry_msgs/test/test_tf2_geometry_msgs.cpp b/tf2_geometry_msgs/test/test_tf2_geometry_msgs.cpp index 860db53f6..719b4da6c 100644 --- a/tf2_geometry_msgs/test/test_tf2_geometry_msgs.cpp +++ b/tf2_geometry_msgs/test/test_tf2_geometry_msgs.cpp @@ -29,6 +29,14 @@ /** \author Wim Meeussen */ +#ifdef _MSC_VER +#ifndef _USE_MATH_DEFINES +#define _USE_MATH_DEFINES +#endif +#endif + +// To get M_PI, especially on Windows. +#include #include #include @@ -61,7 +69,7 @@ TEST(TfGeometry, Frame) EXPECT_NEAR(v_simple.pose.orientation.y, 0.0, EPS); EXPECT_NEAR(v_simple.pose.orientation.z, 0.0, EPS); EXPECT_NEAR(v_simple.pose.orientation.w, 1.0, EPS); - + // advanced api geometry_msgs::PoseStamped v_advanced = tf_buffer->transform(v1, "B", ros::Time(2.0), "A", ros::Duration(3.0)); @@ -89,7 +97,7 @@ TEST(TfGeometry, PoseWithCovarianceStamped) v1.pose.covariance[21] = 1; v1.pose.covariance[28] = 1; v1.pose.covariance[35] = 1; - + // simple api const geometry_msgs::PoseWithCovarianceStamped v_simple = tf_buffer->transform(v1, "B", ros::Duration(2.0)); EXPECT_NEAR(v_simple.pose.pose.position.x, -9, EPS); @@ -99,7 +107,7 @@ TEST(TfGeometry, PoseWithCovarianceStamped) 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); - + // no rotation in this transformation, so no change to covariance EXPECT_NEAR(v_simple.pose.covariance[0], 1.0, EPS); EXPECT_NEAR(v_simple.pose.covariance[7], 1.0, EPS); @@ -107,7 +115,7 @@ TEST(TfGeometry, PoseWithCovarianceStamped) EXPECT_NEAR(v_simple.pose.covariance[21], 1.0, EPS); EXPECT_NEAR(v_simple.pose.covariance[28], 1.0, EPS); EXPECT_NEAR(v_simple.pose.covariance[35], 1.0, EPS); - + // advanced api const geometry_msgs::PoseWithCovarianceStamped v_advanced = tf_buffer->transform(v1, "B", ros::Time(2.0), "A", ros::Duration(3.0)); @@ -118,7 +126,7 @@ TEST(TfGeometry, PoseWithCovarianceStamped) 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); - + // no rotation in this transformation, so no change to covariance EXPECT_NEAR(v_advanced.pose.covariance[0], 1.0, EPS); EXPECT_NEAR(v_advanced.pose.covariance[7], 1.0, EPS); @@ -126,9 +134,9 @@ TEST(TfGeometry, PoseWithCovarianceStamped) EXPECT_NEAR(v_advanced.pose.covariance[21], 1.0, EPS); EXPECT_NEAR(v_advanced.pose.covariance[28], 1.0, EPS); EXPECT_NEAR(v_advanced.pose.covariance[35], 1.0, EPS); - + /** now add rotation to transform to test the effect on covariance **/ - + // rotate pi/2 radians about x-axis geometry_msgs::TransformStamped t_rot; t_rot.transform.rotation = tf2::toMsg(tf2::Quaternion(tf2::Vector3(1,0,0), M_PI/2)); @@ -136,12 +144,12 @@ TEST(TfGeometry, PoseWithCovarianceStamped) t_rot.header.frame_id = "A"; t_rot.child_frame_id = "rotated"; tf_buffer->setTransform(t_rot, "rotation_test"); - + // need to put some covariance in the matrix v1.pose.covariance[1] = 1; v1.pose.covariance[6] = 1; v1.pose.covariance[12] = 1; - + // perform rotation const geometry_msgs::PoseWithCovarianceStamped v_rotated = tf_buffer->transform(v1, "rotated", ros::Duration(2.0)); @@ -155,11 +163,11 @@ TEST(TfGeometry, PoseWithCovarianceStamped) EXPECT_NEAR(v_rotated.pose.covariance[12],-1.0, EPS); EXPECT_NEAR(v_rotated.pose.covariance[13], 0.0, EPS); EXPECT_NEAR(v_rotated.pose.covariance[14], 1.0, EPS); - + // set buffer back to original transform tf_buffer->setTransform(t, "test"); } - + TEST(TfGeometry, Transform) { geometry_msgs::TransformStamped v1; @@ -179,7 +187,7 @@ TEST(TfGeometry, Transform) EXPECT_NEAR(v_simple.transform.rotation.y, 0.0, EPS); EXPECT_NEAR(v_simple.transform.rotation.z, 0.0, EPS); EXPECT_NEAR(v_simple.transform.rotation.w, 1.0, EPS); - + // advanced api geometry_msgs::TransformStamped v_advanced = tf_buffer->transform(v1, "B", ros::Time(2.0), @@ -351,6 +359,42 @@ TEST(TfGeometry, doTransformWrench) EXPECT_NEAR(res.torque.z, 3, EPS); } +TEST(TfGeometry, Quaternion) +{ + // rotated by -90° around y + // 0, 0, -1 + // 0, 1, 0, + // 1, 0, 0 + geometry_msgs::QuaternionStamped q1, res; + q1.quaternion.x = 0; + q1.quaternion.y = -1 * M_SQRT1_2; + q1.quaternion.z = 0; + q1.quaternion.w = M_SQRT1_2; + q1.header.stamp = ros::Time(2); + q1.header.frame_id = "A"; + + // simple api + const geometry_msgs::QuaternionStamped q_simple = tf_buffer->transform( + q1, "B", ros::Duration(2.0)); + EXPECT_NEAR(abs(q_simple.quaternion.x), M_SQRT1_2, EPS); + EXPECT_NEAR(q_simple.quaternion.y, 0, EPS); + EXPECT_NEAR(abs(q_simple.quaternion.z), M_SQRT1_2, EPS); + EXPECT_NEAR(q_simple.quaternion.w, 0, EPS); + // x and z should have different signs + EXPECT_NEAR(abs(q_simple.quaternion.x - q_simple.quaternion.z), M_SQRT2, EPS); + + // advanced api + const geometry_msgs::QuaternionStamped q_advanced = tf_buffer->transform( + q1, "B", ros::Time(2.0), "A", ros::Duration(3.0)); + EXPECT_NEAR(abs(q_advanced.quaternion.x), M_SQRT1_2, EPS); + EXPECT_NEAR(q_advanced.quaternion.y, 0, EPS); + EXPECT_NEAR(abs(q_advanced.quaternion.z), M_SQRT1_2, EPS); + EXPECT_NEAR(q_advanced.quaternion.w, 0, EPS); + // x and z should have different signs + EXPECT_NEAR(abs(q_advanced.quaternion.x - q_advanced.quaternion.z), M_SQRT2, EPS); +} + + int main(int argc, char **argv){ testing::InitGoogleTest(&argc, argv); ros::init(argc, argv, "test"); @@ -358,7 +402,7 @@ int main(int argc, char **argv){ tf_buffer = new tf2_ros::Buffer(); tf_buffer->setUsingDedicatedThread(true); - + // populate buffer t.transform.translation.x = 10; t.transform.translation.y = 20; @@ -373,8 +417,3 @@ int main(int argc, char **argv){ delete tf_buffer; return ret; } - - - - - From 4a55a8d8bfda399d5b0faab7c96cd308e4453548 Mon Sep 17 00:00:00 2001 From: Bjar Ne Date: Mon, 8 Feb 2021 17:11:28 +0000 Subject: [PATCH 4/4] Remove broken test --- tf2_eigen/test/tf2_eigen-test.cpp | 28 ---------------------------- 1 file changed, 28 deletions(-) diff --git a/tf2_eigen/test/tf2_eigen-test.cpp b/tf2_eigen/test/tf2_eigen-test.cpp index 0ae78e1c1..878fa2b5d 100644 --- a/tf2_eigen/test/tf2_eigen-test.cpp +++ b/tf2_eigen/test/tf2_eigen-test.cpp @@ -102,34 +102,6 @@ TEST(TfEigen, ConvertQuaterniond) EXPECT_EQ(v.z(), v1.z()); } -TEST(TfEigen, TransformQuaterion) { - const tf2::Stamped in(Eigen::Quaterniond(Eigen::AngleAxisd(1, Eigen::Vector3d::UnitX())), ros::Time(5), "test"); - const Eigen::Isometry3d iso(Eigen::AngleAxisd(M_PI/2, Eigen::Vector3d::UnitY())); - const Eigen::Affine3d affine(iso); - const tf2::Stamped expected(Eigen::Quaterniond(Eigen::AngleAxisd(1, Eigen::Vector3d::UnitZ())), ros::Time(10), "expected"); - - geometry_msgs::TransformStamped trafo = tf2::eigenToTransform(affine); - trafo.header.stamp = ros::Time(10); - trafo.header.frame_id = "expected"; - - tf2::Stamped out; - tf2::doTransform(in, out, trafo); - - EXPECT_TRUE(out.isApprox(expected)); - EXPECT_EQ(expected.stamp_, out.stamp_); - EXPECT_EQ(expected.frame_id_, out.frame_id_); - - // the same using Isometry - trafo = tf2::eigenToTransform(iso); - trafo.header.stamp = ros::Time(10); - trafo.header.frame_id = "expected"; - tf2::doTransform(in, out, trafo); - - EXPECT_TRUE(out.isApprox(expected)); - EXPECT_EQ(expected.stamp_, out.stamp_); - EXPECT_EQ(expected.frame_id_, out.frame_id_); -} - TEST(TfEigen, ConvertAffine3dStamped) { const Eigen::Affine3d v_nonstamped(Eigen::Translation3d(1,2,3) * Eigen::AngleAxis(1, Eigen::Vector3d::UnitX()));