Eigen知识点2——获取gps坐标，并将其转换为eigen矩阵----poseMsgToEigen解析

#include"ros/ros.h"
#include"std_msgs/String.h"

#include<pcl/common/transforms.h>
#include"nav_msgs/Odometry.h"
#include <tf/tf.h>
#include <tf_conversions/tf_eigen.h>
#include<Eigen/Eigen>
// 可用于geometry_msgs与Eigen相关数据类型之间的转换。
#include <eigen_conversions/eigen_msg.h>
#include<pcl/visualization/pcl_visualizer.h>
#include<pcl/point_cloud.h>

// 包含pcd的输入输出头文件——pcd文件IO操作定义的头文件
#include <pcl/io/pcd_io.h>
// 包含点类型、结构的数据定义——包含若干PointT数据结构定义的头文件
#include <pcl/point_types.h>
using namespace std;
    // ConstPtr常量指针可以修改指针的方向，不可以修改指针的数值
void doMsg(const nav_msgs::Odometry::ConstPtr &gps_msg)
{
    // 通过msg获取并操作订阅到的数据

    // ROS_INFO("翠花订阅的数据：%s",msg->data.c_str());
    Eigen::Affine3d gps_aff;
    cout<<"gps_aff="<<gps_aff.matrix()<<endl;
    // 将获取的gps坐标转换为4维的矩阵
    tf::poseMsgToEigen(gps_msg->pose.pose,gps_aff);
    cout<<gps_aff.matrix()<<endl;
    // 强制类型转换
    Eigen::Matrix4d gps_matrix=gps_aff.matrix().cast<double>();;
    cout<<"gps_matrix="<<endl;
    cout<<gps_matrix.matrix()<<endl;
   // 定义四元素，用四元素表示地理到载体坐标系
     Eigen::Quaterniond q;
     q.x()=gps_msg->pose.pose.orientation.x;
     q.y()=gps_msg->pose.pose.orientation.y;
     q.z()=gps_msg->pose.pose.orientation.z;
     q.w()=gps_msg->pose.pose.orientation.w;
    Eigen::Matrix3d A;
    A(0,0)=2*q.w()*q.w()+2*pow(q.x(),2)-1;
    A(0,1)=2*q.x()*q.y()+2*q.w()*q.z();
    A(0,2)=2*q.x()*q.z()-2*q.w()*q.y();
    A(1,0)=2*q.x()*q.y()-2*q.w()*q.z();
    A(1,1)=2*q.w()*q.w()+2*q.y()*q.y()-1;
    A(1,2)=2*q.y()*q.z()+2*q.w()*q.x();
    A(2,0)=2*q.x()*q.z()+2*q.w()*q.y();
    A(2,1)=2*q.y()*q.z()-2*q.w()*q.x();
    A(2,2)=2*q.w()*q.w()+2*q.z()*q.z()-1;
    cout<<"A="<<endl;
    cout<<A<<endl;
    // 那么载体i坐标系转地理坐标系的方向余弦矩阵就为C，为A的转置
    Eigen::Matrix3d C;
    C=A.transpose();
    cout<<"C="<<endl;
    cout<<C<<endl;
}
int main(int argc, char *argv[])
{
    // 解决中文乱码问题
    setlocale(LC_ALL,"");
    // 初始化ros节点
    ros::init(argc,argv,"GuiHua");
    // 创建节点句柄
    ros::NodeHandle nh;
    // 创建订阅者对象
    ros::Subscriber sub=nh.subscribe("/sensing/gnss/gps/odom",10,doMsg);
    ros::spin();

    return 0;
}

实验结果如下：

程序代码解析：

载体坐标系与地理坐标系间的坐标转换关系
[XgYgZg]=C[XYZ]

（1）获取gps位置信息将其转换为eigen

tf::poseMsgToEigen(gps_msg->pose.pose,gps_aff);

（2）用四元素来表示C

（3）程序运行后发现tf::poseMsgToEigen与C是等价的
（4）tf::poseMsgToEigen获取的pose与四元素解析
其获取的四元素为载体坐标系下的旋转角度，需要用C[Wx]将其转换到地理坐标系下
而其获取的pose为地理坐标下的坐标值