四元素与欧拉角之间的转换

四元素与欧拉角之间的转换

在3D图形学中，最常用的旋转表示方法便是四元数和欧拉角，比起矩阵来具有节省存储空间和方便插值的优点。本文主要归纳了两种表达方式的转换，计算公式采用3D笛卡尔坐标系：

定义分别为绕Z轴、Y轴、X轴的旋转角度，如果用Tait-Bryan angle表示，分别为Yaw、Pitch、Roll。

一、四元数的定义

二.通过旋转轴和绕该轴旋转的角度可以构造一个四元数：

其中是绕旋转轴旋转的角度，为旋转轴在x,y,z方向的分量（由此确定了旋转轴）。

二、欧拉角到四元数的转换

三、四元数到欧拉角的转换

arctan和arcsin的结果是，这并不能覆盖所有朝向(对于角的取值范围已经满足)，因此需要用atan2来代替arctan。

四.代码如下：

#include <iostream>
#include <math.h>
#include <stdio.h>
using namespace std;
double pi=3.141592654;
void quaternionToeularangle(double &qx,double &qy,double &qz,double &qw)
{
    double roll_x,pitch_y,yaw_z,yaw_z_angle;
    yaw_z=std::atan2(2*(qw*qz+qx*qy),1-2*(qy*qy+qz*qz));
    pitch_y=std::asin(2*(qw*qy-qx*qz));
    roll_x=std::atan2(2*(qw*qx+qy*qz),1-2*(qy*qy+qx*qx));

    cout<<"roll_x="<<roll_x<<endl;
    cout<<"pitch_y="<<pitch_y<<endl;
    cout<<"yaw_z="<<yaw_z<<endl;
}

void eularangleToquaternion(double &roll,double &pitch,double &yaw)
{
    double cosroll,sinroll,cospitch,sinpitch,cosyaw,sinyaw,qw,qx,qy,qz;
    cosroll=std::cos(roll*0.5f);
    sinroll=std::sin(roll*0.5f);

    cospitch=std::cos(pitch*0.5f);
    sinpitch=std::sin(pitch*0.5f);

    cosyaw=std::cos(yaw*0.5f);
    sinyaw=std::sin(yaw*0.5f);

    qw=cosroll*cospitch*cosyaw+sinroll*sinpitch*sinyaw;
    qx=sinroll*cospitch*cosyaw-cosroll*sinpitch*sinyaw;
    qy=cosroll*sinpitch*cosyaw+sinroll*cospitch*sinyaw;
    qz=cosroll*cospitch*sinyaw-sinroll*sinpitch*cosyaw;

    cout<<"qx="<<qx<<endl;
    cout<<"qy="<<qy<<endl;
    cout<<"qz="<<qz<<endl;
    cout<<"qw="<<qw<<endl;
}
int main(int argc, char **argv)
{
    ros::init(argc, argv, "quaternionToeularangle");
    ros::NodeHandle n;
    while(n.ok())
    {
    double qx,qy,qz,qw,roll,pitch,yaw;
 //   double roll_angle,pitch_angle,yaw_angle;
    cout<<"please input quaternion"<<endl;
    cin>>qx>>qy>>qz>>qw;
    quaternionToeularangle(qx,qy,qz,qw);
    cout<<"please input eularangle"<<endl;
    cin>>roll>>pitch>>yaw;

    eularangleToquaternion(roll,pitch,yaw);
    }
    return 0;
}