3次元の剛体変換を表すリー群\(\mathrm{SE}(3)\)と、リー代数\(\mathfrak{se}(3)\)に関する、自分用のメモ書きです。
リー代数\(\boldsymbol{\xi} = \left[ \begin{array}{c} \boldsymbol{\phi} \\ \boldsymbol{\rho} \end{array} \right] \in \mathfrak{se}(3)\)と、それに対応するリー群(剛体変換)\(\mathbf{T} = \exp(\boldsymbol{\xi}^\wedge) \in \mathrm{SE}(3)\)を考える。 \(\boldsymbol{\phi} \in \mathfrak{so}(3)\)、\(\boldsymbol{\rho} \in \mathbb{R}^3\)である。 \(\boldsymbol{\xi}\)と\(\mathbf{T}\)との間には、次の関係が成立する。
\[ \mathbf{T} = \exp(\boldsymbol{\xi}^\wedge) = \left[ \begin{array}{cc} \mathbf{C} & \mathbf{J}(\boldsymbol{\phi}) \boldsymbol{\rho} \\ \mathbf{0}^\top & 1 \end{array} \right] \]
ここで、\(\mathbf{C} = \exp(\boldsymbol{\phi}^\wedge) \in \mathrm{SO}(3)\)は、\(\boldsymbol{\phi}\)から得た回転行列である。 またヤコビ行列\(\mathbf{J}\)は、こちらのメモで定義された\(\mathbf{J}_l\)と同じものである(左側バージョン)。 \(\boldsymbol{\phi} = \phi \mathbf{a}\)、\(\phi = | \boldsymbol{\phi} |\)、\(\mathbf{a} = \boldsymbol{\phi} / \phi\)である。
\[ \mathbf{J}(\boldsymbol{\phi}) = \frac{\sin \phi}{\phi} \mathbf{I} + \left( 1 - \frac{\sin \phi}{\phi} \right) \mathbf{a} \mathbf{a}^\top + \frac{1 - \cos \phi}{\phi} \mathbf{a}^\wedge \]
\(\boldsymbol{\xi} = \left[ \begin{array}{c} \boldsymbol{\phi} \\ \boldsymbol{\rho} \end{array} \right] \in \mathfrak{se}(3)\)に対して、\(\wedge\)演算子は次のように定義される(Wedge演算子)。 \(\boldsymbol{\phi} \in \mathfrak{so}(3)\)に対する\(\wedge\)演算子は、こちらのメモを参照する。 \(\vee\)演算子は、\(\wedge\)とは真逆の操作を行う(Vee演算子)。
\[ \boldsymbol{\xi}^\wedge = \left[ \begin{array}{cc} \boldsymbol{\phi}^\wedge & \boldsymbol{\rho} \\ \mathbf{0}^\top & 0 \end{array} \right] \in \mathbb{R}^{4 \times 4} \]
\(\boldsymbol{\xi}\)から\(\mathbf{T}\)を計算するためには、次のようにする。
\(\mathbf{T}\)の逆行列\(\mathbf{T}^{-1}\)は、次のように表される(\(\mathbf{r} = \mathbf{J}(\boldsymbol{\phi}) \boldsymbol{\rho}\))。
\[ \mathbf{T}^{-1} = \left[ \begin{array}{cc} \mathbf{C}^\top & -\mathbf{C}^\top \mathbf{r} \\ \mathbf{0}^\top & 1 \end{array} \right] \]
\(\mathbf{T}\)から\(\mathbf{T}^{-1}\)、あるいは\(\boldsymbol{\xi}\)から\(\mathbf{T}^{-1}\)は、容易に求められる。
\(\mathbf{T}\)から\(\boldsymbol{\xi} = \left[ \begin{array}{c} \boldsymbol{\phi} \\ \boldsymbol{\rho} \end{array} \right]\)への変換は、次のように行う。