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

0 前言

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

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

1.1 实现代码

• 函数声明

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

• 函数实现

// 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

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

N 参考

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