matrix2euler bug

[thorstenwagner]

Hi,

thanks for this handy package :-) We are using eulerangles 1.0.2

We discovered a problem with matrix2euler. In short: For two rotation matrices which are almost the same (except some floating point accuracy problems) the euler angles calculated by matix2euler differ quite a lot. Here the script to reproduce it:

import numpy as np
from eulerangles import matrix2euler

def rotation_matrix_from_vectors(vec1: np.array, vec2: np.array):
    """ Find the rotation matrix that aligns vec1 to vec2
    :param vec1: A 3d "source" vector
    :param vec2: A 3d "destination" vector
    :return mat: A transform matrix (3x3) which when applied to vec1, aligns it with vec2.
    """
    a, b = (vec1 / np.linalg.norm(vec1)).reshape(3), (vec2 / np.linalg.norm(vec2)).reshape(3)
    v = np.cross(a, b)
    if any(v):  # if not all zeros then
        c = np.dot(a, b)
        s = np.linalg.norm(v)
        kmat = np.array([[0, -v[2], v[1]], [v[2], 0, -v[0]], [-v[1], v[0], 0]])
        return np.eye(3) + kmat + kmat.dot(kmat) * ((1 - c) / (s ** 2))

    else:
        return np.eye(3)

def _main_():
    a = np.array([0, 1, 0])
    b = np.array([0, 0, 1])
    rot_mat = rotation_matrix_from_vectors(a, b)
    print("First rotation matrix:")
    print(rot_mat)

    eulers = matrix2euler(rot_mat,
                          axes='zyz',
                          intrinsic=True,
                          right_handed_rotation=True)
    print("matrix2euler output for first rotation matrix:", eulers)

    a = np.array([0.0000001, 1, 0])
    b = np.array([0, 0, 1])
    rot_mat = rotation_matrix_from_vectors(a, b)
    print("Second rotation matrix")
    print(rot_mat)

    eulers = matrix2euler(rot_mat,
                          axes='zyz',
                          intrinsic=True,
                          right_handed_rotation=True)
    print("matrix2euler output for second rotation matrix:",eulers)

if __name__ == "__main__":
    _main_()

That is the output:

First rotation matrix:
[[ 1.  0.  0.]
 [ 0.  0. -1.]
 [ 0.  1.  0.]]
matrix2euler output for first rotation matrix: [ 0. 90.  0.]

Second rotation matrix
[[ 1.00000000e+00 -1.00000000e-07 -1.00000000e-07]
 [-1.00000000e-07  9.76996262e-15 -1.00000000e+00]
 [ 1.00000000e-07  1.00000000e+00 -2.22044605e-16]]
matrix2euler output for second rotation matrix: [  0.         90.        -89.9999944]

[alisterburt]

huh! thanks for the report - I'll take a look now and let you know what I find :)

[alisterburt]

Interesting, I'm not sure what the best path forward is here, I'll summarise the code path below

This is the zyz extrinsic rotation matrix (intrinsic eulers are directly computed from extrinsic eulers)

eulerangles/eulerangles/math/rotation_matrix_to_eulers.py

Lines 199 to 204 in 97a9b70

	def matrix2zyz_extrinsic(rotation_matrices: np.ndarray) -> np.ndarray:
	"""
	Rz(k3) @ Ry(k2) @ Rz(k1) = [[c1c2c3-s1s3, -s1c2c3-c1s3, s2c3],
	[c1c2s3+s1c3, -s1c2s3+c1c3, s2s3],
	[-c1s2, s1s2, c2]]
	"""

We see that the second euler angle can be read directly from the matrix

eulerangles/eulerangles/math/rotation_matrix_to_eulers.py

Lines 208 to 209 in 97a9b70

	# Angle 2 can be taken directly from matrices
	angles_radians[:, 1] = np.arccos(rotation_matrices[:, 2, 2])

If this angle is close to zero, we are in a 'gimbal lock' scenario where the first and third angles are not independent. This is true for both of your example matrices.

eulerangles/eulerangles/math/rotation_matrix_to_eulers.py

Lines 211 to 212 in 97a9b70

	# Gimbal lock case (s2 = 0)
	tolerance = 1e-4

In this case, we explicitly set the third angle to zero and calculate the first angle only

eulerangles/eulerangles/math/rotation_matrix_to_eulers.py

Lines 214 to 221 in 97a9b70

	# Find indices where this is the case
	gimbal_idx = np.abs(rotation_matrices[:, 0, 2]) < tolerance
	# Calculate angle 1 and set angle 3 = 0 for those indices
	r21 = rotation_matrices[gimbal_idx, 1, 0]
	r22 = rotation_matrices[gimbal_idx, 1, 1]
	angles_radians[gimbal_idx, 0] = np.arctan2(r21, r22)
	angles_radians[gimbal_idx, 2] = 0

For your first matrix, r22 and r21 are 0. atan2(r21, r22) is 0
For your second matrix, r22 is 9.8e-15 and r21 is -1e-7, atan2(r21, r22) is -1.57 radians

I think the problem is that with such a small difference between r21 and r22 you can't get a good estimate of the first angle. I could add a check for the relative difference of r21 and r22, if the difference is below the tolerance threshold then set the angle to zero by default. Does this make sense to you?

[thorstenwagner]

Thanks for checking! Why do you want to apply the threshold on the difference? I would rather apply on the entries in the rotation matrix itself?

[alisterburt]

My thought was that the situation which 'breaks' the code is when the two elements passed to atan2 are very small floats on opposite sides of 0 - for different euler angle conventions and depending on whether or not we are in a gimbal lock case the two elements passed to atan2 will be different so I thought maybe checking there was a good solution

A solution you can use within your own code is indeed to sanitise the elements of your rotation matrix though :)

I am happy to try to implement a fix here some time next week though, this is a real issue with the package which I'm sure was not the most fun to debug 😆

[jojoelfe]

Hey,

just wanted to mention that I think I ran into this or similar too: BradyAJohnston/MolecularNodes#366

Euler angles are just the worst sometimes. I guess at some point I should start to understand quaternions....