2
$\begingroup$

I am rotating n 3D shape using Euler angles in the order of XYZ meaning that the object is first rotated along the X axis, then Y and then Z. I want to convert the Euler angle to Quaternion and then get the same Euler angles back from the Quaternion using some [preferably] Python code or just some pseudocode or algorithm. Below, I have some code that converts Euler angle to Quaternion and then converts the Quaternion to get Euler angles. However, this does not give me the same Euler angles.

I think the problem is I don't know how to associate yaw, pitch and roll to X, Y an Z axes. Also, I don't know how to change order of conversions in the code to correctly convert the Euler angles to Quaternion and then convert the Quaternion to Euler angle so that I am able to get the same Euler angle back. Can someone help me with this?

And here's the code I used:

This function converts Euler angles to Quaternions:

def euler_to_quaternion(yaw, pitch, roll):

        qx = np.sin(roll/2) * np.cos(pitch/2) * np.cos(yaw/2) - np.cos(roll/2) * np.sin(pitch/2) * np.sin(yaw/2)
        qy = np.cos(roll/2) * np.sin(pitch/2) * np.cos(yaw/2) + np.sin(roll/2) * np.cos(pitch/2) * np.sin(yaw/2)
        qz = np.cos(roll/2) * np.cos(pitch/2) * np.sin(yaw/2) - np.sin(roll/2) * np.sin(pitch/2) * np.cos(yaw/2)
        qw = np.cos(roll/2) * np.cos(pitch/2) * np.cos(yaw/2) + np.sin(roll/2) * np.sin(pitch/2) * np.sin(yaw/2)

        return [qx, qy, qz, qw]

And this converts Quaternions to Euler angles:

def quaternion_to_euler(x, y, z, w):

        import math
        t0 = +2.0 * (w * x + y * z)
        t1 = +1.0 - 2.0 * (x * x + y * y)
        X = math.degrees(math.atan2(t0, t1))

        t2 = +2.0 * (w * y - z * x)
        t2 = +1.0 if t2 > +1.0 else t2
        t2 = -1.0 if t2 < -1.0 else t2
        Y = math.degrees(math.asin(t2))

        t3 = +2.0 * (w * z + x * y)
        t4 = +1.0 - 2.0 * (y * y + z * z)
        Z = math.degrees(math.atan2(t3, t4))

        return X, Y, Z

And I use them as follow:

import numpy as np
euler_Original = np.random.random(3) * 360).tolist() # Generate random rotation angles for XYZ within the range [0, 360)
quat = euler_to_quaternion(euler_Original[0], euler_Original[1], euler_Original[2]) # Convert to Quaternion
newEulerRot = quaternion_to_euler(quat[0], quat[1], quat[2], quat[3]) #Convert the Quaternion to Euler angles

print (euler_Original)
print (newEulerRot)

The print statements print different numbers for euler_Original and newEulerRot which I don't want to be the case. For example if euler_original contains numbers like (0.2, 1.12, 2.31) in radians I get this Quaternion --> [0.749, 0.290, -0.449, 0.389] and converting the Quaternion to Euler angles gives me this --> (132.35, 64.17, 11.45) which is pretty wrong. I wonder how I can fix this?

Although I'm interested in getting the above code to work by making changes to it but, I would rather learn how to set up the equations correctly. This way I would know how I can get the correct Quaternions even if the order of rotations (XYZ --> YZX etc) for applying Euler angles is changed.

$\endgroup$
  • 1
    $\begingroup$ @HectorBlandin I've literally copied the Python script from the same Wikipedia page to go from Quaternion to Euler angles. Besides being interested in getting the code to work, I would rather learn how to set up the equations correctly so I know how changing the order of rotations (XYZ --> YZX etc) for applying Euler angles still gives me the correct Quaternion. $\endgroup$ – Amir Oct 28 '18 at 20:06
  • $\begingroup$ Did you follow en.wikipedia.org/wiki/… or en.wikipedia.org/wiki/…? Best cite your source, to help readers and to comply with copyright requirements. It seems to me that you failed to convert the initial input from degrees to radians. I also notice that when I restrict angles to $[0°,90°]$ then the numbers are preserved but the order is reversed. So better check which angle is which. $\endgroup$ – MvG Oct 28 '18 at 21:13
3
$\begingroup$

The formula seems to have been derived this way:

First, roll around the world $x$ axis. The quaternion for this is $q_{x,\alpha} = \cos\alpha + (\sin\alpha)i.$

Second, pitch around the world $y$ axis. The quaternion is $q_{y,\beta} = \cos\beta + (\sin\beta)j.$

Third, yaw around the world $z$ axis. The quaternion is $q_{z,\gamma} = \cos\gamma + (\sin\gamma)k.$

A rotation that is done in steps like this is modeled by multiplying the quaternions. The quaternion for the first rotation goes on the right. Multiplying all these together, and recalling that $i^2 = j^2 = k^2 = 1,$ that $ij = k = -ji,$ that $jk = i = -kj,$ and that $ki = j = -ik,$

\begin{align} q_{z,\gamma}q_{y,\beta}q_{x,\alpha} &= \left(\cos\frac\gamma2 + \left(\sin\frac\gamma2\right)\mathrm k\right) \left(\cos\frac\beta2 + \left(\sin\frac\beta2\right)\mathrm j\right) \left(\cos\frac\alpha2 + \left(\sin\frac\alpha2\right)\mathrm i\right) \\ &= \left(\cos\frac\gamma2+ \left(\sin\frac\gamma2\right)\mathrm k\right) \\ & \qquad \left(\cos\frac\alpha2\cos\frac\beta2 + \left(\sin\frac\alpha2\cos\frac\beta2\right)\mathrm i + \left(\cos\frac\alpha2\sin\frac\beta2\right)\mathrm j - \left(\sin\frac\alpha2\sin\frac\beta2\right)\mathrm k\right) \\ &= \cos\frac\alpha2\cos\frac\beta2\cos\frac\gamma2 + \sin\frac\alpha2\sin\frac\beta2\sin\frac\gamma2 \\ & \qquad + \left(\sin\frac\alpha2\cos\frac\beta2\cos\frac\gamma2 - \cos\frac\alpha2\sin\frac\beta2\sin\frac\gamma2\right)\mathrm i\\ & \qquad + \left(\cos\frac\alpha2\sin\frac\beta2\cos\frac\gamma2 + \sin\frac\alpha2\cos\frac\beta2\sin\frac\gamma2\right)\mathrm j\\ & \qquad + \left(\cos\frac\alpha2\cos\frac\beta2\sin\frac\gamma2 - \sin\frac\alpha2\sin\frac\beta2\cos\frac\gamma2\right)\mathrm k. \end{align}

If you want a different order of rotations, rearrange the order of multiplication of the individual axis rotation quaternions accordingly.

To convert a quaternion to Euler angles, we use facts such as

\begin{align} &\left(\cos\frac\alpha2\cos\frac\beta2\cos\frac\gamma2 + \sin\frac\alpha2\sin\frac\beta2\sin\frac\gamma2\right) \\ & \qquad \left(\sin\frac\alpha2\cos\frac\beta2\cos\frac\gamma2 - \cos\frac\alpha2\sin\frac\beta2\sin\frac\gamma2\right)\\ & \quad + \left(\cos\frac\alpha2\sin\frac\beta2\cos\frac\gamma2 + \sin\frac\alpha2\cos\frac\beta2\sin\frac\gamma2\right)\\ & \qquad\qquad \left(\cos\frac\alpha2\cos\frac\beta2\sin\frac\gamma2 - \sin\frac\alpha2\sin\frac\beta2\cos\frac\gamma2\right) = \frac12 \sin\alpha \cos\beta. \end{align} and \begin{align} &\left(\sin\frac\alpha2\cos\frac\beta2\cos\frac\gamma2 - \cos\frac\alpha2\sin\frac\beta2\sin\frac\gamma2\right)^2\\ & \quad + \left(\cos\frac\alpha2\sin\frac\beta2\cos\frac\gamma2 + \sin\frac\alpha2\cos\frac\beta2\sin\frac\gamma2\right)^2 = \frac12(1 - \cos\alpha\cos\beta). \end{align}

Your function quaternion_to_euler is (in effect) setting the roll angle to $\mathrm{atan2}(\sin\alpha \cos\beta, \cos\alpha \cos\beta).$ This works fine if $\cos\beta > 0,$ but if $\cos\beta < 0$ it gives an answer that is $180$ degrees opposite from $\alpha,$ and if $\cos\beta = 0$ it has no valid way to determine $\alpha$ at all. Meanwhile, the formula for the pitch angle uses the $\arcsin$ function, which can only return angles in the range $-\frac\pi2 \leq \beta \leq \frac\pi2.$ In short, if you start with a pitch angle outside the range from $-90$ to $90$ degrees, you will not end up with what you started with.

As long as the pitch angle is not too large, however, the formulas seem to give reasonable results. There are just two things to watch out for: first, make sure you are comparing radians to radians (or degrees to degrees), and second, realize that in quaternion_to_euler, the variables X, Y, and Z are respectively roll, pitch, and yaw. If you list yaw first in the input to euler_to_quaternion but last in the output of quaternion_to_euler, as you did, the output will come out in reverse order from the input.

I implemented the functions as follows:

def euler_to_quaternion(r):
    (yaw, pitch, roll) = (r[0], r[1], r[2])
    qx = np.sin(roll/2) * np.cos(pitch/2) * np.cos(yaw/2) - np.cos(roll/2) * np.sin(pitch/2) * np.sin(yaw/2)
    qy = np.cos(roll/2) * np.sin(pitch/2) * np.cos(yaw/2) + np.sin(roll/2) * np.cos(pitch/2) * np.sin(yaw/2)
    qz = np.cos(roll/2) * np.cos(pitch/2) * np.sin(yaw/2) - np.sin(roll/2) * np.sin(pitch/2) * np.cos(yaw/2)
    qw = np.cos(roll/2) * np.cos(pitch/2) * np.cos(yaw/2) + np.sin(roll/2) * np.sin(pitch/2) * np.sin(yaw/2)
    return [qx, qy, qz, qw]

def quaternion_to_euler(q):
    (x, y, z, w) = (q[0], q[1], q[2], q[3])
    t0 = +2.0 * (w * x + y * z)
    t1 = +1.0 - 2.0 * (x * x + y * y)
    roll = math.atan2(t0, t1)
    t2 = +2.0 * (w * y - z * x)
    t2 = +1.0 if t2 > +1.0 else t2
    t2 = -1.0 if t2 < -1.0 else t2
    pitch = math.asin(t2)
    t3 = +2.0 * (w * z + x * y)
    t4 = +1.0 - 2.0 * (y * y + z * z)
    yaw = math.atan2(t3, t4)
    return [yaw, pitch, roll]

I tried your example input and got the following results:

>>> q = euler_to_quaternion([0.2,1.12,2.31])
>>> r = quaternion_to_euler(q)
>>> r
[0.20000000000000026, 1.1200000000000006, 2.3100000000000005]

It seems this is working fine. Looking at your results, the discrepancies are completely accounted for by the reversal of the order of the angles and the conversion between radians and degrees.


Addendum: As observed in a comment, if we consider all Euler-angle rotations where the angles can be anything in the range $-\pi$ to $\pi,$ every rotation can be expressed in at least two ways. That is because any sequence of rotations of the form $(\pm\pi, \pm\pi - \beta_1, \pm\pi)$ is equivalent to the sequence $(0, \beta_1, 0).$ Naturally these produce equivalent quaternions, but when you convert the quaternion back to Euler angles the quaternion cannot give you any clue as to which of the possible Euler angle inputs it came from. But you can always come up with at least one Euler-angle representation in which the pitch angle has a non-negative cosine. The function quaternion_to_euler is designed to give you that sequence of Euler angles.

The real weakness of the conversion function occurs when the pitch angle is $\pm\frac\pi2.$ Then $\cos\beta = 0$ and the formulas for roll and yaw do not work. You can convert Euler angles to a quaternion and back to non-equivalent Euler angles. You can tell the second set of Euler angles gives a different rotation than the first because it converts to a different quaternion. For example:

>>> q = euler_to_quaternion([0.2,0.5*np.pi,0.4])
>>> q
[0.070592885899994171, 0.70357419257695242, -0.070592885899994171, 0.70357419257695242]
>>> r = quaternion_to_euler(q)
>>> r
[3.141592653589793, 1.5707963267948966, 3.141592653589793]
>>> s = euler_to_quaternion(r)
>>> s
[0.0, 0.70710678118654757, 0.0, 0.70710678118654757]
$\endgroup$
  • $\begingroup$ The "problem" you cite (if a certain cosine is negative, you might get back $\alpha + 180$ instead of $\alpha$) is really intrinsic: going from pitch-roll-yaw angles to SO(3) isn't 1-1 unless we restrict the domain. One might restrict pitch to $-\pi < pitch \le \pi$, and similarly for roll, but restrict yaw to $-\pi/2 < yaw \le \pi/2$, or something like that. Roughly, yaw and pitch tell you which latitude and longitude you're pointing to on the sphere at infinity; roll tells you, within the unit circle in the tangent space to that point at infinity, which way your right wing is pointing. $\endgroup$ – John Hughes Oct 29 '18 at 0:02
  • $\begingroup$ BTW ... this is a really nice and thorough answer! $\endgroup$ – John Hughes Oct 29 '18 at 0:02
  • $\begingroup$ @JohnHughes I hadn't (yet) said it was a "problem," I just said that's how these particular formulas work. But I have added a discussion of the equivalency you point out. There actually is a problem with the implemented functions when $\cos(pitch) = 0,$ however, of which I have added an example to the answer. (BTW, thanks for the kind words!) $\endgroup$ – David K Oct 29 '18 at 0:48
  • $\begingroup$ @DavidK This looks to fail if I input the values [math.radians(89), math.radians(90), math.radians(89.5)] for roll, pitch and yaw. quaternion_to_euler will return weird values. Can you try it? Could this be happening due to some numerical instability issues? Also for [math.radians(179.9), math.radians(179.5), math.radians(179.8)]. I'm pretty new to Quaternions. Do they work with rotations more than 90 degrees in Euler angle space? Basically, whatever number I input from 90 degrees or more for rotation in Euler angle space the quaternion_to_euler function returns weird numbers back. $\endgroup$ – Amir Oct 31 '18 at 15:31
  • $\begingroup$ @DavidK Also, I would appreciate if you can take a look at this and see if you can provide an answer there. Basically, I need to know how I can, deterministically, calculate the bounds for the elements of a Quaternion rotation vector. $\endgroup$ – Amir Oct 31 '18 at 20:03

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service, privacy policy and cookie policy

Not the answer you're looking for? Browse other questions tagged or ask your own question.