ROS之tf空间坐标变换完全详解

本博文主要汇总了自己在使用ROS中tf坐标变换包时查找的一些参考资料和博客，包括了tf和部分tf2的使用，在此感谢本博文中出现的所有的博客链接！
参考资源：
ROS官网详细介绍：
http://wiki.ros.org/tf/Tutorials
http://wiki.ros.org/tf
introduction to tf: http://wiki.ros.org/tf/Tutorials/Introduction to tf

博文：
实用讲解：https://blog.csdn.net/Start_From_Scratch/article/details/50762293
https://blog.csdn.net/u010876294/article/details/75004903
http://www.guyuehome.com/355
https://blog.csdn.net/hcx25909/article/details/9255001 详细例子 Python版乌龟坐标
https://blog.csdn.net/Start_From_Scratch/article/details/50762293

在机器人控制系统中，坐标系是非常重要的，ROS中使用tf软件库进行坐标转换，现在已经有tf2库了，而且比tf库更加功能强大，建议优先选用tf2进行开发。

为了更好的理解tf做的事情，我们需要理解ROS中与坐标相关的概念
https://blog.csdn.net/github_30605157/article/details/72081946
https://www.douban.com/note/574961732/
https://stackoverflow.com/questions/46363618/aruco-markers-with-opencv-get-the-3d-corner-coordinates
http://www.guyuehome.com/355
ROS中对坐标系的定义：包括转换方向、坐标轴方向、旋转手性的定义规范：
http://wiki.ros.org/geometry/CoordinateFrameConventions
在tf中，两个坐标轴的关系（也就是转换信息）用一个6自由度的相对位姿表示：平移量（translation）+旋转量（rotation）。其中平移量就是一个三维向量，旋转量可以用一个旋转量矩阵表示，tf中没有表示旋转量矩阵的类型，而是通过四元数类型tf::Quaternion来表示，
并且定义了从旋转量矩阵得到四元数的方法：http://docs.ros.org/api/tf/html/c++/transform__datatypes_8h.html

static tf::Quaternion   tf::createQuaternionFromRPY (double roll, double pitch, double yaw)
static Quaternion   tf::createQuaternionFromYaw (double yaw)
static geometry_msgs::Quaternion    tf::createQuaternionMsgFromRollPitchYaw (double roll, double pitch, double yaw)

将坐标变换讲解最为透彻的：http://blog.exbot.net/archives/1686

1）tf不是坐标变换那么简单。

很多小伙伴认为tf的作用是便捷的进行坐标变换。这个没错，但没这么简单。
在很多api中，存在着target frame，source frame，parent frame，child frame，这些名字的参数。
看起来很让人糊涂，也很让让人烦，但里面隐藏着很多信息。source、target frame是在进行坐标变换时的概念，source是坐标变换的源坐标系，target是目标坐标系，这个时候，这个变换代表的是坐标变换。
parent、child frame是在描述坐标系变换时的概念，parent是原坐标系，child是变换后的坐标系，这个时候这个变换描述的是坐标系变换，也是child坐标系在parent坐标系下的描述。如果把child固结于一个刚体，那么这个变换描述的就是刚体在parent坐标系下的姿态。这个用处就大多了，可以用它来描述刚体的运动了。
这里有个巧合，当然也不是巧合，那就是从child到parent的坐标变换等于从parent到child的frame transform，等于child在parent frame的姿态描述。这里牵扯到了线性代数里的基变换、线性变换、过渡矩阵的概念。

2）简单的说

在线性空间里，一个矩阵不但可以描述在同一个基下把一个点运动到另一个点的线性变换，还可以描述一个基在另外一个基下的表示，也可以表示一个基到另一个基的线性变换。还是比较复杂呀。那就看书吧，这样比较简单。
不过，如果单说坐标变换就简单了。所谓坐标变换就是把一个点在某个坐标系的描述，变换成在另外一个坐标系下的描述。
要实现这个变换，比较简单的做法就是用当前坐标系在参考坐标系下的描述（一个矩阵）去描述（乘以）这个点在当前坐标系下的描述（坐标）。
所以只要我们能够知道当前坐标系在参考坐标系的描述，我们就可以把当前坐标系里任何一个点的坐标变换成参考坐标系里的坐标。
好吧，不考虑平移，只考虑旋转，参考坐标系换个名字叫固定坐标系，当前坐标系叫动坐标系，那么该如何得到这个描述呢？
用矩阵。矩阵的各列分别是动坐标系在固定坐标系上的投影。当然如果你喜欢用坐标向量右乘矩阵，你也可以把在各轴的投影放在各行上。
一个基本的旋转关系构成的矩阵是很好描述的。那么多次旋转呢？

3）多次旋转、三次旋转、欧拉角、四元数

经过多次旋转的坐标系该怎么在固定坐标系下描述呢。这里有一个前乘和后乘的问题。什么时候前，什么时候后，很简单，绕固定坐标系的轴旋转就前乘，绕定坐标系就后乘。为什么是这样呢？解释起来很复杂，但可以这样理解，绕固定轴旋转时，你把这个旋转看做是线性变换，这个变换把原来的矩阵里的各列向量旋转变换成了新的向量，所以前乘。当绕动坐标系旋转时，你把这个旋转矩阵看做是一个描述了，并且这个描述是在未乘之前的矩阵这个描述下的描述，所以要放到后面。
刚体的任一姿态（任一动坐标系）可以经由三次基本旋转得到，用三个角来描述，这就是欧拉角。在tf里eulerYPR指的是绕动坐标系的zyx旋转，RPY指的是绕固定坐标系xyz旋转，这二者等价，坐标系定义为右手，x前，y左，z上。
四元数是刚体姿态的另一种描述方式，理论基础是，刚体姿态可以经过某一特定轴经一次旋转一定角度得到，等价于欧拉角，等价于旋转矩阵。一个四个参数，一个三个参数，一个九个参数，之所以等价是因为四个参数里有一个约束，九个参数里有六个约束。

tf中的基本数据类型

数据类型定义头文件：#include
tf::Quaternion、tf::Vector3、tf::Point、tf::Pose、tf::Transform等类型，这些数据类型中有些是结构体类型，有些是模板类。
其中坐标系中的点在ROS中用tf::Point表示，
tf::StampedTransform是tf::Transform的特例（继承类），它要求frame_id、stamp、child_frame_id，也就是说tf::StampedTransform才是更好的描述某个坐标系(child_frame_id)相对于另一个参考坐标系(frame_id)在某个时刻的位姿关系,其构造函数中也需要指定参考坐标系id和目前坐标系id（也即child_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(this) = input;};
};

tf维护的坐标系关系其实是将有关坐标系话题发布在tf::tfMessage类型消息中，该消息定义为geometry_msgs/TransformStamped类型的向量：

geometry_msgs/TransformStamped[] transforms
  std_msgs/Header header
    uint32 seq
    time stamp
    string frame_id
  string child_frame_id
  geometry_msgs/Transform transform
    geometry_msgs/Vector3 translation
      float64 x
      float64 y
      float64 z
    geometry_msgs/Quaternion rotation
      float64 x
      float64 y
      float64 z
      float64 w

tf变换存储的是将坐标系"frame_id"变换到"child_frame_id"的坐标系变换，也即将数据在"child_frame_id"坐标系下转换到在"frame_id"下表示。

ROS中tf库坐标系管理代码书写

利用tf库管理坐标系主要要做的就是两件事：监听tf变换和广播tf变换
Listening for transforms - Receive and buffer all coordinate frames that are broadcasted in the system, and query for specific transforms between frames.
http://wiki.ros.org/tf/Tutorials/Writing a tf listener (C%2B%2B)
注： each listener has a buffer where it stores all the coordinate transforms coming from the different tf broadcasters。When a broadcaster sends out a transform, it takes some time before that transform gets into the buffer (usually a couple of milliseconds).
http://wiki.ros.org/tf/Tutorials/tf and Time (C%2B%2B)

Broadcasting transforms - Send out the relative pose of coordinate frames to the rest of the system. A system can have many broadcasters that each provide information about a different part of the robot.
http://wiki.ros.org/tf/Tutorials/Writing a tf broadcaster (C%2B%2B)
**如何给tf添加额外的固定坐标系：**http://wiki.ros.org/tf/Tutorials/Adding a frame (C%2B%2B)
通过这种方法广播的tf变换，其实是直接将变换关系添加到tf::tfMessage消息类型中，然后以话题形式发布出来，默认话题名为/tf，这其实是一种动态坐标系变换，即坐标系的相对关系随着时间在发生变化；

在tf2包中，如果想要发布固定不变的坐标变换，可以发布tf::tfMessage消息类型到/tf_static话题中，此时tf会认为这种坐标关系在任何时候都有效，比如无人车上的传感器之间的相对位姿关系就是一种固定坐标系关系：
http://wiki.ros.org/tf2/Tutorials/Writing a tf2 static broadcaster (C%2B%2B)
http://wiki.ros.org/tf2/Migration#Addition_of_.2BAC8-tf_static_topic
https://answers.ros.org/question/235458/what-is-tf_static/

tf包中常用数据转换函数：
http://docs.ros.org/api/tf/html/c++/transform__datatypes_8h_source.html
四元数与欧拉角之间的转换：
https://blog.csdn.net/u010876294/article/details/75004903
http://docs.ros.org/api/tf/html/c++/transform__datatypes_8h.html

使用示例：
http://www.360doc.com/content/17/0411/11/7821691_644634302.shtml
https://blog.csdn.net/u010876294/article/details/75004903
https://blog.csdn.net/u013453604/article/details/47448527

tf功能包使用

利用一些tf工具能够获知一些坐标系的关系情况
1、view_frames能够监听当前时刻所有通过ROS广播的tf坐标系，并绘制出树状图表示坐标系之间的连接关系保存到离线文件中：

$ rosrun tf view_frames

2、rqt_tf_tree工具
虽然view_frames能够将当前坐标系关系保存在离线文件中，但是无法实时反映坐标关系，所以可以用rqt_tf_tree实时刷新显示坐标系关系：

$rosrun rqt_tf_tree rqt_tf_tree

直接在终端中显示不同坐标系之间的关系：

rosrun tf tf_echo [reference_frame] [target_frame]

3、tf_echo工具
使用tf_echo工具可以查看两个广播参考系之间的关系。

$ rosrun tf tf_echo turtle1 turtle2 

tf在ROS中使用示例
https://blog.csdn.net/u010876294/article/details/75004903
接口函数总结：http://www.360doc.com/content/17/0411/11/7821691_644634302.shtml