\[\def\mbf#1{{\mathbf #1}}
\def\SO(#1){{\mathrm{SO}(#1)}}
\def\so(#1){{\mathfrak{so}(#1)}}
\def\SE(#1){{\mathrm{SE}(#1)}}
\def\se(#1){{\mathfrak{se}(#1)}}
\def\skew#1{{\left[ #1 \right]_{\times}}}\]
リー代数による回転表現¶
なぜリー代数を用いるのか¶
視覚情報ベースのSfMは，カメラから得た視覚情報を用いて，各視点におけるカメラの姿勢とランドマークの3次元座標を求める問題である．一方で，視覚情報ベースのSLAMはカメラから逐次的に得られる視覚情報を用いて環境地図とその中の相対的なカメラ姿勢を求める問題である．これらの問題では，カメラ姿勢とランドマークの3次元座標をパラメータとして最適化問題を解くアプローチがしばしば用いられる．このうちカメラ姿勢は原点座標からの並進 \(\mbf{t} \in \mathbb{R}^{3}\) と回転 \(R \in \SO(3)\) の組み合わせで表現することができる． \(\SO(3)\) は回転行列の集合である．
(1)¶\[\SO(3) = \left\{
R \in \mathbb{R}^{3 \times 3} \mid R^{\top}R = I, \det(R) = 1
\right\}\]
このときカメラの回転 \(R\) をそのままパラメータとして最適化問題を解くと，たいていの場合最適化の結果として回転行列でない行列 \(A \in \mathbb{R}^{3 \times 3} \setminus \SO(3)\) が得られてしまう．したがって，SfMやSLAMでは，回転行列の代わりにできる限り簡潔なパラメータ \(\mbf{\omega} \in \mathbb{S}\) でカメラの回転を表現し，最適化の結果として得られた \(\hat{\mbf{\omega}}\) をある関数 \(\phi: \mathbb{S} \to \SO(3)\) で射影することで回転行列 \(\hat{R} \in \SO(3)\) を得る．
回転を表現するためのパラメータ集合 \(\mathbb{S}\) としては \(\SO(3)\) のリー代数 \(\so(3)\) や単位四元数の集合 \(\left\{\mbf{q} \in \mathbb{H} \mid ||q|| = 1 \right\}\) が用いられる．ここでは前者について解説する．
\(\so(3)\) による回転表現¶
ここでは回転行列 \(R \in \SO(3)\) が3次元ベクトル \(\mbf{\omega} \in \mathbb{R}^{3}\) から生成できることを示す．
\(\so(3)\) は以下で定義される集合である．
\[\so(3) = \left\{
\skew{\mbf{\omega}} \mid \mbf{\omega} \in \mathbb{R}^{3}
\right\}\]
ここで \(\skew{\cdot}\) は3次元ベクトルに対応する歪対称行列を表現する演算子である．
\[\begin{split}\skew{\mbf{\omega}} = \begin{bmatrix}
0 & -\omega_{3} & \omega_{2} \\
\omega_{3} & 0 & -\omega_{1} \\
-\omega_{2} & \omega_{1} & 0
\end{bmatrix}\end{split}\]
\(\so(3)\) の元を指数写像で射影すると回転行列が得られる．
\[\exp : \so(3) \to \SO(3)\]
導出¶
実数から回転行列への写像 \(R(t)\) を考える．ただし \(t=0\) において \(R(t) = I\) を通るものとする．
\[R(t) : \mathbb{R} \to \SO(3), \; R(0) = I\]
回転行列の定義から， \(R(t)\) は次を満たす．
(2)¶\[R(t)^{\top} R(t) = I\]
微分すると
\[\begin{split}\begin{align}
\frac{d}{dt} (R(t)^{\top}R(t)) &= \frac{d R(t)}{dt}^{\top} R(t) + R(t)^{\top}\frac{d R(t)}{dt} = 0, \\ \\
-R(t)^{\top}\frac{d R(t)}{dt} &= {\frac{d R(t)}{dt}}^{\top} R(t) \\
&= \left[R(t)^{\top}\frac{d R(t)}{dt}\right]^{\top}
\end{align}\end{split}\]
したがって \(R(t)^{\top}\frac{d R(t)}{dt}\) は歪対称行列であり， \(\omega \in \mathbb{R}^{3}\) を用いて
(3)¶\[\begin{split}\begin{align}
-R(t)^{\top}(\frac{d R(t)}{dt})
&= \frac{d R(t)}{dt}^{\top} R(t) \\
&= \skew{\mbf{\omega}}, \\\\
\frac{d R(t)}{dt} &= -R(t)\skew{\mbf{\omega}}
\end{align}\end{split}\]
と表すことができる．
\(R\) を3つの直交基底を用いて \(R(t) = \begin{bmatrix} \mbf{e}_{1}(t) & \mbf{e}_{2}(t) & \mbf{e}_{3}(t) \end{bmatrix}^{\top}\) と表現すると， (3) は
\[\begin{split}\begin{align}
\frac{d \mbf{e}_{i}(t)}{dt}^{\top} &= -\mbf{e}_{i}(t)^{\top}\skew{\mbf{\omega}},\; &&i = 1,\dots,3 \\ \\
\frac{d \mbf{e}_{i}(t)}{dt} &= -\skew{\mbf{\omega}}^{\top}\mbf{e}_{i}(t) \\
&= \skew{\mbf{\omega}}\mbf{e}_{i}(t),\; &&i = 1,\dots,3
\end{align}\end{split}\]
という微分方程式であることがわかる．この方程式の解は
\[\mbf{e}_{i}(t) = \exp(\skew{\mbf{\omega}} t)\mbf{e}_{i}(0) ,\; i = 1,\dots,3\]
であることから， \(R(t)\) は \(\mbf{\omega}\) を用いて
(4)¶\[\begin{split}\begin{align}
R(t) &= \exp(\skew{\mbf{\omega}} t) \, R(0) \\
&= \exp(\skew{\mbf{\omega}} t)
\end{align}\end{split}\]
\[\begin{split}\begin{align}
\det(R(t))
&= \det(\exp(\skew{\mbf{\omega}}\, t)) \\
&= \exp(\operatorname{tr}(\skew{\mbf{\omega}}\, t)) \\
&= \exp(0) \\
&= 1
\end{align}\end{split}\]
となり， \(R(t)\) はやはり \(\SO(3)\) の元であることがわかる．
Rodriguesの回転公式¶
\(||\mbf{\omega}||^2 = 1\) のとき， \(\skew{\mbf{\omega}}^3 = -\skew{\mbf{\omega}}\) が成り立つ [1] ．これを利用すると，指数写像 (4) を別のかたちで表現できるようになる [1] ．
行列の指数関数の定義
(5)¶\[\begin{split}\exp(A) = \sum_{n=0}^{\infty} \frac{A^n}{n!} \\\end{split}\]
より，
(6)¶\[\begin{split}\begin{align}
R(t)
&= \exp(\skew{\mbf{\omega}}\, t) \\
&= \sum_{n=0}^{\infty} \frac{(\skew{\mbf{\omega}} \, t)^n}{n!} \\
&= I +
\skew{\mbf{\omega}} \, t +
\frac{\skew{\mbf{\omega}}^2 \, t^2}{2!} +
\frac{\skew{\mbf{\omega}}^3 \, t^3}{3!} +
\frac{\skew{\mbf{\omega}}^4 \, t^4}{4!} +
\frac{\skew{\mbf{\omega}}^5 \, t^5}{5!} +
\frac{\skew{\mbf{\omega}}^6 \, t^6}{6!} +
\dots \\
&= I +
\skew{\mbf{\omega}} \, t +
\frac{\skew{\mbf{\omega}}^2 \, t^2}{2!} +
\frac{-\skew{\mbf{\omega}} \, t^3}{3!} +
\frac{-\skew{\mbf{\omega}}^2 \, t^4}{4!} +
\frac{\skew{\mbf{\omega}} \, t^5}{5!} +
\frac{\skew{\mbf{\omega}}^2 \, t^6}{6!} +
\dots \\
&= I +
\left( t - \frac{t^3}{3!} + \frac{t^5}{5!} - \dots \right)
\skew{\mbf{\omega}} +
\left( \frac{t^2}{2!} - \frac{t^4}{4!} + \frac{t^6}{6!} - \dots \right)
\skew{\mbf{\omega}}^2
\end{align}\end{split}\]
\(\sin(t) = t - \frac{t^3}{3!} + \frac{t^5}{5!} - \dots\) ， \(1 - \cos(t) = \frac{t^2}{2!} - \frac{t^4}{4!} + \frac{t^6}{6!} - \dots\) を利用すれば，Rodriguesの回転公式を導くことができる．
(7)¶\[R(t) = I + \sin(t) \skew{\mbf{\omega}} + \left( 1 - \cos(t) \right) \skew{\mbf{\omega}}^2\]
行列の指数関数 (5) よりもRodriguesの回転公式のほうが計算の効率がよい．なお， \(||\mbf{\omega}||^2 = 1\) のとき \(\skew{\mbf{\omega}}^2 = \mbf{\omega} \mbf{\omega}^{\top} - I\) が成り立つため，これを利用して
(8)¶\[R(t) = \cos(t) I + (1 - \cos(t))\mbf{\omega}\mbf{\omega}^{\top} + \sin(t) \skew{\mbf{\omega}}\]
とする場合もある．実際に実装する際は \(\mbf{\omega},\, t\) という2つの値を与えるのではなく， \(\mbf{u} \in \mathbb{R}^{3}\) を引数として
\[\begin{split}\begin{align}
t &= ||\mbf{u}|| \\
\mbf{\omega} &= \frac{\mbf{u}}{||\mbf{u}||} \\
\exp(\skew{\mbf{u}}) &= \exp(\skew{\mbf{\omega}}\, t) \\
\end{align}\end{split}\]
とすることで，単一の引数からの指数写像の計算を可能にしている．
外積との関係¶
\(\mbf{u}, \mbf{v} \in \mathbb{R}^{3}\) について， \(\skew{\mbf{u}}\) と \(\mbf{v}\) の積は \(\mbf{u}\) と \(\mbf{v}\) の外積に等しい．
\[\begin{split}\skew{\mbf{u}} \mbf{v}
&= \begin{bmatrix}
u_{2}v_{3}-u_{3}v_{2} \\
u_{3}v_{1}-u_{1}v_{3} \\
u_{1}v_{2}-u_{2}v_{1}
\end{bmatrix}
&= \mbf{u} \times \mbf{v}\end{split}\]
また，\(\mbf{u}\) のノルム \(||\mbf{u}||\) がごく小さいとき， \(\exp(\skew{\mbf{u}}) \approx I + \skew{\mbf{u}}\) と近似できる．これは (6) において2次以降の項が無視できることから明らかである．
したがって，ベクトル \(\mbf{v}\) の \(\exp(\skew{\mbf{u}})\) による微小回転は
\[\begin{split}\exp(\skew{\mbf{u}}) \cdot \mbf{v}
&\approx (I + \skew{\mbf{u}}) \cdot \mbf{v} \\
&= \mbf{v} + \mbf{u} \times \mbf{v}\end{split}\]
と近似することができる．