視覺SLAM中的數學基礎第三篇李羣與李代數

時間 2019-11-06

原文原文鏈接

視覺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}$對於乘法是封閉的，但對加法不封閉，即任意兩個變換矩陣相加後並非一個變換矩陣，這主要是由於旋轉矩陣對加法是不封閉的。spa

　　出於這兩個緣由，咱們但願有更好的數學工具幫助咱們作這些事，而李羣與李代數理論正好提供了這樣的工具。李羣與李代數普遍地用於機器人與計算機視覺領域，並在機器人動力學推導上佔據重要地位。不過，因爲SLAM不涉及過多的動力學推導。咱們重點介紹它在SLAM中相關的幾個重要的結果，而略去許多數學性質的證實。特別地，重點介紹$SO(3)$和$SE(3)$這兩個李羣與對應的李代數。blog

李代數基礎

　　首先，咱們來討論較爲簡單的三維旋轉羣。爲了說明它的結構，首先介紹羣的概念。博客

羣數學

　　羣（Group）是一種集合加上一種運算的代數結構，記做$(A,\cdot)$。其中$A$表明集合，$\cdot$是定義在該集合上的二元運算。那麼，若是這個運算知足如下幾個條件，則稱$G=(A, \cdot)$爲羣。it

封閉性: $ \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）。對於矩陣，能夠找到一些常見的矩陣羣，例如：io

通常線性羣$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)$的結構已經在前面介紹羣的時候給出：

　　每一個變換矩陣有六個四由度，故對應的李代數位於$\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)$的指數映射的關係。其對數映射亦可類比推得。