前言
理解李群與李代數，是理解許多SLAM中關鍵問題的基礎。本講我們繼續介紹李群李代數的相關知識，重點放在李群李代數的微積分上，這對解決姿態估計問題具有重要意義。
回顧
為了描述三維空間裡的運動，我們使用3$\times $3的旋轉矩陣$\mathbf{R}$來描述一個剛體的旋轉，並且，用4$\times$4的變換矩陣來描述六自由度的旋轉+平移。這兩種矩陣在傳統的歐氏空間$\mathbb{R}^{3 \times 3}$和$\mathbb{R}^{4 \times 4}$中，不存在加法運算，只有乘法運算，故無法構成線性空間，只能構成群。也就是我們說的李群$SO(3)$和$SE(3)$：
\[\begin{equation}
SO(3) = \{ \mathbf{R} \in \mathbb{R}^{3 \times 3} | \mathbf{R R}^T = \mathbf{I}, det(\mathbf{R})=1 \}
\end{equation}\]
\[\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}\]
在李群中，我們使用矩陣來表達一個旋轉和平移，這存在冗餘的自由度。三維空間的旋轉只有三自由度，旋轉+平移有六自由度。因此，我們希望尋找一個沒有冗餘自由度（但是相應的存在奇異性）的表示，也就是李代數$\mathfrak{so}(3)$和$\mathfrak{se}(3)$：
\[\begin{equation}
\mathfrak{so}(3) = \{ \mathbf{\phi} \in \mathbb{R}^{3} \}
\end{equation}\]
\[\begin{equation}
\mathfrak{se}(3) = \{ \mathbf{\xi } \in \mathbb{R}^{6} \}
\end{equation}\]
怎麼將李代數和李群元素對應起來呢？我們說，從李代數到李群，通過指數對映；反之，從李群到李代數，則通過對數對映。
$\mathfrak{so}(3)$的一個元素$\mathbf{\phi}=\mathbf{a} \theta $的指數對映為：
\[\begin{equation} exp\left( {{\phi ^ \wedge }} \right) = \cos \theta {\bf{I}} + (1 - \cos \theta ){\bf{a}}{{\bf{a}}^T} + \sin \theta {{\bf{a}}^ \wedge } \end{equation}\]
其中$\wedge$算符把一個三維向量變換為它對應的反對稱矩陣（3$\times $3）。
該式和Rodrigues（羅德里格斯）公式是一致的。說明李代數$\mathfrak{so}(3)$裡的向量，其方向指向旋轉軸，而模長表示轉過的角度。因此，平時用的rpy尤拉角，實際上是李代數之一。另一點需要說明的是，指數對映是一個滿射而非單射。由於旋轉具有周期性，多轉360度和不轉，表示的是同一個旋轉。如果我們把旋轉限制在正負180度內，那麼李群到李代數就是一一對應的。
反之，對數對映$\ln {\left( \mathbf{R} \right)^ \vee }$將一個旋轉矩陣轉換為一個李代數。但由於對數的泰勒展開不是很優雅，通常直接按照下式計算李代數向量的大小和方向：
\[ \begin{equation} \begin{array}{l}
\theta = {\cos ^{ - 1}}\frac{{tr\left( \mathbf{R} \right) - 1}}{2} + 2k\pi \\
\mathbf{Ra = a}
\end{array} \end{equation} \]
根據該結果得到的$\mathbf{\phi} = \theta \mathbf{a}$就是對應的李代數了。到這為止的內容都在上一講中介紹過，本講我們就接著往下說。
$SE(3)$中的指數對映和對數對映
$\mathfrak{se}(3)$裡的向量經常寫成$\mathbf{\xi}$。它是一個六維向量，通常前三維為平移，後三維為旋轉（也就是$\mathfrak{so}(3)$裡的元素）。請注意，有些地方的定義是倒過來的，即前三維為旋轉，後三維為平移，在使用的時候一定要小心！例如g2o的李代數和Sohpus的李代數定義就不一樣。
記$\mathbf{\xi}=[\mathbf{\rho}, \mathbf{\phi}]^T$，它們各為三維向量，一共組成了六維，構成一個李代數。我們的記法裡$\rho$為平移，$\phi$為旋轉，與$SO(3)$中一致。
與$SO(3)$類似，$\mathfrak{se}(3)$到$SE(3)$的指數對映為：
\[ \begin{equation} \begin{array}{lll}
\exp \left( {{\xi ^ \wedge }} \right) &=& \sum\limits_{n = 0}^\infty {\frac{1}{{n!}}{{\left( {{\xi ^ \wedge }} \right)}^n}} \\
&=& \left[ {\begin{array}{*{20}{c}}
{\exp \left( {{\phi ^ \wedge }} \right)}&{ \mathbf{J\rho} }\\
{{0^T}}&1
\end{array}} \right]
\end{array} \end{equation} \]
請注意該式左上即 $SO(3)$ 的指數對映，右上較特殊一些，是平移量 $\mathbf{\rho}$ 的線性表示。其係數$\mathbf{J}_{3\times 3}$的計算方式如下：
\[\begin{array}{lll}
\mathbf{J} &=& \sum\limits_{n = 0}^\infty {\frac{1}{{\left( {n + 1} \right)!}}{{\left( {{ \mathbf{\phi} ^ \wedge }} \right)}^n}} \\
&=& \frac{{\sin \theta }}{\theta }I + \left( {1 - \frac{{\sin \theta }}{\theta }} \right) \mathbf{a}{\mathbf{a}^T} + \frac{{1 - \cos \theta }}{\theta }{\mathbf{a}^ \wedge }
\end{array}\]
它與Rodrigues相似但有差別，實際上是對$\phi$求了一次導數，所以這個陣也就記成了$\mathbf{J}$，意為一個雅可比陣。它在後續許多地方會用到，所以現在單獨提一下它。
這個矩陣是可逆的，它的逆形式如下：
\[\begin{equation} {\mathbf{J}^{ - 1}} = \frac{\theta }{2}\cot \frac{\theta }{2}I + \left( {1 - \frac{\theta }{2}\cot \frac{\theta }{2}} \right)a{a^T} - \frac{\theta }{2}{a^ \wedge } \end{equation}\]
嗯，現在我們說清了$SE(3)$上的指數對映。那麼它的對數對映怎麼計算呢？也就是一個$SE(3)$中的矩陣怎麼找到對應的向量呢？我們並不需要實際地計算一個矩陣對數，而只需要計算：
對於
\[\mathbf{T} = \left[ {\begin{array}{*{20}{c}}
\mathbf{R} & \mathbf{t}\\
{{0^T}}&1
\end{array}} \right]\]
它對應的李代數 $\mathbf{\xi}=[\mathbf{\rho}, \mathbf{\phi}]^T$ 為：
\[ \begin{equation} \phi = \ln {\left( R \right)^ \vee },\rho = {J^{ - 1}}t \end{equation} \]
因為已經有了$SO(3)$上的計算方式，這裡的式子就變得十分簡單了。
Baker-Campbell-Hausdorff公式
小蘿蔔：師兄我還有個問題啊。
師兄：嗯你說。
小蘿蔔：李群和李代數的對應關係倒是不難啦。但實際用的時候，我經常在李群上做兩個矩陣的乘法。例如，做$\mathbf{R_1} \cdot \mathbf{R_2}$時，它們對應的李代數發生了什麼呢？是不是直接加起來啊？
對，這就是我們要討論的問題，即：
\[ \begin{equation} \exp \left( {\phi _1^ \wedge } \right)\exp \left( {\phi _2^ \wedge } \right) = \exp \left( {{{\left( {{\phi _1} + {\phi _2}} \right)}^ \wedge }} \right) \end{equation} \]
成立嗎？
如果該式成立的話，李群和李代數的結構就很方便了。因為我們可以直接把李群的乘法變成李代數的加法，而不需要改變任何東西。直觀上，兩個指數函式相乘，也確實等於把冪加起來，再取指數。唯一不同的是，這裡我們不是對一個數求指數，而是求一個反對稱矩陣的指數。
但是成不成立呢？
很遺憾，雖然我們非常希望該式成立，但是實質上它是不滿足的。Baker-Campbell-Hausdorff公式（https://en.wikipedia.org/wiki/Baker%E2%80%93Campbell%E2%80%93Hausdorff_formula）給出了這個運算的答案：
\[\ln \left( {\exp \left( A \right)\exp \left( B \right)} \right) = \sum\limits_{n = 1}^\infty {\frac{{{{\left( { - 1} \right)}^{n - 1}}}}{n}\sum\limits_{{r_i} + {s_i} > 0,i \in \left[ {1,n} \right]}^{} {\frac{{{{\left( {\sum\limits_{i = 1}^n {\left( {{r_i} + {s_i}} \right)} } \right)}^{ - 1}}}}{{\prod\limits_{i = 1}^n {{r_i}!{s_i}!} }}\left[ {{A^{{r_1}}}{B^{{s_1}}}{A^{{r_2}}}{B^{{s_2}}} \cdots {A^{{r_n}}}{B^{{s_n}}}} \right]} } \]
那麼這式子在說什麼鬼呢？
為了避免過於冗長的解釋，我們就直接看它的近似式好了：
\[\ln \left( {\exp \left( A \right)\exp \left( B \right)} \right) = A + B + \frac{1}{2}\left[ {A,B} \right] + \frac{1}{{12}}\left[ {A,\left[ {A,B} \right]} \right] - \frac{1}{{12}}\left[ {B,\left[ {A,B} \right]} \right] + \cdots \]
這裡的方括號指的是李括號，也就是：
\[\left[ {A,B} \right] = AB - BA\]
特別地，在$SO(3)$上，我們可以求它關於某一個變數的一階近似：
\[ \begin{equation} \ln {\left( {{R_1}{R_2}} \right)^ \vee } \approx \left\{ \begin{array}{l}
{J_l}{\left( {{\phi _2}} \right)^{ - 1}}{\phi _1} + {\phi _2}\\
{J_r}{\left( {{\phi _1}} \right)^{ - 1}}{\phi _2} + {\phi _1}
\end{array} \right. \end{equation} \]
對我們用處最大的就是這個近似式。在 $\phi_1$ 較小時，使用第一個式；在在 $\phi_2$ 較小時，使用第二個式。這裡的 $J_l$ 和 $J_r$ 也稱為左/右雅可比——從而李代數就分成了左右兩種模型（因此，李代數程式庫會宣告它使用的是左乘模型還是右乘模型）。
那麼這倆雅可比是啥類？
事實上，前面已經提到過它了：
\[ \begin{equation} {J_l} = J = \sum\limits_{n = 0}^\infty {\frac{1}{{\left( {n + 1} \right)!}}{{\left( {{\phi ^ \wedge }} \right)}^n}} ,{J_r} = J\left( { - \phi } \right) \end{equation}\]
在這個近似下，我們只保留了和小量相關的一階資訊，而捨棄了它的高階項。因此，可以把這個過程類比於保留泰勒展開的一階項而捨棄高階項的過程，其中泰勒展開類比於BCH公式。
BCH近似的直觀意義
以左乘形式（$\phi_1$較小）為例。我們給一個旋轉矩陣 $\mathbf{R}$ 左乘一個微小擾動 $\Delta R = \exp \left( {\Delta {\phi ^ \wedge }} \right)$，得到了$\Delta R \cdot R$。那麼，對應到李代數上的變化量為：
\[ \begin{equation} \ln {\left( {\Delta R \cdot R} \right)^ \vee } = \phi + J_l^{ - 1}\left( \phi \right)\Delta \phi \end{equation} \]
反過來說，如果對某個固定的李代數$\phi$增加一個小量$\delta \phi$，對應到李代數上，為：
\[ \begin{equation} \exp {\left( {\phi + \delta \phi } \right)^ \wedge } = \exp {\left( {{J_l}\delta \phi } \right)^ \wedge } \cdot \exp {\left( \phi \right)^ \wedge } \end{equation} \]
相當於左乘一個 $J_l \phi$ 對應的旋轉陣。類似地，由於左右對稱，有：
\[ \begin{equation} \exp {\left( {\phi + \delta \phi } \right)^ \wedge } = \exp {\left( {{J_l} \delta \phi } \right)^ \wedge } \cdot \exp {\left( \phi \right)^ \wedge } = \exp {\left( \phi \right)^ \wedge }\exp {\left( {{J_r}\delta \phi } \right)^ \wedge } \end{equation} \]
從這兩個式子可以清楚地看到，BCH近似為我們指明瞭，李群與李代數微小量之間的關係——即，在BCH線性近似的意義下，它們只差一個雅可比矩陣。當區別左右乘時，這兩個雅可比自變數差一個符號。
應用：微分模型和擾動模型
實際中，我們經常考慮這樣一個問題（我們先討論$SO(3)$上，再討論$SE(3)$上）：
設一個空間點$P=[x,y,z]$，它經過一次旋轉，變成$RP$。問$RP$隨著$R$是如何變化的？
由於$SO(3)$上面沒有加法，我們無法使用傳統導數的定義（因為 $R+\Delta R$ 不再是旋轉矩陣）：
\[\frac{{d\left( {RP} \right)}}{{dR}} = \mathop {\lim }\limits_{\Delta R \to 0} \frac{{\left( {R + \Delta R} \right)P - RP}}{{\Delta R}}\]
為了解決這個問題，有兩種方式：一是把增量定義在李代數上，對應於微分模型；二是把增量直接乘在$R$上，對應於擾動模型。如果讀者熟悉了前面的公式，那這件事情是很輕鬆的。
-
定義在李代數上的增量
在這個意義下，導數的含義為（最後一步跳過了若干過程，實際需要分開對各分量計算導數）：
\[\begin{array}{lll}
\frac{{d\left( {RP} \right)}}{{d\phi }} &=& \mathop {\lim }\limits_{\delta \phi \to 0} \frac{{\exp {{\left( {\phi + \delta \phi } \right)}^ \wedge }P - \exp {{\left( \phi \right)}^ \wedge }P}}{{\delta \phi }}\\
&=& \mathop {\lim }\limits_{\Delta \phi \to 0} \frac{{(\exp {{\left( {{J_l}\delta \phi } \right)}^ \wedge } - 1)exp{{\left( \phi \right)}^ \wedge }P}}{{\delta \phi }}\\
&=& - {(RP)^ \wedge }{J_l}
\end{array}\]
2. 定義在李群上的乘增量
另一種方式是將小量乘在李群上，而非加在李代數上。這種方式定義的模型稱為擾動模型。由於左右的差異，需要區別這個增量是左乘在旋轉陣，還是右乘在旋轉陣上的。如果使用左乘，那麼導數的含義為：
\[\begin{equation} \begin{array}{l}
\frac{{d\left( {RP} \right)}}{{dR}} = \mathop {\lim }\limits_{\delta \phi \to 0} \frac{{\exp {{\left( {\delta \phi } \right)}^ \wedge } \cdot RP - RP}}{{\delta \phi }}\\
= \mathop {\lim }\limits_{\Delta \phi \to 0} \frac{{(\exp {{\left( {\delta \phi } \right)}^ \wedge } - 1)RP}}{{\delta \phi }}\\
= - {\left( {RP} \right)^ \wedge }
\end{array} \end{equation} \]
可見，由於我們把小量乘在李群上（而非加在李代數上），結果中的導數將少一個雅可比矩陣。因此，擾動模型計算更為簡潔，在實際上更加常用。
$SE(3)$上擾動模型
$SE(3)$上擾動模型為：考慮一個空間點$P$（必須為齊次座標，否則維數不對）受到剛體變換 $T$ ，得到 $TP$ 。求 $TP$ 是如何隨 $T$ 變化的。
類似於$SO(3)$，我們給$T$左乘一個小量$\Delta T=exp(\delta \xi)^\wedge $，得到：
\[ \begin{equation}\begin{array}{l}
\frac{{d\left( {TP} \right)}}{{dT}} = \mathop {\lim }\limits_{\delta \xi \to 0} \frac{{\exp {{\left( {\delta \xi } \right)}^ \wedge }TP - TP}}{{\delta \xi }}\\
= {(TP)^ \odot }
\end{array}\end{equation} \]
其中$^ \odot$算符把一個4$\times$4的矩陣變換成一個4$\times$6的矩陣，計算方式如下：
\[ \begin{equation} p = \left[ \begin{array}{l}
{x_{3 \times 1}}\\
{w_{1 \times 1}}
\end{array} \right], \quad {p^ \odot } = \left[ {\begin{array}{*{20}{c}}
{w{I_{3 \times 3}}}&{ - {x^ \wedge }_{3 \times 3}}\\
{{0^T}_{3 \times 1}}&{{0^T}_{3 \times 1}}
\end{array}} \right] \end{equation} \]
至此，我們明確了一個被變換之後的點，是如何隨變換矩陣而變化的。注意，我們省略了$SE(3)$上BCH近似雅可比的推導（比較複雜且擾動模型裡沒有該雅可比），如果讀者感興趣可參考[1]。
小蘿蔔：師兄，知識我倒是知道了，您能講講$^ \odot$怎麼念嗎？
師兄：誒？……你看它像一個石子掉進井裡，就念"dong"吧。
小蘿蔔：所以$SE(3)$對李代數的擾動的導數就是那個點自己的“dong”嘍？
師兄：你願意怎麼記就怎麼記吧……我不管了。
小結
本節介紹了以下幾個關於李群代數的知識：
1. $SE(3)$的結構，指數對映和對數對映；
2. BCH公式與近似公式；
3. 李代數上的微分運算。
其中，介紹微分是介紹李代數的主要目的所在。知道了如何對位姿求導，我們才能深入地討論姿態估計，直接法VO等話題。下一講我們將介紹direct VO和semi-direct VO的方法。
[1]. Barfoot, State Estimation for Robotics: A Matrix-Lie-Group Approach, 2016