ホームに戻る

このページについて

3次元の剛体変換を表すリー群$\mathrm{SE}(3)$と、リー代数$\mathfrak{se}(3)$に関する、自分用のメモ書きです。

$\mathfrak{se}(3)$から$\mathrm{SE}(3)$への変換

リー代数$\mathit{\xi }=\left[\begin{array}{c}\mathit{\varphi }\\ \mathit{\rho }\end{array}\right]\in \mathfrak{se}(3)$と、それに対応するリー群(剛体変換)$\mathbf{T}=\exp ({\mathit{\xi }}^{\wedge })\in \mathrm{SE}(3)$を考える。 $\mathit{\varphi }\in \mathfrak{so}(3)$、$\mathit{\rho }\in {\mathbb{R}}^3$である。 $\mathit{\xi }$と$\mathbf{T}$との間には、次の関係が成立する。

$$\mathbf{T}=\exp ({\mathit{\xi }}^{\wedge })=\left[\begin{array}{cc}\mathbf{C}& \mathbf{J}(\mathit{\varphi })\mathit{\rho }\\ {\mathbf{0}}^{\mathrm{\top }}& 1\end{array}\right]$$

ここで、$\mathbf{C}=\exp ({\mathit{\varphi }}^{\wedge })\in \mathrm{SO}(3)$は、$\mathit{\varphi }$から得た回転行列である。 またヤコビ行列$\mathbf{J}$は、こちらのメモで定義された${\mathbf{J}}_l$と同じものである(左側バージョン)。 $\mathit{\varphi }=\varphi \mathbf{a}$、$\varphi =|\mathit{\varphi }|$、$\mathbf{a}=\mathit{\varphi }/\varphi$である。

$$\mathbf{J}(\mathit{\varphi })=\frac{\sin \varphi }{\varphi }\mathbf{I}+(1-\frac{\sin \varphi }{\varphi })\mathbf{a}{\mathbf{a}}^{\mathrm{\top }}+\frac{1-\cos \varphi }{\varphi }{\mathbf{a}}^{\wedge }$$

$\mathit{\xi }=\left[\begin{array}{c}\mathit{\varphi }\\ \mathit{\rho }\end{array}\right]\in \mathfrak{se}(3)$に対して、$\wedge$演算子は次のように定義される(Wedge演算子)。 $\mathit{\varphi }\in \mathfrak{so}(3)$に対する$\wedge$演算子は、こちらのメモを参照する。 $\vee$演算子は、$\wedge$とは真逆の操作を行う(Vee演算子)。

$${\mathit{\xi }}^{\wedge }=\left[\begin{array}{cc}{\mathit{\varphi }}^{\wedge }& \mathit{\rho }\\ {\mathbf{0}}^{\mathrm{\top }}& 0\end{array}\right]\in {\mathbb{R}}^{4\times 4}$$

$\mathit{\xi }$から$\mathbf{T}$を計算するためには、次のようにする。

• $\mathit{\xi }$から回転を表す成分$\mathit{\varphi }\in \mathfrak{so}(3)$を取り出して、回転行列$\mathbf{C}=\exp ({\mathit{\varphi }}^{\wedge })\in \mathrm{SO}(3)$を計算する。 計算方法は、こちらのメモを参照する。
• $\mathit{\varphi }\in \mathfrak{so}(3)$から、(左側バージョンの)ヤコビ行列$\mathbf{J}(\mathit{\varphi })$を計算する。 計算方法は、こちらのメモを参照する。
• $\mathit{\xi }$から$\mathit{\rho }$を取り出して、$\mathbf{J}(\mathit{\varphi })\mathit{\rho }$を計算する。
• $\mathbf{C}$、$\mathbf{J}(\mathit{\varphi })\mathit{\rho }$が揃ったので、それらをまとめて$\mathbf{T}$とする。

$\mathbf{T}$の逆行列${\mathbf{T}}^{-1}$は、次のように表される($\mathbf{r}=\mathbf{J}(\mathit{\varphi })\mathit{\rho }$)。

$${\mathbf{T}}^{-1}=\left[\begin{array}{cc}{\mathbf{C}}^{\mathrm{\top }}& -{\mathbf{C}}^{\mathrm{\top }}\mathbf{r}\\ {\mathbf{0}}^{\mathrm{\top }}& 1\end{array}\right]$$

$\mathbf{T}$から${\mathbf{T}}^{-1}$、あるいは$\mathit{\xi }$から${\mathbf{T}}^{-1}$は、容易に求められる。

$\mathrm{SE}(3)$から$\mathfrak{se}(3)$への変換

$\mathbf{T}$から$\mathit{\xi }=\left[\begin{array}{c}\mathit{\varphi }\\ \mathit{\rho }\end{array}\right]$への変換は、次のように行う。

• $\mathbf{T}$の左上のブロック(回転行列)$\mathbf{C}$を取り出して、$\mathbf{C}$に対応するリー代数$\mathit{\varphi }$を計算する。 $\mathbf{C}$から$\mathit{\varphi }$への変換は、こちらのメモを参照する。
• $\mathbf{T}$の右上のブロック$\mathbf{J}(\mathit{\varphi })\mathit{\rho }$を取り出して、ヤコビ行列の逆行列$\mathbf{J}(\mathit{\varphi }{)}^{-1}$を掛けることで、$\mathit{\rho }$を計算する。 $\mathbf{J}(\mathit{\varphi }{)}^{-1}$は、次のように定義される。 計算方法は、こちらのメモを参照する。 $$\mathbf{J}(\mathit{\varphi }{)}^{-1}=\frac{\varphi }{2}\cot \frac{\varphi }{2}\mathbf{I}+(1-\frac{\varphi }{2}\cot \frac{\varphi }{2})\mathbf{a}{\mathbf{a}}^{\mathrm{\top }}-\frac{\varphi }{2}{\mathbf{a}}^{\wedge }$$
• $\mathit{\varphi }$と$\mathit{\rho }$を結合して、$\mathit{\xi }$を得る。

参考文献

• State Estimation for Robotics
• Introduction to Visual SLAM: From Theory to Practice