在SLAM后端非線性優化中，李群和李代數是一個繞不開的玩意兒。我們需要借助李代數來表達旋轉或者位姿（平移加旋轉），進行求導操作。那么，這一篇博客讓我們來扒一扒李群和李代數是什么東西。在此之前，你可能有一連串疑問：
問：群是什么？
答：群是一種代數結構。通俗點說，群就是元素集合加上代數運算，使得集合中任意兩個元素經過運算后形成的第三個元素仍然在這個集合里面。
問：群需要滿足什么性質？
答：任何群都需要滿足群公理。一共有四個：封閉性（closure），結合性（associtivity）, 單位元（identity），逆元（invertibility）。
問：李群是什么？
答：除了需要滿足四個群公理外，李群還需要滿足其他性質，例如不可交換。順便說一下，不可交換的群也稱為非阿貝爾群。除此而外，李群是具有微分流形結構的群，群集合中的元素是矩陣。
有了這樣一個基本概念后，我們來介紹兩個常用李群：特殊正交群$SO(3)$和特殊歐氏群$SE(3)$，以及分別對應的李代數$\mathfrak{so}(3)$和$\mathfrak{se}(3)$。表達旋轉時，我們可以用$SO(3)$或者$\mathfrak{so}(3)$；表達位姿時，我們可以用$SE(3)$或者$\mathfrak{se}(3)$。
另外，澄清一下：博客中所有文章，如果不加特殊說明，所有矢量均表示列矢量：記號$\mathbb{R}^n$表示實數域上的$n$維列矢量。
$SO(3)$與$SE(3)$
特殊正交群用來表達旋轉，簡單的說就是一系列旋轉矩陣的集合：$$SO(3) = \{ \mathbf{R} \in \mathbb{R}^{3 \times 3} | \mathbf{R} \mathbf{R}^T = \mathbf{I}, \det(\mathbf{R}) = 1 \}$$ 正交性條件$\mathbf{R} \mathbf{R}^T = \mathbf{I}$在具有9個分量的矩陣上施加了6個約束，使得矩陣自由度變為3。另外，$$\det(\mathbf{R} \mathbf{R}^T) = \det(\mathbf{I}) = 1 \Rightarrow \det(\mathbf{R}) = \pm 1 $$ 當$\det(\mathbf{R}) = -1$時，我們稱之為非正常轉動（improper rotation），或者旋轉反射 (rotary reflection)。選擇 $\det(\mathbf{R}) = +1$保證了我們得到的是一個正常的轉動 (proper rotation)。
特殊歐氏群用來表達位姿，包括平移和旋轉：$$SE(3) = \left\{ \mathbf{T} = \left [ \begin{array} {*{20}{c}} \mathbf{R} & \mathbf{t} \\ \mathbf{0}^T & 1 \end{array} \right] \in \mathbb{R}^{4 \times 4} \Bigg| \mathbf{R} \in SO(3), \mathbf{t} \in \mathbb{R}^3 \right\} $$
接着，我們來看看$SO(3)$和$SE(3)$需要滿足四個群公理，即：
- 封閉性：$\mathbf{R}_1, \mathbf{R}_2 \in SO(3) \Rightarrow \mathbf{R}_1\mathbf{R}_2 \in SO(3)$ and $\mathbf{T}_1, \mathbf{T}_2 \in SE(3) \Rightarrow \mathbf{T}_1\mathbf{T}_2 \in SE(3)$
- 結合性：$\mathbf{R}_1(\mathbf{R}_2\mathbf{R}_3) = (\mathbf{R}_1\mathbf{R}_2)\mathbf{R}_3 = \mathbf{R}_1\mathbf{R}_2\mathbf{R}_3$ and $\mathbf{T}_1(\mathbf{T}_2\mathbf{T}_3) = (\mathbf{T}_1\mathbf{T}_2)\mathbf{T}_3 = \mathbf{T}_1\mathbf{T}_2\mathbf{T}_3$
- 單位元：$\mathbf{R}, \mathbf{I} \in SO(3) \Rightarrow \mathbf{R}\mathbf{I} = \mathbf{I}\mathbf{R} = \mathbf{R}$ and $\mathbf{T}, \mathbf{I} \in SE(3) \Rightarrow \mathbf{T}\mathbf{I} = \mathbf{I}\mathbf{T} = \mathbf{T}$
- 逆元：$\mathbf{R} \in SO(3) \Rightarrow \mathbf{R}^{-1} \in SO(3) $ and $\mathbf{T} \in SE(3) \Rightarrow \mathbf{T}^{-1} \in SE(3)$
我們以封閉性為例，來證一證。先來看$SO(3)$的封閉性：$$(\mathbf{R}_1\mathbf{R}_2)(\mathbf{R}_1\mathbf{R}_2)^T = \mathbf{R}_1 \mathbf{R}_2\mathbf{R}_{2}^{T} \mathbf{R}_{1}^{T} = \mathbf{R}_1 \mathbf{R}_{1}^{T} = \mathbf{I} \qquad \det(\mathbf{R}_1\mathbf{R}_2) =\det(\mathbf{R}_1) \det(\mathbf{R}_2) = 1$$ 所以，$\mathbf{R}_1\mathbf{R}_2 \in SO(3)$，再來看看$SE(3)$的封閉性：
$$\mathbf{T}_1\mathbf{T}_2 = \left [ \begin{array} {*{20}{c}} \mathbf{R}_1 & \mathbf{t}_1 \\ \mathbf{0}^T & 1 \end{array} \right] \left [ \begin{array} {*{20}{c}} \mathbf{R}_2 & \mathbf{t}_2 \\ \mathbf{0}^T & 1 \end{array} \right] = \left [ \begin{array} {*{20}{c}} \mathbf{R}_1 \mathbf{R}_2 & \mathbf{R}_1\mathbf{t}_2 + \mathbf{t}_1 \\ \mathbf{0}^T & 1 \end{array} \right] \in SE(3)$$ 其中，$\mathbf{R}_1\mathbf{R}_2 \in SO(3)$ 且 $\mathbf{R}_1\mathbf{t}_2 + \mathbf{t}_1 \in \mathbb{R}^3$
需要說明的是，以上都是在說$SO(3)$和$SE(3)$對乘法封閉，然而兩者對加法並不封閉：$$\mathbf{R}_1, \mathbf{R}_2 \in SO(3) \nRightarrow \mathbf{R}_1 + \mathbf{R}_2 \in SO(3) \qquad and \qquad \mathbf{T}_1, \mathbf{T}_2 \in SE(3) \nRightarrow \mathbf{T}_1 + \mathbf{T}_2 \in SE(3)$$ 正因為對加法的不封閉，后面我們會引入李代數利用擾動法進行導數計算。
其他三個公理有興趣大家可以自己試着證明一下。不過不證也沒關系，因為這在實際工程應用中，這些問題比較trivial，證明的過程純粹是為了鍛煉自己的思維能力。
$\mathfrak{so}(3)$與$\mathfrak{se}(3)$
不論$SO(3)$還是$SE(3)$，都伴隨着一個相應的李代數。李代數是定義在某個數域$\mathbb{F}$上的集合$\mathbb{V}$，同時定義了一個稱為李括號$[\cdot , \cdot]$的雙目運算符。我們把藍色字體部分稱為李代數的三個要素。同李群類似，對所有的$\mathbf{X}, \mathbf{Y}, \mathbf{Z} \in \mathbb{V}$ 和 $a, b \in \mathbb{F}$，李代數也必須滿足四個性質：
- 封閉性：$[\mathbf{X}, \mathbf{Y}] \in \mathbb{V}$
- 雙線性：$[a\mathbf{X} + b\mathbf{Y}, \mathbf{Z}] = a[\mathbf{X}, \mathbf{Z}] + b[\mathbf{Y}, \mathbf{Z}] \qquad and \qquad [ \mathbf{Z}, a\mathbf{X} + b\mathbf{Y}] = a[\mathbf{Z}, \mathbf{X}] + b[\mathbf{Z}, \mathbf{Y}]$
- 自反性：$[\mathbf{X}, \mathbf{X}] = \mathbf{0}$
- 雅可比恆等：$[\mathbf{X}, [\mathbf{Y}, \mathbf{Z}]] + [\mathbf{Y}, [\mathbf{Z}, \mathbf{X}]] + [\mathbf{Z}, [\mathbf{X}, \mathbf{Y}]] = \mathbf{0}$
李代數所表達的矢量空間是對應李群在單位陣附近的切空間。下面我們分別來介紹$\mathfrak{so}(3)$與$\mathfrak{se}(3)$。
- $\mathfrak{so}(3)$
$SO(3)$對應的李代數$\mathfrak{so}(3)$是定義在$\mathbb{R}^3$上的向量，記作$\mathbf{\boldsymbol{\phi}}$。來看看$\mathfrak{so}(3)$的三個要素： - 集合：$\mathfrak{so}(3) = \{ \mathbf{\Phi} = \boldsymbol{\phi}^{\wedge} \in \mathbb{R}^{3 \times 3} | \boldsymbol{\phi} \in \mathbb{R}^3\} $
- 數域：實數域$\mathbb{R}$
- 李括號：$[\mathbf{\Phi}_1, \mathbf{\Phi}_2] = \mathbf{\Phi}_1\mathbf{\Phi}_2 - \mathbf{\Phi}_2\mathbf{\Phi}_1 \in \mathbb{R}^{3 \times 3}$
其中，$$\mathbf{\Phi} = \boldsymbol{\phi}^{\wedge} = \left [ \begin{array} {*{20}{c}} 0 & -\boldsymbol{\phi}_3 & \boldsymbol{\phi}_2 \\ \boldsymbol{\phi}_3 & 0 & -\boldsymbol{\phi}_1 \\ -\boldsymbol{\phi}_2 & \boldsymbol{\phi}_1 & 0\end{array} \right] \in \mathbb{R}^{3 \times 3}, \quad \boldsymbol{\phi} \in \mathbb{R}^3$$
另外，我們定義運算$[\cdot]^{\vee}$為$[\cdot]^{\wedge}$的逆運算：$$\mathbf{\Phi} = \boldsymbol{\phi}^{\wedge} \Leftrightarrow \boldsymbol{\phi} = \mathbf{\Phi}^{\vee}$$
作為比較trivial的內容，我們來驗證一下這樣定義的李括號，是否滿足上面的四個性質。這里只驗證前兩個，留兩個大家自行驗證。
首先，封閉性：$$[\mathbf{\Phi}_1, \mathbf{\Phi}_2] = (\mathbf{\Phi}_1\mathbf{\Phi}_2 - \mathbf{\Phi}_2\mathbf{\Phi}_1) = (\boldsymbol{\phi}_1^{\wedge}\boldsymbol{\phi}_2^{\wedge} - \boldsymbol{\phi}_2^{\wedge}\boldsymbol{\phi}_1^{\wedge}) = (\underbrace{\boldsymbol{\phi}_1^{\wedge}\boldsymbol{\phi}_2}_{\in \mathbb{R}^3})^{\wedge} \in \mathfrak{so}(3) $$ 其次，自反性：$$[\mathbf{\Phi}, \mathbf{\Phi}] = \mathbf{\Phi}\mathbf{\Phi} - \mathbf{\Phi}\mathbf{\Phi} = \mathbf{0}$$ - $\mathfrak{se}(3)$
$SE(3)$對應的李代數$\mathfrak{se}(3)$是定義在$\mathbb{R}^6$上的向量，記作$\boldsymbol{\xi}$。來看看$\mathfrak{se}(3)$的三個要素：
- 集合：$\mathfrak{se}(3) = \{ \mathbf{\Xi} = \boldsymbol{\xi}^{\wedge} \in \mathbb{R}^{4 \times 4} | \boldsymbol{\xi} \in \mathbb{R}^6\} $
- 數域：實數域$\mathbb{R}$
- 李括號：$[\mathbf{\Xi}_1, \mathbf{\Xi}_2] = \mathbf{\Xi}_1\mathbf{\Xi}_2 - \mathbf{\Xi}_2\mathbf{\Xi}_1 \in \mathbb{R}^{4 \times 4}$
其中，$$\mathbf{\Xi} = \boldsymbol{\xi}^{\wedge} = \left [ \begin{array} {*{20}{c}} \boldsymbol{\rho} \\ \boldsymbol{\phi} \end{array} \right] ^{\wedge} = \left [ \begin{array} {*{20}{c}} \boldsymbol{\phi}^{\wedge} & \boldsymbol{\rho} \\ \mathbf{0}^T & 0 \end{array} \right] \in \mathbb{R}^{4 \times 4}, \quad \boldsymbol{\rho}, \boldsymbol{\phi} \in \mathbb{R}^3$$
上式中，$\boldsymbol{\rho}$表示$\mathfrak{se}(3)$上的平移，它和$SE(3)$上的平移$\mathbf{t}$還有一些不同，后面會說明。
類似地，$$\mathbf{\Xi} = \boldsymbol{\xi}^{\wedge} \Leftrightarrow \boldsymbol{\xi} = \mathbf{\Xi}^{\vee}$$
同樣，我們來驗證一下封閉性：$$[\mathbf{\Xi}_1, \mathbf{\Xi}_2] = (\mathbf{\Xi}_1\mathbf{\Xi}_2 - \mathbf{\Xi}_2\mathbf{\Xi}_1) = (\boldsymbol{\xi}_1^{\wedge}\boldsymbol{\xi}_2^{\wedge} - \boldsymbol{\xi}_2^{\wedge}\boldsymbol{\xi}_1^{\wedge}) = (\underbrace{\boldsymbol{\xi}_1^{\curlywedge}\boldsymbol{\xi}_2}_{\in \mathbb{R}^6})^{\wedge} \in \mathfrak{se}(3)$$ 注意到運算$[\cdot]^{\curlywedge}$，它表示：$$\boldsymbol{\xi}^{\curlywedge} = \left [ \begin{array} {*{20}{c}} \boldsymbol{\rho} \\ \boldsymbol{\phi} \end{array} \right] ^{\curlywedge} = \left [ \begin{array} {*{20}{c}} \boldsymbol{\phi}^{\wedge} & \boldsymbol{\rho}^{\wedge} \\ \mathbf{0} & \boldsymbol{\phi}^{\wedge} \end{array} \right] \in \mathbb{R}^{6 \times 6}, \quad \boldsymbol{\rho}, \boldsymbol{\phi} \in \mathbb{R}^3 $$
可以看到：旋轉如果用李代數$\boldsymbol{\phi}$來表示，依然是3個自由度，這和旋轉矩陣$\mathbf{R}$自由度一致；而位姿用李代數$\boldsymbol{\xi}$來表示，也仍然是6個自由度，這和用姿態矩陣$\mathbf{T}$表達也一致，因為其中旋轉3個自由度，平移也有3個自由度。那么，我們的問題來了：李群和對應李代數究竟是什么關系？另外，還有一個問題就是用李代數$\boldsymbol{\phi}$表達旋轉，或者$\boldsymbol{\xi}$表達位姿有何意義？因為畢竟它們看起來並不直觀。不急，下一節指數映射會為我們揭開這些問題的答案！
指數映射
先直接搬結論：李代數經過指數映射會得到對應李群，反過來，李群經過對數映射得到對應李代數 (這個結論可以通過正交性條件對時間求導，然后求解一個常微分方程得出，這里不加證明)。
這里引入方陣的指數映射：$$\exp(\mathbf{A}) = \mathbf{I} + \mathbf{A} + \frac{1}{2!} \mathbf{A}^2 + ... = \sum_{0}^{\infty}{\frac{1}{n!}\mathbf{A}^n} \in \mathbb{R}^{n \times n}, \quad \mathbf{A} \in \mathbb{R}^{n \times n}$$
對於旋轉而言，$SO(3)$中的元素和$\mathfrak{so}(3)$中的元素是通過指數映射聯系的：$$ \mathbf{R} = \exp(\boldsymbol{\phi}^{\wedge}) = \sum_{0}^{\infty}{\frac{1}{n!} (\boldsymbol{\phi}^{\wedge})^n}$$
同時， 我們記$\boldsymbol{\phi} = \phi \mathbf{a}$，其中，$\phi = \| \boldsymbol{\phi} \| $表示$\boldsymbol{\phi}$的模長，而$\mathbf{a} = \frac{\boldsymbol{\phi}}{\phi}$表示與$\boldsymbol{\phi}$方向一致的單位向量。這樣，指數映射就可表達為：
\begin{equation*}
\begin{split}
\exp(\boldsymbol{\phi}^{\wedge}) &= \exp((\phi \mathbf{a})^{\wedge}) = \exp(\phi \mathbf{a}^{\wedge}) \\
&= \underbrace{\mathbf{I}}_{\mathbf{a} \mathbf{a}^T - \mathbf{a}^{\wedge}\mathbf{a}^{\wedge}} + \phi \mathbf{a}^{\wedge} + \frac{1}{2!}\phi^2\mathbf{a}^{\wedge}\mathbf{a}^{\wedge} + \frac{1}{3!}\phi^3\underbrace{\mathbf{a}^{\wedge}\mathbf{a}^{\wedge}\mathbf{a}^{\wedge}}_{-\mathbf{a}^{\wedge}} + \frac{1}{4!}\phi^4\underbrace{\mathbf{a}^{\wedge}\mathbf{a}^{\wedge}\mathbf{a}^{\wedge}\mathbf{a}^{\wedge}}_{-\mathbf{a}^{\wedge}\mathbf{a}^{\wedge}} + ... \\
& = \mathbf{a} \mathbf{a}^T + \underbrace{(\phi - \frac{1}{3!}\phi^3+ \frac{1}{5!}\phi^5 - ...)}_{\sin \phi}\mathbf{a}^{\wedge} - \underbrace{(1-\frac{1}{2!}\phi^2 + \frac{1}{4!}\phi^4 - ...)}_{\cos \phi} \underbrace{\mathbf{a}^{\wedge} \mathbf{a}^{\wedge}}_{-\mathbf{I} + \mathbf{a} \mathbf{a}^T} \\
&= \cos \phi \mathbf{I} + (1 - \cos \phi) \mathbf{a} \mathbf{a}^T + \sin \phi \mathbf{a}^{\wedge}
\end{split}
\end{equation*}
也就是說：$$\mathbf{R} = \cos \phi \mathbf{I} + (1 - \cos \phi) \mathbf{a} \mathbf{a}^T + \sin \phi \mathbf{a}^{\wedge}$$ 最后這個式子，跟旋轉向量轉成旋轉矩陣的羅德里格斯公式一模一樣。 因此，在數學意義上，我們可以說： 李代數$\boldsymbol{\phi}$表征的就是一個旋轉向量，$\boldsymbol{\phi}$的模長表達了旋轉的角度，$\boldsymbol{\phi}$的方向表達了旋轉軸的方向。
反過來，當給定一個旋轉矩陣$\mathbf{R}$，我們怎么轉成李代數$\boldsymbol{\phi}$形式？這個問題也等價於，如何把一個旋轉矩陣轉成對應的旋轉向量形式？你會說，很簡單對數映射嘛！$$\boldsymbol{\phi} = \ln(\mathbf{R})^{\vee}$$ 但從計算角度來說，這不太方便。我們有更簡單的求解方式：
首先，來說角度$\phi$，我們把羅德里格斯公式兩邊同時取跡$\text{tr}(\cdot)$: $$\text{tr}(\mathbf{R}) = \cos \phi \underbrace{\text{tr}(\mathbf{I})}_{=3} +(1 - \cos \phi) \underbrace{\text{tr}(\mathbf{a} \mathbf{a}^T)}_{=1} + \sin \phi \underbrace{\text{tr}(\mathbf{a}^{\wedge})}_{=0} = 3 \cos \phi + (1 - \cos \phi) = 1 + 2 \cos \phi$$ 因此，$$\phi = \arccos(\frac{\text{tr}(\mathbf{R}) - 1}{2})$$ 需要說明的是，旋轉角度由於具有周期性（旋轉$2\pi$和沒有旋轉是一回事），$SO(3)$到$\mathfrak{so}(3)$的映射不是唯一的，但是我們利用了$\arccos(\cdot)$來限制角度的取值范圍，這種意義下，$SO(3)$和$\mathfrak{so}(3)$之間的映射可以保證是單射。
其次，來求解旋轉軸$\mathbf{a}$，如果我們把旋轉軸拿來繞着自身旋轉，那么它的結果也不會變。因此：$$\mathbf{R} \mathbf{a} = \mathbf{a}$$ 代數意義就是說，旋轉軸是旋轉矩陣特征值為1所對應的特征向量。那么，這個問題只需要做特征分解就可以搞定了。
同樣地，我們通過指數映射也可以把$\mathfrak{se}(3)$映射到$SE(3)$：$$\mathbf{T} = \exp(\boldsymbol{\xi}^{\wedge}) = \sum_{0}^{\infty}{\frac{1}{n!}(\boldsymbol{\xi}^{\wedge})^n}$$ 反過來，從$SE(3)$到$\mathfrak{se}(3)$的對數映射為：$$\boldsymbol{\xi} = \ln(\mathbf{T})^{\vee}$$
現在我們來導出指數映射的簡潔結果：
\begin{equation*}
\begin{split}
\exp(\boldsymbol{\xi}^{\wedge}) &= \sum_{0}^{\infty}{\frac{1}{n!}(\boldsymbol{\xi}^{\wedge})^n} \\
&= \sum_{0}^{\infty}\frac{1}{n!}\left( \left [ \begin{array} {*{20}{c}} \boldsymbol{\rho} \\ \boldsymbol{\phi} \end{array} \right] ^ {\wedge}\right) ^{n} \\
&= \sum_{0}^{\infty}\frac{1}{n!}\left [\begin{array} {*{20}{c}} \boldsymbol{\phi}^{\wedge} & \boldsymbol{\rho} \\ \mathbf{0}^T & 0 \end{array} \right]^{n} \\
&=\left [ \begin{array} {*{20}{c}} \sum_{0}^{\infty}\frac{1}{n!}(\boldsymbol{\phi}^{\wedge})^n & \sum_{0}^{\infty}\frac{1}{(n+1)!}(\boldsymbol{\phi}^{\wedge})^n\boldsymbol{\rho} \\ \mathbf{0}^{T} & 1 \end{array} \right] \\
&= \left [ \begin{array} {*{20}{c}} \mathbf{R} & \mathbf{t} \\ \mathbf{0}^{T} & 1\end{array} \right] \in SE(3)
\end{split}
\end{equation*}
其中，$\mathbf{t} = \mathbf{J} \boldsymbol{\rho} \in \mathbb{R}^3, \quad \mathbf{J} = \sum_{0}^{\infty}\frac{1}{(n+1)!}(\boldsymbol{\phi}^{\wedge})^n$。利用反對稱矩陣乘積的性質，像之前導出旋轉指數映射的過程一樣化簡就可以得到（$\boldsymbol{\phi} = \phi \mathbf{a}$）：$$\mathbf{J} = \frac{\sin \phi}{\phi} \mathbf{I} + \left(1 - \frac{\sin \phi}{\phi} \right) \mathbf{a} \mathbf{a}^T + \frac{1-\cos \phi}{\phi} \mathbf{a}^{\wedge} \in \mathbb{R}^{3 \times 3}$$ 現在，我們清楚了$\mathfrak{se}(3)$上的平移$\boldsymbol{\rho}$和$SE(3)$上的平移$\mathbf{t}$的聯系，兩者並非一回事。反過來，我們也可以知道：$\boldsymbol{\rho} = \mathbf{J}^{-1} \mathbf{t}$，其中，$\mathbf{J}^{-1}$也有解析的表達式：$$ \mathbf{J}^{-1} = \frac{\phi}{2}\cot\frac{\phi}{2}\mathbf{I} + \left(1- \frac{\phi}{2}\cot\frac{\phi}{2} \right) \mathbf{a} \mathbf{a}^T -\frac{\phi}{2}\mathbf{a}^{\wedge}$$
反過來，當給定一個位姿矩陣$\mathbf{T}$，我們怎么轉成李代數$\boldsymbol{\xi}$形式？答案當然還是對數映射，但是仍然計算不太方便。
一種比較簡便的方式是：通過$\mathbf{R}$先計算$\boldsymbol{\phi}$，然后通過$\boldsymbol{\phi}$計算$\mathbf{J}$，再通過$\mathbf{t} = \mathbf{J} \boldsymbol{\rho}$去求解線性方程組，從而算出$\boldsymbol{\rho}$得到$\boldsymbol{\xi}$。
幾個恆等式
現在給出一些非常有用的恆等式，在利用李代數進行導數計算的時候，我們經常會用到，請務必掌握。
等式一：$$\mathbf{R} \mathbf{p}^{\wedge}\mathbf{R}^T = (\mathbf{R}\mathbf{p})^{\wedge}, \quad \mathbf{R} \in SO(3), \mathbf{p} \in \mathbb{R}^3$$