将一个3x3矩阵转换为欧拉/泰特布莱恩角度（俯仰、偏航、翻滚）。

Question

将一个3x3矩阵转换为欧拉/泰特布莱恩角度（俯仰、偏航、翻滚）。

6

我这里有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]);

但是这似乎会给我错误的结果。

有人知道我如何轻松地翻译这个，并且我做错了什么吗？

- Rob

如果你会使用Eigen库，你应该查看以下链接：https://dev59.com/0V4c5IYBdhLWcg3wzs-Q - Nicolas Diremdjian

2个回答

2

这是一个公式，需要记住的是，精度越高，旋转矩阵中的变量就越重要：

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。

- Martijn van Wezel

谢谢Martijn，我用过这个，但效果不太好 - https://www.youtube.com/watch?v=loQWMElHVuk。当它旋转超过180度时，会发生奇怪的事情。 - Rob

请对欧拉角锁定进行一些研究，这就是奇怪现象的原因，这个公式只适用于小旋转。 - Martijn van Wezel

网页内容由stack overflow 提供, 点击上面的

可以查看英文原文，
原文链接

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