tf变换（1）
TF库的目的是实现系统中任一个点在所有坐标系之间的坐标变换，也就是说，只要给定一个坐标系下的一个点的坐标，就能获得这个点在其他坐标系的坐标.
使用tf功能包，a. 监听tf变换： 接收并缓存系统中发布的所有参考系变换，并从中查询所需要的参考系变换。
b.广播 tf变换: 向系统中广播参考系之间的坐标变换关系。系统中更可能会存在多个不同部分的tf变换广播，每个广播都可以直接将参考系变换关系直接插入tf树中，不需要再进行同步。
首先介绍关于TF的API的一些数据结构：
基本的数据类型有（Quaternion, Vector, Point, Pose, Transform）
这其中Quaternion 是表示四元数，vector3是一个3*1 的向量，point是一个表示一个点坐标，Pose是位姿（位姿是包括坐标以及方向） Transform是一个转换的模版
tf::Stamped <T>
是一种包含了除了Transform的其他几种基本的数据结构的一种数据结构:
template <typename T> //模版结构可以是tf::Pose tf:Point 这些基本的结构 class Stamped : public T{ public: ros::Time stamp_; //记录时间 std::string frame_id_; //ID Stamped() :frame_id_ ("NO_ID_STAMPED_DEFAULT_CONSTRUCTION"){}; //Default constructor used only for preallocation Stamped(const T& input, const ros::Time& timestamp, const std::string & frame_id); void setData(const T& input); };
tf::StampedTransform
TF::stampedtransform是TF的一种特殊情况：它需要frame_id和stamp以及child_frame_id。
/** rief 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 定义转换坐标框架的frameID std::string child_frame_id_; ///< The frame_id of the coordinate frame this transform defines 的坐标系变换定义的id 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){ }; /** rief Default constructor only to be used for preallocation */ StampedTransform() { }; /** rief Set the inherited Traonsform data */ void setData(const tf::Transform& input){*static_cast<tf::Transform*>(this) = input;}; };
举个例子
在机器人的定位领域有蒙特卡罗定位（AMCL）的算法，这个算法是根据给定的地图，结合粒子滤波获取最佳定位点Mp，这个定位点是相对于地图map上的坐标,也就是base_link（也就是机器人的基坐标）相对map上的坐标。我们知道 odom 的原点是机器人启动时刻的位置，它在map上的位置或转换矩阵是未知的。但是AMCL可以根据最佳粒子的位置推算出 odom->map（就是说通过最佳粒子推算出机器人在地图的中的位置）的tf转换信息并发布到 tf主题上。因为base_link->odom的tf转换信息是每时每刻都在发布的，所以它是已知的
，所以这里有个这样的tf关系
map->base_link（就是地图中机器人的位置 是根据最佳粒子推算的）
base_link->odom（这是现实中机器人的位姿可以说是里程计的信息）
可以理解：机器人的里程计的信息 = 当前地图中的机器人的的位置 减去 地图中机器人的起点位置。
转为公式可以写成 ：map->odom = map->base_link - base_link->odom
或者写为：
base_link->odom = map->base_link - map->odom （这样更容易理解）
提示：首先我们可以先了解关于PRY这三个概念关于pitch yaw roll的博客 http://blog.csdn.net/yuzhongchun/article/details/22749521）
pitch是围绕X轴旋转，也叫做俯仰角，
yaw是围绕Y轴旋转，也叫偏航角，
roll是围绕Z轴旋转，也叫翻滚角
1. ROS_DEBUG("New pose: %6.3f %6.3f %6.3f", 2. hyps[max_weight_hyp].pf_pose_mean.v[0], 3. hyps[max_weight_hyp].pf_pose_mean.v[1], 4. hyps[max_weight_hyp].pf_pose_mean.v[2]); 5. // hyps[max_weight_hyp].pf_pose_mean.v[0], [1], [2] 就代表了Mp 也就是机器人的位姿那么位姿的格式是（x,y,theta）最后一个是yaw偏航角， 6. // subtracting base to odom from map to base and send map to odom instead 7. tf::Stamped<tf::Pose> odom_to_map; 8. try 9. { 10. tf::Transform tmp_tf(tf::createQuaternionFromYaw(hyps[max_weight_hyp].pf_pose_mean.v[2]), tf::Vector3(hyps[max_weight_hyp].pf_pose_mean.v[0], 11. hyps[max_weight_hyp].pf_pose_mean.v[1], 12. 0.0)); 13. // tmp_tf = 从map原点看base_link的位置 为yaw生成四元数，最后的0.0是（x,y,z）的Z的值为0 因为这是在二维平面中。 14. tf::Stamped<tf::Pose> tmp_tf_stamped (tmp_tf.inverse(), 15. laser_scan->header.stamp, 16. base_frame_id_); 17. //tmp_tf.inverse() = 以为Mp为坐标的原点，地图map原点相对于Mp的位置，就是在base_link坐标系下map 原点的位置 18. this->tf_->transformPose(odom_frame_id_, 19. tmp_tf_stamped, 20. odom_to_map); 21. //进行 base_link坐标系下的点转换到odom坐标系，也就是把map原点转换到odom坐标系下，等于从odom原点看map原点的位置。放到latest_tf_变量里面 22. } 23. catch(tf::TransformException) 24. { 25. ROS_DEBUG("Failed to subtract base to odom transform"); 26. return; 27. }
TF命令行工具
（1） tf_monitor工具的功能是打印tf树中的所有参考系信息，通过输入参数来查看指定参考系之间的信息 用法： rosrun tf tf_monitor
tf_monitor <source_frame> <target_target> 监视一个特定的转换 For example, to monitor the transform from /base_footprint to /odom:
（2） tf_echo工具的功能是查看指定参考系之间的变换关系。命令的格式： tf_echo <source_frame> <target_frame>
（3）static_transform_publisher工具的功能是发布两个参考系之间的静态坐标变换，两个参考系一般不发生相对位置变化
（4）view_frames 是可视化的调试工具，可以生成pdf文件，来显示整棵tf树的信息。用法：rosrun tf view_frames
具体可以查看http://wiki.ros.org/tf/