tf2_eigen.h

Go to the documentation of this file.

/*
 * Copyright (c) Koji Terada
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 *     * Redistributions of source code must retain the above copyright
 *       notice, this list of conditions and the following disclaimer.
 *     * Redistributions in binary form must reproduce the above copyright
 *       notice, this list of conditions and the following disclaimer in the
 *       documentation and/or other materials provided with the distribution.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 */
 
#ifndef TF2_EIGEN_H
#define TF2_EIGEN_H
 
#include <tf2/convert.h>
#include <tf2_ros/buffer_interface.h>
#include <Eigen/Geometry>
#include <geometry_msgs/msg/point_stamped.hpp>
#include <geometry_msgs/msg/pose_stamped.hpp>
#include <geometry_msgs/msg/quaternion_stamped.hpp>
#include <geometry_msgs/msg/twist.hpp>
#include <geometry_msgs/msg/point.hpp>
#include <geometry_msgs/msg/quaternion.hpp>
#include <tf2_ros/buffer.h>
 
 
namespace tf2
{
 
 inline
 Eigen::Isometry3d transformToEigen(const geometry_msgs::msg::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));
}
 
inline
Eigen::Isometry3d transformToEigen(const geometry_msgs::msg::TransformStamped& t) {
  return transformToEigen(t.transform);
}
 
inline
geometry_msgs::msg::TransformStamped eigenToTransform(const Eigen::Affine3d& T)
{
  geometry_msgs::msg::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.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;
}
 
inline
geometry_msgs::msg::TransformStamped eigenToTransform(const Eigen::Isometry3d& T)
{
  geometry_msgs::msg::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;
}
 
template <>
inline
void doTransform(const Eigen::Vector3d& t_in, Eigen::Vector3d& t_out, const geometry_msgs::msg::TransformStamped& transform)
{
  t_out = Eigen::Vector3d(transformToEigen(transform) * t_in);
}
 
inline
geometry_msgs::msg::Point toMsg(const Eigen::Vector3d& in)
{
  geometry_msgs::msg::Point msg;
  msg.x = in.x();
  msg.y = in.y();
  msg.z = in.z();
  return msg;
}
 
inline
void fromMsg(const geometry_msgs::msg::Point& msg, Eigen::Vector3d& out)
{
  out.x() = msg.x;
  out.y() = msg.y;
  out.z() = msg.z;
}
 
inline
geometry_msgs::msg::Vector3& toMsg(const Eigen::Vector3d& in, geometry_msgs::msg::Vector3& out)
{
  out.x = in.x();
  out.y = in.y();
  out.z = in.z();
  return out;
}
 
inline
void fromMsg(const geometry_msgs::msg::Vector3& msg, Eigen::Vector3d& out)
{
  out.x() = msg.x;
  out.y() = msg.y;
  out.z() = msg.z;
}
 
template <>
inline
void doTransform(const tf2::Stamped<Eigen::Vector3d>& t_in,
         tf2::Stamped<Eigen::Vector3d>& t_out,
         const geometry_msgs::msg::TransformStamped& transform) {
  t_out = tf2::Stamped<Eigen::Vector3d>(transformToEigen(transform) * t_in,
                                        tf2_ros::fromMsg(transform.header.stamp),
                    transform.header.frame_id);
}
 
inline
geometry_msgs::msg::PointStamped toMsg(const tf2::Stamped<Eigen::Vector3d>& in)
{
  geometry_msgs::msg::PointStamped msg;
  msg.header.stamp = tf2_ros::toMsg(in.stamp_);
  msg.header.frame_id = in.frame_id_;
  msg.point = toMsg(static_cast<const Eigen::Vector3d&>(in));
  return msg;
}
 
inline
void fromMsg(const geometry_msgs::msg::PointStamped& msg, tf2::Stamped<Eigen::Vector3d>& out) {
  out.stamp_ = tf2_ros::fromMsg(msg.header.stamp);
  out.frame_id_ = msg.header.frame_id;
  fromMsg(msg.point, static_cast<Eigen::Vector3d&>(out));
}
 
template <>
inline
void doTransform(const Eigen::Affine3d& t_in,
                 Eigen::Affine3d& t_out,
                 const geometry_msgs::msg::TransformStamped& transform) {
  t_out = Eigen::Affine3d(transformToEigen(transform) * t_in);
}
 
template <>
inline
void doTransform(const Eigen::Isometry3d& t_in,
                 Eigen::Isometry3d& t_out,
                 const geometry_msgs::msg::TransformStamped& transform) {
  t_out = Eigen::Isometry3d(transformToEigen(transform) * t_in);
}
 
inline
geometry_msgs::msg::Quaternion toMsg(const Eigen::Quaterniond& in) {
 geometry_msgs::msg::Quaternion msg;
 msg.w = in.w();
 msg.x = in.x();
 msg.y = in.y();
 msg.z = in.z();
 return msg;
}
 
inline
void fromMsg(const geometry_msgs::msg::Quaternion& msg, Eigen::Quaterniond& out) {
  out = Eigen::Quaterniond(msg.w, msg.x, msg.y, msg.z);
}
 
template<>
inline
void doTransform(const Eigen::Quaterniond& t_in,
                 Eigen::Quaterniond& t_out,
                 const geometry_msgs::msg::TransformStamped& transform) {
  Eigen::Quaterniond t;
  fromMsg(transform.transform.rotation, t);
  t_out = t.inverse() * t_in * t;
}
 
inline
geometry_msgs::msg::QuaternionStamped toMsg(const Stamped<Eigen::Quaterniond>& in) {
  geometry_msgs::msg::QuaternionStamped msg;
  msg.header.stamp = tf2_ros::toMsg(in.stamp_);
  msg.header.frame_id = in.frame_id_;
  msg.quaternion = toMsg(static_cast<const Eigen::Quaterniond&>(in));
  return msg;
}
 
inline
void fromMsg(const geometry_msgs::msg::QuaternionStamped& msg, Stamped<Eigen::Quaterniond>& out) {
  out.frame_id_ = msg.header.frame_id;
  out.stamp_ = tf2_ros::fromMsg(msg.header.stamp);
  fromMsg(msg.quaternion, static_cast<Eigen::Quaterniond&>(out));
}
 
template <>
inline
void doTransform(const tf2::Stamped<Eigen::Quaterniond>& t_in,
     tf2::Stamped<Eigen::Quaterniond>& t_out,
     const geometry_msgs::msg::TransformStamped& transform) {
  t_out.frame_id_ = transform.header.frame_id;
  t_out.stamp_ = tf2_ros::fromMsg(transform.header.stamp);
  doTransform(static_cast<const Eigen::Quaterniond&>(t_in), static_cast<Eigen::Quaterniond&>(t_out), transform);
}
 
inline
geometry_msgs::msg::Pose toMsg(const Eigen::Affine3d& in) {
  geometry_msgs::msg::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();
  return msg;
}
 
inline
geometry_msgs::msg::Pose toMsg(const Eigen::Isometry3d& in) {
  geometry_msgs::msg::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();
  return msg;
}
 
inline
geometry_msgs::msg::Vector3 toMsg2(const Eigen::Vector3d& in) {
  geometry_msgs::msg::Vector3 msg;
  msg.x = in(0);
  msg.y = in(1);
  msg.z = in(2);
  return msg;
}
 
inline
void fromMsg(const geometry_msgs::msg::Pose& msg, Eigen::Affine3d& out) {
  out = Eigen::Affine3d(
      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));
}
 
inline
void fromMsg(const geometry_msgs::msg::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));
}
 
inline
geometry_msgs::msg::Twist toMsg(const Eigen::Matrix<double,6,1>& in) {
  geometry_msgs::msg::Twist msg;
  msg.linear.x = in[0];
  msg.linear.y = in[1];
  msg.linear.z = in[2];
  msg.angular.x = in[3];
  msg.angular.y = in[4];
  msg.angular.z = in[5];
  return msg;
}
 
inline
void fromMsg(const geometry_msgs::msg::Twist &msg, Eigen::Matrix<double,6,1>& out) {
  out[0] = msg.linear.x;
  out[1] = msg.linear.y;
  out[2] = msg.linear.z;
  out[3] = msg.angular.x;
  out[4] = msg.angular.y;
  out[5] = msg.angular.z;
}
 
template <>
inline
void doTransform(const tf2::Stamped<Eigen::Affine3d>& t_in,
         tf2::Stamped<Eigen::Affine3d>& t_out,
         const geometry_msgs::msg::TransformStamped& transform) {
  t_out = tf2::Stamped<Eigen::Affine3d>(transformToEigen(transform) * t_in, tf2_ros::fromMsg(transform.header.stamp), transform.header.frame_id);
}
 
template <>
inline
void doTransform(const tf2::Stamped<Eigen::Isometry3d>& t_in,
         tf2::Stamped<Eigen::Isometry3d>& t_out,
         const geometry_msgs::msg::TransformStamped& transform) {
  t_out = tf2::Stamped<Eigen::Isometry3d>(transformToEigen(transform) * t_in, tf2_ros::fromMsg(transform.header.stamp), transform.header.frame_id);
}
 
inline
geometry_msgs::msg::PoseStamped toMsg(const tf2::Stamped<Eigen::Affine3d>& in)
{
  geometry_msgs::msg::PoseStamped msg;
  msg.header.stamp = tf2_ros::toMsg(in.stamp_);
  msg.header.frame_id = in.frame_id_;
  msg.pose = toMsg(static_cast<const Eigen::Affine3d&>(in));
  return msg;
}
 
inline
geometry_msgs::msg::PoseStamped toMsg(const tf2::Stamped<Eigen::Isometry3d>& in)
{
  geometry_msgs::msg::PoseStamped msg;
  msg.header.stamp = tf2_ros::toMsg(in.stamp_);
  msg.header.frame_id = in.frame_id_;
  msg.pose = toMsg(static_cast<const Eigen::Isometry3d&>(in));
  return msg;
}
 
inline
void fromMsg(const geometry_msgs::msg::PoseStamped& msg, tf2::Stamped<Eigen::Affine3d>& out)
{
  out.stamp_ = tf2_ros::fromMsg(msg.header.stamp);
  out.frame_id_ = msg.header.frame_id;
  fromMsg(msg.pose, static_cast<Eigen::Affine3d&>(out));
}
 
inline
void fromMsg(const geometry_msgs::msg::PoseStamped& msg, tf2::Stamped<Eigen::Isometry3d>& out)
{
  out.stamp_ = tf2_ros::fromMsg(msg.header.stamp);
  out.frame_id_ = msg.header.frame_id;
  fromMsg(msg.pose, static_cast<Eigen::Isometry3d&>(out));
}
 
} // namespace
 
 
namespace Eigen {
// This is needed to make the usage of the following conversion functions usable in tf2::convert().
// According to clangs error note 'fromMsg'/'toMsg' should be declared prior to the call site or
// in an associated namespace of one of its arguments. The stamped versions of this conversion
// functions work because they have tf2::Stamped as an argument which is the same namespace as
// which 'fromMsg'/'toMsg' is defined in. The non-stamped versions have no argument which is
// defined in tf2, so it take the following definitions in Eigen namespace to make them usable in
// tf2::convert().
 
inline
geometry_msgs::msg::Pose toMsg(const Eigen::Affine3d& in) {
  return tf2::toMsg(in);
}
 
inline
geometry_msgs::msg::Pose toMsg(const Eigen::Isometry3d& in) {
  return tf2::toMsg(in);
}
 
inline
void fromMsg(const geometry_msgs::msg::Point& msg, Eigen::Vector3d& out) {
  tf2::fromMsg(msg, out);
}
 
inline
geometry_msgs::msg::Point toMsg(const Eigen::Vector3d& in) {
  return tf2::toMsg(in);
}
 
inline
void fromMsg(const geometry_msgs::msg::Pose& msg, Eigen::Affine3d& out) {
  tf2::fromMsg(msg, out);
}
 
inline
void fromMsg(const geometry_msgs::msg::Pose& msg, Eigen::Isometry3d& out) {
  tf2::fromMsg(msg, out);
}
 
inline
geometry_msgs::msg::Quaternion toMsg(const Eigen::Quaterniond& in) {
  return tf2::toMsg(in);
}
 
inline
geometry_msgs::msg::Twist toMsg(const Eigen::Matrix<double,6,1>& in) {
  return tf2::toMsg(in);
}
 
inline
void fromMsg(const geometry_msgs::msg::Twist &msg, Eigen::Matrix<double,6,1>& out) {
  tf2::fromMsg(msg, out);
}
 
} // namespace
 
#endif // TF2_EIGEN_H