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

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

在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;}