Euler pour les Quaternions / Quaternion d'Euler en utilisant Eigen

Je suis en train de mettre en œuvre une fonctionnalité qui permet de convertir un angle d'Euler dans un Quaternion et retour "YXZ"-convention d'utilisation Propres. Plus tard, cela devrait être utilisé pour permettre à l'utilisateur de vous donner des angles d'Euler et tourner autour comme les Quaternions et les convertir en Arrière pour l'utilisateur. En fait, je suis vraiment mauvais en maths, mais essayé de faire de mon mieux. Je n'ai aucune Idée si ce matrices sont corrects ou quoi que ce soit. Le code Fonctionne, mais mes résultats sont à off, je suppose. Une idée d'où je prends le mauvais tour? C'est ce que mon Quat.cpp ressemble:

#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;
}

Entrée: x = 55.0, y = 80.0, z = 12.0
Quaternion: w:0.872274 x: -0.140211, y:0.447012, z:-0.140211
Valeur de retour: x:-55.5925, y: -6.84901, z:-21.8771
La Valeur X semble sur le droit de méconnaître le préfixe, mais Y et z sont éteints.

OriginalL'auteur Little-God | 2015-07-23

  1. 15

    D'Euler pour les Quaternions:

    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;

    De Quaternion d'Euler:

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

    Prises de https://eigen.tuxfamily.org/dox/classEigen_1_1AngleAxis.html

    Faute de frappe dans "eulerAnglers".
    Pour compiler votre code, je dois apporter un argument de modèle (Scalaire) lors de la construction d'un Quaternion, par exemple, Quternion<float> q ou Quaternionf q pour votre cas. (avec [email protected] et [email protected])

    OriginalL'auteur Ross

  2. 2

    Voici une approche (pas testé):

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

    OriginalL'auteur Shital Shah

  3. 0

    Lorsque j'utilise

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

    Il peut ne pas fonctionner parfaitement tout le temps, la droite d'euler de l'angle a toujours des battements réguliers (la valeur réelle et la valeur calculée avoir une déviation de ±π).
    Par exemple, de lire et d'afficher soit l'angle de lacet par rqt
    image.

    Je n'ai aucune idée à ce sujet, mais je trouve ros tf::getYaw() peut également atteindre "Quaternion d'Euler" (parce que j'ai juste besoin d'angle de lacet).

    OriginalL'auteur JKTesla