TF坐标变换学习整理

1、什么是tf变换

       以机器人为例，定义两个坐标系，一个坐标系以机器人移动平台的中心为原点，称为base_link参考系，另一个坐标系以激光雷达的中心为原点，称为base_laser参考系。

      tf变换树定义了不同坐标系之间的平移与旋转变换关系，tf功能包提供了存储、计算不同数据在不同参考系之间变换的功能，因此只需要告诉tf树这些参考系之间的变换公式即可。

// 初始化tf数据
tf::Transform transform;
transform.setOrigin( tf::Vector3(0.1, 0.0, 0.2) );
transform.setRotation( tf::Quaternion(0,0,0,1) );

tf::Transform transform;  定义存放转换信息（平动，转动的变量)    setOrigin();设置坐标原点

setRotation是设置这个子坐标系与父坐标系的角度关系，前三个参数为pitch,row,yaw，最后一个参数为角速度。

q=tf.Quaternion()  四元数表示

q.setRPY(Rtheta,Ptheta,Ytheta);//根据已经知道的欧拉角进行设置q，函数的参数为小车base_link在map坐标系下的roll(绕x轴），pitch(绕y轴），yaw(绕z轴），

 

tf::StampedTransform stamped_transform; //定义存放变换关系的变量,  这种类型是由tf::Transform作为父类继承而来的,可以获得信息：

得到baselink坐标系的原点在map下的坐标，和旋转的四元数，

transform.getOrigin().x()
transform.getOrigin().y()
 
transform.getRotation().getW();
transform.getRotation().getX();
transform.getRotation().getY();
transform.getRotation().getZ();

tf_.transformPose(）

bool Costmap2DROS::getRobotPose(tf::Stamped<tf::Pose>& global_pose) const
{
  global_pose.setIdentity();
  tf::Stamped < tf::Pose > robot_pose;                      //定义一个位姿变量用来存储坐标值
  robot_pose.setIdentity();                                  //位姿各成员值初始化为0
  robot_pose.frame_id_ = robot_base_frame_;                //定义需要获得在那个坐标系下的坐标
  robot_pose.stamp_ = ros::Time();
                                                        // get the global pose of the robot
  tf_.transformPose(global_frame_, robot_pose, global_pose);//获取global_pose在robot_base_frame_坐标系下的坐标
}

transformPoint()函数的使用

在实际应用中我们肯定会需要在一个坐标系下的点转换到另一个坐标系下，这就需要transformPoint()函数。

listener_.transformPoint("map",laser_pose,map_pose)；

（1）其中laser_pose,world_pose的数据类型都是 geometry_msgs::PointStamped， 需要定义laser_pose.header.frame.id即该点所属的坐标系（比如激光坐标系）

（2）”map“是指，我要将aser_pose转换到map坐标系下，map_pose是转换的结果。

geometry_msgs::PointStamped turtle1;
turtle1.header.stamp=ros::Time();
turtle1.header.frame_id="turtle1";
turtle1.point.x=1;
turtle1.point.y=2;
turtle1.point.z=3;
geometry_msgs::PointStamped turtle1_world;
        try{
listener_.transformPoint("PTAM_world",turtle1,turtle1_world);
         }
             catch (tf::TransformException &ex) {
          ROS_ERROR("%s",ex.what());
          ros::Duration(1.0).sleep();
    }