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

Jump to bottom

conversions for Eigen::Isometry3d #335

Merged
merged 4 commits into from
Oct 19, 2018
Merged

conversions for Eigen::Isometry3d #335

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
e5fc156
improve computation efficiency
rhaschke Oct 17, 2018
a853261
normalize quaternions to be in half-space w >= 0
rhaschke Oct 17, 2018
34832e2
add Eigen::Isometry3d conversions
rhaschke Oct 17, 2018
89a668b
improve comments
rhaschke Oct 17, 2018
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
150 changes: 135 additions & 15 deletions 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 @@ -42,21 +42,20 @@ namespace tf2

/** \brief Convert a timestamped transform to the equivalent Eigen data type.
* \param t The transform to convert, as a geometry_msgs Transform message.
* \return The transform message converted to an Eigen Affine3d transform.
* \return The transform message converted to an Eigen Isometry3d transform.
*/
inline
Eigen::Affine3d transformToEigen(const geometry_msgs::Transform& t) {
return Eigen::Affine3d(Eigen::Translation3d(t.translation.x, t.translation.y, t.translation.z)
* Eigen::Quaterniond(t.rotation.w,
t.rotation.x, t.rotation.y, t.rotation.z));
Eigen::Isometry3d transformToEigen(const geometry_msgs::Transform& t) {
return Eigen::Isometry3d(Eigen::Translation3d(t.translation.x, t.translation.y, t.translation.z)
* Eigen::Quaterniond(t.rotation.w, t.rotation.x, t.rotation.y, t.rotation.z));
}

/** \brief Convert a timestamped transform to the equivalent Eigen data type.
* \param t The transform to convert, as a geometry_msgs TransformedStamped message.
* \return The transform message converted to an Eigen Affine3d transform.
* \return The transform message converted to an Eigen Isometry3d transform.
*/
inline
Eigen::Affine3d transformToEigen(const geometry_msgs::TransformStamped& t) {
Eigen::Isometry3d transformToEigen(const geometry_msgs::TransformStamped& t) {
return transformToEigen(t.transform);
}

Expand All @@ -72,6 +71,27 @@ geometry_msgs::TransformStamped eigenToTransform(const Eigen::Affine3d& T)
t.transform.translation.y = T.translation().y();
t.transform.translation.z = T.translation().z();

Eigen::Quaterniond q(T.linear()); // assuming that upper 3x3 matrix is orthonormal
t.transform.rotation.x = q.x();
t.transform.rotation.y = q.y();
t.transform.rotation.z = q.z();
t.transform.rotation.w = q.w();

return t;
}

/** \brief Convert an Eigen Isometry3d transform to the equivalent geometry_msgs message type.
* \param T The transform to convert, as an Eigen Isometry3d transform.
* \return The transform converted to a TransformStamped message.
*/
inline
geometry_msgs::TransformStamped eigenToTransform(const Eigen::Isometry3d& T)
{
geometry_msgs::TransformStamped t;
t.transform.translation.x = T.translation().x();
t.transform.translation.y = T.translation().y();
t.transform.translation.z = T.translation().z();

Eigen::Quaterniond q(T.rotation());
t.transform.rotation.x = q.x();
t.transform.rotation.y = q.y();
Expand Down Expand Up @@ -195,10 +215,10 @@ void fromMsg(const geometry_msgs::PointStamped& msg, tf2::Stamped<Eigen::Vector3
fromMsg(msg.point, static_cast<Eigen::Vector3d&>(out));
}

/** \brief Apply a geometry_msgs Transform to a Eigen-specific affine transform data type.
/** \brief Apply a geometry_msgs Transform to an Eigen Affine3d transform.
* This function is a specialization of the doTransform template defined in tf2/convert.h,
* although it can not be used in tf2_ros::BufferInterface::transform because this
* functions rely on the existence of a time stamp and a frame id in the type which should
* function relies on the existence of a time stamp and a frame id in the type which should
* get transformed.
* \param t_in The frame to transform, as a Eigen Affine3d transform.
* \param t_out The transformed frame, as a Eigen Affine3d transform.
Expand All @@ -212,6 +232,14 @@ void doTransform(const Eigen::Affine3d& t_in,
t_out = Eigen::Affine3d(transformToEigen(transform) * t_in);
}

template <>
inline
void doTransform(const Eigen::Isometry3d& t_in,
Eigen::Isometry3d& t_out,
const geometry_msgs::TransformStamped& transform) {
t_out = Eigen::Isometry3d(transformToEigen(transform) * t_in);
}

/** \brief Convert a Eigen Quaterniond type to a Quaternion message.
* This function is a specialization of the toMsg template defined in tf2/convert.h.
* \param in The Eigen Quaterniond to convert.
Expand Down Expand Up @@ -309,10 +337,42 @@ geometry_msgs::Pose toMsg(const Eigen::Affine3d& in) {
msg.position.x = in.translation().x();
msg.position.y = in.translation().y();
msg.position.z = in.translation().z();
msg.orientation.x = Eigen::Quaterniond(in.rotation()).x();
msg.orientation.y = Eigen::Quaterniond(in.rotation()).y();
msg.orientation.z = Eigen::Quaterniond(in.rotation()).z();
msg.orientation.w = Eigen::Quaterniond(in.rotation()).w();
Eigen::Quaterniond q(in.linear());
Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

compute the quaternion only once!

msg.orientation.x = q.x();
msg.orientation.y = q.y();
msg.orientation.z = q.z();
msg.orientation.w = q.w();
if (msg.orientation.w < 0) {
Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Constrain quaternions to the half-space w >= 0 as in tf1.

msg.orientation.x *= -1;
msg.orientation.y *= -1;
msg.orientation.z *= -1;
msg.orientation.w *= -1;
}
return msg;
}

/** \brief Convert a Eigen Isometry3d transform type to a Pose message.
* This function is a specialization of the toMsg template defined in tf2/convert.h.
* \param in The Eigen Isometry3d to convert.
* \return The Eigen transform converted to a Pose message.
*/
inline
geometry_msgs::Pose toMsg(const Eigen::Isometry3d& in) {
geometry_msgs::Pose msg;
msg.position.x = in.translation().x();
msg.position.y = in.translation().y();
msg.position.z = in.translation().z();
Eigen::Quaterniond q(in.linear());
msg.orientation.x = q.x();
msg.orientation.y = q.y();
msg.orientation.z = q.z();
msg.orientation.w = q.w();
if (msg.orientation.w < 0) {
msg.orientation.x *= -1;
msg.orientation.y *= -1;
msg.orientation.z *= -1;
msg.orientation.w *= -1;
}
return msg;
}

Expand All @@ -331,6 +391,21 @@ void fromMsg(const geometry_msgs::Pose& msg, Eigen::Affine3d& out) {
msg.orientation.z));
}

/** \brief Convert a Pose message transform type to a Eigen Isometry3d.
* This function is a specialization of the toMsg template defined in tf2/convert.h.
* \param msg The Pose message to convert.
* \param out The pose converted to a Eigen Isometry3d.
*/
inline
void fromMsg(const geometry_msgs::Pose& msg, Eigen::Isometry3d& out) {
out = Eigen::Isometry3d(
Eigen::Translation3d(msg.position.x, msg.position.y, msg.position.z) *
Eigen::Quaterniond(msg.orientation.w,
msg.orientation.x,
msg.orientation.y,
msg.orientation.z));
}

/** \brief Convert a Eigen 6x1 Matrix type to a Twist message.
* This function is a specialization of the toMsg template defined in tf2/convert.h.
* \param in The 6x1 Eigen Matrix to convert.
Expand Down Expand Up @@ -363,10 +438,10 @@ void fromMsg(const geometry_msgs::Twist &msg, Eigen::Matrix<double,6,1>& out) {
out[5] = msg.angular.z;
}

/** \brief Apply a geometry_msgs TransformStamped to a Eigen-specific affine transform data type.
/** \brief Apply a geometry_msgs TransformStamped to an Eigen Affine3d transform.
* This function is a specialization of the doTransform template defined in tf2/convert.h,
* although it can not be used in tf2_ros::BufferInterface::transform because this
* functions rely on the existence of a time stamp and a frame id in the type which should
* function relies on the existence of a time stamp and a frame id in the type which should
* get transformed.
* \param t_in The frame to transform, as a timestamped Eigen Affine3d transform.
* \param t_out The transformed frame, as a timestamped Eigen Affine3d transform.
Expand All @@ -380,6 +455,23 @@ void doTransform(const tf2::Stamped<Eigen::Affine3d>& t_in,
t_out = tf2::Stamped<Eigen::Affine3d>(transformToEigen(transform) * t_in, transform.header.stamp, transform.header.frame_id);
}

/** \brief Apply a geometry_msgs TransformStamped to an Eigen Isometry transform.
* This function is a specialization of the doTransform template defined in tf2/convert.h,
* although it can not be used in tf2_ros::BufferInterface::transform because this
* function relies on the existence of a time stamp and a frame id in the type which should
* get transformed.
* \param t_in The frame to transform, as a timestamped Eigen Isometry transform.
* \param t_out The transformed frame, as a timestamped Eigen Isometry transform.
* \param transform The timestamped transform to apply, as a TransformStamped message.
*/
template <>
inline
void doTransform(const tf2::Stamped<Eigen::Isometry3d>& t_in,
tf2::Stamped<Eigen::Isometry3d>& t_out,
const geometry_msgs::TransformStamped& transform) {
t_out = tf2::Stamped<Eigen::Isometry3d>(transformToEigen(transform) * t_in, transform.header.stamp, transform.header.frame_id);
}

/** \brief Convert a stamped Eigen Affine3d transform type to a Pose message.
* This function is a specialization of the toMsg template defined in tf2/convert.h.
* \param in The timestamped Eigen Affine3d to convert.
Expand All @@ -395,6 +487,16 @@ geometry_msgs::PoseStamped toMsg(const tf2::Stamped<Eigen::Affine3d>& in)
return msg;
}

inline
geometry_msgs::PoseStamped toMsg(const tf2::Stamped<Eigen::Isometry3d>& in)
{
geometry_msgs::PoseStamped msg;
msg.header.stamp = in.stamp_;
msg.header.frame_id = in.frame_id_;
msg.pose = toMsg(static_cast<const Eigen::Isometry3d&>(in));
return msg;
}

/** \brief Convert a Pose message transform type to a stamped Eigen Affine3d.
* This function is a specialization of the toMsg template defined in tf2/convert.h.
* \param msg The PoseStamped message to convert.
Expand All @@ -408,6 +510,14 @@ void fromMsg(const geometry_msgs::PoseStamped& msg, tf2::Stamped<Eigen::Affine3d
fromMsg(msg.pose, static_cast<Eigen::Affine3d&>(out));
}

inline
void fromMsg(const geometry_msgs::PoseStamped& msg, tf2::Stamped<Eigen::Isometry3d>& out)
{
out.stamp_ = msg.header.stamp;
out.frame_id_ = msg.header.frame_id;
fromMsg(msg.pose, static_cast<Eigen::Isometry3d&>(out));
}

} // namespace


Expand All @@ -425,6 +535,11 @@ geometry_msgs::Pose toMsg(const Eigen::Affine3d& in) {
return tf2::toMsg(in);
}

inline
geometry_msgs::Pose toMsg(const Eigen::Isometry3d& in) {
return tf2::toMsg(in);
}

inline
void fromMsg(const geometry_msgs::Point& msg, Eigen::Vector3d& out) {
tf2::fromMsg(msg, out);
Expand All @@ -440,6 +555,11 @@ void fromMsg(const geometry_msgs::Pose& msg, Eigen::Affine3d& out) {
tf2::fromMsg(msg, out);
}

inline
void fromMsg(const geometry_msgs::Pose& msg, Eigen::Isometry3d& out) {
tf2::fromMsg(msg, out);
}

inline
geometry_msgs::Quaternion toMsg(const Eigen::Quaterniond& in) {
return tf2::toMsg(in);
Expand Down
52 changes: 50 additions & 2 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 @@ -90,10 +90,11 @@ TEST(TfEigen, ConvertQuaterniond)

TEST(TfEigen, TransformQuaterion) {
const tf2::Stamped<Eigen::Quaterniond> in(Eigen::Quaterniond(Eigen::AngleAxisd(1, Eigen::Vector3d::UnitX())), ros::Time(5), "test");
const Eigen::Affine3d r(Eigen::AngleAxisd(M_PI/2, Eigen::Vector3d::UnitY()));
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(r);
geometry_msgs::TransformStamped trafo = tf2::eigenToTransform(affine);
trafo.header.stamp = ros::Time(10);
trafo.header.frame_id = "expected";

Expand All @@ -103,6 +104,16 @@ TEST(TfEigen, TransformQuaterion) {
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)
Expand All @@ -121,6 +132,22 @@ TEST(TfEigen, ConvertAffine3dStamped)
EXPECT_EQ(v.stamp_, v1.stamp_);
}

TEST(TfEigen, ConvertIsometry3dStamped)
{
const Eigen::Isometry3d v_nonstamped(Eigen::Translation3d(1,2,3) * Eigen::AngleAxis<double>(1, Eigen::Vector3d::UnitX()));
const tf2::Stamped<Eigen::Isometry3d> v(v_nonstamped, ros::Time(42), "test_frame");

tf2::Stamped<Eigen::Isometry3d> v1;
geometry_msgs::PoseStamped p1;
tf2::convert(v, p1);
tf2::convert(p1, v1);

EXPECT_EQ(v.translation(), v1.translation());
EXPECT_EQ(v.rotation(), v1.rotation());
EXPECT_EQ(v.frame_id_, v1.frame_id_);
EXPECT_EQ(v.stamp_, v1.stamp_);
}

TEST(TfEigen, ConvertAffine3d)
{
const Eigen::Affine3d v(Eigen::Translation3d(1,2,3) * Eigen::AngleAxis<double>(1, Eigen::Vector3d::UnitX()));
Expand All @@ -134,6 +161,19 @@ TEST(TfEigen, ConvertAffine3d)
EXPECT_EQ(v.rotation(), v1.rotation());
}

TEST(TfEigen, ConvertIsometry3d)
{
const Eigen::Isometry3d v(Eigen::Translation3d(1,2,3) * Eigen::AngleAxis<double>(1, Eigen::Vector3d::UnitX()));

Eigen::Isometry3d v1;
geometry_msgs::Pose p1;
tf2::convert(v, p1);
tf2::convert(p1, v1);

EXPECT_EQ(v.translation(), v1.translation());
EXPECT_EQ(v.rotation(), v1.rotation());
}

TEST(TfEigen, ConvertTransform)
{
Eigen::Matrix4d tm;
Expand All @@ -155,6 +195,14 @@ TEST(TfEigen, ConvertTransform)
EXPECT_TRUE(T.isApprox(Tback));
EXPECT_TRUE(tm.isApprox(Tback.matrix()));

// same for Isometry
Eigen::Isometry3d I(tm);

msg = tf2::eigenToTransform(T);
Eigen::Isometry3d Iback = tf2::transformToEigen(msg);

EXPECT_TRUE(I.isApprox(Iback));
EXPECT_TRUE(tm.isApprox(Iback.matrix()));
}


Expand Down