-
Notifications
You must be signed in to change notification settings - Fork 84
Open
Description
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)
Metadata
Metadata
Assignees
Labels
No labels