四元数を用いたプロクルステス問題の解法

概要

以下の内容の個人的まとめ。

1. 四元数（クォータニオン）を用いたプロクルステス問題の解法
2. 回転行列の最適補正
3. その周辺の話題

参考資料

３次元回転に関して丁寧に解説されている本

金谷健一著　「３次元回転　ーパラメータKさんとリー代数による最適化」共立出版

こちらの本は非常に丁寧に書かれているので、ご購入されるとよいと思います。
以下ではこの本のことを回転本といいます。

四元数を用いたプロクルステス問題の解法を示した論文

B.K.P.Horn Closed-form solution of absolute orientation using unit quaternions

1. 四元数（クォータニオン）を用いたプロクルステス問題の解法

1.1 回転行列を用いたプロクルステス問題

剛体の回転を考える。剛体上のNこの観測点に関して、

${\bf a}_\alpha \ \ (\alpha=1, \ldots ,N)$ :回転前の点の座標

${\bf a}_\alpha' \ \ (\alpha=1, \ldots ,N)$ :回転後の点の座標

とする。

図を入れる

回転行列 ${\rm \bf R}$ を用いたプロクルステス問題を考える。

J = \frac{1}{2} \sum^{N}_{\alpha = 1} \| {\bf a}_{\alpha}' - {\bf R}{\bf a}_{\alpha} \|^2 \rightarrow min \tag{1.1.1}

これを展開して

\begin{eqnarray}
J &=& \frac{1}{2} \sum^{N}_{\alpha = 1} \| {\bf a}_{\alpha}' - {\bf R}{\bf a}_{\alpha} \|^2 \tag{1.1.2} \\
&=& \frac{1}{2} \sum^{N}_{\alpha = 1} \| {\bf a}_{\alpha}'\|^2 - \sum^{N}_{\alpha = 1} \langle {\bf R}{\bf a}_\alpha , {\bf a}_{\alpha}' \rangle + \frac{1}{2} \sum^{N}_{\alpha = 1} \| {\bf a}_{\alpha} \|^2  \tag{1.1.3} \\
\end{eqnarray}

１項目と３項目はconstなので J を最小化するには２項目の

K = \sum^{N}_{\alpha = 1} \langle {\bf R}{\bf a}_\alpha , {\bf a}_{\alpha}' \rangle \tag{1.1.4}

を最大化させればいい。

1.2 四元数によるプロクルステス問題の表記

回転行列 ${\bf R}$ は四元数 $q = q_0 + q_1i + q_2j + q_3k$ を用いて

{\bf R} = 
\begin{pmatrix}
q_0^2+q_1^2-q_2^2-q_3^2 & 2(q_1q_2 - q_0q_3)            & 2(q_1q_3 + q_0q_2) \\
2(q_2q_1 + q_0q_3)      & q_0^2 - q_1^2 + q_2^2 - q_3^2 & 2(q_2q_3 - q_0q_1) \\
2(q_3q_1 - q_0q_2)      & 2(q_3q_2 + q_0q_1)            & q_0^2-q_1^2-q_2^2+q_3^2 
\end{pmatrix} \tag{1.2.1}

とかけるので、(1.1.4)式 K 内の ${\bf R}{\bf a}_{\alpha}$ は

{\bf a}_{\alpha} =
\begin{pmatrix}
a_{\alpha (1)} \\
a_{\alpha (2)} \\
a_{\alpha (3)}
\end{pmatrix}\tag{1.2.2}

を用いて

\begin{eqnarray}
{\bf R}{\bf a}_{\alpha} &=& \begin{pmatrix}
q_0^2+q_1^2-q_2^2-q_3^2 & 2(q_1q_2 - q_0q_3)            & 2(q_1q_3 + q_0q_2) \\
2(q_2q_1 + q_0q_3)      & q_0^2 - q_1^2 + q_2^2 - q_3^2 & 2(q_2q_3 - q_0q_1) \\
2(q_3q_1 - q_0q_2)      & 2(q_3q_2 + q_0q_1)            & q_0^2-q_1^2-q_2^2+q_3^2 
\end{pmatrix} 
\begin{pmatrix}
a_{\alpha (1)} \\
a_{\alpha (2)} \\
a_{\alpha (3)}
\end{pmatrix} \\

&=& 
\begin{pmatrix}
(q_0^2+q_1^2-q_2^2-q_3^2) a_{\alpha (1)}+2(q_1q_2 - q_0q_3)a_{\alpha (2)}+2(q_1q_3 + q_0q_2) a_{\alpha (3)} \\
2(q_2q_1 + q_0q_3) a_{\alpha (1)}+(q_0^2 - q_1^2 + q_2^2 - q_3^2) a_{\alpha (2)} + 2(q_2q_3 - q_0q_1) a_{\alpha (3)} \\
2(q_3q_1 - q_0q_2) a_{\alpha (1)} + 2(q_3q_2 + q_0q_1) a_{\alpha (2)} + (q_0^2-q_1^2-q_2^2+q_3^2 )a_{\alpha (3)}
\end{pmatrix}\tag{1.2.3}
\end{eqnarray}

となるので、$\langle {\bf R}{\bf a}_\alpha , {\bf a}_{\alpha}' \rangle$ は

\begin{eqnarray}
\langle {\bf R}{\bf a}_\alpha , {\bf a}_{\alpha}' \rangle &=& 
\begin{pmatrix}
(q_0^2+q_1^2-q_2^2-q_3^2) a_{\alpha (1)}+2(q_1q_2 - q_0q_3)a_{\alpha (2)}+2(q_1q_3 + q_0q_2) a_{\alpha (3)} \\
2(q_2q_1 + q_0q_3) a_{\alpha (1)}+(q_0^2 - q_1^2 + q_2^2 - q_3^2) a_{\alpha (2)} + 2(q_2q_3 - q_0q_1) a_{\alpha (3)} \\
2(q_3q_1 - q_0q_2) a_{\alpha (1)} + 2(q_3q_2 + q_0q_1) a_{\alpha (2)} + (q_0^2-q_1^2-q_2^2+q_3^2 )a_{\alpha (3)}
\end{pmatrix} \ast 
\begin{pmatrix}
a_{\alpha (1)}' \\
a_{\alpha (2)}' \\
a_{\alpha (3)}'
\end{pmatrix} \\ \tag{1.2.4}

&=& \ \ 

(q_0^2+q_1^2-q_2^2-q_3^2) a_{\alpha (1)}a_{\alpha (1)}'+2(q_1q_2 - q_0q_3)a_{\alpha (2)}a_{\alpha (1)}'+2(q_1q_3 + q_0q_2) a_{\alpha (3)}a_{\alpha (1)}' \\
\ \   &+& 2(q_2q_1 + q_0q_3) a_{\alpha (1)}a_{\alpha (2)}'+(q_0^2 - q_1^2 + q_2^2 - q_3^2) a_{\alpha (2)}a_{\alpha (2)}' + 2(q_2q_3 - q_0q_1) a_{\alpha (3)}a_{\alpha (2)}' \\
\ \  &+& 2(q_3q_1 - q_0q_2) a_{\alpha (1)}a_{\alpha (3)}' + 2(q_3q_2 + q_0q_1) a_{\alpha (2)}a_{\alpha (3)}' + (q_0^2-q_1^2-q_2^2+q_3^2 )a_{\alpha (3)}a_{\alpha (3)}'


\end{eqnarray}

となる。ここで＊は内積。よって (1.1.4) のKは

\begin{eqnarray}
K &=& \sum^{N}_{\alpha = 1} \langle {\bf R}{\bf a}_\alpha , {\bf a}_{\alpha}' \rangle \\
 &=& \sum^{N}_{\alpha = 1} \{
(q_0^2+q_1^2-q_2^2-q_3^2) a_{\alpha (1)}a_{\alpha (1)}'+2(q_1q_2 - q_0q_3)a_{\alpha (2)}a_{\alpha (1)}' \\ &+&  2(q_1q_3 + q_0q_2) a_{\alpha (3)}a_{\alpha (1)}' + 2(q_2q_1 + q_0q_3) a_{\alpha (1)}a_{\alpha (2)}' \\ &+& (q_0^2 - q_1^2 + q_2^2 - q_3^2) a_{\alpha (2)}a_{\alpha (2)}' + 2(q_2q_3 - q_0q_1) a_{\alpha (3)}a_{\alpha (2)}' \\
&+& 2(q_3q_1 - q_0q_2) a_{\alpha (1)}a_{\alpha (3)}' + 2(q_3q_2 + q_0q_1) a_{\alpha (2)}a_{\alpha (3)}' \\ &+& (q_0^2-q_1^2-q_2^2+q_3^2 )a_{\alpha (3)}a_{\alpha (3)}' \}\tag{1.2.5}


\end{eqnarray}

これを ${\bf q}$ に関してまとめたい。よって各 $q_x$ でまとめ直す。

\begin{eqnarray}
K  &=& \sum^{N}_{\alpha = 1} \{
q_0^2 (a_{\alpha (1)}a_{\alpha (1)}' + a_{\alpha (2)}a_{\alpha (2)}' + a_{\alpha (3)}a_{\alpha (3)}) +
q_1^2 (a_{\alpha (1)}a_{\alpha (1)}' - a_{\alpha (2)}a_{\alpha (2)}' - a_{\alpha (3)}a_{\alpha (3)}) \\
&+& q_2^2 (- a_{\alpha (1)}a_{\alpha (1)}' + a_{\alpha (2)}a_{\alpha (2)}' - a_{\alpha (3)}a_{\alpha (3)}) 
+ q_3^2 (- a_{\alpha (1)}a_{\alpha (1)}' - a_{\alpha (2)}a_{\alpha (2)}' + a_{\alpha (3)}a_{\alpha (3)}) \\
&+& 2q_0q_1(a_{\alpha (2)}a_{\alpha (3)}' - a_{\alpha (3)}a_{\alpha (2)}') 
+ 2q_0q_2(-a_{\alpha (1)}a_{\alpha (3)}' + a_{\alpha (3)}a_{\alpha (1)}') \\
&+& 2q_0q_3(a_{\alpha (1)}a_{\alpha (2)}' - a_{\alpha (2)}a_{\alpha (1)}') 
+ 2q_1q_2(a_{\alpha (1)}a_{\alpha (2)} + a_{\alpha (2)}a_{\alpha (1)}') \\
&+& 2q_1q_3(a_{\alpha (1)}a_{\alpha (3)} + a_{\alpha (3)}a_{\alpha (1)})
 + 2q_2q_3(a_{\alpha (2)}a_{\alpha (3)} + a_{\alpha (3)}a_{\alpha (2)})
 \} \tag{1.2.6}
\end{eqnarray}

ごちゃごちゃしてきたので回転本やHornの論文の表記に従って、

N_{ij} = \sum_{\alpha=1}^{N} a_{\alpha (i)}a_{\alpha (j)} \tag{1.2.7}

とすると、

\begin{eqnarray}
K  &=&  
q_0^2 (N_{1,2} + N_{2,2} + N_{3,3}) + q_1^2 (N_{1,1} - N_{2,2} - N_{3,3}) \\
&+& q_2^2 (- N_{1,1} + N_{2,2} - N_{3,3}) + q_3^2 (- N_{1,1} - N_{2,2} + N_{3,3}) \\
&+& 2q_0q_1(N_{2,3} - N_{3,2}) + 2q_0q_2(-N_{1,3} + N_{3,1}) \\
&+& 2q_0q_3(N_{1,2} - N_{2,1}) + 2q_1q_2(N_{1,2} + N_{2,1}) \\
&+& 2q_1q_3(N_{1,3} + N_{3,1}) + 2q_2q_3(N_{2,3} + N_{3,2}) \tag{1.2.8}
\end{eqnarray}

そうすると、$q_x$ に関して二次式となっているので、これのまとめ方として

{\bf q}{\bf q}^{\top}{\bf \tilde{N}} \tag{1.2.9}

とか

\langle {\bf q}, {\bf \tilde{N}}{\bf q} \rangle \tag{1.2.10}

といったものが想像できる。

後者で考えると、$q^2_x$ の係数が $\tilde{N}$ の対角成分に並ぶので

\tilde{N} =

\begin{pmatrix}
N_{1,2} + N_{2,2} + N_{3,3} & \cdots & \cdots & \cdots \\
\cdots & N_{1,2} - N_{2,2} - N_{3,3} & \cdots & \cdots \\
\cdots & \cdots & - N_{1,2} + N_{2,2} - N_{3,3} & \cdots \\
\cdots & \cdots & \cdots & -N_{1,2} - N_{2,2} + N_{3,3}
\end{pmatrix} \tag{1.2.11}

となればいいだろう。

それ以外の成分に関しては、例えば $q_0q_1$ の係数は この行列の(0, 1)成分と(1, 0)成分に対応するので、

\tilde{N} =

\begin{pmatrix}
\cdots & N_{2,3} - N_{3,2} & \cdots & \cdots \\
N_{2,3} - N_{3,2} & \cdots & \cdots & \cdots \\
\cdots & \cdots & \cdots & \cdots \\
\cdots & \cdots & \cdots & \cdots
\end{pmatrix} \tag{1.2.12}

とすれば足して２倍となってちょうどいい。そうすると、(1.2.8)式の $q_iq_j \left( i\neq j \right)$ となる６つの項は $\tilde{N}$ の非対角成分と対応する。

よって、求める行列は

\tilde{N} =
\begin{pmatrix}
N_{1,2} + N_{2,2} + N_{3,3} & N_{2,3} - N_{3,2} & -N_{1,3} + N_{3,1} & N_{1,2} - N_{2,1} \\
N_{2,3} - N_{3,2} & N_{1,1} - N_{2,2} - N_{3,3} & N_{1,2} + N_{2,1} & N_{1,3} + N_{3,1} \\
-N_{1,3} + N_{3,1} &  N_{1,2} + N_{2,1} & - N_{1,1} + N_{2,2} - N_{3,3} & N_{2,3} + N_{3,2} \\
N_{1,2} - N_{2,1} & N_{1,3} + N_{3,1} & N_{2,3} + N_{3,2} & -N_{1,1} - N_{2,2} + N_{3,3}
\end{pmatrix} \tag{1.2.13}

とわかる。これにより求める最大化問題は

K = \langle {\bf q}, {\bf \tilde{N}}{\bf q} \rangle \rightarrow max \tag{1.2.14}

となる。

${\bf q}$ は４次元だが、わかりやすく３次元で考えてみる。

この図のように ${\bf q}$ ベクトルがあって、それが一次変換${\bf \tilde{N}}$ によって回転・伸縮させられる。

一次変換${\bf \tilde{N}}$ はconstで、 ${\bf q}$ ベクトルは任意に選択できる。ただし、 ${\bf q}$ は四元数なのでノルムは１である。

このとき、どのような ${\bf q}$ ベクトルが内積 $ \langle {\bf q}, {\bf \tilde{N}}{\bf q} \rangle$ を最大化させられるだろうか。

考えられる１つは、以下の図のように $\tilde{N}$ によって向きが変わらないもの。

そうすると両者の $\cos \theta$ が最大なので、筋がよさそう。その場合、${\bf q}$ は $\tilde{N}$ の固有ベクトル。

さらに、同じ向きの中ではより大きさが増大する方がよい。

なので、結局、${\bf q}$ は $\tilde{N}$ の固有ベクトルの中で最大固有値に対応するものがよさげ。 -> 証明は？

回転行列の最適補正

プロクルステス問題を解いて回転行列を得たとしても、誤差のためにそれらが正しい回転行列・・つまりその列ベクトルが正規直交系をなしている・・・とは限らない。

今、

${\bf \hat{R}}$ : 観測点から推定した回転行列

${\bf R}$ : 補正後の（烈ベクトルをより正規直交系に近づけた？）回転行列

とする。

やりたいことは、できるだけ値を変えずに ${\bf \hat{R}}$ を ${\bf R}$ に変換すること。そこで ${\bf R}$ と ${\bf \hat{R}}$ との距離の測り方が重要となるが、ここでは回転本に従って以下のフロベニウスノルムを用いる。

\| {\bf A} \|_F = \sqrt{\sum_{i=1}^{m} \sum_{j=1}^{n} A_{ij}^2} \tag{2.1}

よって、問題の定式化をすると

\newcommand{\argmin}{\mathop{\rm arg~min}\limits}

{\bf R} = \argmin_{R} \| {\bf R} - \hat{\bf R} \|_F^2 \tag{2.2}

という感じか。後の計算のため２乗とした。具体的にはフロベニウスノルムの２乗がtransposeとの積のトレースとなることを用いる。

\| {\bf A} \|_F^2 = {\rm tr} ({\bf A}{\bf A}^\top ) = {\rm tr} ({\bf A}^\top {\bf A} )

そうすると(2.2)式は

\begin{eqnarray}

\| {\bf R} - \hat{\bf R} \|_F^2 &=& {\rm tr}(({\bf R} - \hat{\bf R})({\bf R} - \hat{\bf R})^{\top}) \tag{2.3} \\
&=& {\rm tr}(({\bf R} - \hat{\bf R})({\bf R}^{\top} - \hat{\bf R}^{\top})) \tag{2.4} \\
&=& {\rm tr}({\bf R}{\bf R}^\top - {\bf R}\hat{\bf R}^\top - \hat{\bf R}{\bf R}^{\top} + \hat{\bf R} \hat{\bf R}^\top) \tag{2.5} \\
&=& {\rm tr}({\bf R}{\bf R}^\top) - {\rm tr}({\bf R}\hat{\bf R}^\top) - {\rm tr}(\hat{\bf R}{\bf R}^{\top}) + {\rm tr}(\hat{\bf R} \hat{\bf R}^\top) \tag{2.6} \\
&=& {\rm tr}{\bf I} - {\rm tr}({\bf R}\hat{\bf R}^\top) - {\rm tr}({\bf R}\hat{\bf R}^\top)^\top + {\rm tr}(\hat{\bf R} \hat{\bf R}^\top) \tag{2.7} \\
&=& 3 - 2{\rm tr}({\bf R}\hat{\bf R}^\top) + \|\hat{\bf R}\|_F^2 \tag{2.8} \\
\end{eqnarray}

となる。

(2.3)式から(2.4)式へは引いてからtransposeしてもtransposeして引いても同じとなる性質、(2.5)式から(2.6)式へは足したり引いたりしてtraceを求めてもtrace求めて足したり引いても同じとなる性質、(2.7)式から(2.8)式へはtransposeしてもtraceは変わらない性質及びフロベニウスノルムの２乗がtransposeとの積のtraceとなる性質をそれぞれ用いた。

(2.8)式において、３項目はconst。よって(2.8)式を最小化することは２項目を最大化させること。つまり求める ${\bf R}$ は

\newcommand{\argmax}{\mathop{\rm arg~max}\limits}

{\bf R} = \argmax_{R} {\rm tr}({\bf R}  \hat{\bf R}^\top)  \tag{2.9}

これは $\hat{\bf R}$ の特異値分解を

\hat{\bf R} = {\bf U \Sigma V}^\top

として

{\bf R} = {\bf U}
\begin{pmatrix}
1 & 0 &  0 \\
0 & 1 & 0 \\
0 &0 & | {\bf UV} |
\end{pmatrix}
{\bf V}^\top

となる。（途中過程は回転本第四章の３など参照されたし）

個人的な疑問

前節で書いた四元数を用いてプロクルステス問題を解いた場合、得られる四元数のノルムが１となるよう各要素 $q_0, q_1, q_2, q_3$ を補正したのではダメなのか？それを求めて(1.1.2)式で回転行列に変換した方が早そうだが。。。