Euler to Quaternion / Quaternion to Euler using Eigen答案

【问题标题】：Euler to Quaternion / Quaternion to Euler using EigenEuler to Quaternion / Quaternion to Euler using Eigen
【发布时间】：2015-07-23 14:10:40
【问题描述】：

我正在尝试实现一种功能，该功能可以将欧拉角转换为四元数并使用 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 值似乎与前缀无关，但 Y 和 z 已关闭。

【问题讨论】：

标签： c++ eigen quaternions euler-angles

【解决方案1】：

从欧拉到四元数：

using namespace Eigen;
//Roll pitch and yaw in Radians
float roll = 1.5707, pitch = 0, yaw = 0.707;    
Quaternionf q;
q = AngleAxisf(roll, Vector3f::UnitX())
    * AngleAxisf(pitch, Vector3f::UnitY())
    * AngleAxisf(yaw, Vector3f::UnitZ());
std::cout << "Quaternion" << std::endl << q.coeffs() << std::endl;

从四元数到欧拉：

auto euler = q.toRotationMatrix().eulerAngles(0, 1, 2);
std::cout << "Euler from quaternion in roll, pitch, yaw"<< std::endl << euler << std::endl;

取自https://eigen.tuxfamily.org/dox/classEigen_1_1AngleAxis.html

【讨论】：

“eulerAnglers”中的错字。
要编译你的代码，我必须在构造四元数时提供一个模板参数（标量），例如，Quternion<float> q 或 Quaternionf q 对于你的情况。（使用 Eigen@3.3.4 和 gcc@7.3.0）

【解决方案2】：

这是一种方法（未测试）：

  Vector3d euler = quaternion.toRotationMatrix().eulerAngles(2, 1, 0);
  yaw = euler[0]; pitch = euler[1]; roll = euler[2];

【讨论】：

【解决方案3】：

当我使用时

auto euler = q.toRotationMatrix().eulerAngles(0, 1, 2)

不可能一直完美，欧拉角总是有规律的节拍（实际值和计算值有±π的偏差）。例如通过rqt读取并显示偏航角 picture.

我对此一无所知，但我发现 ros tf::getYaw() 也可以实现“四元数到欧拉”（因为我只需要偏航角）。

【讨论】：

我看到了完全相同的行为 - 四元数绘制平稳，但转换为 Euler roll/pitch/yaw 表现出 +/- pi 不稳定性。

【解决方案4】：

没有 Eigen（以防万一），我做到了：

tf2::Matrix3x3 ( quat ) . getEulerYPR( &roll, &pitch, &yaw );
// and
tf2::Matrix3x3 ( quat ) . getRPY( &roll, &pitch, &yaw );

不过，这些只能提供 24 种可能的配置中的两种。

【讨论】：