Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

fix doTransform() for Eigen::Quaternion #496

Open
wants to merge 4 commits into
base: noetic-devel
Choose a base branch
from
Open

fix doTransform() for Eigen::Quaternion #496

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
4 commits
Select commit Hold shift + click to select a range
182a7ee
Add Eigen buffer transform tests
gleichdick Jan 28, 2021
28fd107
Fix Eigen Quaternion transform
gleichdick Jan 28, 2021
a26f09e
Unit test for doTransform(QuaternionStamped)
gleichdick Jan 28, 2021
4a55a8d
Remove broken test
gleichdick Feb 8, 2021
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
1 change: 1 addition & 0 deletions tf2_eigen/CMakeLists.txt
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
2 changes: 1 addition & 1 deletion tf2_eigen/include/tf2_eigen/tf2_eigen.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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.
Expand Down
1 change: 1 addition & 0 deletions tf2_eigen/package.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -11,6 +11,7 @@

<depend>geometry_msgs</depend>
<depend>tf2</depend>
<depend>tf2_ros</depend>

<build_depend>cmake_modules</build_depend>
<build_depend>eigen</build_depend>
Expand Down
162 changes: 132 additions & 30 deletions tf2_eigen/test/tf2_eigen-test.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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 <math.h>


#include <tf2_eigen/tf2_eigen.h>
#include <gtest/gtest.h>
#include <tf2/convert.h>
#include <tf2_eigen/tf2_eigen.h>
#include <tf2_ros/buffer.h>
#include <tf2_ros/transform_listener.h>


#include <memory>

TEST(TfEigen, ConvertVector3dStamped)
{
Expand Down Expand Up @@ -87,34 +102,6 @@ TEST(TfEigen, ConvertQuaterniond)
EXPECT_EQ(v.z(), v1.z());
}

TEST(TfEigen, TransformQuaterion) {
const tf2::Stamped<Eigen::Quaterniond> 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<Eigen::Quaterniond> 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<Eigen::Quaterniond> 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<double>(1, Eigen::Vector3d::UnitX()));
Expand Down Expand Up @@ -204,9 +191,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<int mode>
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<int mode>
void EigenTransform::testEigenTransform()
{
using T = Eigen::Transform<double, 3, mode>;
using stampedT = tf2::Stamped<T>;

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<Eigen::Affine>();
}

TEST_F(EigenTransform, Isometry3d) {
testEigenTransform<Eigen::Isometry>();
}

TEST_F(EigenTransform, Vector)
{
const tf2::Stamped<Eigen::Vector3d> v1{{1, 2, 3}, ros::Time(2), "A"};

// simple api
tf2::Stamped<Eigen::Vector3d> v_simple;
tf2::doTransform(v1, v_simple, transform);

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<Eigen::Quaterniond> q1{parameter, ros::Time(2), "A"};
// avoid linking error
const double eps = EPS;

// simple api
tf2::Stamped<Eigen::Quaterniond> 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){
int main(int argc, char ** argv)
{
testing::InitGoogleTest(&argc, argv);

return RUN_ALL_TESTS();
Expand Down
75 changes: 57 additions & 18 deletions tf2_geometry_msgs/test/test_tf2_geometry_msgs.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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 <math.h>

#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
#include <tf2_ros/transform_listener.h>
Expand Down Expand Up @@ -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));
Expand Down Expand Up @@ -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);
Expand All @@ -99,15 +107,15 @@ 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);
EXPECT_NEAR(v_simple.pose.covariance[14], 1.0, EPS);
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));
Expand All @@ -118,30 +126,30 @@ 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);
EXPECT_NEAR(v_advanced.pose.covariance[14], 1.0, EPS);
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));
t_rot.header.stamp = ros::Time(2.0);
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));

Expand All @@ -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;
Expand All @@ -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),
Expand Down Expand Up @@ -351,14 +359,50 @@ 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");
ros::NodeHandle n;

tf_buffer = new tf2_ros::Buffer();
tf_buffer->setUsingDedicatedThread(true);

// populate buffer
t.transform.translation.x = 10;
t.transform.translation.y = 20;
Expand All @@ -373,8 +417,3 @@ int main(int argc, char **argv){
delete tf_buffer;
return ret;
}