tf变换及工具说明（1）

tf说明解释

在ros下有一个库：tf，其主要功能是维护整个系统中所有坐标系，形成一个完整的坐标转换树。利用tf库，可以描述一个物体相对于另一个物体坐标系的坐标转换，从而形成单链的描述。而将整个系统中所有物体均链接此树后，则可以得到任何两个坐标的相互关系；如下图所示。
已知如下关系：
laser->baselink
depth->baselink
baselink->odom
odom->map
gps->map
如此当利用tf进行管理时，laser、baselink、depth、odom、map、gps任意之间均可获取相互转换关系。
需要注意的是，一个系统中必须仅有一个tf树，即所有坐标系都应在此坐标树中，否则无法使用；

tf 库常用方法

tf数据类型

基本的数据类型有（Quaternion, Vector, Point, Pose, Transform）；

• Quaternion 是表示四元数(表示旋转变换)；
• vector3是一个3*1 的向量；
• point是一个表示一个点坐标（x,y,z）；
• Pose是位姿（x,y,z,pitch,roll,yaw）；
• Transform是一个转换的模版；

tf::Stamped 为tf变换中使用的数据类型，其中T为上述除了Transform其他类型；tf::Stamped 主要根据监听的tf进行转标转换的类型；

静态tf发布

1. tf::TransformBroadcaster br;
   创建一个TransformBroadcaster对象，是用来发布tf变换树;其类说明如下：

namespace tf
{
class TransformBroadcaster{
public:

  TransformBroadcaster();
  void sendTransform(const StampedTransform & transform);
  void sendTransform(const std::vector<StampedTransform> & transforms);
  void sendTransform(const geometry_msgs::TransformStamped & transform);
  void sendTransform(const std::vector<geometry_msgs::TransformStamped> & transforms);
private:
  tf2_ros::TransformBroadcaster tf2_broadcaster_;
};
}

2. tf::Transform transform;
   创建一个Transform对象,用于描述两个坐标系之间的转化关系，即两个点的相对坐标;

 transform.setOrigin( tf::Vector3(2.0*sin(ros::Time::now().toSec()), 2.0*cos(ros::Time::now().toSec()), 0.0) );
 transform.setRotation( tf::Quaternion(0, 0, 0, 1) );

Transform对象内包含两种信息，即平移和旋转量。其中旋转量采用4元数表示；

3. tf::StampedTransform
   发布tf变换树的类型，一个变换树包含 变换、时间戳、父坐标系frame_id、子坐标系frame_id；
   tf::StampedTransform(transform, ros::Time::now(), “turtle1”, “carrot1”)

例子：

static tf::TransformBroadcaster br;
  tf::Transform transform;
  transform.setOrigin( tf::Vector3(msg->x, msg->y, 0.0) );
  tf::Quaternion q;
  q.setRPY(0, 0, msg->theta);
  transform.setRotation(q);
  br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "world", turtle_name));

tf动态发布

geometry_msgs::TransformStamped transformStamped;
//坐标转换广播
  tf::TransformBroadcaster broadcaster;
 
  //初始转换参数
  transformStamped.header.frame_id = "origin_base";
  transformStamped.child_frame_id = "trans_base";
  transformStamped.transform.translation.x = 0;
  transformStamped.transform.translation.y = 0;
  transformStamped.transform.translation.z = 0;
  transformStamped.transform.rotation.x = 0;
  transformStamped.transform.rotation.y = 0;
  transformStamped.transform.rotation.z = 0;
  transformStamped.transform.rotation.w = 1;
  broadcaster.sendTransform(transformStamped);

tf接收

1. 创建tf监听器

tf::TransformListener listener(ros::Duration(10))；   // 缓存10s内的变换
tf::TransformListener listener；                                // 缓存默认是时间数据

2. 获取目标的两个坐标系之间坐标转换
   结合waitForTransform()使用,可以等待某时刻的tf变换数据，防止lookupTransform获取失败；因为可在duration时间内进行等待，直到获取的transform是可用的；

tf::StampedTransform transform;
//
listener.waitForTransform("world" ,"base_link"，  ros::Time(0)，ros::Duration(3.0));     
// transform则为获取当前base_link在world坐标系下的坐标，即为从“world”到“baselink”下的转换
// 其中ros::Time(0)表示获取最近时刻的转换参数，因为无法实时，故不能修改为 ros::Time::now()；
listener.lookupTransform("world" ,"base_link"  ros::Time(0), transform);     

3. 已知当前f坐标系下坐标获取在任意其他坐标系下的坐标
   概念：
   第2节中说的转标转换的解释如下简称：WAT；
   指的是从W的原点看A所在的位置坐标 p，就是A->W转换矩阵，称为WAT；其实也可以认为是A在W坐标系下的坐标或者A坐标系到W坐标系下的坐标转换。假设某一点 A_p为A坐标系的某一点， 它在W上的坐标
   $$Wp=WAT\ast Ap$$

坐标转换

转换信息是描述两个坐标系的相互关系，采用一个6自由度的相对位姿表示：平移量(translation)和旋转量(rotation)
平移量(translation)是一个向量，用$W_{tA}$。在ROS中用tf::Vector3来表示，包括$(x,y,z)$;
旋转量(rotation)用一个旋转量矩阵(rotation matrices)来表示,符号为$W_{A^R}$，同样为３维向量，表示为$(pitch,roll,yaw)$。　但是在ros的tf变换中采用四元素表示tf::Quaternion；
但是TF库中可以将4元数和旋转矩阵进行转换；

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)

如果转换信息可用4*4矩阵表示，我们称为$W_{A^T}$，其表示形式为
$$\begin{array}{cc}{W}_{A^R}& W_{tA}\\ 0& 1\end{array}$$

平面坐标系例子

在平面坐标下，如已知A点在W下在坐标为$W_p（x，y，\theta ）$即所谓的$W_{A^T}$（即transform）。假设A_p点在A坐标下坐标为（x_a,y_b）;则$A_p$点在W下的坐标为
1.采用公式直接转换
$$W_{A^T}\times A_p=W_p$$
$$\left[\begin{array}{c}{x}^{\prime }\\ y^{\prime }\\ 1\end{array}\right]=\left[\begin{array}{ccc}cos(\theta )& -sin(\theta )& x\\ sin(\theta )& cos(\theta ))& y\\ 0& 0& 1\end{array}\right]\ast \left[\begin{array}{c}{x}_a\\ y_a\\ 1\end{array}\right]$$
$$x_w=x+x_a\ast cos(\theta )-y_a\ast sin(\theta )$$
$$y_w=y+y_a\ast cos(\theta )+x_a\ast sin(\theta )$$

2. 采用eigen库进行矩形运算

Eigen::Affine3d W_to_A_eig;                                  // 仿射变换矩阵
 tf::Transform W_to_A;
 tf::transformTFToEigen(W_to_A, W_to_A_eig);    // 转换eigen矩阵格式
 Eigen::Vector3d pos(x_a,y_a,z_a); 
 Eigen::Vector3d pos = W_to_A_eig * pos;

3. 采用tf::tranform对基本类型转换库

// 对坐标点进行转换
void TransformListener::transformPoint	(	const std::string & 	target_frame,
const geometry_msgs::PointStamped & 	stamped_in,
geometry_msgs::PointStamped & 	stamped_out	 
)	
// 对点云坐标转换
void TransformListener::transformPointCloud	(	const std::string & 	target_frame,
const sensor_msgs::PointCloud & 	pcin,
sensor_msgs::PointCloud & 	pcout	 
)	

// 对pose进行转换，包含 point和Quaternion
void TransformListener::transformPose	(	const std::string & 	target_frame,
const geometry_msgs::PoseStamped & 	stamped_in,
geometry_msgs::PoseStamped & 	stamped_out	 
)	

// 对方向进行转换
void TransformListener::transformQuaternion	(	const std::string & 	target_frame,
const geometry_msgs::QuaternionStamped & 	stamped_in,
geometry_msgs::QuaternionStamped & 	stamped_out	 
)	

// 对坐标向量进行转换，同样为point，仅类型为vector
void TransformListener::transformVector	(	const std::string & 	target_frame,
const geometry_msgs::Vector3Stamped & 	stamped_in,
geometry_msgs::Vector3Stamped & 	stamped_out	 
)	

坐标转换传递

例如：若已知map到baselink的坐标转换，同时已知里程计到baselink下的坐标转换，则可获取odom到map下的坐标转换。

 tf::Transform map_to_base;      // 已知baselink在 map下的坐标
 tf::Transform odom_to_base;    // 已知baselink在odom下的坐标
 odom_to_map_ = odom_to_base * map_to_base.inverse();