Skip to content

Commit

Permalink
add Eigen::Isometry3d conversions
Browse files Browse the repository at this point in the history
  • Loading branch information
@rhaschke @tfoote
rhaschke authored and tfoote committed Oct 19, 2018
1 parent b1f5287 commit d1b297b
Show file tree
Hide file tree
Showing 2 changed files with 170 additions and 9 deletions.
127 changes: 120 additions & 7 deletions tf2_eigen/include/tf2_eigen/tf2_eigen.h
View file
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 @@ -81,6 +80,27 @@ geometry_msgs::TransformStamped eigenToTransform(const Eigen::Affine3d& T)
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();
t.transform.rotation.z = q.z();
t.transform.rotation.w = q.w();

return t;
}

/** \brief Apply a geometry_msgs TransformStamped to an Eigen-specific Vector3d type.
* 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
Expand Down Expand Up @@ -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 @@ -323,6 +351,31 @@ geometry_msgs::Pose toMsg(const Eigen::Affine3d& in) {
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;
}

/** \brief Convert a Pose message transform type to a Eigen Affine3d.
* This function is a specialization of the toMsg template defined in tf2/convert.h.
* \param msg The Pose message to convert.
Expand All @@ -338,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 @@ -387,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 @@ -402,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 @@ -415,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 @@ -432,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 @@ -447,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
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

0 comments on commit d1b297b

Please sign in to comment.