Euler to Quaternion / Quaternion to Euler using Eigen

Question

我正在尝试实现一种功能，可以将欧拉角转换为四元数并使用 Eigen 转换回“YXZ”约定。稍后这应该用于让用户给你欧拉角并作为四元数旋转并为用户转换回。事实上，我的数学很差，但我尽力了。我不知道这个矩阵是否正确或任何东西。代码有效，但我想我的结果很糟糕。知道我在哪里走错了吗？这就是我的 Quat.cpp 的样子：

 #include "Quat.h"
#include <Eigen/Geometry>
#include <Eigen/Dense>
#include <cmath>
#include <iostream>

using namespace Eigen;

Vector3f Quat::MyRotation(const Vector3f YPR)
{
    Matrix3f matYaw(3, 3), matRoll(3, 3), matPitch(3, 3), matRotation(3, 3);
    const auto yaw = YPR[2]*M_PI / 180;
    const auto pitch = YPR[0]*M_PI / 180;
    const auto roll = YPR[1]*M_PI / 180;

    matYaw << cos(yaw), sin(yaw), 0.0f,
        -sin(yaw), cos(yaw), 0.0f,  //z
        0.0f, 0.0f, 1.0f;

    matPitch << cos(pitch), 0.0f, -sin(pitch),
        0.0f, 1.0f, 0.0f,   // X
        sin(pitch), 0.0f, cos(pitch);

    matRoll << 1.0f, 0.0f, 0.0f,
        0.0f, cos(roll), sin(roll),   // Y
        0.0f, -sin(roll), cos(roll);

    matRotation = matYaw*matPitch*matRoll;

    Quaternionf quatFromRot(matRotation);

    quatFromRot.normalize(); //Do i need to do this?

    return Quat::toYawPitchRoll(quatFromRot);
}

Vector3f Quat::toYawPitchRoll(const Eigen::Quaternionf& q)
{
    Vector3f retVector;

    const auto x = q.y();
    const auto y = q.z();
    const auto z = q.x();
    const auto w = q.w();

    retVector[2] = atan2(2.0 * (y * z + w * x), w * w - x * x - y * y + z * z);
    retVector[1] = asin(-2.0 * (x * z - w * y));
    retVector[0] = atan2(2.0 * (x * y + w * z), w * w + x * x - y * y - z * z);

#if 1
    retVector[0] = (retVector[0] * (180 / M_PI));
    retVector[1] = (retVector[1] * (180 / M_PI))*-1;
    retVector[2] = retVector[2] * (180 / M_PI);
#endif
    return retVector;
}

输入：x = 55.0, y = 80.0, z = 12.0 四元数：w:0.872274, x:-0.140211, y:0.447012, z:-0.140211 返回值：x:-55.5925, y: -6.84901, z:-21.8771 X-Value 似乎与前缀无关，但 Y 和 z 已关闭。

原文由 Little-God 发布，翻译遵循 CC BY-SA 4.0 许可协议

Answer 1

Quaternation to Euler 解决方案对我不起作用，所以我研究并修改了代码，现在它适用于我的目的：

 Vector3f ToEulerAngles(const Eigen::Quaternionf& q) {
    Vector3f angles;    //yaw pitch roll
    const auto x = q.x();
    const auto y = q.y();
    const auto z = q.z();
    const auto w = q.w();

    // roll (x-axis rotation)
    double sinr_cosp = 2 * (w * x + y * z);
    double cosr_cosp = 1 - 2 * (x * x + y * y);
    angles[2] = std::atan2(sinr_cosp, cosr_cosp);

    // pitch (y-axis rotation)
    double sinp = 2 * (w * y - z * x);
    if (std::abs(sinp) >= 1)
        angles[1] = std::copysign(M_PI / 2, sinp); // use 90 degrees if out of range
    else
        angles[1] = std::asin(sinp);

    // yaw (z-axis rotation)
    double siny_cosp = 2 * (w * z + x * y);
    double cosy_cosp = 1 - 2 * (y * y + z * z);
    angles[0] = std::atan2(siny_cosp, cosy_cosp);
    return angles;
}

我受到这个 wiki 条目的启发，并用这里提供的解决方案做了一些基准测试。查看维基： https ://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles

原文由 Michael Hauptvogel 发布，翻译遵循 CC BY-SA 4.0 许可协议