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

Question

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?

Answer 1

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

Answer 2

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