matthew-brett / transforms3d

3 dimensional spatial transformations
http://matthew-brett.github.io/transforms3d/
Other
467 stars 83 forks source link

quat2euler gives negative values for second angle in symmetric sequences #53

Open evbernardes opened 1 year ago

evbernardes commented 1 year ago

I noticed that quat2euler gives negative values for the middle angle in every symmetric sequence. By symmetric sequence, I mean sequences like zyz, xyx, etc.

This is inconsistent, for example, with SciPy. Applying this simple correction solves the problem:

def correct(angles, axes):
    if axes[1] != axes[-1]:
        return angles

Here's how the code I used to show that my correction makes it consistent with SciPy:

from scipy.spatial.transform import Rotation
from transforms3d import euler
from itertools import permutations
import numpy as np
from numpy.testing import assert_allclose

# this corrects the angles
def correct(angles, seq):
    if seq[1] != seq[-1]:
        return angles

    angles_new = angles.copy()
    idx = angles[:, 1] < 0
    angles_new[idx, 0] += np.pi
    angles_new[idx, 1] *= -1
    angles_new[idx, 2] += np.pi
    return angles_new

# this gets the angles on a big array
def quat2euler(quat, seq, apply_correct=True, direct_formula=False):
    angles = np.array([euler.quat2euler(quat[i], seq, direct_formula) for i in range(n)])
    if apply_correct:
        angles = correct(angles, seq)
    return (angles + np.pi) % (2 * np.pi) - np.pi

# this compares with the results from SciPy
n = 1000
rotation = Rotation.random(n)
quat = rotation.as_quat()[:,[3,0,1,2]]

for xyz in ('xyz', 'ZYX'):
    for seq_tuple in permutations(xyz):
        # symmetric
        seq = "".join([seq_tuple[0], seq_tuple[1], seq_tuple[0]])
        # print(seq)
        extra = 'r' if seq.isupper() else 's'
        angles_scipy = rotation.as_euler(seq)
        angles_transforms = quat2euler(quat, extra+seq.lower(), True)
        assert_allclose(angles_scipy, angles_transforms)

        # asymmetric
        seq = "".join(seq_tuple)
        # print(seq)
        extra = 'r' if seq.isupper() else 's'
        angles_scipy = rotation.as_euler(seq)
        angles_transforms = quat2euler(quat, extra+seq.lower(), False)
        assert_allclose(angles_scipy, angles_transforms)
matthew-brett commented 1 year ago

Am I right in saying that we do not guarantee postive angles? In which case, why would we prefer the positive angles - just for consistency with Scipy?

I am not sure how much to worry about this - but returning different angles for the same query would be an API break. I can't easily see why that would matter to a user, but I guess it could.

evbernardes commented 1 year ago

Basically, while the first and third angles have a span of 360 degrees, the second angle only spans 180 degrees. This can be seen, for example, on the fact that in direct single-sequence implementations, the second angle is usually either defined as an asin or an acos, while the others are always a arctan2. You can see this here.

For asymmetric axes, this second angle should be between -90 and +90 degrees, the two possibly singularities.

For symmetric axes, this second angle should be between 0 and 180 degrees, the two possibly singularities. Or, equivalently, they could be between 0 and -180 degrees which is also possible and is what is being given by transforms3d.

In this case, a whole different set of Euler angles are chosen, but I think this is probably inconsistent with other implementations, not only Scipy. And I'd argue it is also inconsistent with what one might expect: having this angle only be negative seems like an unnatural choice!

evbernardes commented 1 year ago

Hey there again, if you're interested, there's some discussion about this exact problem in Eigen's Gitlab repo now. It started here: https://gitlab.com/libeigen/eigen/-/issues/2617 And ended up creating this: https://gitlab.com/libeigen/eigen/-/merge_requests/1301