ROS TF坐标变换

在ROS下开发自动驾驶系统，第一步遇到的问题就是数据预处理，包括时间同步、坐标变换，而在ROS中应用坐标变换就会用到TF库

一. 监听TF

通过监听tf,我们可以避免繁琐的旋转矩阵计算，而直接获取我们需要的相关信息。在监听中最常用的又两个函数

(1）lookupTransform（）

（2）transformPoint（）； 

（一）先来看lookupTransform（），

函数功能：可以获得两个坐标系之间转换的关系，包括旋转和平移。主要步骤有

 1. 定义监听器   tf::TransformListener listener

2. 定义存放变换关系的变量 tf::StampedTransform transform;

(3) 监听两个坐标系之间的变换，实现base_link到map的转换
 

一.  tf::StampedTransform <-------------> Eigen::Matrix4f

tf::StampedTransform是ros里代表TF变换的数据类型,一般在机器人上读取雷达坐标系到机器人坐标系的信息就是通过读取tf信息.

tf::StampedTransform transform; //雷达到机器人的静态变换，transform为StampedTransform类型实例

而transform数据在坐标变换中无法直接使用，需要转换为另一种位姿(也就是变换矩阵)的表示方式, 即Eigen::Matrix4f数据类型，表示4*4的变换矩阵,包括平移和旋转.

 Eigen::Matrix4f tf_btol;

以下是这两种数据格式之间的转换:

由transform获取位移矩阵:Eigen::Translation3f  (3 * 3)旋转通过以下形式从TF获得:

Eigen::Translation3f tl_btol(transform.getOrigin().getX(), transform.getOrigin().getY(), transform.getOrigin().getZ());
Eigen::AngleAxisf rot_x_btol(roll, Eigen::Vector3f::UnitX());   //轴角表达形式
Eigen::AngleAxisf rot_y_btol(pitch, Eigen::Vector3f::UnitY());
Eigen::AngleAxisf rot_z_btol(yaw, Eigen::Vector3f::UnitZ());
tf_btol = (tl_btol * rot_z_btol * rot_y_btol * rot_x_btol).matrix(); //雷达到机器人的静态变换矩阵   平移*旋转

这样就从TF变换转换到了变换矩阵.

二. Eigen::Matrix4f<-------------> pose

一般我们通过点云匹配或者里程计产生的都是运动表达形式，都是变换矩阵(Eigen::Matrix4f )形式，但是我们在实际使用中更多是直观的使用 平移 x, y, z和旋转 roll, pitch, yaw  这些直接数据.所以要将得到的变换矩阵转换成 自定义的pose格式,也就是将 x ,y ,z roll, pitch, yaw 信息提取出来.

一般在程序中我们自己定义一个相对直观的位姿表示结构体:

struct pose

{

double x, y, z;

double pitch, roll, yaw;

...... //一些其他成员函数

}

pose localizer_pose;  //定义一个 pose结构体实例
Eigen::Matrix4f t_localizer(Eigen::Matrix4f::Identity());  //ndt定位产生的变换矩阵      4*4
 
tf::Matrix3x3 mat_l,; // 旋转变换矩阵,作为转换的中间量,  来求rpy转换角度
mat_l.setValue(
static_cast<double>(t_localizer(0, 0)), static_cast<double>(t_localizer(0, 1)),static_cast<double>(t_localizer(0, 2)),
static_cast<double>(t_localizer(1, 0)),static_cast<double>(t_localizer(1, 1)), static_cast<double>(t_localizer(1, 2)),
static_cast<double>(t_localizer(2, 0)), static_cast<double>(t_localizer(2, 1)),static_cast<double>(t_localizer(2, 2))
);
// Update localizer_pose. // 更新局部下的坐标
localizer_pose.x = t_localizer(0, 3);
localizer_pose.y = t_localizer(1, 3);
localizer_pose.z = t_localizer(2, 3);
mat_l.getRPY(localizer_pose.roll, localizer_pose.pitch, localizer_pose.yaw, 1);

以上就将Eigen::Matrix4f 的变换矩阵 转换成了 pose,我们可以从pose直接获取平移 x, y, z和旋转 roll, pitch, yaw 

三. 将位姿通过 tf::TransformBroadcaster 发送出去

我们一般直接获取到的转换形式是上文的 变换矩阵形式(Eigen::Matrix4f),首先我们要将变换矩阵根据上文转换成 Pose格式,然后再将Pose通过一下步骤转换为 tf::StampedTransform:

pose current_pose;  //得到的Pose
tf::StampedTransform transform_broadcaster
transform_broadcaster.setOrigin( tf::Vector3(current_pose.x, current_pose.y, current_pose.z) );    //设置平移
tf::Quaternion q;
q.setRPY(current_pose.roll, current_pose.pitch, current_pose.yaw);  //四元数 自动从欧拉角获得 旋转量
transform_broadcaster.setRotation(q);    //TF 的transform 必须从四元数获取旋转信息