PCL点云库必备知识点8——四元素转方向余弦矩阵C

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

#include
#include"nav_msgs/Odometry.h"
#include 
#include 
#include
// 可用于geometry_msgs与Eigen相关数据类型之间的转换。
#include 
#include
#include

// 包含pcd的输入输出头文件——pcd文件IO操作定义的头文件
#include 
// 包含点类型、结构的数据定义——包含若干PointT数据结构定义的头文件
#include 
using namespace std;
    // ConstPtr常量指针可以修改指针的方向，不可以修改指针的数值
    // 将点云map坐标系转换到导航坐标系下
void doMsg(const nav_msgs::Odometry::ConstPtr &gps_msg)
{
    // 通过msg获取并操作订阅到的数据

    // ROS_INFO("翠花订阅的数据：%s",msg->data.c_str());
    Eigen::Affine3d gps_aff;
    // cout<<"gps_aff="<pose.pose,gps_aff);
    // cout<();;
    cout<<"方法1：gps_matrix="<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;
    //  四元素转方向余矩阵的方法2：
    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="< 
实验结果： 
 
**程序说明： 
四元素转方向余弦矩阵的三种方法如下：**
 X轴指向北，Y轴指向天，Z轴指向东，符合右手坐标系
 注意：方向余弦矩阵是分别沿着Y（yaw）、Z(pitch)、X(roll)旋转而来的
 
方法1： 
Eigen::Affine3d gps_aff;
tf::poseMsgToEigen(gps_msg->pose.pose,gps_aff);
 
方法2： 
 定义四元素并获取四元素值    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;
 
手动写方向余弦矩阵C
 
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;
    Eigen::Matrix3d C;
    C=A.transpose();//转置
 

 
 
方法3： 
cout<