c++ 四元数转欧拉角

c++ 四元数转欧拉角

两种方法，供你选择；

方法１;

/*
输入：x,y,z,w　为四元数
输出：roll，pitch，yaw欧拉角
**/
static void toEulerAngle(const double x,const double y,const double z,const double w, double& roll, double& pitch, double& yaw)
{
// roll (x-axis rotation)
    double sinr_cosp = +2.0 * (w * x + y * z);
    double cosr_cosp = +1.0 - 2.0 * (x * x + y * y);
    roll = atan2(sinr_cosp, cosr_cosp);

// pitch (y-axis rotation)
    double sinp = +2.0 * (w * y - z * x);
    if (fabs(sinp) >= 1)
        pitch = copysign(M_PI / 2, sinp); // use 90 degrees if out of range
    else
        pitch = asin(sinp);

// yaw (z-axis rotation)
    double siny_cosp = +2.0 * (w * z + x * y);
    double cosy_cosp = +1.0 - 2.0 * (y * y + z * z);
    yaw = atan2(siny_cosp, cosy_cosp);
//    return yaw;
}

调用

            cv::Vec3d euler;
            toEulerAngle(theta[0],theta[1],theta[2],theta[3],euler[0],euler[1],euler[2]);
            cout <<"euler "<< euler << endl;

方法二－依赖库Eigen;

eigen安装；有就跳过；

sudo apt install libeigen3-dev

#include <Eigen/Dense>
#include <Eigen/Geometry>
#include <Eigen/StdVector>
#include <Eigen/Core>

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 << "x = "<< euler[2] << endl ;
    cout << "y = "<< euler[1] << endl ;
    cout << "z = "<< euler[0] << endl << endl;
    return euler;
}

调用；theta是四元数；返回 Eigen::Vector3d euler

 Eigen::Vector3d euler = Quaterniond2Euler(theta[0],theta[1],theta[2],theta[3] );