※この記事はQiitaからの引越し記事です。

物体の姿勢の表し方には以下の２種類があります。

連続的に表現するオイラー角
離散的に表現するクオータニオン

このうちクオータニオンはオイラー角とは異なり、ある姿勢を一意に決めてしまうものなので、オイラー角のように見た目の姿勢変化をそのまま値に反映することはできません。

本稿ではそんなクオータニオンの平均を取る手法について話します。

補足
「平均化」とは言わず、普通は「線形補間」と言います。
ただ、今回の目的が線形補間の中間地点を取得することなので、わかりやすさのために敢えて「平均化」と言ってます。

lerp() / slerp()

Unityで主に使われている手法で、２点間の異なるパラメータを[0〜1]の範囲で線形補間することができます。詳しい話は以下参照

対数化による線形補間

$\displaystyle \bar{q} = \exp (\sum{\omega_i\log{}q_i} )$

これは前項のlerp()/slerp()にも使われてる気がする。。

クオータニオンの平均（Averaging Quaternions）

平均化されたクオータニオンは次のように表すことができます。

$\displaystyle \bar{q} = \arg min_{Q \in R^{3 \times 3}} \sum{\| Q-Q_i \|}^2_F \\ q : quaternion~vector~(q \in R^4 , |q| = 1) \\ Q : rotate~matrix~(Q \in R^{3 \times 3})$

なお、クオータニオンの特徴で、回転行列 $Q$ は直交行列（orthogonal matrix）となります。

Step 1　数式の簡略化

$\displaystyle \begin{align} \bar{q} &= \arg min~\sum{\| Q-Q_i \|}^2_F \\ &= \arg min~\sum{\mathrm{tr}\{ (Q-Q_i)^T(Q-Q_i) \}} \end{align}$

ここで、

$\displaystyle \begin{align} \mathrm{tr}\{ (Q-Q_i)^T(Q-Q_i) \} &= \mathrm{tr}(Q^TQ-Q^TQ_i-Q_i^TQ+Q_i^TQ_i) \\ &= \mathrm{tr}(Q^TQ)-\mathrm{tr}(Q^TQ_i)-\mathrm{tr}(Q_i^TQ)+\mathrm{tr}(Q_i^TQ_i) \\ &= 6-\mathrm{tr}(Q^TQ_i)-\mathrm{tr}(Q_i^TQ) \\ &= 6-2\mathrm{tr}(Q_i^TQ) \end{align}$

となるので、

$\displaystyle \begin{align} \bar{q} &= \arg min~\sum{\mathrm{tr}\{ (Q-Q_i)^T(Q-Q_i) \}} \\ &= \arg min~\sum{\{ 6-2\mathrm{tr}(Q_i^TQ)} \} \\ &= \arg max~\sum{\mathrm{tr}(Q_i^TQ)} \\ &= \arg max~\mathrm{tr}(\sum{Q_i^TQ}) \\ &= \arg max~\mathrm{tr}(Q\sum{Q_i^T}) \end{align}$

Step 2　魔法の力による簡略化

ここでは魔法の力を借りることで更なる簡略化を施します。（魔法の力については最後の章で話します。ひとまず受け入れてクダサイ。）

$\displaystyle \begin{align} \bar{q} &= \arg max~\mathrm{tr}(Q\sum{Q_i^T}) \\ &= \arg max_{q \in R^4/|q|=1}~q^TKq \\ &= \arg max~q^T(4M - I)q \\ &= \arg max~q^TMq \end{align}$

ここでの $K$ 、 $M$ は以下の通り。

$\displaystyle K = 4M - I_{4 \times 4} \\ M = \sum{q_iq_i^T}$

Step 3　座標空間の変換

$M$ は先程の式より、対称行列（symmetric matrix）であることがわかります。（すべての要素が実数である場合、「実対称行列」と呼びます。）

よって $M$ に対角化を行い、元の数式に適用します。 $M$ が対称行列なので、対角化によって現れる行列 $P$ は直交行列であり、その大きさは１となります。

$\displaystyle M = P^T \Delta P$

$\displaystyle \begin{align} \bar{q} &= \arg max~q^T M q \\ &= \arg max~q^T P^T \Delta P q \end{align}$

ここで、 $y = Pq$ により、数式の空間を $q$ から $y$ へと変換します。 $q$ のノルムが１であることと、行列 $P$ が直交行列であることから、この変換によってベクトル $q$ はベクトル $y$ へ、大きさは変わらずに、向きだけが変わったものとみなせます。

$\displaystyle \begin{align} \bar{y} &= \arg max_{y \in R^4/|y|=1}~y^T \Delta y \\ &= \arg max~ y^T \left( \begin{array}{ccccc} \lambda_1 & & 0 \\ & \ddots & \\ 0 & & \lambda_n \end{array} \right) y\\ \end{align}$

Step 4　固有値・固有ベクトル

ここで、行列 $\Delta$ の固有値 $\lambda_i$ の中で、ある固有値 $\lambda_k$ が最も大きい値であるとき、行列 $\Delta$ の固有ベクトル $y_i$ の、ある固有ベクトル $y_k$ も最大となります。

$\displaystyle y_k^T = \left( \begin{array}{ccccc} 0 & \ldots & 0 & 1 & 0 & \ldots & 0 \end{array} \right)$

したがって、クオータニオンの平均値は $y_k$ となることがわかりました。

$\displaystyle \begin{align} \bar{y} &= \arg max_{y \in R^4/|y|=1}~y^T \Delta y = y_k \end{align}$

補足

前項での説明の補足を行っていきます。リクエストなどがあればコメントください。

魔法の力について

Step 2では「魔法の力」ということにして説明をごっそり省きました。なぜなら、この部分の変換がかなりハードであるためです。。（ユルシテクダサイ）

$\displaystyle \begin{align} \bar{q} &= \arg max~\mathrm{tr}(Q\sum{Q_i^T}) \\ &= \arg max_{q \in R^{4\times1}/|q|=1}~q^TKq \end{align}$

この節ではこのとき何が行われていたのかを（ざっくり）説明していきます。

回転行列Q

クオータニオンのベクトル $q$ を次のように表すとします。

$\displaystyle \begin{align} q &= {\begin{bmatrix} q_1 & q_2 & q_3 & q_4 \end{bmatrix}}^T \\ &= {\begin{bmatrix} ϱ^T & q_4 \end{bmatrix}}^T \end{align}$