Эйлер в кватернион / кватернион в эйлер, используя Эйген

Question

Эйлер в кватернион / кватернион в эйлер, используя Эйген

Я пытаюсь реализовать функциональность, которая может преобразовать угол Эйлера в кватернион и обратно «YXZ» -конвенция с помощью Eigen. Позже это следует использовать, чтобы позволить пользователю дать вам углы Эйлера и повернуть его как Quaternion и преобразовать Back для пользователя. На самом деле я очень плохо разбираюсь в математике, но старался изо всех сил. Я понятия не имею, правильны ли эти матрицы или что-то еще. Код работает, но мои результаты, я полагаю, далеко не верны. Есть идеи, где я поверну не туда? Вот как выглядит мой 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
Кватернион: ш: 0,872274, х: -0,140211, у: 0,447012, з: -0,140211
Возвращаемое значение: x: -55.5925, y: -6.84901, z: -21.8771
Значение X кажется правильным без учета префикса, но Y и z отключены.

4

c++eigen euler-angles quaternions

Решение

Другие решения

Вот один из подходов (не проверен):

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

2

Источник

Accepted Answer

От Эйлера до Кватерниона:

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

13