四元素与欧拉角之间的转换
在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;}