Pythonでクォータニオンをz-y-x系オイラー角に変換するコード書いてみた
クォータニオンからz-y-x系オイラー角の計算をしなければならなくなりました。
しかも、Python2.7で書かなければいけない案件なので、scipy.spatial.transform.Rotation みたいな便利なやつ使えません。
公式を調べてみたら、WikipediaにC++のサンプルコードが載っていました。
Wikipedia / Quaternion to Euler Angles Conversion
Pythonで書いてみた
Wikipediaのサンプルコードを参考にして、Python2.7のコードを書いてみました。
import math
import numpy as np
from pyquaternion import Quaternion
def quaternion_to_euler_zyx(q):
"""
クォータニオンをz-y-x系オイラー角に変換する。
Parameters
----------
q : Quaternion
クォータニオン(pyquaternion形式)
Returns
-------
np.array
z-y-x系オイラー角
"""
# roll : x軸回転
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 : y軸回転
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 : z軸回転
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)
# オイラー角
retrun np.array([
math.degrees(roll),
math.degrees(pitch),
math.degrees(yaw)
])
依存ライブラリ
numpy 1.16.6
ベクトル計算とかに使うやつです。1.16.6がPython2.7対応の中では最新っぽいです。
pyquaternion 0.9.5
Quaternionの計算に pyquaternion 使ってます。
numpy-quaternion の方がnumpyと相性はよさそうなのですが、Python2.7環境だとインストールできませんでした。
Python3以降ならscipy.spatial.transform.Rotationが楽
scipy.spatial.transform.Rotation を使えば、もっとシンプルに書けそうです。
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 はPython2.7には対応してないみたいです。残念。
さいごに
普段はUnityのz-x-y系オイラー角ばかり触ってるので、z-y-x系はかなり混乱しました。
本記事作成にあたり、以下の記事を参考にさせていただきました。ありがとうございました！