四元数、欧拉角、旋转矩阵之间互相转换

一、理论

四元数

 

欧拉角

用R表示旋转矩阵，yaw(偏航) 、pitch（俯仰角） rol（横滚角）l分别表示Z　Ｙ　Ｘ轴的转角，q=[q0,q1,q2,q3]'表示单位四元数;

欧拉角有两种：

静态：即绕世界坐标系三个轴的旋转，由于物体旋转过程中坐标轴保持静止，所以称为静态

动态：即绕物体坐标系三个轴的旋转，由于物体旋转过程中坐标轴随着物体做相同的转动，所以称为动态

使用动态欧拉角会出现万向锁现象；静态欧拉角不存在万向锁的问题。一个典型的万向锁问题可以表述如下：

先仰45°再俯90°，这与先俯90°再仰45°是等价的。事实上，一旦选择±90°作为俯角，就会导致第一次旋转和第三次旋转等价，

整个旋转表示系统被限制在只能绕竖直轴旋转，丢失了一个表示维度；这种角度为±90°的第二次旋转使得第一次和第三次旋转的旋转轴相同的现象，称作万向锁。

相关连接： http://v.youku.com/v_show/id_XNzkyOTIyMTI=.html

 

二、代码

#include <iostream>
#include <Eigen/Eigen>
#include <stdlib.h>
#include <Eigen/Geometry>
#include <Eigen/Core>
#include <vector>
#include <math.h>
 
using namespace std;
using namespace Eigen;
 
Eigen::Quaterniond euler2Quaternion(const double roll, const double pitch, const double yaw)
{
    Eigen::AngleAxisd rollAngle(roll, Eigen::Vector3d::UnitZ());
    Eigen::AngleAxisd yawAngle(yaw, Eigen::Vector3d::UnitY());
    Eigen::AngleAxisd pitchAngle(pitch, Eigen::Vector3d::UnitX());
 
    Eigen::Quaterniond q = rollAngle * yawAngle * pitchAngle;
    cout << "Euler2Quaternion result is:" <<endl;
    cout << "x = " << q.x() <<endl;
    cout << "y = " << q.y() <<endl;
    cout << "z = " << q.z() <<endl;
    cout << "w = " << q.w() <<endl<<endl;
    return q;
}
Eigen::Vector3d Quaterniond2Euler(const double x,const double y,const double z,const double w)
{
    Eigen::Quaterniond q;
    q.x() = x;
    q.y() = y;
    q.z() = z;
    q.w() = w;
    Eigen::Vector3d euler = q.toRotationMatrix().eulerAngles(2, 1, 0);
    cout << "Quaterniond2Euler result is:" <<endl;
    cout << "z = "<< euler[2] << endl ;
    cout << "y = "<< euler[1] << endl ;
    cout << "x = "<< euler[0] << endl << endl;
}
Eigen::Matrix3d Quaternion2RotationMatrix(const double x,const double y,const double z,const double w)
{
    Eigen::Quaterniond q;
    q.x() = x;
    q.y() = y;
    q.z() = z;
    q.w() = w;
    Eigen::Matrix3d R = q.normalized().toRotationMatrix();
    cout << "Quaternion2RotationMatrix result is:" <<endl;
    cout << "R = " << endl << R << endl<< endl;
    return R;
}
Eigen::Quaterniond rotationMatrix2Quaterniond(Eigen::Matrix3d R)
{
    Eigen::Quaterniond q = Eigen::Quaterniond(R);
    q.normalize();
    cout << "RotationMatrix2Quaterniond result is:" <<endl;
    cout << "x = " << q.x() <<endl;
    cout << "y = " << q.y() <<endl;
    cout << "z = " << q.z() <<endl;
    cout << "w = " << q.w() <<endl<<endl;
    return q;
}
Eigen::Matrix3d euler2RotationMatrix(const double roll, const double pitch, const double yaw)
{
    Eigen::AngleAxisd rollAngle(roll, Eigen::Vector3d::UnitZ());
    Eigen::AngleAxisd yawAngle(yaw, Eigen::Vector3d::UnitY());
    Eigen::AngleAxisd pitchAngle(pitch, Eigen::Vector3d::UnitX());
    Eigen::Quaterniond q = rollAngle * yawAngle * pitchAngle;
    Eigen::Matrix3d R = q.matrix();
    cout << "Euler2RotationMatrix result is:" <<endl;
    cout << "R = " << endl << R << endl<<endl;
    return R;
}
Eigen::Vector3d RotationMatrix2euler(Eigen::Matrix3d R)
{
    Eigen::Matrix3d m;
    m = R;
    Eigen::Vector3d euler = m.eulerAngles(0, 1, 2);
    cout << "RotationMatrix2euler result is:" << endl;
    cout << "x = "<< euler[2] << endl ;
    cout << "y = "<< euler[1] << endl ;
    cout << "z = "<< euler[0] << endl << endl;
    return euler;
}
int main(int argc, char **argv)
{
  //example
   Eigen::Vector3d x_axiz,y_axiz,z_axiz;
   x_axiz << 1,0,0;
   y_axiz << 0,1,0;
   z_axiz << 0,0,1;

   Eigen::Matrix3d R;
   R << x_axiz,y_axiz,z_axiz;
   rotationMatrix2Quaterniond(R);
   euler2RotationMatrix(0,0,0);
   RotationMatrix2euler(R);
}