為什麼會有李群和李代數的引出。在通常的 SLAM 中，我們估計的無非就是在極短的時間內物體的一個相對位姿運動，然後進行累加，即可得到物體的當前位置，即 SLAM 中的定位問題，但是往往該運動在較短的時間內其變化量是極小的。
通常其運動變化我們可以使用旋轉加平移進行表示，即 \((R ,t )= T\) 。但是在實際中，我們時常想要知道這一微小的變化量，不能一直將每一次變化都記錄下來，這時候就聯想到了導數，表示的正是在某一點的一個變化率。可是對於平移 \(t\) 來說，其是具有加法運算的，但是對於 \(R\) 來說，兩個旋轉矩陣相加，是沒有意義的，而且相加之後是破壞了旋轉矩陣 \(RR^T=I\) 的性質的。
怎麼解決呢，這就要引出李群和李代數來解決。
\(R\) 和 $ T$ 都是李群的一種，雖然其不具有加法運算，但是當把其對映到李代數空間上時，就具有了加法的性質，在李代數上進行加法計算後，或者說是導數運算後，再將其對映到對應的李群即可。
前面我們介紹了若干種描述三維世界剛體運動的方式。除了用它們來描述相機的位姿之外，還要對它們進行估計和優化。這個問題可以描述為什麼樣的相機位姿最符合當前觀測資料。而典型的做法就是將其構建成一個優化問題來求解。
上一講在介紹旋轉矩陣和變換矩陣時，我們說三維旋轉矩陣構成了特殊正交群 \(SO(3)\)，變換矩陣構成了特殊歐式群 \(SE(3)\)。我們知道，矩陣對於乘法是封閉的，即假設相機進行了兩次連續的旋轉，旋轉矩陣分別為\(\mathbf{R}_1\) 和\(\mathbf{R}_2\), 這兩個矩陣相乘後得到的也是個旋轉矩陣，表示了總的旋轉。也即：
對變換矩陣也是如此。但這二者對於加法是不封閉的。兩個變換矩陣相加後得到的矩陣並不是一個變換矩陣。
如此，我們就可以引出群的定義：群（group）是一種集合加上一種運算的代數結構。把集合記為\(\mathbf{A}\)，運算記為\(\cdot\), 群就可以記為\(\mathbf{G}=(\mathbf{A},\ \cdot)\)。
三維旋轉矩陣構成了特殊正交群( \(Special Orthogonal Group\) )
三維變換矩陣構成了特殊歐氏群（\(Special Euclidean Group\)）
什麼是群？群（Group）是一種集合加上一種運算的代數結構。
記集合為\(A\)，運算為 · ，那麼當運算滿足以下性質時，稱 (\(A\), · )成群：
- 封閉性：
\[\quad \forall a_{1}, a_{2} \in A, \quad a_{1} \cdot a_{2} \in A .\\ \]
- 結合率：
\[ \quad \forall a_{1}, a_{2}, a_{3} \in A, \quad\left(a_{1} \cdot a_{2}\right) \cdot a_{3}=a_{1} \cdot\left(a_{2} \cdot a_{3}\right) . \\ \]
- 么元：
\[ \exists a_{0} \in A , s.t. \forall a \in A, \quad a_{0} \cdot a=a \cdot a_{0}=a . \\ \]
- 逆：
\[\forall a \in A, \quad \exists a^{-1} \in A , s.t. \quad a \cdot a^{-1}=a_{0} . \]
可以驗證，旋轉矩陣集合和變換矩陣集合分別和矩陣乘法構成群。還有其他很多群，如整數的加法等。群結構保證了在群上的運算具有良好的性質。
定義：李群就是具有連續（光滑）性質的群。
前面舉的整數的加法的例子顯然不是連續的，因而它不是李群。但 \(SO(3)\) 和 \(SE(3)\) 在實數空間上是連續的（機器人在三維空間中顯然是連續地運動，而不會進行「瞬移」）。
介紹完李群，在引入李代數之前，我們來回顧下開頭我們提到的問題：為什麼要用到李群和李代數？避免一直是數學上的推導。我們用一個比較實際的例子。假設某個時刻我們預測機器人的位姿為\(\mathbf{T}\)（待定值）, 它觀測到了一個慣性座標系下的點\(\mathbf{p}\) 而產生了一個觀測資料 \(\mathbf{z}\)，它是該點在相機座標系下的座標，則可得
其中，\(\mathbf{\omega}\) 是觀測噪聲。由於觀測噪聲的存在，\(\mathbf{z}\) 無法嚴格滿足式\(\mathbf{z}=\mathbf{T}\mathbf{p}\)。因此而產生的誤差\(\mathbf{e}\) 為
若共有N 個觀測值，那麼就有N 個這樣的式子。機器人的位姿估計就轉變成尋找一個最優的\(\mathbf{T}\) 使得整體的誤差最小化：
通常，直接求解上式得出最優的\(\mathbf{T}\) 是很困難的（或計算量很大）。我們常常先給定一個猜測值（初始值）\(\mathbf{T}_0\)，然後不斷地對它進行迭代更新。而這個過程需要用到導數（可以想想梯度下降法）。回顧導數的定義，\(\dot{f(x)}=\lim_{\Delta x \to 0} \frac{f(x+\Delta x)-f(x)}{\Delta x}\)。顯然計算導數和進行更新時都要用到加法。但SO(3) 和SE(3) 上對矩陣加法的運算並不封閉。如果要繼續採取這個迭代更新的策略勢必要再想想辦法，使得導數「可行」。而這就可以通過李群及其對應的李代數來實現。
現在我們考慮任意的旋轉矩陣\(\mathbf{R}\)，根據上一講的知識，有
因為機器人在不斷運動，所以\(\mathbf{R}\) 也在隨時間不斷變換，引入時間\(t\) 就得到\(\mathbf{R}(t)\)，上式也變成
等式兩邊一起對時間求導可得：
亦即
一個矩陣等於其自身轉置後取負號，可知\(\dot{\mathbf{R}}(t)\mathbf{R}(t)^T\) 是一個反對稱矩陣。我們之前就介紹過反對稱矩陣和 ^ 符號，可知對應地，對於任意的反對稱矩陣，可以找到一個與之對應的向量，這個關係可以用\(^\vee\) 來表示：
設這個對應的向量為\(\phi(t)\in R^3\)，就有
等式兩邊同時右乘\(\mathbf{R}(t)\) 則有
從式 \((11)\) 可以看到，每對旋轉矩陣求一次導數，只需要左乘一個\(\phi(t)^\wedge\) 就可以了。假設 \(t_0 = 0\) 並設此時\(\mathbf{R}(0)=\mathbf{I}\)，對式 \((10)\) 進行一階泰勒展開就有：
NOTE：\(\phi\) 反應了\(\mathbf{R}\) 的導數的性質，故稱它在 \(SO(3)\) 原點附近的正切空間（\(Tangent Space\)）上。
同時，在 $t_0 $ 附近，設\(\phi\) 也是一個常數，即\(\phi(t_0)=\phi_0\)，根據式(11) 就有
顯然，上式是一個關於\(\mathbf{R}\) 的微分方程，而且知道初始值\(\mathbf{R}(0)=\mathbf{I}\)，他的解為
由於我們之前做了一些假設，所以這個式子只在 \(t\) 附近有效。
但通過式 \((14)\) 我們知道，當某個時刻的\(\mathbf{R}\) 已知時，存在一個向量\(\phi\)，二者滿足這個矩陣指數關係 。而這個\(\phi\)，就是對應到 \(SO(3)\) 上的李代數 \(so(3)\)。下面我們就介紹一下李代數的定義和如何計算矩陣指數\(\exp(\phi^{\wedge})\).
每個李群都有與之對應的李代數。李代數描述了李群的區域性性質。其定義為：李代數由一個集合\(V\), 一個數域\(F\) 和一個二元運算\([,]\), 組成。如果它們滿足以下幾條性質，則稱\((V, F, [,])\)為一個李代數，記作g。
其中的二元運算\([,]\) 稱為李括號，它表達了兩個元素的差異。三維向量 \(R^3\)上定義的叉積就是一種李括號，\(g=(R^3,R,\times)\) 就構成了李代數。
之前我們提到的SO(3) 對應的向量\(\phi\) 就是一種李代數，是定義在R3 上的向量。它的反對稱矩陣記為\(\mathbf{\Phi}=\phi^\wedge\). 在定義下面，兩個向量\(\phi_1, \phi_2\) 的李括號為：
高翔在書中沒有明說，但經過計算可以得到結果為\(\phi_1, \phi_2\) 的叉積（又叫外積）。所以這裡我猜測so(3) 對應的李括號為三維向量R3 上的叉積。
至此，我們已經清楚了\(so(3)\) 的內容：它們是一個由三維向量組成的集合；每個向量對應到一個反對稱矩陣；（李括號為三維向量的外積運算）；可以用來表達旋轉矩陣的導數，和 \(SO(3)\) 的關係由指數對映給定：\(\mathbf{R}=\exp (\phi^\wedge)\).
下面我們先看李代數 \(se(3)\) 再來解釋指數對映。
對於\(SE(3)\)，它也有對應的李代數 \(se(3)\)。和 \(so(3)\) 類似，\(se(3)\) 在 $R^6 $ 空間中：
把每個 \(se(3)\) 元素記做\(\xi\)，它是一個六維向量：前三維為平移，記作\(\rho\)；後三維是旋轉，記作\(\phi\)，實質上是 \(s0(3)\) 元素。此外，在 \(se(3)\) 中，\(^\wedge\) 符號的含義被拓展了：這裡它將一個六維向量轉換為四維矩陣，但這裡不再表示反對稱矩陣。
同樣，李代數 $ se(3) $ 也有類似的李括號：
如前所述，李代數到李群是一個指數對映。現在來看看指數對映是如何計算的。
任意矩陣的指數對映都可以寫成一個泰勒展開。但要注意的是展開式只有在收斂的情況下才有解，其結果仍然是一個矩陣。所以，對 \(so(3)\) 中的元素\(\phi\)，其指數對映可以寫成：
將\(\phi\) 記為\(\theta \mathbf{a}\)，並利用其性質：(偶次項) \(\mathbf{a}^\wedge \mathbf{a}^\wedge=\mathbf{a}\mathbf{a}^T-\mathbf{I}\)，(奇次項) \(\mathbf{a}^\wedge \mathbf{a}^\wedge \mathbf{a}^\wedge=-\mathbf{a}^\wedge\) 可得：
回想前一講，可以發現式\((20)\) 和羅德里格斯公式有著相同的形式。這表明 \(so(3)\) 實際上就是由旋轉向量組成的空間，而指數對映就是羅德里格斯公式。也可以定義一個對數對映，將 $SO(3) $ 對映到 \(so(3)\) 上：
但上式太過複雜了。通常我們會用前一講介紹的旋轉向量到旋轉矩陣的方式來計算旋轉矩陣。
NOTE：指數對映是一個滿射，即對每個SO(3) 中的元素都可以找到一個so(3) 的元素與之對應；但存在多個so(3) 元素對應到同一個SO(3) 上（一個旋轉角為\(\theta\) 的旋轉向量多旋轉一週得到的結果顯然是一樣的）。如果把旋轉角固定在\(\pm \pi\) 或者\([0,2\pi]\) 之間，那麼李群和李代數中的元素就是一一對應的。
SE(3) 上的指數對映推導原理與SO(3) 類似，但更復雜。這裡直接給出結論：
\(\exp(\xi^\wedge)\) 的左上角是SO(3) 中的元素\(\mathbf{R}\)，是其旋轉部分。而矩陣\(\mathbf{J}\) 則為：
式(23) 與羅德里格斯公式相似。\(\xi\) 中的平移部分\(\rho\) 經過指數變換後，發生了一次以\(\mathbf{J}\) 為係數的線性變換。
相反的，SE(3) 到se(3) 也有對應的對數對映。但一般先利用左上角的旋轉矩陣計算出旋轉向量；對右上角的平移向量\(\mathbf{t}\) 則有：
即可得到\(\rho\)。
至此，我們已經介紹完了李群和李代數還有它們之間的對應關係。但最後還有一個問題。我們知道，對於兩次連續的旋轉可以用矩陣乘法來描述，但用李代數如何表示呢？我們自然是希望李群中兩個矩陣相乘對應李代數上對應元素的相加，這樣我們才能夠利用加法來計算導數和進行更新。但很遺憾，這是不成立的。
兩個李代數指數對映的乘積，由BCH 公式給出：
其中，\([,]\) 為李括號。可以看到，兩個矩陣指數的乘積是兩個矩陣之和再加上一些由李括號組成的餘項。
想象下機器人的運動，連續兩幀間相機的位姿變化是一個小量；在優化過程中，通過求導而得到的更新量也是一個小量。更具體地，考慮SO(3) 上的李代數\(\ln(\exp(\phi^\wedge_1)\exp(\phi^\wedge_2))^\vee\)，當\(\phi_1\) 或者\(\phi_2\) 為小量時，顯然，小量二次以上的項都可以忽略。此時，BCH 的線性近似為：
解釋：以第一個式子為例，當對一個旋轉矩陣\(\mathbf{R_2}\)，對應李代數為\(\phi_2\)，左乘一個微小的旋轉\(\mathbf{R}_1\) (李代數為\(\phi_1\)) 時，對應的李代數間的關係可以近似為原有的李代數\(\phi_2\) 加上一項\(\mathbf{J}_l(\phi_2)^{-1}\phi_1\)。同理，第二個式子則描述了右乘的情況。所以，在使用 $BCH $ 近似公式時必須分清楚是左乘模型還是右乘模型。
對於左乘 \(BCH\) 近似模型，雅可比矩陣 \(\mathbf{J}_l\) 就是式 $ (23)$ 中的內容：
它的逆為：
對於右乘模型，右乘雅可比矩陣就是將左乘雅可比的自變數取負號：
小結一下，假設有某個旋轉\(\mathbf{R}\)，對應的李代數為\(\phi\)。對它左乘一個微小旋轉，記作\(\Delta\mathbf{R}\)，對應的李代數為\(\Delta\phi\)。那麼，在李群上，得到的結果就是\(\Delta\mathbf{R}\cdot\mathbf{R}\)。而在李代數上，為\(\mathbf{J}_l(\phi)^{-1}\Delta\phi+\phi\)，即
反之，如果我們在李代數上進行加法，讓\(\phi\) 加上\(\Delta\phi\)，那麼可以近似為李群上帶左/右雅可比矩陣的乘法：
同樣地，對於\(SE(3)\)，也有類似的 \(BCH\) 近似公式：
這裡\(\cal{J}\) 是一個\(6*6\) 的矩陣，形式較為複雜且在實際計算中並不用到矩該陣，所以略去其具體形式。
回顧介紹李群時舉的那個例子。現在我們有了李代數，which 具有良好的加法運算。因此，我們可以用它來解決求導問題了。具體的思路有兩種：
假設空間點\(\mathbf{p}\) 進行了一次矩陣表示為\(\mathbf{R}\) 的旋轉，得到了\(\mathbf{R}\mathbf{p}\)。要計算旋轉後的點的座標相對於旋轉的導數，可以不嚴謹的寫為：
前面提到，SO(3)沒有加法，所以上式無法按照導數的定義進行計算。設\(\mathbf{R}\) 對應的李代數為\(\phi\)，就可以寫成：
按照導數的定義展開，並在推導中利用 \(BCH\) 公式和一階泰勒展開，可以得到下式：
這個公式是直接對旋轉矩陣 \(\mathbf{R}\) 進行求導，最後的結果中含有左雅可比矩陣 \(\mathbf{J}_l\)。
另一種方法就是先對旋轉矩陣\(\mathbf{R}\) 進行一次擾動\(\Delta\mathbf{R}\)，然後再對個擾動進行求導。這裡以左乘擾動模型為例。設左擾動的李代數為\(\varphi\)，直接對\(\varphi\)（而不是\(\mathbf{R}\)）進行求導：
和李代數求導模型進行對比，最明顯的就是式(37) 少了左雅可比矩陣\(\mathbf{J}_l\)。在擾動模型中，我們把小量\(\Delta\mathbf{R}\) 先直接乘在了李群上，然後再對這個小量的李代數進行求導。而在李代數求導模型中，我們是把這個小量\(\delta\phi\) 加在了李代數上，然後直接對對應的李代數進行求導。因此在所得的結果中就會有一個表示李群和李代數運算間關係（不是轉換關係）的左雅可比矩陣\(\mathbf{J}_l\)。
假設某空間點\(\mathbf{p}\) 經過了一次變換\(\mathbf{T}\)，對應的李代數為\(\xi\)，得到了\(\mathbf{T}\mathbf{p}\)。給\(\mathbf{T}\) 左乘一個微小擾動\(\Delta\mathbf{T}\)，擾動項的李代數為\(\delta\xi=[\delta\rho,\delta\phi]^T\)。則有：
最後的結果被定義為符號\(^\bigodot\) 的一個運算：把一個齊次座標下的空間點變換成一個4*6 的矩陣。
最後介紹下單目SLAM中會用到的相似變換群Sim(3)。由於單目視覺中存在一個尺度不確定性（簡單說，把單目相機兩次拍照的兩張相片想象成雙目相機一次拍的兩張照片，這兩次拍照中單目相機的位移（對應於雙目中的基線）是不知道的，導致了在畫素深度估計上的尺度不確定），因此在單目的情況下，需要把尺度因子顯式表達出來。就由歐式變換到了相似變換。
設空間點為\(\mathbf{p}\)，相似變換為：
這裡，s 就是尺度因子，它相當於對p 進行了一次縮放。
顯然，相似變換也對矩陣乘法構成群，稱為相似變換群：
它對應的李代數為sim(3)，是一個7維向量：前六維就是se(3)，最後一項為尺度因子\(\sigma\)。
二者間仍然是通過指數對映和對數對映相關聯。指數對映為：
其中，
可以看到，相似變換的雅可比矩陣遠比前面的複雜。而對數對映可以寫為：
同樣地，\(Sim(3)\) 也具有 \(BCH\) 近似公式，也有李代數倒數模型和擾動模型。這裡我們介紹擾動模型。給\(\mathbf{S}\mathbf{p}\) 左乘一個微小擾動\(\exp(\zeta^\wedge)\) 並對該微小擾動求導，記\(\mathbf{S}\mathbf{p}\) 的前三維座標為q，有
顯然，\(\mathbf{S}\mathbf{p}\) 是一個4維齊次座標，\(\zeta\) 是一個7維向量。該導數就是一個\(4*7\) 大小的雅可比矩陣。
Sophus 程式碼解析Sophus 庫。fmt#include <iostream>
#include <cmath>
#include <Eigen/Core>
#include <Eigen/Geometry>
#include "sophus/se3.hpp"
using namespace std;
using namespace Eigen;
/// 本程式演示sophus的基本用法
int main(int argc, char **argv) {
// 沿Z軸轉90度的旋轉矩陣
Matrix3d R = AngleAxisd(M_PI / 2, Vector3d(0, 0, 1)).toRotationMatrix();
// 或者四元數
Quaterniond q(R);
Sophus::SO3d SO3_R(R); // Sophus::SO3d可以直接從旋轉矩陣構造
Sophus::SO3d SO3_q(q); // 也可以通過四元數構造
// 二者是等價的
cout << "SO(3) from matrix:\n" << SO3_R.matrix() << endl;
cout << "SO(3) from quaternion:\n" << SO3_q.matrix() << endl;
cout << "they are equal" << endl;
// 使用對數對映獲得它的李代數
Vector3d so3 = SO3_R.log();
cout << "so3 = " << so3.transpose() << endl;
// hat 為向量到反對稱矩陣
cout << "so3 hat=\n" << Sophus::SO3d::hat(so3) << endl;
// 相對的，vee為反對稱到向量
cout << "so3 hat vee= " << Sophus::SO3d::vee(Sophus::SO3d::hat(so3)).transpose() << endl;
// 增量擾動模型的更新
Vector3d update_so3(1e-4, 0, 0); //假設更新量為這麼多
Sophus::SO3d SO3_updated = Sophus::SO3d::exp(update_so3) * SO3_R;
cout << "SO3 updated = \n" << SO3_updated.matrix() << endl;
cout << "*******************************" << endl;
// 對SE(3)操作大同小異
Vector3d t(1, 0, 0); // 沿X軸平移1
Sophus::SE3d SE3_Rt(R, t); // 從R,t構造SE(3)
Sophus::SE3d SE3_qt(q, t); // 從q,t構造SE(3)
cout << "SE3 from R,t= \n" << SE3_Rt.matrix() << endl;
cout << "SE3 from q,t= \n" << SE3_qt.matrix() << endl;
// 李代數se(3) 是一個六維向量，方便起見先typedef一下
typedef Eigen::Matrix<double, 6, 1> Vector6d;
Vector6d se3 = SE3_Rt.log();
cout << "se3 = " << se3.transpose() << endl;
// 觀察輸出，會發現在Sophus中，se(3)的平移在前，旋轉在後.
// 同樣的，有hat和vee兩個算符
cout << "se3 hat = \n" << Sophus::SE3d::hat(se3) << endl;
cout << "se3 hat vee = " << Sophus::SE3d::vee(Sophus::SE3d::hat(se3)).transpose() << endl;
// 最後，演示一下更新
Vector6d update_se3; //更新量
update_se3.setZero();
update_se3(0, 0) = 1e-4;
Sophus::SE3d SE3_updated = Sophus::SE3d::exp(update_se3) * SE3_Rt;
cout << "SE3 updated = " << endl << SE3_updated.matrix() << endl;
return 0;
}