5

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.

Amir
  • 10,600
  • 9
  • 48
  • 75

2 Answers2

10

We can use Rotation from scipy.spatial.transform.

from scipy.spatial.transform import Rotation

# Create a rotation object from Euler angles specifying axes of rotation
rot = Rotation.from_euler('xyz', [90, 45, 30], degrees=True)

# Convert to quaternions and print
rot_quat = rot.as_quat()
print(rot_quat)

The result would be:

[ 0.56098553  0.43045933 -0.09229596  0.70105738]

Then, you can also get it back in Euler angles:

print(rot.as_euler('xyz', degrees=True))

Which results in:

[90. 45. 30.]

As a final check, create a rotation object from the quaternions calculated above and get it as Euler angles:

rot = Rotation.from_quat(rot_quat)

# Convert the rotation to Euler angles given the axes of rotation
print(rot.as_euler('xyz', degrees=True))

Which results in:

[90. 45. 30.]
SaTa
  • 2,422
  • 2
  • 14
  • 26
5

Major problem:

The input order of euler_to_quaternion is different to the output order of quaternion_to_euler

The former takes angles in the order Z, Y, X (yaw, pitch, roll), and the latter returns X, Y, Z. Fix:

def euler_to_quaternion(roll, pitch, yaw):
# or
euler_to_quaternion(euler_Original[2], euler_Original[1], euler_Original[0])

Minor problem

euler_to_quaternion takes radians whereas quaternion_to_euler returns degrees.

Not really a problem per se, but it's always better to keep angles in radians as most library functions use them.

X = math.atan2(t0, t1)
Y = math.asin(t2)
Z = math.atan2(t3, t4)
meowgoesthedog
  • 14,670
  • 4
  • 27
  • 40
  • This fails 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 this? – Amir Oct 31 '18 at 15:20
  • Yes it's possible. 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. – Amir Oct 31 '18 at 15:27
  • @Amir oops ignore my previous comment. The weird values were due to **gimbal lock**, a troublesome property of Euler angles; setting Y (pitch) to 90 effectively "folds" the X and Z axes together, so there is no unique solution for the yaw and roll – both of them have the same effect so a degree of freedom is lost. As for the other case, you are exceeding the valid range of Y (`[-90, 90]`), which means your camera becomes "inverted". – meowgoesthedog Oct 31 '18 at 15:39
  • Is it generally the case that you cannot have values more than -90 degrees in Euler angle space when converting them to Quaternions or is it because of the way the formulas have been set up here? – Amir Oct 31 '18 at 16:31
  • @Amir it is due to the nature of the angles themselves (or what they represent). Only certain combinations will produce gimbal lock, e.g. setting only roll or yaw to 90. See the [Wiki page](https://en.wikipedia.org/wiki/Gimbal_lock). – meowgoesthedog Oct 31 '18 at 16:33