几种位姿转换表示方式----tf::StampedTransform & Eigen::Matrix4f & Pose, 及其相互转换

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

 

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

tf::StampedTransform transform; //雷达到机器人的静态变换 tf

然后就是另一种位姿(也就是变换矩阵)的表示方式, Eigen::Matrix4f是4*4的变换矩阵,包括平移和旋转.

 Eigen::Matrix4f tf_btol;

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

由transform获取位移矩阵:Eigen::Translation3f  (3 * 3)

Eigen::Translation3f tl_btol(transform.getOrigin().getX(), transform.getOrigin().getY(), transform.getOrigin().getZ());

旋转通过以下形式从TF获得:

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变换转换到了变换矩阵.

下面是完整的程序:

tf::TransformBroadcaster tf_broadcaster;
tf::TransformListener tf_listener;
tf::StampedTransform transform;  //雷达到机器人的静态变换 tf

    try
    {
        ros::Time now = ros::Time::now();
        ROS_INFO("now:%f listen from static_tf and set tf_btol");
        tf_listener.waitForTransform(param_base_frame, param_laser_frame, ros::Time(0), ros::Duration(param_tf_timeout * 10), ros::Duration(param_tf_timeout / 3));
        tf_listener.lookupTransform(param_base_frame, param_laser_frame, ros::Time(0), transform);
        ROS_INFO("success listen from tf");
    }
    catch (const tf::TransformException &ex)
    {
        ROS_ERROR("Error waiting for tf in init: %s", ex.what());
        return;
    }
    Eigen::Translation3f tl_btol(transform.getOrigin().getX(), transform.getOrigin().getY(), transform.getOrigin().getZ());
    double roll, pitch, yaw;
    tf::Matrix3x3(transform.getRotation()).getEulerYPR(yaw, pitch, roll);
    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_ltob = tf_btol.inverse();  //机器人到雷达的静态变换矩阵

 

二. 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 必须从四元数获取旋转信息

 //四元数 -->> 欧拉角
tf::Quaternion quat;
tf::quaternionMsgToTF(odom.pose.pose.orientation, quat);
double roll, pitch, yaw;   //定义存储r\p\y的容器
tf::Matrix3x3(quat).getRPY(roll, pitch, yaw);//进行转换