回転の平均、回転同士の距離

距離とは

回転同士の距離がどういうものか、といったことはここに日本語で詳しく書いてあります。とても良いページです。

簡単にまとめてみると、数学的に「距離」というためには、以下の条件を満たす必要があります。

  • 正定値性: 常に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)