La conversion d'une matrice de 3x3 à Euler/Tait Bryan angles (pitch yaw roll)

J'ai le Razer Hydra SDK ici, et j'ai envie de transformer la matrice de rotation-je obtenir du matériel en tangage, lacet et roulis.

Les états de documentation:

rot_mat - A 3x3 matrix describing the rotation of the controller.

Mon code est actuellement:

roll = atan2(rot_mat[2][0], rot_mat[2][1]);
pitch = acos(rot_mat[2][2]);
yaw = -atan2(rot_mat[0][2], rot_mat[1][2]);

Pourtant, cela semble me donner de mauvais résultats.

Serait quelqu'un sait comment je peux traduire facilement, et ce que je fais de mal?

OriginalL'auteur Rob Quist | 2013-08-25

  1. 10

    Vous pouvez calculer le tangage, le roulis et le lacet comme cette.
    Basé sur:

    #include <array>
#include <limits>
typedef std::array<float, 3> float3;
typedef std::array<float3, 3> float3x3;
const float PI = 3.14159265358979323846264f;
bool closeEnough(const float& a, const float& b, const float& epsilon = std::numeric_limits<float>::epsilon()) {
return (epsilon > std::abs(a - b));
}
float3 eulerAngles(const float3x3& R) {
//check for gimbal lock
if (closeEnough(R[0][2], -1.0f)) {
float x = 0; //gimbal lock, value of x doesn't matter
float y = PI / 2;
float z = x + atan2(R[1][0], R[2][0]);
return { x, y, z };
} else if (closeEnough(R[0][2], 1.0f)) {
float x = 0;
float y = -PI / 2;
float z = -x + atan2(-R[1][0], -R[2][0]);
return { x, y, z };
} else { //two solutions exist
float x1 = -asin(R[0][2]);
float x2 = PI - x1;
float y1 = atan2(R[1][2] / cos(x1), R[2][2] / cos(x1));
float y2 = atan2(R[1][2] / cos(x2), R[2][2] / cos(x2));
float z1 = atan2(R[0][1] / cos(x1), R[0][0] / cos(x1));
float z2 = atan2(R[0][1] / cos(x2), R[0][0] / cos(x2));
//choose one solution to return
//for example the "shortest" rotation
if ((std::abs(x1) + std::abs(y1) + std::abs(z1)) <= (std::abs(x2) + std::abs(y2) + std::abs(z2))) {
return { x1, y1, z1 };
} else {
return { x2, y2, z2 };
}
}
}

    Si vous avez toujours des mauvais angles avec cela, vous pouvez utiliser une ligne-major de la matrice, par opposition à la colonne principale, ou vice versa - dans ce cas, vous devrez retourner tous les R[i][j] instances de R[j][i].

    Selon le système de coordonnées utilisé (gaucher, droitier) x,y,z peuvent ne pas correspondre aux mêmes axes, mais une fois que vous commencez à obtenir les bons chiffres, à comprendre l'axe qui est ce qui devrait être facile 🙂

    Sinon, pour convertir à partir d'un Quaternion d'angles d'euler comme montré ici:

    float3 eulerAngles(float q0, float q1, float q2, float q3)
{
return
{
atan2(2 * (q0*q1 + q2*q3), 1 - 2 * (q1*q1 + q2*q2)),
asin( 2 * (q0*q2 - q3*q1)),
atan2(2 * (q0*q3 + q1*q2), 1 - 2 * (q2*q2 + q3*q3))
};
}
    Wikipedia a un article qui explique comment convertir les quaternions pour les angles d'euler ici
    t2 au carré, et t3 au carré 🙂
    J'ai remarqué un problème majeur avec mon matrice->angles d'euler code a été l'arbitraire de la sélection de la solution - revenant toujours x1,y1,z1 produit beaucoup de résultats plus stables
    Oui. Lorsque l'on compare les résultats à l'origine des orientations, j'ai trouvé qu'ils ne peuvent pas être dans les X-Y-Z oder...en essayant de différents ordres donné des résultats variés; parfois, l'orientation finale a été mis en miroir le long d'un ou deux axes, parfois en dehors de 180° autour d'un axe...mais je n'ai pas remarqué de bizarre sauts.
    Heh, je pensais que la comparaison est à l'empirer. Mais si cela fonctionne pour vous... 🙂

    OriginalL'auteur melak47

  2. 1

    C'est l'une formule qui va faire, gardez à l'esprit que plus la précision la plus variables dans la matrice de rotation sont importants:

    roll = atan2(rot_mat[2][1], rot_mat[2][2]);
pitch = asin(rot_mat[2][0]);
yaw = -atan2(rot_mat[1][0], rot_mat[0][0]);

    http://nghiaho.com/?page_id=846

    Il est également utilisé dans le nuage de points de la bibliothèque, de la fonction : pcl::getEulerAngles

    Grâce martijn, j'ai utilisé de cela, mais il n'a pas vraiment bien youtube.com/watch?v=loQWMElHVuk Vous voyez qu'il tourne à plus de 180 degrés de substance étrange commence à se produire
    Faites quelques recherches sur les eurler angle de serrure, c'est la raison pour les trucs bizarres, cette formule ne fonctionne qu'avec peu de rotations

    OriginalL'auteur Martijn van Wezel