几种位姿的表示方式的转换

一. tf::StampedTransform to Eigen::Matrix4f

tf::StampedTransform 是ROS上TF变换的格式，Eigen::Matrix4f 是代码中用来矩阵运算的常用方法。下面这段代码直接抄的，数学推导没有不知道为啥，才疏学浅。

/** param transfrom 就是監聽到的tf::StampedTransform
 ** param transform_matrix 返回的Eigen::Matrix4f
*/
bool TFListener::TransformToMatrix(const tf::StampedTransform& transform, Eigen::Matrix4f& transform_matrix) {
   
    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());

    // 此矩阵为 child_frame_id 到 base_frame_id 的转换矩阵
    transform_matrix = (tl_btol * rot_z_btol * rot_y_btol