はじめに
クォータニオンでの回転表現やロドリゲスの回転公式などをPythonで実装してみます．
数式レベルでは前々回・前回で世界一スッキリ？紹介しましたが，コードで数式を確認できるのと，やっぱり動けば楽しいということで．
サンプルプログラム – ライブラリ部分
前半は主要な数式を実装したライブラリ風のコード例です．
Numpyで線形代数をそこそこ扱えるので，数式のコード化は簡単です．エラー処理，計算速度，使い勝手などは度外視しています．
R()がロドリゲスの回転公式，Q()がそれと同じ回転を表す単位クォータニオンの算出，R2Q()とQ2R()が回転行列と単位クォータニオンとの相互換算，Rq()が単位クォータニオンによる回転計算です．(3-18, 29-30行目)R2Q_stable()は，R2Q()に桁落ち・ゼロ除算の対策(前回記事末尾の「下記2案ご参考」の第2案)を加えたものです．(20-27行目)- 関数の引数は，
th: 回転角[rad],u[3]: 単位ベクトル(回転軸)，q[4]: 単位クォータニオン，r[3,3]: 回転行列，などです．配列はNumpy配列型を想定しています．
import numpy as np
def R(u,th): # u軸θ回転の行列R (Rodriguesの回転公式)
u = u.reshape([3,1])
return np.cos(th)*np.eye(3) + np.sin(th)*vec2skew(u) + (1-np.cos(th))*u@u.T
def Q(u,th): # u軸θ回転を表す単位四元数q
return np.array([np.cos(th/2), *(np.sin(th/2)*u)]);
def Q2R(q): # q-->R換算
qr = q[0]
qi = q[1:].reshape([3,1])
return (2*qr**2-1)*np.eye(3) + 2*qr*vec2skew(qi) + 2*qi@qi.T
def R2Q(r): # R-->q換算
qr = np.sqrt(1 + np.trace(r)) / 2
qi = skew2vec(r-r.T) / (4*qr) # qr≒0(θ≒180度)のとき不安定
return np.array([qr, *qi])
def R2Q_stable(r): # R-->q換算 安定版
# qr:=1+tr(R・R0)が大きいR0=Q2R(q0)を選び，q=R2Q(R・R0^T)・q0 と計算
axis = np.argmax([np.trace(r), r[0,0], r[1,1], r[2,2]]) # qr最大軸
if axis==0: return R2Q(r) # q0=1, R0=I
if axis==1: q0,r0 = [0,1,0,0],[1,-1,-1] # q0=i,R0=i軸半回転
if axis==2: q0,r0 = [0,0,1,0],[-1,1,-1] # q0=j,R0=j軸半回転
if axis==3: q0,r0 = [0,0,0,1],[-1,-1,1] # q0=k,R0=k軸半回転
return qmul(R2Q(r@np.diag(r0)), np.array(q0)) # q=R2Q(R・R0^T)・q0
def Rq(x,q): # 回転の計算 x-->qxq~
return qmul(qmul(q,np.array([0,*x[-3:]])),qconj(q))[1:]
def vec2skew(v): # v∈R^3-->v_× (外積作用の行列)
v = v.reshape([3,])
return np.array([[0,-v[2],v[1]], [v[2],0,-v[0]], [-v[1],v[0],0]])
def skew2vec(a): # v_×-->v
return np.array([a[2,1], a[0,2], a[1,0]])
def qmul(q1,q2): # 四元数の積
qr = q1[0]*q2[0] - np.dot(q1[1:],q2[1:])
qi = q1[0]*q2[1:] + q1[1:]*q2[0] + np.cross(q1[1:],q2[1:])
return np.array([qr,*qi])
def qconj(q): # 四元数の共役
return np.array([q[0], -q[1], -q[2], -q[3]])サンプルプログラム – アプリ部分
後半は回転の計算＆検算をするユーザコード例です．python application.py で計算内容を出力．
数値を変えたり，$\theta \approx 180°$ での R2Q() の挙動を見たりして遊べます．Q() と R2Q_stable() は，戻り値の実部が非負と保証していませんのでご留意を．
from rotation import Q, R, R2Q_stable, Q2R, R2Q, Rq
import numpy as np
# 回転軸u(単位ベクトル), 回転角θ[rad], 検算用ベクトルx
u = np.random.normal(size=3)
u /= np.linalg.norm(u)
th = np.random.uniform(low=-np.pi, high=+np.pi)
#th = np.pi - 1.e-10
x = np.random.normal(size=3)
print('# u=%s th=%s x=%s' % (u, th, x))
# 回転を表す単位四元数qと回転行列rを算出, 相互換算, 回転計算
q1,r1 = Q(u,th), R(u,th)
q2,r2 = R2Q_stable(r1), Q2R(q1)
#q2 = R2Q(r1) # θ≒180度のとき不安定
xq,xr = Rq(x,q1), r1@x
print('q1=%s\nq2=%s\nr1=\n%s\nr2=\n%s\nxq=%s\nxr=%s' % (q1,q2,r1,r2,xq,xr))
# 各種検算
def equal(a, b, tolerance=1.e-8):
return np.abs(a-b).max()<=tolerance
assert (equal(q1,q2) or equal(q1,-q2)) and equal(r1,r2) and equal(xq,xr) # 2計算法が一致
assert equal(r1.T@r1,np.eye(3)) and np.linalg.det(r1)>0 # Rは回転行列
assert equal(np.dot(q1,q1),1) # qは単位四元数
xu = np.dot(x,u)*u
xv = x-xu
xw = np.cross(u,xv)
assert equal(xr, xu + np.cos(th)*xv + np.sin(th)*xw) # u軸θ回転Accutheraでは加速器・医療システム・機械学習・放射線シミュレーションなどの専門技術を軸に開発やコンサルティングを承っております。お困りの案件がございましたら、こちら からお気軽にお問合せください。
