C++欧拉角、四元数互相转换

问题描述：最近视觉和机械臂通讯，位姿需要使用欧拉角进行表示，视觉这边一直使用的是四元数进行计算，我心想转一下就好了，只前写过，就发现了一个究极无敌大bug，我原来用的都是错的！！！！，下面是修正之后的

欧拉角转四元数

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;

}

注：要特别小心roll pitch yaw对应的顺序。

四元数转欧拉角

注：要特别小心返回roll pitch yaw对应的顺序。

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;

    double yaw = atan2(2.0 * (q.w() * q.z() + q.x() * q.y()),
                      1.0 - 2.0 * (q.y() * q.y() + q.z() * q.z()));
    double pitch = asin(2.0 * (q.w() * q.y() - q.z() * q.x()));
    double roll = atan2(2.0 * (q.w() * q.x() + q.y() * q.z()),
                       1.0 - 2.0 * (q.x() * q.x() + q.y() * q.y()));

        cout << "roll = "<< roll  << endl;
        cout << "pitch = "<< pitch << endl ;
        cout << "yaw = "<< yaw << endl ;
    return Vector3d(roll, pitch, yaw);
}