How to convert Euler angles to Quaternions and get the same Euler angles back from Quaternions?

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.

Solution 1:

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\frac\alpha2 + \left(\sin\frac\alpha2\right)\mathrm i.$

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

Third, yaw around the world $z$ axis. The quaternion is $q_{z,\gamma} = \cos\frac\gamma2 + \left(\sin\frac\gamma2\right)\mathrm 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]