ROS中的tf与Eigen的转换

写ROS程序时会经常遇到tf与Eigen库的转换，即算法中大多会使用Eigen来进行运算和表示机器人的位姿，但是最终需要tf将pose发布出去，所以需要将Eigen表示的pose转换为tf以及相应的msgs。

首先，在“Transform.h”头文件中定义了Transform类，类成员函数 void mult(const Transform& t1, const Transform& t2) 可以很方便地用于机器人的坐标转换。在"transform_datatypes.h"头文件中又定义了StampedTransform类，该类继承自Transform类，新增加了时间戳以及frame_id：

/** \brief The Stamped Transform datatype used by tf */
class StampedTransform : public tf::Transform
{
public:
  ros::Time stamp_; ///< The timestamp associated with this transform
  std::string frame_id_; ///< The frame_id of the coordinate frame  in which this transform is defined
  std::string child_frame_id_; ///< The frame_id of the coordinate frame this transform defines
  StampedTransform(const tf::Transform& input, const ros::Time& timestamp, const std::string & frame_id, const std::string & child_frame_id):
    tf::Transform (input), stamp_ ( timestamp ), frame_id_ (frame_id), child_frame_id_(child_frame_id){ };

  /** \brief Default constructor only to be used for preallocation */
  StampedTransform() { };

  /** \brief Set the inherited Traonsform data */
  void setData(const tf::Transform& input){*static_cast<tf::Transform*>(this) = input;};

};

ROS中对外发布tf的方式为调用“tf::TransformBroadcaster”类的"sendTransform()"函数，在头文件"transform_broadcaster.h”中定义：

/** \brief Send a StampedTransform 
   * The stamped data structure includes frame_id, and time, and parent_id already.  */
  void sendTransform(const StampedTransform & transform);

  /** \brief Send a TransformStamped message
   * The stamped data structure includes frame_id, and time, and parent_id already.  */
  void sendTransform(const geometry_msgs::TransformStamped & transform);

 可见该函数不仅可以接收标准的 “geometry_msgs::TransformStamped” 数据，也可以接收"tf::StampedTransform"类型的数据，虽然在"transform_datatypes.h"中也定义了一个静态函数 “static void transformStampedTFToMsg(const StampedTransform& bt, geometry_msgs::TransformStamped & msg)” 用于将“StampedTransform”类型的数据转换为”geometry_msgs::TransformStamped“类型的数据。

1. 车体与激光传感器的位姿变换关系

假设我们使用激光传感器来做机器人定位， 算法给出的是激光传感器在地图坐标系中的位姿，即tf_map2scan，而我们实际需要的是车体在地图坐标系中位姿tf_map2base。又已知车体中心和激光传感器之间有一个固定的变换关系tf_base2scan，这个tf是由static_transform_publisher发布的，与激光传感器的安装位置有关，故程序中需要监听该Transform：

    // 先获得base_link到scan的tf转换关系，以便发布map到base_link的tf
    tf::StampedTransform tf_base2scan;
    tf::TransformListener tf_listener;
    tf_listener.waitForTransform("/base_link", "/scan", ros::Time(0), ros::Duration(1.0));
    tf_listener.lookupTransform("/base_link", "/scan", ros::Time(0), tf_base2scan);

2. 激光传感器到地图坐标系中的位姿

定位算法的结果是使用 ”Eigen::Matrix4f globalPose“ 这个Eigen的变换矩阵来表示，我们要做的是转换为tf来表示。首先将变换矩阵拆分为一个旋转矩阵（实际使用四元数表示）和一个平移矩阵：

Eigen::Quaterniond eigen_quat(globalPose.block<3,3>(0,0).cast<double>());
Eigen::Vector3d eigen_trans(globalPose.block<3,1>(0,3).cast<double>());

然后将其转换为相应的tf变量：

tf::Quaternion tf_quat;
tf::Vector3 tf_trans;
tf::quaternionEigenToTF(eigen_quat, tf_quat);
tf::vectorEigenToTF(eigen_trans, tf_trans);

”tf::quaternionEigenToTF“ 和 "tf::vectorEigenToTF" 函数在头文件<tf_conversions/tf_eigen.h>中定义，但是在实际编译的时候报错，提示undefined reference to ‘tf::matrixEigenToTF()'，暂时放弃这种写法，改为手动赋值：

tf::Quaternion tf_quat(eigen_quat.x(), eigen_quat.y(), eigen_quat.z(), eigen_quat.w());
tf::Vector3 tf_trans(eigen_trans(0), eigen_trans(1), eigen_trans(2));

最后构建激光传感器与地图坐标系的tf变换：

tf::StampedTransform tf_map2scan(tf::Transform(tf_quat, tf_trans), current_time, "map", "scan");

3. 车体在地图坐标系中的位姿

得到激光传感器与地图坐标系的tf关系 ”tf_map2scan“ 以及激光传感器与车体坐标系的tf关系 ”tf_base2scan“ 以后，便可以计算地图坐标系到车体坐标系的tf，即 tf_map2base = tf_map2scan * tf_scan2base：

tf::StampedTransform tf_map2base;
tf_map2base.mult(tf_map2scan, tf_base2scan.inverse());
tf_map2base.stamp_ = current_time;
tf_map2base.frame_id_ = "map";
tf_map2base.child_frame_id_ = "base_link";

最后发布该tf关系： 

broadcaster.sendTransform(tf_map2base);