41

I have a rotation quaternion and want to extract the angle of rotation about the Up axis (the yaw). I am using XNA and as far as I can tell there is no inbuilt function for this. What is the best way to do this?

Thanks for any help, Venatu

Venatu
  • 1,264
  • 1
  • 13
  • 24

6 Answers6

46

The quaternion representation of rotation is a variation on axis and angle. So if you rotate by r radians around axis x, y, z, then your quaternion q is:

q[0] = cos(r/2);
q[1] = sin(r/2)*x;
q[2] = sin(r/2)*y;
q[3] = sin(r/2)*z;

If you want to create a quaternion that only rotates around the y axis, you zero out the x and z axes and then re-normalize the quaternion:

q[1] = 0;
q[3] = 0;
double mag = sqrt(q[0]*q[0] + q[2]*q[2]);
q[0] /= mag;
q[2] /= mag;

If you want the resulting angle:

double ang = 2*acos(q[0]);

This assumes that the quaternion representation is stored: w,x,y,z. If both q[0] and q[2] are zero, or close to it, the resulting quaternion should just be {1,0,0,0}.

JCooper
  • 6,395
  • 1
  • 25
  • 31
  • 1
    Does this also work to 'only' extract pitch (or roll) from a Quaternion, e.g. by setting x and y to zero. If not, why not? – Alexander Pacha Sep 19 '13 at 02:13
  • 12
    @AlexanderPacha It doesn't quite work for extracting pitch or roll. The reason is that 'yaw' is typically defined as rotation around the world's 'up' axis. But pitch and roll are defined relative to object's internal axes. When a craft pitches, it does so around its own wings, regardless of what world axis the wings are aligned with. Since the quaternion is typically in world coordinates, it takes extra steps to convert things to/from the object local frame of reference. – JCooper Sep 21 '13 at 04:07
  • Forgive my stupidity, but I can not get out of place here, I have the quaternion matrix wxyz, where does this value of R? – jucajl Mar 29 '16 at 20:01
  • This fails if r is negative – Eric May 05 '16 at 20:03
  • @Eric It should still work when r is negative. Rotating around x,y,z by -r is the same as rotating around -x,-y,-z by r. If you negate all elements of the quaternion, you have the same rotation. `acos()` Always returns a positive value. So you'll get `0 <= ang < 2*pi` back. But if you want a negative angle (e.g., if 0 means forward and +/- angles are left/right turns) you just do `if (ang > pi) ang -= 2*pi` – JCooper May 12 '16 at 22:00
  • My mistake, your quaternion order is different to the one I was assuming, and your answer is correct – Eric May 13 '16 at 05:44
  • 3
    N.B. If you are just interested in ang, then instead of computing `2*acos(q0/sqrt(q0*q0+q2*q2))`, you can directly compute `atan(q[2],q[0])*2` which is also already normalized to [-pi, pi]. – chtz Nov 16 '16 at 19:00
41

Having given a Quaternion q, you can calculate roll, pitch and yaw like this:

var yaw = atan2(2.0*(q.y*q.z + q.w*q.x), q.w*q.w - q.x*q.x - q.y*q.y + q.z*q.z);
var pitch = asin(-2.0*(q.x*q.z - q.w*q.y));
var roll = atan2(2.0*(q.x*q.y + q.w*q.z), q.w*q.w + q.x*q.x - q.y*q.y - q.z*q.z);

This should fit for intrinsic tait-bryan rotation of xyz-order. For other rotation orders, extrinsic and proper-euler rotations other conversions have to be used.

user1767754
  • 23,311
  • 18
  • 141
  • 164
thewhiteambit
  • 1,365
  • 16
  • 31
  • 3
    I just double checked, but this is actually my working code transforming Quaternion-Values to Yaw, Pitch, Roll for usage in Maya. Tried the commonly accepted method first, but it does not work as expected! You might better first try, before voting down... – thewhiteambit Dec 05 '13 at 00:10
  • I say yes (testet at least for some [f.e. Autodesk Maya] implementation of yaw, pitch, roll) - others vote it down but don't comment anything. Make up your own mind, or just try if it fits your needs. It is just 3 lines of code, if it does not work for you try to look for something differend and drop me a line what was wrong. If it works for you please confirm it with a comment and vote up :D – thewhiteambit Apr 22 '14 at 21:24
  • 2
    Hmm, it returns me some weird results at the moment: If I yaw my camera a bit, only the yaw changes, which is what I expect. If I then pitch it, every component returned changes, which is unexpected. E.g. if I yaw and pitch my cam by 0.3 radians, I would expect this to return 0.3 for botch and 0 roll, but every component has a weird number for me then ;S – Ray Apr 23 '14 at 08:19
  • Strange, I have testes the usual implementations in Maya, and it resulted in strange rotations when looking upwards beyond 'north-pole' (like it would try to work around some gimbal), where this exact implementation works perfect for all conditions (f.e. looking 180° upwards results in a upside-down look backwards, were the ususal implementation results in just looking backwards because the horizon flips back to normal). Thanks for your comment :) – thewhiteambit Apr 23 '14 at 13:03
  • You can convert it from radiant to degree by multiplying each value by "180.0/M_PI" - pitch has a value range from -90° to +90° and then jumps, where yaw and roll switch from -180° to + 180° what it no jump because it is the same... maybe it is your quaternion values you start with? – thewhiteambit Apr 23 '14 at 13:12
  • 2
    some might use different order of assignment: roll, yaw, pitch - depending on coordinate system. – thewhiteambit Apr 23 '14 at 13:19
  • You're right. Yaw and Pitch was swapped for me, and I had to negate every value. – Ray Apr 23 '14 at 17:30
  • maybe you like to rate it up now? :D – thewhiteambit Apr 23 '14 at 20:21
  • Yeah, even though it still doesn't work as intended :S – Ray Apr 23 '14 at 21:23
  • Are you shure you want a yaw, pitch, roll conversion and no conversion to euler angles? http://www.euclideanspace.com/maths/geometry/rotations/conversions/quaternionToEuler/ – thewhiteambit Apr 24 '14 at 19:31
  • Yeah, thats needed here – Ray Apr 24 '14 at 20:19
  • But now you are doing yaw, roll, and yaw again - not yaw, pitch, roll: http://math.stackexchange.com/questions/147028/are-euler-angles-the-same-with-pitch-roll-yaw – thewhiteambit Apr 26 '14 at 01:13
  • 2
    I posted my specific question here: http://stackoverflow.com/questions/23310299/quaternion-from-tait-bryan-angles The termination "euler angles" put me in big confusion until Wikipedia clarified it I am searching for "Tait-Bryan" angles (XYZ rotation, not XYX). – Ray Apr 26 '14 at 11:38
  • This should fit for XYZ rotate order assuming yaw=X, pitch=Y, roll=Z. Afaik "Tait-Bryan" is ZYX order by convention. – thewhiteambit Apr 26 '14 at 13:27
  • Best explanaition here: http://www.answers.com/topic/euler-angles#Tait.E2.80.93Bryan_angles Maya default convention: http://download.autodesk.com/global/docs/maya2014/en_us/index.html?url=files/Animation_Basics_Animated_rotation_in_Maya.htm,topicNumber=d30e236648 – thewhiteambit Apr 26 '14 at 14:15
  • 2
    Wait, "aircraft principal axes" are again something different? I thought it's the same. God... who made up these terms... – Ray Apr 26 '14 at 15:49
  • 1
    This seems to work with normalized invensense DMP data except that I had to swap the ordering of the results from yaw,pitch, roll to pitch, roll, yaw to have the angles make sense – user1792021 Dec 04 '14 at 20:05
  • 1
    Of course this can happen, depending on the coordinate system orientation for your destination the axes are also changing on yaw, pitch, roll. – thewhiteambit Dec 05 '14 at 03:45
17

Note: I've verified below code against Wikipedia's equations plus Pixhawk's documentation and it is correct.

If you are working with drones/aviation, below is the code (taken directly from DJI SDK). Here q0, q1, q2, q3 corresponds to w,x,y,z components of the quaternion respectively. Also note that yaw, pitch, roll may be referred to as heading, attitude and bank respectively in some literature.

float roll  = atan2(2.0 * (q.q3 * q.q2 + q.q0 * q.q1) , 1.0 - 2.0 * (q.q1 * q.q1 + q.q2 * q.q2));
float pitch = asin(2.0 * (q.q2 * q.q0 - q.q3 * q.q1));
float yaw   = atan2(2.0 * (q.q3 * q.q0 + q.q1 * q.q2) , - 1.0 + 2.0 * (q.q0 * q.q0 + q.q1 * q.q1));

If you need to calculate all 3 then you can avoid recalculating common terms by using following functions:

//Source: http://docs.ros.org/latest-lts/api/dji_sdk_lib/html/DJI__Flight_8cpp_source.html#l00152
EulerianAngle Flight::toEulerianAngle(QuaternionData data)
{
    EulerianAngle ans;

    double q2sqr = data.q2 * data.q2;
    double t0 = -2.0 * (q2sqr + data.q3 * data.q3) + 1.0;
    double t1 = +2.0 * (data.q1 * data.q2 + data.q0 * data.q3);
    double t2 = -2.0 * (data.q1 * data.q3 - data.q0 * data.q2);
    double t3 = +2.0 * (data.q2 * data.q3 + data.q0 * data.q1);
    double t4 = -2.0 * (data.q1 * data.q1 + q2sqr) + 1.0;

    t2 = t2 > 1.0 ? 1.0 : t2;
    t2 = t2 < -1.0 ? -1.0 : t2;

    ans.pitch = asin(t2);
    ans.roll = atan2(t3, t4);
    ans.yaw = atan2(t1, t0);

    return ans;
}

QuaternionData Flight::toQuaternion(EulerianAngle data)
{
    QuaternionData ans;
    double t0 = cos(data.yaw * 0.5);
    double t1 = sin(data.yaw * 0.5);
    double t2 = cos(data.roll * 0.5);
    double t3 = sin(data.roll * 0.5);
    double t4 = cos(data.pitch * 0.5);
    double t5 = sin(data.pitch * 0.5);

    ans.q0 = t2 * t4 * t0 + t3 * t5 * t1;
    ans.q1 = t3 * t4 * t0 - t2 * t5 * t1;
    ans.q2 = t2 * t5 * t0 + t3 * t4 * t1;
    ans.q3 = t2 * t4 * t1 - t3 * t5 * t0;
    return ans;
}

Note on Eigen Library

If you are using Eigen library, it has another way to do this conversion, however, this may not be as optimized as above direct code:

  Vector3d euler = quaternion.toRotationMatrix().eulerAngles(2, 1, 0);
  yaw = euler[0]; pitch = euler[1]; roll = euler[2];
William Grand
  • 1,033
  • 2
  • 12
  • 24
Shital Shah
  • 63,284
  • 17
  • 238
  • 185
  • @thewhiteambit - It's not "your" answer. Above code is from DJI SDK as I've mentioned which should give people more confidence in using. Also I've added the version of code which avoids doing same dot products again if you wanted to calculate all 3. It's very bad taste to vote down because you think you own a math formula AND the fact that you didn't wrote efficient code in first place which is very important because these calculations needs to be done 100s of times per second. – Shital Shah Jun 10 '16 at 03:10
  • 1
    @thewhiteambit The reason I wrote this answer is to help others save some time for the code that I'd to write myself. The toEularianAngle() does eliminate one redundancy (q2sqr), its more clean and its far from "shameless copy" that you claim to be. The toQuternion() makes it more complete. – Shital Shah Jun 10 '16 at 20:19
  • since someone deleted half of my answers, I deleted the remaining ones. great work eliminating that very single one multiplication. – thewhiteambit Jun 20 '16 at 13:18
  • Dude, I have no idea what you are talking about. As far as I know, only you can delete your own answers. Someone else can't just delete other people's answers. That would be very scary. I've also filed issue with DJI folks to see if their code is correct: https://github.com/dji-sdk/Onboard-SDK/issues/59#issuecomment-225851991. That's how you fix problems, not by creating and protecting your "turfs". Anyway, whatever. Best of luck. – Shital Shah Jul 08 '16 at 01:14
4

Conversion Quaternion to Euler

I hope you know that yaw, pitch and roll are not good for arbitrary rotations. Euler angles suffer from singularities (see the above link) and instability. Look at 38:25 of the presentation of David Sachs

http://www.youtube.com/watch?v=C7JQ7Rpwn2k

Good luck!

Ali
  • 56,466
  • 29
  • 168
  • 265
  • 1
    Euler Angels don't always have this Gimbal-Lock problem, if you handle them correctly (and this means in first case no usage of workarounds like in the video - but a correct handling of multiplication order). Euler can even have advantages like Rotation with >360°. But I also prefer Quaternions in most cases. – thewhiteambit Aug 08 '13 at 00:14
  • 1
    I'm working with implementations that don't cause singularities in working with euler angles... for example, extraction with asin causes gimbal lock... I'm currently working on an implementation using 3 atan2 functions to avoid such a circumstance. – Tcll May 19 '17 at 01:50
2

A quaternion consists of two components: a 3d vector component and a scalar component.

The vector component of the quaternion describes independent rotations about each axis, so zero'ing out the x- and y-components of the vector component and leaving z-component as-is is all you need to do in order to solve for the vector term:

// Don't modify qz
double qx = 0;
double qy = 0;  

The scalar term represents the magnitude of rotation. For a unit quaternion (such as one used to represent attitude), the entire quaternion must have a magnitude of 1. Thus, the scalar term can be solved by:

double qw = sqrt(1 - qx*qx - qy*qy - qz*qz);

Since qx and qy are zero, the scalar component is given by

double qw = sqrt(1 - qz*qz); 

Thus, the full quaternion representing yaw is given by

double qx = 0;
double qy = 0;
// Don't modify qz
double qw = sqrt(1 - qz*qz);
Eric Cox
  • 19
  • 2
0

The transformation from quaternion to yaw, pitch, and roll depends on the conventions used to define the quaternion and the yaw, pitch, and roll. For a given convention there are many "almost correct" transformations that will work for the majority of angles but only one truly correct transformation that will work for all angles including south and north poles where the "almost correct" transformations produce gimbal locks (spurious flips and rotations).

See this tutorial for more information:

https://youtu.be/k5i-vE5rZR0

Dimples
  • 21
  • 3
  • If there are key-concept or key-element in the link you provid, please consider summarizing it in your answer(unless it's already in you first paragraph). Do not forget that link or videos are note eternal! – XouDo Jan 17 '22 at 10:33