SE（3） 和 se（3）

SE（3） 是旋轉加上位移， 也稱歐式變換（Euclidean transformation），剛體變換（Rigid Transformation），一般我們用矩陣

\begin{bmatrix} \mathbf{R} & t \\ \mathbf{0} & 1 \end{bmatrix}

來表示，其中

\mathbf{R}

為旋轉， t 為位移，所以有6個自由度，3個旋轉，3個位置。

SO（3） 和 so（3） 的數學推導

在開始 SE（3） 和 se（3） 之前，我們先在此複習 SO（3） 和 so（3）， 上篇寫到指數對映的式子可以寫成：

\exp(\boldsymbol{\omega}) = I + \frac{\sin \theta}{\theta} [\boldsymbol{\omega}]_x + \frac{1 - \cos \theta}{\theta^2}[\boldsymbol{\omega}]_x^2 \\

和

\exp(\Omega) = I + \frac{\sin \theta}{\theta} \Omega + \frac{1 - \cos \theta}{\theta^2}{\Omega}^2 \\

我們是根據之前的

\exp(\boldsymbol{\theta}) = \exp(\theta \mathbf{u}) = I + [\mathbf{u}]_x \sin \theta + [\mathbf{u}]_x^2 (1 - \cos \theta)

和 把

\boldsymbol{\omega}

分解成大小和方向推論出來的，這裡我們將再次推導指數對映的結論，因為在推導 se（3） 的指數對映之時也會用到。

先再次觀察 so（3），so（3） 中的元素可以看成矩陣，這個矩陣為反對稱矩陣，我們用

\Omega

來表示：

\Omega = [\boldsymbol{\omega}]_x= \begin{bmatrix} 0 & -\omega_3 & \omega_2 \\ \omega_3 & 0 & -\omega_1 \\ -\omega_2 & \omega_1 & 0 \\ \end{bmatrix}\\

這個矩陣可以看成是

E_1 = \begin{bmatrix}0 & 0 & 0 \\0 & 0 & -1 \\ 0 & 1 & 0 \\\end{bmatrix}, E_2 = \begin{bmatrix}0 & 0 & 1 \\0 & 0 & 0 \\ -1 & 0 & 0 \\\end{bmatrix}, E_3 = \begin{bmatrix}0 & -1 & 0 \\1 & 0 & 0 \\ 0 & 0 & 0 \\\end{bmatrix}

組合而成，

\Omega = \omega_1E_1+\omega_2E_2 + \omega_3E_3

也就是：

\begin{align*} \boldsymbol{\omega} {}& \in R^3\\ \omega_1E_1+\omega_2E_2 + \omega_3E_3 &\in so(3) \end{align*}\\

觀察

\boldsymbol{\omega}= \begin{bmatrix} \omega_1 \\ \omega_2 \\ \omega_3 \\ \end{bmatrix}

， 作為向量，它的模長：

\theta^2 = \omega_1^2 + \omega_2^2 + \omega_3^2\\

\theta^2 = \begin{bmatrix}\omega_1 & \omega_2 & \omega_3 \ \end{bmatrix} \cdot \begin{bmatrix}\omega_1 \\ \omega_2 \\ \omega_3 \ \end{bmatrix} = \boldsymbol{\omega}^T\boldsymbol{\omega}\\

計算

\Omega^n, n = 1, 2, ...

：

\Omega^0 = I\\

\Omega = \begin{bmatrix} 0 & -\omega_3 & \omega_2 \\ \omega_3 & 0 & -\omega_1 \\ -\omega_2 & \omega_1 & 0 \\ \end{bmatrix}\\

\Omega^2 = \begin{bmatrix} 0 & -\omega_3 & \omega_2 \\ \omega_3 & 0 & -\omega_1 \\ -\omega_2 & \omega_1 & 0 \\\end{bmatrix}\begin{bmatrix} 0 & -\omega_3 & \omega_2 \\ \omega_3 & 0 & -\omega_1 \\ -\omega_2 & \omega_1 & 0 \\\end{bmatrix} = \begin{bmatrix} -\omega_3^2-\omega_2^2 & \omega_1\omega_2 & \omega_1\omega_3 \\ \omega_2\omega_1 & -\omega_3^2-\omega_1^2 & \omega_2\omega_3 \\ \omega_3\omega_1 & \omega_2\omega_3 & -\omega_1^2-\omega_2^2 \\\end{bmatrix} \\

這裡我們可以觀察到

\operatorname {tr}(\Omega^2) = -2(\omega_1^2+\omega_2^2+\omega_3^2) = -2\theta^2

計算

\Omega^3 = \Omega^2 \Omega= \begin{bmatrix} -\omega_3^2-\omega_2^2 & \omega_1\omega_2 & \omega_1\omega_3 \\ \omega_2\omega_1 & -\omega_3^2-\omega_1^2 & \omega_2\omega_3 \\ \omega_3\omega_1 & \omega_2\omega_3 & -\omega_1^2-\omega_2^2 \\\end{bmatrix}\begin{bmatrix} 0 & -\omega_3 & \omega_2 \\ \omega_3 & 0 & -\omega_1 \\ -\omega_2 & \omega_1 & 0 \\\end{bmatrix} =- \theta^2\begin{bmatrix} 0 & -\omega_3 & \omega_2 \\ \omega_3 & 0 & -\omega_1 \\ -\omega_2 & \omega_1 & 0 \\\end{bmatrix} = -\theta^2 \Omega\\

所以有

\Omega^3 = -\theta^2 \Omega

， 繼續：

\Omega^4 = \Omega \Omega^3 = -\theta^2 \Omega^2\\

\Omega^5 = \Omega^2 \Omega^3 =-\Omega^2\theta^2\Omega = -\theta^2\Omega^3 = \theta^4\Omega \\ \dots\\

所以有

\begin{align*} \theta^2 {}&= \boldsymbol{\omega}^T\boldsymbol{\omega}\\ \Omega^{2i+1} &= (-1)^i\theta^{2i}\Omega \\ \Omega^{2i+2} &= (-1)^i\theta^{2i}\Omega^2 \\ \end{align*} \\

展開：

\begin{align*} \exp(\Omega) {}&= \sum_{n = 0}^{\infty} \frac{1}{n!} \Omega ^n \\ &= I + \bigg( \sum_{i=0}^{\infty} \frac {(-1)^i\theta^{2i}}{(2i+1)!}\bigg) \Omega + \bigg( \sum_{i=0}^{\infty} \frac {(-1)^i\theta^{2i}}{(2i+2)!}\bigg) \Omega^2\\ &= I + (1 - \frac{\theta^2}{3!} + \frac{\theta^4}{5!} + \cdots)\Omega + (\frac{1}{2!} - \frac{\theta^2}{4!} + \frac{\theta^4}{6!}+ \cdots) \Omega^2 \end{align*} \\

有尤拉公式：

\sin x = x - \frac{x^3}{3!} + \frac{x^5}{5!} - \frac{x^7}{7!} + \cdots\\ \cos x = 1 - \frac{x^2}{2!} + \frac{x^4}{4!} - \frac{x^6}{6!} + \cdots

所以：

\exp(\Omega) = I + \big(\frac{\sin \theta}{\theta}\big) \Omega + \big(\frac{1- \cos \theta}{\theta^2} \big) \Omega^2\\

如果我們需要更嚴密一點的話 expso（3） → SO（3）：

\Omega \in so(3)\\ \theta = \sqrt{-\frac{1}{2}tr(\Omega^2)}\\ \exp(\Omega) = \begin{cases} I & \theta \simeq 0\\ I + \big(\dfrac{\sin \theta}{\theta}\big) \Omega + \big(\dfrac{1- \cos \theta}{\theta^2} \big) \Omega^2 & \theta \neq 0 \end{cases}\\

同樣 SO（3） → so（3），也就是旋轉矩陣到旋轉向量。如果也更嚴密一點，首先就是我們限制旋轉角度在［0，π］之間，畢竟可能要用acos函式和保證一對一，否則角度加上2π 計算出來的sin cos 總是相等，其次就是當

\theta

足夠小的時候的處理：

R \in SO(3)\\ \theta = \arccos(\frac{1}{2}[tr(R) - 1])\\ \log(R) = \begin{cases} \theta \begin{bmatrix}0 & -1 & 1 \\1 & 0 & -1 \\ -1 & 1 & 0 \\\end{bmatrix} & \theta \simeq 0\\ \dfrac{\theta}{\sin \theta}\big( R - R^T \big) & \theta \neq 0 \end{cases}\\

se（3） → SE（3）

模仿之前對於$so（3）$ 中的元素的看法， se（3） 中的元素可以這樣看：

\begin{align*} \begin{pmatrix} \boldsymbol{\rho} & \boldsymbol{\omega} \end{pmatrix}^T {}& \in R^6\\ \rho_1P_1+ \rho_2P_2 + \rho_3P_3 + \omega_1E_1+\omega_2E_2 + \omega_3E_3&\in se(3) \end{align*}\\

其中：

P_1 = \begin{bmatrix}0 & 0 & 0 & 1 \\0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 \end{bmatrix},P_2 = \begin{bmatrix}0 & 0 & 0 & 0 \\0 & 0 & 0 & 1 \\ 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 \end{bmatrix}, P_3 = \begin{bmatrix}0 & 0 & 0 & 0 \\0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 1 \\ 0 & 0 & 0 & 0 \end{bmatrix},

E_1 = \begin{bmatrix}0 & 0 & 0 & 0 \\0 & 0 & -1 &0 \\ 0 & 1 & 0 & 0 \\0 & 0 & 0 & 0 \end{bmatrix}, E_2 = \begin{bmatrix}0 & 0 & 1 & 0\\0 & 0 & 0 & 0 \\ -1 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 \\\end{bmatrix}, E_3 = \begin{bmatrix}0 & -1 & 0 & 0\\1 & 0 & 0 & 0 \\ 0 & 0 & 0 &0 \\ 0 & 0 & 0 & 0\\\end{bmatrix}

那麼計算它的指數對映：

\begin{align*} \exp \bigg( \begin{bmatrix} \Omega & p \\ 0 & 0 \end{bmatrix} \bigg) {}&= \sum_{n = 0}^{\infty} \frac{1}{n!} \bigg( \begin{bmatrix} \Omega & p \\ 0 & 0 \end{bmatrix} \bigg)^n \\ &= I + \begin{bmatrix} \Omega & p \\ 0 & 0 \end{bmatrix} + \frac{1}{2!} \begin{bmatrix} \Omega^2 & \Omega p \\ 0 & 0 \end{bmatrix} + \frac{1}{3!}\begin{bmatrix} \Omega^3 & \Omega^2 p \\ 0 & 0 \end{bmatrix} + \cdots \end{align*} \\

所以：

\exp \bigg( \begin{bmatrix} \Omega & p \\ 0 & 0 \end{bmatrix} \bigg) = \begin{bmatrix} \exp(\Omega) & Vp \\ 0 & 1 \end{bmatrix} \\

其中

V = I + \frac{1}{2!}\Omega + \frac{1}{3!}\Omega^2 + \cdots\\

在根據之前的結論：

V = I + \bigg( \sum_{i=0}^{\infty} \frac {(-1)^i\theta^{2i}}{(2i+2)!}\bigg) \Omega + \bigg( \sum_{i=0}^{\infty} \frac {(-1)^i\theta^{2i}}{(2i+3)!}\bigg) \Omega^2\\

同樣展開和利用尤拉公式：

V = I + \big( \frac{1 - \cos \theta}{\theta^2} \big) \Omega + \big( \frac{\theta - \sin \theta}{\theta^3}\big)\Omega^2\\

所以可得公式：

\exp \bigg( \begin{bmatrix} \Omega & p \\ 0 & 0 \end{bmatrix} \bigg) = \begin{bmatrix} \exp(\Omega) & Vp \\ 0 & 1 \end{bmatrix}\\

同樣，嚴密一點寫出結論：

$$ \begin{bmatrix} \Omega & p \\ 0 & 0 \end{bmatrix} \in se(3)\\ \exp(\begin{bmatrix} \Omega & p \\ 0 & 0 \end{bmatrix}) = \begin{bmatrix} \exp(\Omega) & Vp \\ 0 & 1 \end{bmatrix} \\ V = I + \big( \dfrac{1 - \cos \theta}{\theta^2}\big) \Omega + \big( \dfrac{\theta - \sin \theta}{\theta^3}\big)\Omega^2 \\ \theta = \sqrt{-\frac{1}{2}tr(\Omega^2)}\\

SE（3） → se（3）

SE（3） 到 se（3） 同樣是用對數對映可以得到，不過我們繼續用已經得到的結論：

\begin{bmatrix} R & t \\ 0 & 1 \end{bmatrix} \in SE(3)\\ \log(\begin{bmatrix} R & t \\ 0 & 1 \end{bmatrix}) = \begin{bmatrix} \log(R) & V^{-1}t \\ 0 & 0 \end{bmatrix} \\ \theta = \arccos(\frac{1}{2}[tr(R) - 1])\\ V = I + \big( \frac{1 - \cos \theta}{\theta^2}\big) \Omega + \big( \frac{\theta - \sin \theta}{\theta^3}\big)\Omega^2\\

V^{-1}

也有closed form，可以透過計算得到，可以寫成如下：

V^{-1} = I - \frac{1}{2} \Omega + \frac{1}{\theta^2}(1 - \frac{A}{2B})\Omega^2\\ A = \frac{\sin \theta}{\theta}, B = \frac{1 - \cos \theta}{\theta^2}\\

參考：

Lie Groups for 2D and 3D Transformations Ethan Eade