admin 管理员组

文章数量: 888297

tf变换及工具说明（1）

tf变换及工具说明（1））

• tf说明解释
• tf 库常用方法
• • tf数据类型
  • 静态tf发布
  • tf动态发布
  • tf接收
  • 坐标转换
  • • 平面坐标系例子
  • 坐标转换传递

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上的坐标
   W p = W A T ∗ A p Wp = WAT*Ap Wp=WAT∗Ap

坐标转换

转换信息是描述两个坐标系的相互关系，采用一个6自由度的相对位姿表示：平移量(translation)和旋转量(rotation)
平移量(translation)是一个向量，用 W t A W_{tA} WtA​。在ROS中用tf::Vector3来表示，包括 ( x , y , z ) (x,y,z) (x,y,z);
旋转量(rotation)用一个旋转量矩阵(rotation matrices)来表示,符号为 W A R W_{A^R} WAR​，同样为３维向量，表示为 ( p i t c h , r o l l , y a w ) (pitch,roll,yaw) (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 W_{A^T} WAT​，其表示形式为
W A R W t A 0 1 \begin{matrix} W_{A^R} & W_{tA} \\ 0 & 1 \end{matrix} WAR​0​WtA​1​

平面坐标系例子

在平面坐标下，如已知A点在W下在坐标为 W p （ x ， y ， θ ） W_p（x，y，\theta） Wp​（x，y，θ）即所谓的 W A T W_{A^T} WAT​（即transform）。假设A_p点在A坐标下坐标为（x_a,y_b）;则 A p A_p Ap​点在W下的坐标为
1.采用公式直接转换
W A T × A p = W p W_{A^T}×A_p=W_p WAT​×Ap​=Wp​
[ x ′ y ′ 1 ] = [ c o s ( θ ) − s i n ( θ ) x s i n ( θ ) c o s ( θ ) ) y 0 0 1 ] ∗ [ x a y a 1 ] \left[ \begin{matrix} x'\\ y'\\ 1 \end{matrix} \right] = \left[ \begin{matrix} cos(\theta) & -sin(\theta) & x\\ sin(\theta) & cos(\theta)) & y\\ 0& 0 & 1 \end{matrix} \right] *\left[ \begin{matrix} x_a\\ y_a\\ 1\\ \end{matrix} \right] ⎣⎡​x′y′1​⎦⎤​=⎣⎡​cos(θ)sin(θ)0​−sin(θ)cos(θ))0​xy1​⎦⎤​∗⎣⎡​xa​ya​1​⎦⎤​
x w = x + x a ∗ c o s ( θ ) − y a ∗ s i n ( θ ) x_w = x + x_a*cos(\theta) - y_a*sin(\theta) xw​=x+xa​∗cos(θ)−ya​∗sin(θ)
y w = y + y a ∗ c o s ( θ ) + x a ∗ s i n ( θ ) y_w = y + y_a* cos(\theta) + x_a*sin(\theta) yw​=y+ya​∗cos(θ)+xa​∗sin(θ)

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(); 

本文标签： tf变换及工具说明（1）