#include <iostream>
#include <cmath>
#include <Eigen/Dense>
Quaterniond q(0.704,-0.708,0.05,0.002)//定義一個(gè)四元數(shù);
Matrix3d m = q.toRotationMatrix();//四元數(shù)轉(zhuǎn)旋轉(zhuǎn)矩陣;
cout << q << endl;
cout << m << endl;
Quaterniond q1(m);//旋轉(zhuǎn)矩陣轉(zhuǎn)四元數(shù)，驗(yàn)證下四元數(shù)是否和原來(lái)輸入的四元數(shù)一致
cout << q1 << endl;
cout << "XYZ------------" << endl;
Vector3d v=m.eulerAngles(0,1,2);//旋轉(zhuǎn)矩陣轉(zhuǎn)歐拉角，以XYZ為旋轉(zhuǎn)方向
cout << v << endl;
cout << "ZYX------------" << endl;
Vector3d v1=m.eulerAngles(2,1,0);//旋轉(zhuǎn)矩陣轉(zhuǎn)歐拉角，以ZYX為旋轉(zhuǎn)方向
cout << v1 << endl;
Vector3d v2;

//接著上面代碼段補(bǔ)充運(yùn)行即可
double sy=sqrt(m(0,0) * m(0,0) +  m(0,1) * m(0,1));
        if (sy>0.00001)//判斷萬(wàn)向鎖奇異
        {
            v2(0) = std::atan2(-m(1, 2), m(2, 2));
            v2(1) = std::atan2(m(0, 2), sy);
            v2(2) = std::atan2(-m(0, 1), m(0, 0));
        }
        else//奇異
        {
            v2(0) = 0;
            v2(1) = std::atan2(m(0, 2), sy);
            v2(2) = std::atan2(-m(1, 0), m(2, 0));
        }
cout << "XYZ------------" << endl;
cout << v2 << endl;
        if (sy>0.00001)//判斷萬(wàn)向鎖奇異
        {
            v2(0) = std::atan2(m(2, 1), m(2, 2));
            v2(1) = std::atan2(m(2, 0), sy);
            v2(2) = std::atan2(-m(1, 0), m(0, 0));
        }
        else//奇異
        {
            v2(0) = std::atan2(-m(1, 2), m(1, 1));;
            v2(1) = std::atan2(m(2, 0), sy);
            v2(2) = 0;
        }
cout << "ZYX------------" << endl;
cout << v2 << endl;