Ich musste den z-y-x-Ölerwinkel aus dem Quaternion berechnen.
Da es sich um eine Angelegenheit handelt, die in Python 2.7 geschrieben werden muss, können Sie außerdem keine praktische wie "scipy.spatial.transform.Rotation" verwenden.
Als ich die Formel nachgeschlagen habe, habe ich auf Wikipedia C ++ - Beispielcode gefunden.
Wikipedia / Quaternion to Euler Angles Conversion
Ich habe den Python 2.7-Code unter Bezugnahme auf den Beispielcode von Wikipedia geschrieben.
import math
import numpy as np
from pyquaternion import Quaternion
def quaternion_to_euler_zyx(q):
"""
Quarternion z-y-In X-Typ-Ölerwinkel umrechnen.
Parameters
----------
q : Quaternion
Quarternion(Pyquaternion-Format)
Returns
-------
np.array
z-y-x-basierter Ölerwinkel
"""
# roll :Drehung der x-Achse
sinr_cosp = 2 * (q[0] * q[1] + q[2] * q[3])
cosr_cosp = 1 - 2 * (q[1] * q[1] + q[2] * q[2])
roll = math.atan2(sinr_cosp, cosr_cosp)
# pitch :Drehung der y-Achse
sinp = 2 * (q[0] * q[2] - q[3] * q[1])
if math.fabs(sinp) >= 1:
pitch = math.copysign(math.pi / 2, sinp)
else:
pitch = math.asin(sinp)
# yaw :Drehung der Z-Achse
siny_cosp = 2 * (q[0] * q[3] + q[1] * q[2])
cosy_cosp = 1 - 2 * (q[2] * q[2] + q[3] * q[3])
yaw = math.atan2(siny_cosp, cosy_cosp)
#Ölerwinkel
retrun np.array([
math.degrees(roll),
math.degrees(pitch),
math.degrees(yaw)
])
numpy 1.16.6
Dies ist diejenige, die für die Vektorberechnung verwendet wird. 1.16.6 scheint die neueste Version der Python 2.7-Unterstützung zu sein.
pyquaternion 0.9.5
Ich benutze "Pyquaternion", um Quaternion zu berechnen.
numpy-quaternion scheint besser mit numpy kompatibel zu sein, aber ich konnte es nicht in der Python 2.7-Umgebung installieren.
Wenn Sie "scipy.spatial.transform.Rotation" verwenden, können Sie es einfacher schreiben.
import numpy as np
from pyquaternion import Quaternion
from scipy.spatial.transform import Rotation as R
def quaternion_to_euler_zyx(q):
r = R.from_quat([q[0], q[1], q[2], q[3]])
return r.as_euler('zyx', degrees=True)
Scipy.spatial.transform.Rotation scheint Python 2.7 jedoch nicht zu unterstützen. Es tut uns leid.
Normalerweise berühre ich nur den Z-x-y-Ölerwinkel von Unity, daher war das z-y-x-System ziemlich verwirrt.
Bei der Erstellung dieses Artikels habe ich auf die folgenden Artikel verwiesen. Vielen Dank!
Recommended Posts