Преобразование матрицы 3х3 в углы Эйлера / Тейта Брайана (шаг тангажа)

Question

Преобразование матрицы 3х3 в углы Эйлера / Тейта Брайана (шаг тангажа)

У меня здесь есть Razer Hydra SDK, и я хочу преобразовать матрицу вращения, которую я получаю от аппаратного обеспечения, в тангаж, рыскание и крен.

В документации говорится:

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

Мой код в настоящее время:

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]);

Все же это, кажется, дает мне неправильные результаты.

Кто-нибудь знает, как я могу легко перевести это, и что я делаю неправильно?

5

algorithm c++math orientation

Решение

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

Это формула, которая будет делать, имейте в виду, что чем выше точность, тем больше переменных в матрице вращения:

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

Это также используется в библиотеке облаков точек, функция: pcl :: getEulerAngles

1

Источник

Accepted Answer

Вы можете рассчитать тангаж, крен и рыскание, как этот.
Исходя из этого:

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

Если вы по-прежнему получаете неправильные углы с этим, вы можете использовать матрицу основной строки, а не основной столбец, или наоборот — в этом случае вам нужно будет перевернуть все R[i][j] случаи к R[j][i],

В зависимости от используемой системы координат (левша, правша) x, y, z могут не соответствовать тем же осям, но как только вы начнете получать правильные числа, выясните, какая ось должна быть легкой 🙂

Кроме того, для преобразования из кватерниона в эйлеровы углы, такие как показано здесь:

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

8