距離とは
回転同士の距離がどういうものか、といったことはここに日本語で詳しく書いてあります。とても良いページです。
簡単にまとめてみると、数学的に「距離」というためには、以下の条件を満たす必要があります。
- 正定値性: 常に0以上の値になる
- 非退化性: 同じものの距離は0で、距離が0でないなら同じものではない
- 対称性: AとBの距離は、BとAの距離と同じ
- 三角不等式を満たす: AとBの距離とBとCの距離の合計は、AとCの距離以上になる
回転の平均と距離の計算方法
これより下では、回転における距離の計算方法を紹介します。サンプルデータとして、以下のように、クオータニオンの集合と3x3行列の集合を作っておきます。Pythonでクオータニオンを扱うときは、pyquaternionが便利です。
from pyquaternion import Quaternion
import numpy as np
qlist = [Quaternion.random().elements for i in range(10)]
mlist = [q.rotation_matrix for q in qlist]
クオータニオンの場合
平均
クオータニオンの列があったとき、新しいクオータニオンをもう1つ考えます。このクオータニオンとクオータニオン列との内積の和が最小になるようにすれば、平均クオータニオンが計算されます。実際には、クオータニオン列をNx4の行列Xにしてしまい、X.T・X (4x4行列)の最大の固有ベクトルを求めればよいです。
def mean_q(qlist):
x = np.array([q.elements for q in qlist])
m = x.T @ x
w, v = np.linalg.eig(m)
return Quaternion(v[:, np.argmax(w)])
距離
内積をとってarccosして、2倍する、と、クオータニオン間の角度として距離が計算できます。
def dist_q(q1, q2):
return 2 * np.arccos(np.dot(q1.elements, q2.elements))
3x3行列の場合
平均
そのままでは難しいので、クオータニオンを経由して計算します。
距離
クオータニオンの方で求めたものと対応する角度として、以下のようなコードで距離が計算できます。
def dist_m(m1, m2):
return np.arccos((np.trace(m1 @ m2.T) - 1) / 2)
