C++利用eigen库实现求欧拉角

#include <iostream>
#include <cmath>
#include <Eigen/Dense>
Quaterniond q(0.704,-0.708,0.05,0.002)//定义一个四元数;
Matrix3d m = q.toRotationMatrix();//四元数转旋转矩阵;
cout << q << endl;
cout << m << endl;
Quaterniond q1(m);//旋转矩阵转四元数，验证下四元数是否和原来输入的四元数一致
cout << q1 << endl;
cout << "XYZ------------" << endl;
Vector3d v=m.eulerAngles(0,1,2);//旋转矩阵转欧拉角，以XYZ为旋转方向
cout << v << endl;
cout << "ZYX------------" << endl;
Vector3d v1=m.eulerAngles(2,1,0);//旋转矩阵转欧拉角，以ZYX为旋转方向
cout << v1 << endl;
Vector3d v2;

//接着上面代码段补充运行即可
double sy=sqrt(m(0,0) * m(0,0) +  m(0,1) * m(0,1));
        if (sy>0.00001)//判断万向锁奇异
        {
            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)//判断万向锁奇异
        {
            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;