Logo Questions Linux Laravel Mysql Ubuntu Git Menu
 

Converting a 3x3 matrix to Euler/Tait Bryan angles (pitch yaw roll)

I have the Razer Hydra SDK here, and I want to transform the rotation matrix I get from the hardware, into pitch, yaw and roll.

The documentation states:

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

My code is currently:

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

Yet this seems to give me wrong results.

Would somebody know how I can easily translate this, and what I am doing wrong?

like image 927
Rob Avatar asked Aug 25 '13 21:08

Rob


People also ask

How do you convert a rotation matrix to Euler angles?

Given a rotation matrix R, we can compute the Euler angles, ψ, θ, and φ by equating each element in R with the corresponding element in the matrix product Rz(φ)Ry(θ)Rx(ψ). This results in nine equations that can be used to find the Euler angles. Starting with R31, we find R31 = − sin θ.

Are Euler angles the same as that of roll pitch and yaw?

XYZ Roll-Pitch-Yaw Angles in Robotics In the previous lesson, we learned about Euler angles, and we saw that Euler angles refer to the angles in a sequence of rotations in a body-fixed frame. On the other hand, the roll-pitch-yaw angles are a sequence of rotations about axes of the space frame.

What are Tait Bryan angles?

The Euler or Tait–Bryan angles (α, β, γ) are the amplitudes of these elemental rotations. For instance, the target orientation can be reached as follows (note the reversed order of Euler angle application): The XYZ system rotates about the z axis by γ. The X axis is now at angle γ with respect to the x axis.

What are the 3 Euler angles?

The relative orientation between two orthogonal righthanded 3D cartesian coordinate systems, let's call them xyz and ABC, is described by a real orthogonal 3x3 rotation matrix R, which is commonly parameterized by three so-called Euler angles α, β and γ.


2 Answers

You can calculate pitch, roll and yaw like this. Based on that:

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

If you still get wrong angles with this, you may be using a row-major matrix as opposed to column-major, or vice versa - in that case you'll need to flip all R[i][j] instances to R[j][i].

Depending on the coordinate system used (left handed, right handed) x,y,z may not correspond to the same axes, but once you start getting the right numbers, figuring out which axis is which should be easy :)

Alternatively, to convert from a Quaternion to euler angles like shown here:

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))
    };
}
like image 184
melak47 Avatar answered Sep 28 '22 04:09

melak47


This is the an formula that will do, keep in mind that the higher the precision the more variables in the rotation matrix are important:

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

This is also used in the point cloud library, function : pcl::getEulerAngles

like image 29
Martijn van Wezel Avatar answered Sep 28 '22 04:09

Martijn van Wezel