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

然而，这似乎给了我错误的结果。

有人会知道我如何轻松翻译这个，以及我做错了什么吗？

4

2 回答 2

9

你可以像这样计算俯仰、滚动和偏航。基于此：

#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))
    };
}
于 2013-08-26T03:30:23.030 回答
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

于 2015-10-16T10:42:34.803 回答