6

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?

Peter O.
  • 32,158
  • 14
  • 82
  • 96
Rob
  • 4,927
  • 4
  • 26
  • 41
  • If you can use Eigen lib you should have a look to https://stackoverflow.com/questions/27508242/roll-pitch-and-yaw-from-rotation-matrix-with-eigen-library – Nicolas Diremdjian Oct 09 '19 at 14:32

2 Answers2

9

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))
    };
}
melak47
  • 4,772
  • 2
  • 26
  • 37
  • So, Melak47, i've implemented your code and messed around with the Yaw / Pitch / Roll. Excellent work! It works for 90%. I have added a little video here: http://youtu.be/loQWMElHVuk (upload as we speak, done in ~ 20 minutes) where you can see the results. When i rotate the hardware to the right, something goes wrong. My guess is that this is because i'm missing one rotation, or adding one too much. Can you see what is wrong? – Rob Aug 26 '13 at 13:45
  • Melak, i also have quaternion values. I noticed the numbers when rotating the device. This piece of code looks even better; Rotation on X: q0 * 180; (pitch) Rotation on Z: q1 * 180; (roll) Rotation on Y: q2 * 180; (yaw) – Rob Aug 26 '13 at 16:05
  • @RobQuist sorry, I did not see your comments until now. Are the virtual sticks oriented using the raw data, or the euler angles obtained from that data? – melak47 Aug 28 '13 at 14:12
  • Hi Melak, in the video they are oriented based on your calculations - you notice the weird angle when turning it to the right. Currently i took the raw quat data, and simply multiplied the first 3 values by 180, to get Roll Pitch and Yaw degree angles. The latter looks better but still isn't perfect. – Rob Aug 29 '13 at 10:59
  • 1
    @RobQuist Wikipedia has an article that describes how to convert quaternions to euler angles [here](https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles) – melak47 Aug 30 '13 at 02:24
  • Yes, but the problem is, i do not know how to convert these "LateX" images into code. - I mean, what does the Q23 mean? (with 2 and 3 on top of each other) – Rob Aug 30 '13 at 16:13
  • 1
    @RobQuist q2 squared, and q3 squared :) – melak47 Aug 31 '13 at 11:48
  • yes. I updated my answer with an example for the quaternion->euler angle conversion. if that *still* doesn't work, then I don't know why. – melak47 Sep 06 '13 at 00:57
  • The lower part gives weird results and even the arctan2 / arcsin functions crash because of wrong values. – Rob Oct 30 '13 at 09:54
  • @RobQuist let's talk [in chat](http://chat.stackoverflow.com/rooms/40260/room-for-rob-quist-and-melak47) ? – melak47 Oct 30 '13 at 16:01
  • @RobQuist did you ever figure this out? I'm also trying to convert quaternions from the razer hydra to roll/pitch/yaw. This code seems to work properly for 2 axes, but when I rotate the controller about the 3rd axis, it changes the values of the other two. I can't tell if it's the controller itself spitting out wonky data or if it's that the quaternion indexes don't line up to what the wiki equation is expecting. – meta-meta Dec 11 '13 at 04:13
  • Exactly my point meta-meta! I've been having those issues as well. See; http://www.youtube.com/watch?v=PqIVxAtAMNI I think that q0 and q4 are swapped, because changing that makes it a little better. Not perfect though. So, no, i have no good answer :( – Rob Dec 13 '13 at 13:53
  • 1
    @RobQuist I've noticed a major problem with my matrix->euler angles code was the arbitrary selection of the solution - always returning x1,y1,z1 produces much more stable results – melak47 Dec 13 '13 at 22:53
  • How do you mean? use x1, y1 and z1 as pitch, yaw and roll radians? – Rob Dec 18 '13 at 13:52
  • 1
    @RobQuist Yes. When comparing the results to the original orientations, I've found that they may not be in X-Y-Z oder...trying different orders yielded varied results; sometimes the final orientation was mirrored along one or two axes, sometimes off by 180° around one axis...but I haven't noticed any odd jumps. – melak47 Dec 18 '13 at 16:13
  • So with this code (matrix), the pitch and roll seem to work great. They correspond one on one with my hardware. Problem is the yaw! if i have the joystick facing forward, pitch and roll work good. When i rotate it to the left (-90 degrees) it seems to be okay, and also to the right (90 degrees) its okay. But if i rotate it _over_ the 90 degrees (so that the controller is starting to face me) - the yaw starts to turn back, and the pitch and yaw both start to flip. Ill up a vid. Is this the hardware? or is this something in my calculations that isn't right? I'll see if i can upload a video. – Rob Jan 06 '14 at 14:18
  • PS; weird thing is that the hydra demo application seems to use the Matrix and that seems to work good. – Rob Jan 06 '14 at 14:31
  • THANK YOU! I´ve been fiddling around with stuff, and decided to go with your older solution. The weird flipping was because... I wasn´t picking the shortest solution! The code now works and is visible here; https://github.com/RobQuistNL/GMHydra/blob/master/GMHydraDll/GMHydraDll/GMHydraDll.cpp#L29 Thanks a ton! – Rob Jan 06 '14 at 20:20
  • 1
    @RobQuist Heh, I thought that comparison was making it worse. But if it works for you... :) – melak47 Jan 07 '14 at 01:55
  • 1
    Thanks for linking the paper @melak47, I find it regrettable that people have difficulty adhering to conventions, if indeed conventions actually exist on this topic. XYZ is Tait-Bryan angles not Euler angles. Euler angles (supposedly) always only use two axes to rotate around, not all 3. – Steven Lu Feb 28 '14 at 20:21
  • seriously? noone noticed that the angles are confused? in the special case you need to replace x and z, in the last case x and y – Markus Zancolò May 10 '17 at 15:07
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

Martijn van Wezel
  • 1,120
  • 14
  • 25
  • Thanks martijn, i've used this but it didn't really work out well - https://www.youtube.com/watch?v=loQWMElHVuk You see as it revolves over 180 degrees weird stuff starts to happen – Rob Oct 16 '15 at 12:31
  • Do some research about eurler angle lock, that is the reason about the weird stuff, this formula only works with little rotations – Martijn van Wezel Oct 16 '15 at 17:02