視覺SLAM中的數學基礎 第三篇 李群與李代數
前言
在SLAM中，除了表達3D旋轉與位移之外，我們還要對它們進行估計，因為SLAM整個過程就是在不斷地估計機器人的位姿與地圖。為了做這件事，需要對變換矩陣進行插值、求導、迭代等操作。例如，在經典ICP問題中，給定了兩組3D點，我們要計算它們之間的變換矩陣。假設第一組的3D點為$\mathbf{P}=\{ \mathbf{p}_i | i = [1,2, \ldots, N] \}$，第二組3D點為$\mathbf{Q}=\{ \mathbf{q}_i | i = [1,2, \ldots, N] \}$，那我們實際要做的事情是求一個歐氏變換$\mathbf{T}$，使得$\mathbf{T}$滿足：
\[\begin{equation}
\forall i, \quad \mathbf{q}_i = \mathbf{T} \mathbf{p}_i
\end{equation}\]
注意這裡使用了齊次座標表示。通常，這許多個匹配過的點是通過特徵匹配得到的，構成了一個超定方程。而由於噪聲的存在，這個方程往往是無解的。因此我們轉而計算一個最小二乘：
\[\begin{equation}
\mathop {\min }\limits_{\mathbf{T}} u\left( {\mathbf{T}} \right) = \sum\limits_{i = 1}^N {{{\left\| {{\mathbf{q}_i} - \mathbf{T} {\mathbf{p}_i}} \right\|}^2}}
\end{equation}\]
這時問題就來了：如果用迭代方式求解這個優化時（儘管可以不用迭代方式來求），如何求目標函式$u$相對於$\mathbf{T}$的導數呢？首先，$\mathbf{T}$只有6 個自由度，最好能夠在一個六維空間表達它，那麼$u(\mathbf{T})$相對於這個六維空間的導數（雅可比矩陣）是一個$6 \times 6$的矩陣。其次，$\mathbf{T}$對於乘法是封閉的，但對加法不封閉，即任意兩個變換矩陣相加後並不是一個變換矩陣，這主要是因為旋轉矩陣對加法是不封閉的。
出於這兩個原因，我們希望有更好的數學工具幫助我們做這些事，而李群與李代數理論正好提供了這樣的工具。李群與李代數廣泛地用於機器人與計算機視覺領域，並在機器人動力學推導上佔據重要地位。不過，由於SLAM不涉及過多的動力學推導。我們重點介紹它在SLAM中相關的幾個重要的結果，而略去許多數學性質的證明。特別地，重點介紹$SO(3)$和$SE(3)$這兩個李群與對應的李代數。
李代數基礎
首先，我們來討論較為簡單的三維旋轉群。為了說明它的結構，首先介紹群的概念。
群
群（Group）是一種集合加上一種運算的代數結構，記作$(A,\cdot)$。其中$A$代表集合，$\cdot$是定義在該集合上的二元運算。那麼，如果這個運算滿足以下幾個條件，則稱$G=(A, \cdot)$為群。
- 封閉性: $ \quad \forall a_1, a_2, \quad a_1 \cdot a_2 \in A$
- 結合律: $ \quad \forall a_1, a_2, a_3, \quad (a_1 \cdot a_2) \cdot a_3 = a_1 \cdot ( a_2 \cdot a_3) $
- 么元: $ \quad \exists a_0 \in A, \quad s.t. \quad \forall a \in A, \quad a_0 \cdot a = a \cdot a_0 = a $
- 逆: $ \quad \forall a \in A, \quad \exists a^{-1} \in A, \quad s.t. \quad a \cdot a^{-1} = a_0 $
讀者可以記作“封結么逆”（諧音鳳姐咬你），並可以把一些常見的群放進去驗證。例如整數的加法（么元為0），去掉0後的有理數的乘法（么元為1）。對於矩陣，可以找到一些常見的矩陣群，例如：
- 一般線性群$GL(n)$ 指$n \times n$的可逆矩陣，它們對矩陣乘法成群。
- 特殊正交群$SO(n)$ 也就是所謂的旋轉矩陣群，其中$SO(2)$ 和$SO(3)$最為常見。正式的記法是：
\[\begin{equation}
SO(n) = \{ \mathbf{R} \in \mathbb{R}^{n \times n} | \mathbf{R R}^T = \mathbf{I}, det(\mathbf{R})=1 \}
\end{equation}\]
- 特殊歐氏群$SE(n)$ 也就是前面提到的$n$維歐氏變換，如$SE(2)$和$SE(3)$。這裡給出$SE(3)$的記法：
\[\begin{equation}
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} | \mathbf{R} \in SO(3), \mathbf{t} \in \mathbb{R}^3\right\}
\end{equation}\]
群結構保證了在群上的運算具有良好的性質，而群論則研究群的各種結構和性質，但我們在此不多加介紹。感興趣的讀者可以參考任意一本近世代數教材。
李群是指具有連續性質的群。並且，一般連續群上的運算還是無限可微，乃至解析的（解析比無限可微更強，它還要求任意點鄰域的泰勒展開都收斂）。這個問題在20世紀初被稱為希爾伯特第五問題，並已得到了解決。而李群，則指實數空間上的連續群。常見的李群包括上邊提到的$GL(n), SO(n), SE(n)$，以及其他的如酉群$U(n)$，辛群$Sp(2n)$等等。
三維旋轉群$SO(3)$
三維旋轉群$SO(3)$是特殊正交群$SO(n)$在$n=3$時的特例，它們可以用來描述三維空間的旋轉，其元素都是$3 \times3$ 的正交且行列式為$+1$的矩陣。假設有這樣一個矩陣$\mathbf{R}$，滿足$\mathbf{R} \mathbf{R}^T＝\mathbf{I}$。現在，考慮它隨時間發生變化，即從$\mathbf{R}$ 變成了$\mathbf{R}(t)$，仍有$\mathbf{R}(t) \mathbf{R}(t) ^T = \mathbf{I}$。在等式兩邊對時間求導，得到：
\[\begin{equation}
\mathbf{\dot{R}} (t) \mathbf{R} {(t)^T} + \mathbf{R} (t) \mathbf{\dot{R}} {(t)^T} = 0
\end{equation}\]
於是：
\[\begin{equation}
\mathbf{\dot{R}} (t) \mathbf{R} {(t)^T} = - \left( \mathbf{\dot{R}} (t) \mathbf{R} {(t)^T} \right)^T
\end{equation}\]
可以看出$\mathbf{\dot{R}} (t) \mathbf{R} {(t)^T}$是一個反對稱矩陣。注意到對於任意一個$3 \times 3$的反對稱矩陣，我們記它為$\mathbf{A}$。由於$\mathbf{A}^T=-\mathbf{A}$，所以它主對角線元素必為$0$，而非對角線元素則只有三個自由度。我們可以把它對應到一個向量$\mathbf{a}=[a_1, a_2, a_3]^T$中去：
\[\begin{equation}
{\mathbf{a}^ \wedge } = \mathbf{A} = \left[ {\begin{array}{*{20}{c}}
0&{ - {a_3}}&{{a_2}}\\
{{a_3}}&0&{ - {a_1}}\\
{ - {a_2}}&{{a_1}}&0
\end{array}} \right]
\end{equation}\]
其中$^{\wedge}$符號表示由向量轉換為矩陣，反之我們也可以用符號$^{\vee}$定義由矩陣轉換為向量的方式：
\[\begin{equation}
{ \mathbf{A}^ \vee } = \mathbf{a}
\end{equation}\]
注意到這樣定義的好處之一，是它與叉積的相容性。我們可以直接把矩陣與任意向量的乘積$\mathbf{A} \mathbf{b} $寫成 $\mathbf{a} \times \mathbf{b}$。讀者可以自行驗證這個相容性。除此之外，這樣定義的向量還有一些較好的性質，後文會提到。
現在，由於$\mathbf{\dot{R}} (t) \mathbf{R} {(t)^T}$是一個反對稱矩陣，我們可以找到一個三維向量$\mathbf{\phi} (t) \in \mathbb{R}^3$與之對應。於是有：
\[\begin{equation}
\mathbf{ \dot{R} } (t) \mathbf{R}(t)^T = \mathbf{\phi} (t) ^ {\wedge}
\end{equation}\]
左右各右乘$\mathbf{R}(t)$，由於$\mathbf{R}$為正交陣，有：
\[\begin{equation}
\mathbf{ \dot{R} } (t) = \mathbf{\phi} (t)^{\wedge} \mathbf{R}(t) =
\left[ {\begin{array}{*{20}{c}}
0&{ - {\phi _3}}&{{\phi _2}}\\
{{\phi _3}}&0&{ - {\phi _1}}\\
{ - {\phi _2}}&{{\phi _1}}&0
\end{array}} \right] \mathbf{R} (t)
\end{equation}\]
可以看到，每對旋轉矩陣求一次導數，只需左乘一個$\mathbf{\phi}$矩陣即可。由於$\mathbf{\phi}$反映了$\mathbf{R}$的導數性質，故稱它在$SO(3)$的正切空間(tangent space)上。同時，將上式類比於一個關於$\mathbf{R}$的微分方程，可得：
\[\begin{equation}
\label{eq:so3ode}
\mathbf{R}(t) = \exp \left( \mathbf{\phi} (t) ^\wedge \right) \mathbf{R}(t_0)
\end{equation}\]
由此我們可以引出兩個概念。（1）求$\mathbf{\phi}$的方法以及它的結構？——$\mathbf{\phi}$是對應到$SO(3)$上的李代數$\mathfrak{so}(3)$；（2）$\exp( \mathbf{\phi})$如何計算？——李群與李代數間的指數/對數對映。下面我們一一加以介紹。
什麼是李代數
對於$SO(3)$和$SE(3)$，李代數可定義於李群的正切空間上，描述了李群中元素區域性性質，分別把它們記作小寫的$\mathfrak{so}(3)$和$\mathfrak{se}(3)$。首先，給出通用的李代數的定義。
李代數由一個集合$\mathbb{V}$，一個數域$\mathbb{F}$和一個二元運算$[]$組成。如果它們滿足以下幾條性質，稱$(\mathbb{V}, \mathbb{F}, [])$ 為一個李代數，記作$\mathfrak{g}$。
- 封閉性 $\forall \mathbf{X}, \mathbf{Y} \in \mathbb{V}, [\mathbf{X} \mathbf{Y}] \in \mathbb{V}$
- 雙線性 $\forall \mathbf{X,Y,Z} \in \mathbb{V}, a,b \in \mathbb{F}, $ 有 $$ [a\mathbf{X}+b\mathbf{Y}, \mathbf{Z}] = a[\mathbf{X}\mathbf{Z}] + b [ \mathbf{Y} \mathbf{Z} ] \quad [\mathbf{Z}, a \mathbf{X}+b\mathbf{Y}] = a [\mathbf{Z} \mathbf{X} ]+ b [\mathbf{ZY}] $$
- 自反性 $\forall \mathbf{X} \in \mathbb{V}, [\mathbf{X} \mathbf{X}] = \mathbf{0}$
- 雅可比等價 $\forall \mathbf{X,Y,Z} \in \mathbb{V}, [\mathbf{X}, [\mathbf{YZ}] ] + [\mathbf{Z}, [\mathbf{YX}] ] + [\mathbf{Y}, [\mathbf{ZX}]] $
從表面上來看，李代數所需要的性質還是挺多的。其中二元運算被稱為李括號。相比於群中的較為簡單的二元運算，李括號表達了兩個集合元素的差異。它不要求結合律，而滿足反對稱性，以及元素和自己做李括號之後為零的性質。作為類比，三維向量$\mathbb{R}^3$ 上定義的叉積$\times$是一種李括號，因此$\mathfrak{g} = (\mathbb{R}^3, \mathbb{R}, \times)$構成了一個李代數。讀者可以嘗試將叉積的性質代入到上面四條性質中。
三維旋轉群與對應的李代數
$SO(3)$對應的李代數是定義在$\mathbb{R}^3$上的向量，我們記作$\mathbf{\phi}$（注意這是個向量，雖然希臘字母的粗體不明顯）。根據前面的推導，每個$\mathbf{\phi}$都可以生成一個反對稱矩陣：
\[\begin{equation}
\label{eq:phi}
\mathbf{\Phi} = \mathbf{\phi}^{\wedge} = \left[ {\begin{array}{*{20}{c}}
0&{ - {\phi _3}}&{{\phi _2}}\\
{{\phi _3}}&0&{ - {\phi _1}}\\
{ - {\phi _2}}&{{\phi _1}}&0
\end{array}} \right] \in \mathbb{R}^{3 \times 3}
\end{equation}\]
在此定義下，兩個向量$\mathbf{\phi}_1, \mathbf{\phi}_2$的李括號為：
\[\begin{equation}
[\mathbf{\phi}_1, \mathbf{\phi}_2] = \mathbf{ \Phi }_1 \mathbf{ \Phi }_2 - \mathbf{ \Phi }_2 \mathbf{ \Phi }_1
\end{equation}\]
讀者可以去驗證該定義下的李括號滿足上面的幾條性質。由於$\mathbf{\phi}$ 與反對稱矩陣關係很緊密，在不引起歧義的情況下，就說$\mathfrak{so}(3)$的元素是3維向量或者3維反對稱矩陣，不加區別：
\[\begin{equation}
\mathfrak{so}(3) = \left\{ \Phi = \mathbf{\phi^\wedge} \in \mathbb{R}^{3 \times 3} | \mathbf{\phi} \in \mathbb{R}^3 \right\}
\end{equation}\]
反對稱矩陣有一些重要的性質，重點包括以下兩條：
\[\begin{equation}
\mathbf{\phi} \mathbf{\phi}^T = \mathbf{\phi}^{\wedge} \mathbf{\phi}^{\wedge} + \| \mathbf{\phi} \|^2 \mathbf{I}_{3 \times 3}
\end{equation}\]
當$\mathbf{\phi}$為單位向量時，進而有：
\[\begin{equation}
\mathbf{\phi} \mathbf{\phi}^T = \mathbf{\phi}^{\wedge} \mathbf{\phi}^{\wedge} + \mathbf{I}1
\end{equation}\]
以及
\[\begin{equation}
\mathbf{\phi}^{\wedge} \mathbf{\phi}^{\wedge} \mathbf{\phi}^{\wedge} = - \mathbf{\phi}^{\wedge}
\end{equation}\]
這兩條性質讀者也可以自行驗證，我們在指數對映中會用到。
至此，我們已清楚了$\mathfrak{so}(3)$的結構。它們是一個由三維向量組成的集合，每個向量對應到一個反對稱矩陣，可以表達旋轉矩陣的導數。現在來考慮$\exp ( \mathbf{\phi}^{\wedge} )$是如何計算的，為此我們引入指數對映。
指數對映
首先，回憶任意矩陣的指數對映。它可以寫成一個泰勒展開，但是隻有在收斂的情況下才會有結果，其結果仍是一個矩陣。
\[\begin{equation}
\exp(\mathbf{A}) = \sum\limits_{n = 0}^\infty {\frac{1}{{n!}}{ \mathbf{A}^n}}
\end{equation}\]
同樣地，對$\mathfrak{so}(3)$中任意一元素$\mathbf{\phi}$，我們亦可按此方式定義它的指數對映：
\[\begin{equation}
\exp(\mathbf{\phi}^\wedge) = \sum\limits_{n = 0}^\infty {\frac{1}{{n!}}{ (\mathbf{\phi}^{\wedge})^n}}
\end{equation}\]
現在我們來仔細看看它的含義。由於$\mathbf{\phi}$是三維向量，我們可以定義它的模長和它的方向，分別記作$\theta$和$\mathbf{a}$（注意這裡記號是有含義的，此時$\mathbf{a}$是一個單位長度的向量），那麼按照上式，可以推出如下公式，注意中間使用了上面講到了兩個反對稱矩陣的性質：
\[\begin{align*}
\exp \left( {{\mathbf{\phi} ^ \wedge }} \right) &= \exp \left( {\theta {\mathbf{a}^ \wedge }} \right) = \sum\limits_{n = 0}^\infty {\frac{1}{{n!}}{{\left( {\theta {\mathbf{a}^ \wedge }} \right)}^n}} \\
&= \mathbf{I} + \theta {\mathbf{a}^ \wedge } + \frac{1}{{2!}}{\theta ^2}{\mathbf{a}^ \wedge }{\mathbf{a}^ \wedge } + \frac{1}{{3!}}{\theta ^3}{\mathbf{a}^ \wedge }{\mathbf{a}^ \wedge }{\mathbf{a}^ \wedge } + \frac{1}{{4!}}{\theta ^4}{\left( {{\mathbf{a}^ \wedge }} \right)^4} + ...\\
&= \mathbf{a} {\mathbf{a}^T} - {\mathbf{a}^ \wedge }{\mathbf{a}^ \wedge } + \theta {\mathbf{a}^ \wedge } + \frac{1}{{2!}}\theta {\mathbf{a}^ \wedge }{\mathbf{a}^ \wedge } - \frac{1}{{3!}}{\theta ^3}{\mathbf{a}^ \wedge } + \frac{1}{{4!}}{\theta ^4}{\left( {{\mathbf{a}^ \wedge }} \right)^4} + ...\\
&= \mathbf{a}{\mathbf{a}^T} + \left( {\theta - \frac{1}{{3!}}{\theta ^3} + \frac{1}{{5!}}{\theta ^5} - ...} \right){\mathbf{a}^ \wedge } - \left( {1 - \frac{1}{{2!}}{\theta ^2} + \frac{1}{{4!}}{\theta ^4} - ...} \right){\mathbf{a}^ \wedge }{\mathbf{a}^ \wedge }\\
&= {\mathbf{a}^ \wedge }{\mathbf{a}^ \wedge } + \mathbf{I} + \sin \theta {\mathbf{a}^ \wedge } - \cos \theta {\mathbf{a}^ \wedge }{\mathbf{a}^ \wedge }\\
&= (1 - \cos \theta ){\mathbf{a}^ \wedge }{\mathbf{a}^ \wedge } + I + \sin \theta {\mathbf{a}^ \wedge }\\
&= \cos \theta \mathbf{I} + (1 - \cos \theta )\mathbf{a}{\mathbf{a}^T} + \sin \theta {\mathbf{a}^ \wedge } \\
\end{align*}\]
最後我們得到了一個似曾相識的式子：
\[\begin{equation}
\exp( \theta \mathbf{a} ) = \cos \theta \mathbf{I} + (1 - \cos \theta )\mathbf{a}{\mathbf{a}^T} + \sin \theta {\mathbf{a}^ \wedge }
\end{equation}\]
回憶前一節內容，它和羅德里格斯公式（參觀本系列第一篇）如出一輒。這表明，$\mathfrak{so}(3)$實際上就是由所謂的旋轉向量組成的空間。特別地，當轉軸取一定順序時，李代數$\mathfrak{so}(3)$還會變為對應的尤拉角。通過羅德里格斯公式或者指數對映，我們把$\mathbb{R}^3$ 中的一個向量對應到了一個位於$SO(3)$中的3D旋轉。
反之，如果定義對數對映，我們也能把$SO(3)$中的元素對應到$\mathfrak{so}(3)$中：
\[\begin{equation}
\mathbf{\phi} = \ln {\left( \mathbf{R} \right)^ \vee } = {\left( {\sum\limits_{n = 0}^\infty {\frac{{{{\left( { - 1} \right)}^n}}}{{n + 1}}{{\left( { \mathbf{R} - \mathbf{I}} \right)}^{n + 1}}} } \right)^ \vee }
\end{equation}\]
其中$^\vee$表示從反對稱矩陣到向量的對應關係，為$^\wedge$的逆運算。
讀者可能會問，指數對映性質如何呢？它是一個雙射嗎？很遺憾，它只是一個滿射。每個$SO(3)$中的元素，都可以找到$\mathfrak{so}(3)$中至少一個與之對應；但是可能存在多個$\mathfrak{so}(3)$中的元素，對應到同一個$SO(3)$元素上。至少對於旋轉角$\theta$，我們知道它具有周期性。
$SO(3)$與$\mathfrak{so}(3)$的結論似乎在我們意料之中。它和我們前面講的旋轉向量與旋轉矩陣很相似，而指數對映即是羅德里格斯公式。旋轉向量可以視為旋轉矩陣的導數，指導如何在旋轉矩陣中進行微積分運算。
三維歐氏群與對應的李代數
下面我們來介紹三維歐氏群$SE(3)$以及對應的李代數$\mathfrak{se}(3)$。有了前面的基礎，我們可以直接介紹它們的結構及運算了。$SE(3)$的結構已經在前面介紹群的時候給出：
\[\begin{equation}
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} | \mathbf{R} \in SO(3), \mathbf{t} \in \mathbb{R}^3\right\}
\end{equation}\]
每個變換矩陣有六個四由度，故對應的李代數位於$\mathbb{R}^6$中：
\[\begin{equation}
\mathfrak{se}(3) = \left\{ \mathbf{ \Xi } = \mathbf{\xi}^\wedge \in \mathbb{R}^{4 \times 4} | \mathbf{\xi} \in \mathbb{R}^6 \right\}
\end{equation}\]
但是$^\wedge$不再對應到一個反對稱關係，而是：
\[\begin{equation}
\mathbf{\xi}^\wedge = {\left[ \begin{array}{l}
\mathbf{\rho} \\
\mathbf{\phi}
\end{array} \right]^ \wedge } = \left[ {\begin{array}{*{20}{c}}
{{\mathbf{\phi} ^ \wedge }}&\mathbf{\rho} \\
{{\mathbf{0}^T}}&0
\end{array}} \right] = \mathbf{\Xi}
\end{equation}\]
可以看到，$\mathbf{\xi}$ 的前三維為旋轉向量，後三維為平移向量，其定義也十分的直觀。該李代數對應於微分方程：
\[\begin{equation}
\mathbf{\dot{T}}(t) = \mathbf{\xi}^\wedge(t) \mathbf{T}(t)
\end{equation}\]
因此
\[\begin{equation}
\mathbf{T}(t) = \exp ( \mathbf{\xi}(t)^\wedge ) \mathbf{T}(t)
\end{equation}\]
那麼$\mathfrak{se}(3)$上的指數對映如何呢？略加推導可得：
\[\begin{align}
\exp \left( {{ \mathbf{\xi} ^ \wedge }} \right) &= \left[ {\begin{array}{*{20}{c}}
{\sum\limits_{n = 0}^\infty {\frac{1}{{n!}}{{\left( {{\mathbf{\phi} ^ \wedge }} \right)}^n}} }&{\sum\limits_{n = 0}^\infty {\frac{1}{{\left( {n + 1} \right)!}}{{\left( {{\mathbf{\phi} ^ \wedge }} \right)}^n} \mathbf{\rho} } }\\
{{\mathbf{0}^T}}&1
\end{array}} \right] \\
&= \left[ {\begin{array}{*{20}{c}}
\mathbf{\Phi} &{\mathbf{J\rho} } \\
{{\mathbf{0}^T}}&1
\end{array}} \right]
\end{align}\]
左上角的$\mathbf{\Phi}$是我們熟知的$\mathfrak{so}(3)$中的元素，前文已經介紹過了。而右上角的$\mathbf{J}$則可整理為（設$\mathbf{\phi}=\theta\mathbf{a}$）：
\[\begin{equation}
\mathbf{J} = \frac{{\sin \theta }}{\theta } \mathbf{I} + \left( {1 - \frac{{\sin \theta }}{\theta }} \right) \mathbf{a} { \mathbf{a}^T} + \frac{{1 - \cos \theta }}{\theta }{ \mathbf{a}^ \wedge }
\end{equation}\]
因此我們就得到了$\mathfrak{se}(3)$的指數對映的關係。 其對數對映亦可類比推得。
小結
最後，我們對之前介紹的李群李代數進行一個簡單的小結。概而言之，李群有以下兩個重要用處：
- 李代數表達的正切空間，具有和對應李群相同的自由度。
- 指數對映能把正切空間中任意向量正好對映到原李群。
下篇中，我們將教大家用Eigen和Sophus庫處理變換矩陣與李代數。敬請期待。
參考資料
[1]. Yi Ma, An Invitation to 3D Vision. 2001.
[2]. Timothy D. Barfoot, State Estimation for Robotics: A Matrix-Lie-Group Approach, 2015.
如果你覺得我的部落格有幫助，可以進行幾塊錢的小額贊助，幫助我把部落格寫得更好。
