Eigen类型与ROS中tf相关消息类型进行相互转换工具

在ROS机器人编程资料书中，有个Eigen消息类型与TF相关消息类型进行互相转换的工具包感觉挺好使，修改一下方便以后使用。

1、头文件

#include <Eigen/Eigen> 
#include <Eigen/Dense>
#include <Eigen/Geometry>
#include <Eigen/Eigenvalues>
#include <geometry_msgs/TransformStamped.h>
#include <math.h>
#include <stdlib.h>
#include <string>
#include <vector>
#include <ros/ros.h> 
#include <geometry_msgs/Twist.h>
#include <geometry_msgs/TwistStamped.h>
#include <geometry_msgs/Point.h>
#include <geometry_msgs/PointStamped.h>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/PoseWithCovarianceStamped.h>
#include <nav_msgs/Odometry.h>
#include <std_msgs/Float64.h>
 #include <tf/transform_listener.h>
#include <tf/LinearMath/Vector3.h>
#include <tf/LinearMath/QuadWord.h>
#include <tf/transform_broadcaster.h>
class TfEigenUtils {
public:
   Eigen::Affine3f tfTransformToEigenAffine3f(const tf::Transform &t);
   Eigen::Affine3d tfTransformToEigenAffine3d(const tf::Transform &t);
   double geometrymsgsQuaternionToPhi(geometry_msgs::Quaternion quaternion);
   geometry_msgs::Quaternion psiToGeometrymsgsQuaternion(double psi);
   tf::Transform tfStampedTransformTotfTransform(tf::StampedTransform sTf);
   geometry_msgs::PoseStamped tfStampedTransformToGeometrymsgsPoseStamped(tf::StampedTransform sTf);
   bool multiplyStampedTfs(tf::StampedTransform A_stf,tf::StampedTransform B_stf, tf::StampedTransform &C_stf);
   tf::StampedTransform stampedTransformInverse(tf::StampedTransform sTf);
   geometry_msgs::Pose eigenAffine3dToGeometrymsgsPose(Eigen::Affine3d e);
   geometry_msgs::PoseStamped eigenAffine3dToGeometrymsgsPoseStamped(Eigen::Affine3d e);
   geometry_msgs::PoseStamped eigenAffine3dToGeometrymsgsPoseStamped(Eigen::Affine3d e,std::string reference_frame_id);   
   Eigen::Affine3d geometrymsgsPoseToEigenAffine3d(geometry_msgs::Pose pose);
   Eigen::Affine3d geometrymsgsPoseStampedToEigenAffine3d(geometry_msgs::PoseStamped stPose); 
   Eigen::Affine3d tfStampedTransformToEigenAffine3d(tf::StampedTransform sTf);   
   tf::StampedTransform geometrymsgsPoseStampedToTfStampedTransform(geometry_msgs::PoseStamped stPose, std::string child_frame_id);  
   geometry_msgs::PoseStamped navmsgsOdometryToGeometrymsgsPoseStamped(nav_msgs::Odometry odom);
};

2、函数的实现

Eigen::Affine3f TfEigenUtils::tfTransformToEigenAffine3f(const tf::Transform &t) {
    Eigen::Affine3f e;
    for (int i = 0; i < 3; i++) {
        e.matrix()(i, 3) = t.getOrigin()[i]; 
        for (int j = 0; j < 3; j++) {
            e.matrix()(i, j) = t.getBasis()[i][j]; 
        }
    }
    // Fill in identity in last row
    for (int col = 0; col < 3; col++)
        e.matrix()(3, col) = 0;
    e.matrix()(3, 3) = 1;
    return e;
}

Eigen::Affine3d TfEigenUtils::tfTransformToEigenAffine3d(const tf::Transform &t) {
    Eigen::Affine3d e;
    for (int i = 0; i < 3; i++) {
        e.matrix()(i, 3) = t.getOrigin()[i]; 
        for (int j = 0; j < 3; j++) {
            e.matrix()(i, j) = t.getBasis()[i][j]; 
        }
    }
    // Fill in identity in last row
    for (int col = 0; col < 3; col++)
        e.matrix()(3, col) = 0;
    e.matrix()(3, 3) = 1;
    return e;
}

double TfEigenUtils::geometrymsgsQuaternionToPhi(geometry_msgs::Quaternion quaternion) {
    double quat_z = quaternion.z;
    double quat_w = quaternion.w;
    double phi = 2.0 * atan2(quat_z, quat_w); motion
    return phi;
}

geometry_msgs::Quaternion  TfEigenUtils::psiToGeometrymsgsQuaternion(double psi) {
    geometry_msgs::Quaternion quaternion;
    quaternion.x = 0.0;
    quaternion.y = 0.0;
    quaternion.z = sin(psi / 2.0);
    quaternion.w = cos(psi / 2.0);
    return (quaternion);
}

tf::Transform TfEigenUtils::tfStampedTransformTotfTransform(tf::StampedTransform sTf) {
    tf::Transform tf(sTf.getBasis(), sTf.getOrigin()); 
    return tf;
}

//tf::StampedTransform 到geometry_msgs::PoseStamped
geometry_msgs::PoseStamped TfEigenUtils::tfStampedTransformToGeometrymsgsPoseStamped(tf::StampedTransform tf) {
    
    geometry_msgs::PoseStamped stPose;
    geometry_msgs::Quaternion quat; 
    tf::Quaternion tfQuat; 
    tfQuat = tf.getRotation(); 
    quat.x = tfQuat.x(); 
    quat.y = tfQuat.y();
    quat.z = tfQuat.z();
    quat.w = tfQuat.w();
    stPose.pose.orientation = quat; 
    
    tf::Vector3 tfVec; 
    geometry_msgs::Point pt; 
    tfVec = tf.getOrigin(); 
    pt.x = tfVec.getX(); 
    pt.y = tfVec.getY();
    pt.z = tfVec.getZ();
    stPose.pose.position = pt; 
    stPose.header.frame_id = tf.frame_id_; //坐标系
    stPose.header.stamp = tf.stamp_; //保留原始转换的时间戳
    return stPose;
}

//compute C_stf = A_stf*B_stf; 
bool TfEigenUtils::multiplyStampedTfs(tf::StampedTransform A_stf,
        tf::StampedTransform B_stf, tf::StampedTransform &C_stf) {
    if (A_stf.child_frame_id_.compare(B_stf.frame_id_) != 0) {    
        std::cout << "can't multiply transforms; mismatched frames" << endl;
	std::cout << A_stf.child_frame_id_ << " is not " << B_stf.frame_id_ << '\n';
        return false;
    }
    //坐标系原则上是一样的
        tf::Transform C = A_stf*B_stf; //multiplication is defined for transforms
	C_stf.setData(C);
	C_stf.frame_id_ = A_stf.frame_id_;
	C_stf.child_frame_id_ = B_stf.child_frame_id_;
	C_stf.stamp_ = ros::Time::now();
    return true; //if got here, the multiplication is valid
}

//tf::StampedTransform到tf::StampedTransform 
tf::StampedTransform TfEigenUtils::stampedTransformInverse(tf::StampedTransform stf) {
//使用构造函数参数实例化带戳记的转换
//注意:child_frame和frame_id是反转的，对应于反转变换
    tf::StampedTransform stf_inv(stf.inverse(), stf.stamp_, stf.child_frame_id_, stf.frame_id_);
    return stf_inv;
}

//Eigen::Affine3d到geometry_msgs::Pose
geometry_msgs::Pose TfEigenUtils::eigenAffine3dToGeometrymsgsPose(Eigen::Affine3d e) {
    Eigen::Vector3d Oe;
    Eigen::Matrix3d Re;
    geometry_msgs::Pose pose;
    Oe = e.translation();
    Re = e.linear();
    Eigen::Quaterniond q(Re); // 将旋转矩阵Re转换为四元数q
    pose.position.x = Oe(0);
    pose.position.y = Oe(1);
    pose.position.z = Oe(2);
    pose.orientation.x = q.x();
    pose.orientation.y = q.y();
    pose.orientation.z = q.z();
    pose.orientation.w = q.w();
    return pose;
}

//Eigen::Affine3d  到 geometry_msgs::PoseStamped 
geometry_msgs::PoseStamped TfEigenUtils::eigenAffine3dToGeometrymsgsPoseStamped(Eigen::Affine3d e) {
    geometry_msgs::Pose pose;
    geometry_msgs::PoseStamped poseStamped;
    pose = eigenAffine3dToGeometrymsgsPose(e);
    poseStamped.pose = pose;
    poseStamped.header.stamp = ros::Time::now();
    return poseStamped;
}

//Eigen::Affine3d e到geometry_msgs::PoseStamped 
geometry_msgs::PoseStamped TfEigenUtils::eigenAffine3dToGeometrymsgsPoseStamped(Eigen::Affine3d e,std::string frame_id) {
  geometry_msgs::PoseStamped poseStamped;
  poseStamped = eigenAffine3dToGeometrymsgsPose(e);
  poseStamped.header.frame_id = frame_id;
  return poseStamped;
} 

//geometry_msgs::PoseStamped到Eigen::Affine3d 
Eigen::Affine3d TfEigenUtils::geometrymsgsPoseStampedToEigenAffine3d(geometry_msgs::PoseStamped stPose) {
    Eigen::Affine3d affine;
    geometry_msgs::Pose pose = stPose.pose;
    Eigen::Vector3d Oe;
    Oe(0) = pose.position.x;
    Oe(1) = pose.position.y;
    Oe(2) = pose.position.z;
    affine.translation() = Oe;
    Eigen::Quaterniond q;
    q.x() = pose.orientation.x;
    q.y() = pose.orientation.y;
    q.z() = pose.orientation.z;
    q.w() = pose.orientation.w;
    Eigen::Matrix3d Re(q);
    affine.linear() = Re;
    affine.translation() = Oe;
    return affine;
}
//geometry_msgs::Pose 到 Eigen::Affine3d 
Eigen::Affine3d TfEigenUtils::geometrymsgsPoseToEigenAffine3d(geometry_msgs::Pose pose) {
    Eigen::Affine3d affine;
    Eigen::Vector3d Oe;
    Oe(0) = pose.position.x;
    Oe(1) = pose.position.y;
    Oe(2) = pose.position.z;
    affine.translation() = Oe;
    Eigen::Quaterniond q;
    q.x() = pose.orientation.x;
    q.y() = pose.orientation.y;
    q.z() = pose.orientation.z;
    q.w() = pose.orientation.w;
    Eigen::Matrix3d Re(q);
    affine.linear() = Re;
    affine.translation() = Oe;
    return affine;
}

//StampedTransform 到  Eigen::Affine3d
   Eigen::Affine3d TfEigenUtils::tfStampedTransformToEigenAffine3d(tf::StampedTransform sTf) {
       tf::Transform transform = get_tf_from_stamped_tf(sTf);
       Eigen::Affine3d affine = transformTFToAffine3d(transform);
       return affine;
   } 

tf::StampedTransform TfEigenUtils::geometrymsgsPoseStampedToTfStampedTransform(geometry_msgs::PoseStamped stPose, std::string child_frame_id) {
 tf::Transform transform;
 geometry_msgs::Pose pose = stPose.pose;

 geometry_msgs::Point position = pose.position;
 geometry_msgs::Quaternion orientation = pose.orientation;
 transform.setOrigin( tf::Vector3(position.x, position.y, position.z) );
 transform.setRotation( tf::Quaternion( orientation.x, orientation.y, orientation.z, orientation.w) );
 tf::StampedTransform stTransform(transform, stPose.header.stamp, stPose.header.frame_id,child_frame_id);
 return stTransform;
}

geometry_msgs::PoseStamped TfEigenUtils::navmsgsOdometryToGeometrymsgsPoseStamped(nav_msgs::Odometry odom)
{
    geometry_msgs::PoseStamped stPose;
    stPose.pose = odom.pose.pose;
    stPose.header = odom.header;
    return stPose;
}

参考ROS机器人编程原理与应用