【Eigen和tf中位姿表达方式转换】【tf::StampedTransform】【Eigen::Matrix4f】0 前言1 tf::StampedTransform<——>Eigen::Matrix4f2 tf::Quaternion和geometry_msgs/Quaternion的变换，变出roll, pitch, yawN 参考

118 阅读 0 评论 78 点赞

我是靠谱客的博主笑点低书本，这篇文章主要介绍【Eigen和tf中位姿表达方式转换】【tf::StampedTransform】【Eigen::Matrix4f】0 前言1 tf::StampedTransform<——>Eigen::Matrix4f2 tf::Quaternion和geometry_msgs/Quaternion的变换，变出roll, pitch, yawN 参考，现在分享给大家，希望可以做个参考。

【tf的使用】【Eigen和tf中位姿表达方式转换】【tf::StampedTransform】【Eigen::Matrix4f】

0 前言
1 tf::StampedTransform<——>Eigen::Matrix4f
- 1.1 实现代码
2 tf::Quaternion和geometry_msgs/Quaternion的变换，变出roll, pitch, yaw
N 参考

0 前言

1 tf::StampedTransform<——>Eigen::Matrix4f

tf::StampedTransform：是ros里代表TF变换的数据类型,一般在机器人上读取雷达坐标系到机器人坐标系的信息就是通过读取tf信息.
Eigen::Matrix4f：是变换矩阵的位姿格式，4*4的矩阵，包括平移和旋转.

1.1 实现代码

函数声明

复制代码

1
2
bool TransformToMatrix(const tf::StampedTransform& transform, Eigen::Matrix4f& transform_matrix);

函数实现

复制代码

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
// transform是 child_frame 到 base_frame 的静态变换 tf
//  Eigen::Matrix4f 是4*4的变换矩阵,包括平移和旋转.

bool TFListener::TransformToMatrix(const tf::StampedTransform& transform, Eigen::Matrix4f& transform_matrix) {
    //由transform获取位移矩阵 Eigen::Translation3f  (3 * 3)
    Eigen::Translation3f tl_btol(transform.getOrigin().getX(), transform.getOrigin().getY(), transform.getOrigin().getZ());
    // 旋转通过以下形式从TF获得:
    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());

    // 此矩阵为 child_frame 到 base_frame 的转换矩阵
    // child_frame 到 base_frame 的静态变换矩阵   平移*旋转 
    transform_matrix = (tl_btol * rot_z_btol * rot_y_btol * rot_x_btol).matrix();

    return true;
}

2 tf::Quaternion和geometry_msgs/Quaternion的变换，变出roll, pitch, yaw

复制代码

1
2
3
4
5
const sensor_msgs::Imu::ConstPtr& imuIn
        tf::Quaternion orientation;
        tf::quaternionMsgToTF(imuIn->orientation, orientation);
        tf::Matrix3x3(orientation).getRPY(roll, pitch, yaw);