李群李代數(shù)是視覺SLAM里幾乎每個(gè)人都要翻一次的高墻。很多同學(xué)學(xué)到第三講就開始勸退我當(dāng)初也一樣盯著SO(3)、se(3)這些符號(hào)看了兩天完全想不明白為什么要這么折騰。但說實(shí)話這一講是整個(gè)SLAM后端優(yōu)化的地基搞不懂它之后看BA、看位姿圖優(yōu)化都是云里霧里。這篇筆記我按自己的理解重新梳理了一遍核心概念附帶了課后典型習(xí)題的詳細(xì)求解思路最后補(bǔ)充了一些動(dòng)手實(shí)操時(shí)容易踩的坑。1. 李群與李代數(shù)到底在解決什么問題1.1 旋轉(zhuǎn)矩陣的加法不封閉困局先問一個(gè)最基礎(chǔ)的問題為什么要引入李群李代數(shù)答案要從旋轉(zhuǎn)矩陣的運(yùn)算性質(zhì)說起。我們知道旋轉(zhuǎn)矩陣構(gòu)成的特殊正交群SO(3)滿足兩個(gè)關(guān)鍵性質(zhì)正交性$R^T R I$和行列式為1$\det(R) 1$。這意味著兩個(gè)旋轉(zhuǎn)矩陣相乘的結(jié)果仍然是旋轉(zhuǎn)矩陣群結(jié)構(gòu)對(duì)乘法是封閉的。但問題來了——兩個(gè)旋轉(zhuǎn)矩陣相加結(jié)果還是旋轉(zhuǎn)矩陣嗎答案顯然是否定的。這就是SLAM中最麻煩的一件事。我們?cè)趦?yōu)化位姿時(shí)本質(zhì)上是在調(diào)整旋轉(zhuǎn)矩陣的數(shù)值讓重投影誤差最小化。最自然的做法是像優(yōu)化普通向量一樣給當(dāng)前的$R$加上一個(gè)增量$\Delta R$然后繼續(xù)迭代。但$R \Delta R$根本不在SO(3)上它的正交性和行列式約束全被破壞了。你強(qiáng)行把加法的結(jié)果當(dāng)作旋轉(zhuǎn)矩陣使用很快就會(huì)得到一堆毫無意義的數(shù)值。所以問題的核心就變成了如何在保持旋轉(zhuǎn)矩陣約束的前提下對(duì)旋轉(zhuǎn)量進(jìn)行正常的加法式優(yōu)化。李群李代數(shù)給出的答案是——把旋轉(zhuǎn)矩陣換個(gè)坐標(biāo)系表示在那個(gè)坐標(biāo)系里可以做加法算完再映射回來。1.2 群的定義與李群的結(jié)構(gòu)要理解李群先得知道群是什么。群是一個(gè)集合加上一種運(yùn)算滿足封閉性、結(jié)合律、存在幺元、存在逆元這四個(gè)條件。SO(3)和SE(3)都是典型的李群因?yàn)樗鼈兂藵M足群的性質(zhì)之外還具備流形的結(jié)構(gòu)——也就是光滑可微。這句話聽起來很抽象你可以把李群理解成一個(gè)帶約束的光滑曲面每個(gè)旋轉(zhuǎn)矩陣都是這個(gè)曲面上的一個(gè)點(diǎn)曲面在局部看起來像平直的歐氏空間。關(guān)鍵洞察來了既然曲面在每個(gè)點(diǎn)附近都像平面那我們就可以在這個(gè)局部平面上研究旋轉(zhuǎn)的變化規(guī)律。這個(gè)局部平面就是該點(diǎn)處的切空間而李代數(shù)恰恰就是這個(gè)切空間。所以李代數(shù)并不是什么玄乎的東西它就是李群在單位元處的切空間描述了李群局部無窮小的運(yùn)動(dòng)方式。1.3 SLAM中實(shí)際使用的李群與李代數(shù)在視覺SLAM中我們最關(guān)心四樣?xùn)|西$SO(3)$、$SE(3)$以及它們對(duì)應(yīng)的李代數(shù)$\mathfrak{so}(3)$、$\mathfrak{se}(3)$。它們的關(guān)系可以整理成一張表名稱含義維度代表元素$SO(3)$三維旋轉(zhuǎn)矩陣群3$R_{3\times3}$$SE(3)$三維剛體變換矩陣群6$T_{4\times4}$$\mathfrak{so}(3)$旋轉(zhuǎn)矩陣對(duì)應(yīng)的李代數(shù)3三維向量$\phi$$\mathfrak{se}(3)$變換矩陣對(duì)應(yīng)的李代數(shù)6六維向量$\xi$有一點(diǎn)要特別注意$SE(3)$是6維的但變換矩陣$T$是4×4共16個(gè)元素所以$SE(3)$實(shí)際上是一個(gè)16維空間中的6維約束流形。這也就是為什么我們不能直接對(duì)$T$做加法優(yōu)化——16個(gè)元素之間存在著很強(qiáng)的內(nèi)在約束只有6個(gè)自由度是真正獨(dú)立的。2. 指數(shù)映射與對(duì)數(shù)映射李代數(shù)與李群之間的橋梁2.1 從旋轉(zhuǎn)向量到旋轉(zhuǎn)矩陣的指數(shù)映射李代數(shù)$\mathfrak{so}(3)$里的元素是一個(gè)三維向量$\phi$它的物理意義其實(shí)就是旋轉(zhuǎn)向量方向代表旋轉(zhuǎn)軸模長(zhǎng)代表旋轉(zhuǎn)角度。所以從$\mathfrak{so}(3)$到$SO(3)$的指數(shù)映射本質(zhì)上是把旋轉(zhuǎn)向量變成旋轉(zhuǎn)矩陣的過程。這個(gè)映射的公式就是大名鼎鼎的羅德里格斯公式$$R \cos\theta \cdot I (1 - \cos\theta) \cdot aa^T \sin\theta \cdot a^{\wedge}$$其中$\theta$是旋轉(zhuǎn)向量的模長(zhǎng)$a$是單位旋轉(zhuǎn)軸向量。這個(gè)公式完美體現(xiàn)了旋轉(zhuǎn)向量的幾何含義。但很多同學(xué)并不知道這個(gè)公式是怎么來的這里我用自己的理解推導(dǎo)一遍關(guān)鍵思路。從矩陣指數(shù)的定義出發(fā)$e^{\phi^{\wedge}} \sum_{n0}^{\infty} \frac{1}{n!}(\phi^{\wedge})^n$。直接展開這個(gè)級(jí)數(shù)是很難算的。關(guān)鍵在于利用反對(duì)稱矩陣的一個(gè)性質(zhì)三維反對(duì)稱矩陣$A$滿足$A^3 -|A|^2 A$這里的$|A|$不是矩陣的行列式而是對(duì)應(yīng)向量的模長(zhǎng)。利用這個(gè)性質(zhì)$A$的奇數(shù)次冪都能化簡(jiǎn)成$A$的倍數(shù)偶數(shù)次冪都能化簡(jiǎn)成$A^2$的倍數(shù)。把無窮級(jí)數(shù)拆成奇數(shù)項(xiàng)和偶數(shù)項(xiàng)就能湊出$\sin\theta$和$\cos\theta$的級(jí)數(shù)展開式。整個(gè)推導(dǎo)過程就像把復(fù)雜的指數(shù)運(yùn)算降維成三角函數(shù)運(yùn)算這也是羅德里格斯公式的精髓所在。2.2 為什么矩陣指數(shù)能收斂成有限項(xiàng)你可能好奇一個(gè)無限級(jí)數(shù)求和為什么最后收斂成一個(gè)有限項(xiàng)的組合關(guān)鍵就隱藏在反對(duì)稱矩陣的冪次周期性上。三維反對(duì)稱矩陣的平方乘以自身的模長(zhǎng)平方等于立方這使得冪次呈現(xiàn)出3階循環(huán)的特征。換句話說$A$的冪次只存在兩種獨(dú)立形式$A$和$A^2$更高次的冪能不斷化簡(jiǎn)回這兩種形式。這樣的性質(zhì)大大簡(jiǎn)化了計(jì)算。理論上$e^{A}$涉及無窮多次矩陣乘法但因?yàn)閮绱蔚闹芷谛詿o窮級(jí)數(shù)被壓縮成了對(duì)$\sin$和$\cos$兩個(gè)標(biāo)量三角函數(shù)的求和。這就是為什么羅德里格斯公式不僅形式優(yōu)美而且計(jì)算效率極高——每次矩陣指數(shù)運(yùn)算只需要計(jì)算兩個(gè)三角函數(shù)值和三次矩陣加法乘法。2.3 SE(3)的指數(shù)映射而不僅僅是平移疊加$\mathfrak{se}(3)$的情況比$\mathfrak{so}(3)$更復(fù)雜。$\mathfrak{se}(3)$中的元素是一個(gè)六維向量$\xi [\rho, \phi]^T$其中$\rho$是平移相關(guān)的分量$\phi$是旋轉(zhuǎn)分量。它的指數(shù)映射可以分塊處理$$T \exp(\xi^{\wedge}) \begin{bmatrix} R J\rho \ 0 1 \end{bmatrix}$$這里的$R$就是由$\phi$決定的旋轉(zhuǎn)矩陣$J$是一個(gè)3×3的矩陣通常被稱為雅可比矩陣。注意$J\rho$并不等于$\rho$本身說明李代數(shù)中的平移分量和平移向量之間差了一個(gè)$J$變換。這件事在SLAM初始化代碼中特別容易出錯(cuò)很多人直接從$\mathfrak{se}(3)$里取出平移分量去更新位置結(jié)果發(fā)現(xiàn)軌跡出現(xiàn)奇怪的漂移往往就是忽略了$J$的作用。$J$的具體表達(dá)式為$$J \frac{\sin\theta}{\theta}I \left(1-\frac{\sin\theta}{\theta}\right)aa^T \frac{1-\cos\theta}{\theta}a^{\wedge}$$當(dāng)旋轉(zhuǎn)角度很小時(shí)$J$趨近于單位陣但這個(gè)近似只有在角度趨近于0時(shí)才成立。實(shí)際處理中位姿變化較大時(shí)不能隨便做近似。2.4 對(duì)數(shù)映射與李代數(shù)坐標(biāo)提取反過來從李群到李代數(shù)的映射叫對(duì)數(shù)映射。給定旋轉(zhuǎn)矩陣$R$可以通過$\theta \arccos\left(\frac{\mathrm{tr}(R)-1}{2}\right)$求出旋轉(zhuǎn)角再通過$Rn n$求出旋轉(zhuǎn)軸。這里$n$是$R$特征值為1時(shí)對(duì)應(yīng)的特征向量換句話說旋轉(zhuǎn)軸就是$R$不變的那條方向。從$SE(3)$提取$\mathfrak{se}(3)$也是兩步先通過對(duì)數(shù)映射提取旋轉(zhuǎn)部分得到$\phi$然后用$t J\rho$反解出$\rho J^{-1}t$。$J$的逆有解析表達(dá)式不需要做矩陣求逆。在實(shí)際工程里如果你用的是Sophus庫(kù)這些映射都封裝成了庫(kù)函數(shù)不需要手推但理解原理對(duì)調(diào)試幫助極大——我遇到過好幾次位姿圖優(yōu)化發(fā)散最后查出就是因?yàn)槭謩?dòng)提取擾動(dòng)增量時(shí)搞錯(cuò)了平移部分的坐標(biāo)變換。3. 伴隨性質(zhì)與BCH公式理解擾動(dòng)的兩種姿勢(shì)3.1 李群乘法與李代數(shù)加法的對(duì)應(yīng)關(guān)系引入李代數(shù)之后有一個(gè)快樂把李群的乘法對(duì)應(yīng)到李代數(shù)的加法。但實(shí)際計(jì)算中這個(gè)對(duì)應(yīng)并不完美存在一個(gè)重要的修正公式——BCH公式。用數(shù)學(xué)公式表達(dá)的話$\exp(A)\exp(B)$并不等于$\exp(AB)$中間還差一些高階項(xiàng)。這個(gè)差異在SLAM中不能被忽略因?yàn)樗苯記Q定了優(yōu)化迭代時(shí)的增量更新方式。BCH公式的完整形式極其復(fù)雜但在SLAM中我們通常只需要兩個(gè)近似形式左乘小擾動(dòng)的近似和右乘小擾動(dòng)的近似。例如當(dāng)$\phi_1$很小時(shí)有$$\ln(\exp(\phi_1^{\wedge})\exp(\phi_2^{\wedge}))^{\vee} \approx J_l^{-1}(\phi_2)\phi_1 \phi_2$$這里的$J_l$就是前面提到的左雅可比矩陣。這個(gè)公式的含義是在旋轉(zhuǎn)矩陣上左乘一個(gè)小量對(duì)應(yīng)的李代數(shù)增量近似等于在當(dāng)前李代數(shù)上加一個(gè)經(jīng)過$J_l^{-1}$修正的量。3.2 左擾動(dòng)模型與右擾動(dòng)模型的選擇在視覺SLAM后端的求導(dǎo)中最常用的操作是對(duì)$\exp(\phi^{\wedge}) p$求$\phi$的導(dǎo)數(shù)。直接按定義求導(dǎo)需要處理矩陣指數(shù)的無窮級(jí)數(shù)推導(dǎo)過程非常痛苦。于是有了兩個(gè)替代方案左擾動(dòng)模型和右擾動(dòng)模型。左擾動(dòng)模型的核心想法是給$R$左乘一個(gè)微小的旋轉(zhuǎn)擾動(dòng)$\Delta R \exp(\delta\phi^{\wedge})$然后按導(dǎo)數(shù)定義展開$$\frac{\partial (Rp)}{\partial \phi} \lim_{\delta\phi\rightarrow 0} \frac{\exp(\delta\phi^{\wedge})\exp(\phi^{\wedge})p - \exp(\phi^{\wedge})p}{\delta\phi} \approx -(Rp)^{\wedge}$$右擾動(dòng)模型則是右乘一個(gè)微小擾動(dòng)結(jié)果類似于$-p^{\wedge}R^T$形式上差了一個(gè)旋轉(zhuǎn)的共軛變換。兩者都是有效的只是最終展開的雅可比矩陣形式不同。我個(gè)人的經(jīng)驗(yàn)是在視覺SLAM殘差模型中優(yōu)先使用左擾動(dòng)模型因?yàn)樗慕Y(jié)論形式更簡(jiǎn)潔視覺幾何中通常已經(jīng)約定以左乘旋轉(zhuǎn)為主這樣維護(hù)代碼不容易混亂。3.3 擾動(dòng)模型與導(dǎo)數(shù)模型的內(nèi)在區(qū)別很多同學(xué)區(qū)分不開擾動(dòng)模型和導(dǎo)數(shù)模型這是兩個(gè)完全不同的事。導(dǎo)數(shù)模型是對(duì)李代數(shù)直接求導(dǎo)相當(dāng)于把旋轉(zhuǎn)矩陣看成李代數(shù)的一個(gè)函數(shù)然后對(duì)這個(gè)函數(shù)取微分結(jié)果形式復(fù)雜必須借助左雅可比修正。擾動(dòng)模型則是把李群上的乘法擾動(dòng)當(dāng)作變量通過極限過程求導(dǎo)數(shù)結(jié)果形式簡(jiǎn)單干凈。如果類比理解的話導(dǎo)數(shù)模型相當(dāng)于你坐在旋轉(zhuǎn)的車上觀察整個(gè)世界的變化擾動(dòng)模型相當(dāng)于你在車外輕輕推一下車再觀察變化。后者在實(shí)際工程中更常用因?yàn)樗恍枰?jì)算復(fù)雜的$J_l^{-1}$項(xiàng)每一步迭代的開銷更小。SLAM這種對(duì)實(shí)時(shí)性要求極高的場(chǎng)景省掉一次3×3矩陣求逆都是值得的。4. 課后典型習(xí)題思路精講4.1 旋轉(zhuǎn)矩陣與反對(duì)稱矩陣的性質(zhì)驗(yàn)證題這類題目通常要求證明$(R\phi)^{\wedge} R\phi^{\wedge}R^T$或者驗(yàn)證$\mathbf{a}^{\wedge}\mathbf -\mathbf^{\wedge}\mathbf{a}$。核心思路只有一個(gè)把$\phi^{\wedge}$還原到叉乘的定義。叉乘滿足$\mathbf{a} \times \mathbf -\mathbf \times \mathbf{a}$利用這個(gè)反交換性質(zhì)很多看似復(fù)雜的矩陣恒等式能快速化簡(jiǎn)。舉個(gè)例子要證明$u^{\wedge}v -v^{\wedge}u$你只需要明白左端等于$u \times v$右端等于$-v \times u$而$v \times u -u \times v$所以兩邊恰好相等。這類題目主要是幫助你建立反對(duì)稱矩陣就是叉乘的矩陣表示這個(gè)直覺在后續(xù)推導(dǎo)李代數(shù)雅可比時(shí)這個(gè)直覺會(huì)反復(fù)用到。4.2 指數(shù)映射的級(jí)數(shù)展開計(jì)算題有一類高頻習(xí)題是給定具體的$\phi$向量要求計(jì)算$e^{\phi^{\wedge}}$。常規(guī)做法是打開羅德里格斯公式硬算但不推薦因?yàn)橛?jì)算量大且容易出錯(cuò)。更好的做法是分三步走先求模長(zhǎng)$\theta$再求單位旋轉(zhuǎn)軸$\mathbf{a}$最后代入羅德里格斯公式。這里有一個(gè)容易被忽略的小細(xì)節(jié)當(dāng)$\theta$非常接近0時(shí)羅德里格斯公式中的$\sin\theta/\theta$和$(1-\cos\theta)/\theta^2$會(huì)面臨數(shù)值不穩(wěn)定的問題。工程上的處理方式是當(dāng)$\theta$小于某個(gè)閾值時(shí)直接取近似$R \approx I \phi^{\wedge}$這個(gè)近似在微小旋轉(zhuǎn)下精度完全夠用。4.3 加速度計(jì)或角速度積分相關(guān)的推導(dǎo)題第三講的部分習(xí)題會(huì)引出旋轉(zhuǎn)的時(shí)間導(dǎo)數(shù)關(guān)系$\dot{R} \phi^{\wedge} R$。這其實(shí)是旋轉(zhuǎn)矩陣的運(yùn)動(dòng)學(xué)方程。解題時(shí)從$R R^T I$出發(fā)兩邊對(duì)時(shí)間求導(dǎo)得到$\dot{R}R^T R\dot{R}^T 0$這說明$\dot{R}R^T$是反對(duì)稱的可以寫成某個(gè)向量$\omega$的反對(duì)稱矩陣形式于是得到$\dot{R} \omega^{\wedge}R$。這個(gè)$\omega$就是角速度的坐標(biāo)表示。這個(gè)結(jié)論在SLAM的IMU預(yù)積分中至關(guān)重要。IMU測(cè)得的角速度正是這里的$\omega$通過對(duì)$\omega^{\wedge}$做指數(shù)積分就能從起始旋轉(zhuǎn)外推出后續(xù)時(shí)刻的旋轉(zhuǎn)。搞懂了這道題的推導(dǎo)過程再看IMU預(yù)積分公式會(huì)輕松一大截不然那些帶時(shí)間積分的旋轉(zhuǎn)公式就像是天書。5. Sophus庫(kù)實(shí)操與常見坑記錄5.1 Sophus的安裝與基本用法李群李代數(shù)的手推是一回事工程落地是另一回事。視覺SLAM中最常用的李代數(shù)庫(kù)是Sophus它提供了$SO(3)$、$SE(3)$等類型的完整實(shí)現(xiàn)同時(shí)支持?jǐn)?shù)值導(dǎo)數(shù)驗(yàn)證學(xué)習(xí)階段極為好用。安裝方式通常是通過源碼構(gòu)建依賴fmt庫(kù)。在較新的系統(tǒng)中可以用CMake直接集成也可以在項(xiàng)目CMakeLists中通過FetchContent方式拉取。具體做法不再贅述但有一條建議一定要統(tǒng)一fmt版本我遇到過幾次編譯報(bào)錯(cuò)最后都是因?yàn)閒mt新舊版本接口不兼容導(dǎo)致的。5.2 代碼中的典型操作示例Sophus中常用操作可以整理成以下示例#include sophus/se3.hpp #include sophus/so3.hpp // 由旋轉(zhuǎn)向量構(gòu)造SO3 Sophus::SO3d R Sophus::SO3d::exp(phi); // 由旋轉(zhuǎn)矩陣構(gòu)造SO3 Sophus::SO3d R2(matrix_3d); // 從SO3提取旋轉(zhuǎn)矩陣 Eigen::Matrix3d rotation_matrix R.matrix(); // 對(duì)數(shù)映射從SO3提取李代數(shù) Eigen::Vector3d phi_again R.log(); // 左擾動(dòng)更新 Sophus::SO3d updated_R Sophus::SO3d::exp(delta_phi) * R; // SE3的構(gòu)造與分解 Sophus::SE3d T(Eigen::Quaterniond(qw, qx, qy, qz), translation_vector); Eigen::Vector3d t T.translation(); Sophus::SO3d R_part T.so3();有一個(gè)細(xì)節(jié)值得注意Sophus中T.translation()返回的是平移向量$t$也就是$J\rho$之后的結(jié)果而不是$\mathfrak{se}(3)$里的$\rho$。搞混這兩個(gè)量會(huì)導(dǎo)致調(diào)試時(shí)出現(xiàn)明明位姿更新的量看起來沒問題但軌跡就是不對(duì)的詭異現(xiàn)象。5.3 三個(gè)最常踩的坑第一個(gè)坑是更新順序。剛接觸SLAM時(shí)容易寫成R R * exp(dphi)但優(yōu)化器要求的是世界系到相機(jī)系的位姿擾動(dòng)應(yīng)該作用在合適的側(cè)。視覺幾何中重投影誤差對(duì)相機(jī)位姿的雅可比是按左擾動(dòng)推導(dǎo)的所以更新時(shí)必須用左乘方式否則整個(gè)梯度方向就反了迭代過程當(dāng)場(chǎng)發(fā)散。第二個(gè)坑是旋轉(zhuǎn)矩陣的數(shù)值歸一化。有些外部庫(kù)返回的旋轉(zhuǎn)矩陣由于數(shù)值誤差可能不再是嚴(yán)格正交的直接喂給Sophus的構(gòu)造函數(shù)可能導(dǎo)致斷言失敗。我在調(diào)試中遇到過好幾次奇怪的內(nèi)存錯(cuò)誤最后定位到是SO3構(gòu)造時(shí)檢查行列式不為1而崩潰。正確的做法是先用Eigen::Quaterniond做一次歸一化再傳給Sophus。第三個(gè)坑是矩陣與向量的數(shù)據(jù)類型不匹配。Sophus與Eigen結(jié)合時(shí)對(duì)浮點(diǎn)類型敏感單精度類型的SE3f和雙精度的Eigen::Matrix3d混用會(huì)導(dǎo)致編譯錯(cuò)誤。建議整個(gè)工程統(tǒng)一使用double類型除非是在嵌入式環(huán)境中有明確的內(nèi)存限制否則單精度在做非線性優(yōu)化時(shí)精度真的不夠用。6. 學(xué)習(xí)路徑上的建議與避坑感悟6.1 李群李代數(shù)要不要手推公式很多同學(xué)糾結(jié)要不要把每一個(gè)公式都自己推導(dǎo)一遍。我的看法是必考的核心公式要會(huì)推應(yīng)用型的公式會(huì)用就行。羅德里格斯公式、指數(shù)映射、擾動(dòng)模型這些骨架必須親自動(dòng)手因?yàn)樗鼈儧Q定了你對(duì)后續(xù)內(nèi)容的理解。而BCH公式的完整展開、$J$矩陣的逆推導(dǎo)這類細(xì)枝末節(jié)只需要知道存在性和物理含義用的時(shí)候查庫(kù)即可。更合理的學(xué)習(xí)節(jié)奏是先通讀一遍書把概念框架建立起來然后動(dòng)手做第3講的習(xí)題鞏固推導(dǎo)最后上手Sophus寫一段包含球面插值或位姿變換的測(cè)試代碼。整個(gè)過程控制在3到5天比較合適拖太久容易遺忘前面的數(shù)學(xué)基礎(chǔ)。6.2 學(xué)習(xí)中的常見誤區(qū)第一個(gè)誤區(qū)是死摳李代數(shù)完整定義和泛函分析背景。李群李代數(shù)在數(shù)學(xué)上是一個(gè)非常深刻的分支但SLAM領(lǐng)域用到的只是其中很小一部分。在兩個(gè)小擾動(dòng)近似的層面理解它遠(yuǎn)比研究群論本身更有利于你寫出能跑的SLAM系統(tǒng)。不用因?yàn)閿?shù)學(xué)背景不夠而產(chǎn)生焦慮。第二個(gè)誤區(qū)是跳過習(xí)題直接看代碼。視覺SLAM十四講最值錢的部分恰恰是那些課后推導(dǎo)題它們把概念和工程問題擰在了一起。直接看代碼容易陷入代碼能跑但是換一個(gè)場(chǎng)景就不會(huì)改的尷尬局面。我自己做了三遍習(xí)題每做一遍對(duì)李代數(shù)的理解都上一個(gè)臺(tái)階。第三個(gè)誤區(qū)是從頭手寫李代數(shù)實(shí)現(xiàn)。學(xué)習(xí)階段可以使用Sophus完全沒有必要自己實(shí)現(xiàn)一個(gè)李代數(shù)庫(kù)。自己寫不等于理解了原理反而容易被各種邊界條件和數(shù)值陷阱消耗大量時(shí)間。先會(huì)用再理解最后才是自己造輪子。6.3 這一講與后續(xù)章節(jié)的聯(lián)系接下來進(jìn)入第四講的相機(jī)模型與非線性優(yōu)化時(shí)你會(huì)發(fā)現(xiàn)李代數(shù)無處不在。對(duì)空間點(diǎn)$P$投影到像素坐標(biāo)系的過程求導(dǎo)本質(zhì)上是同時(shí)對(duì)旋轉(zhuǎn)量$\phi$和平移量$\rho$做擾動(dòng)這就直接用到左擾動(dòng)模型。第五講的多視圖幾何和第七講之后的BA優(yōu)化也全部建立在位姿的李代數(shù)表示上。所以這一講不是孤立的數(shù)學(xué)游戲而是整個(gè)SLAM系統(tǒng)優(yōu)化框架的地基。如果你現(xiàn)在感覺概念還有點(diǎn)飄非常正常后續(xù)章節(jié)會(huì)反復(fù)使用這些概念用得多自然就熟了。遇到不懂的地方隨時(shí)回來翻這一講每次都會(huì)有新的收獲。最后分享一個(gè)我在實(shí)際學(xué)習(xí)中的方法每學(xué)完一小節(jié)就在紙上用一句話總結(jié)這一小節(jié)到底在干什么比如指數(shù)映射是把旋轉(zhuǎn)向量變成旋轉(zhuǎn)矩陣擾動(dòng)模型是換了一種求導(dǎo)姿勢(shì)。這種概括能力會(huì)在你組合各個(gè)模塊時(shí)爆發(fā)出巨大的作用。李群李代數(shù)這一關(guān)一旦跨過去后面的路就順暢多了。