李群與李代數2：李代數求導和李群擾動模型

1. 整體誤差最小化引出求導問題
2. BCH公式與近似形式
- 2.1 BCH公式
- 2.2 BCH線性近似
- 2.3 BCH近似的意義
3. 微分模型——李代數求導
4. 擾動模型（左乘）
- 4.1 SO(3)上的擾動模型
- 4.2 SE(3)上的擾動模型
- 4.3 伴随性質
5. 相似變換群相關
- 5.1 相似變換群 S i m ( 3 ) Sim(3) Sim(3)
- 5.2 李代數 s i m ( 3 ) \mathfrak{sim}(3) sim(3)及指數/對數映射
- - 5.2.1 李代數 s i m ( 3 ) \mathfrak{sim}(3) sim(3)
  - 5.2.2 李代數到李群的指數映射
  - 5.2.3 李群到李代數的對數映射
- 5.3 擾動模型
- 5.4 總結
6. 編碼實戰
- 6.1 李群與李代數的運算庫：Sophus
- 6.2 評估軌迹的誤差
後記：
參考文獻：

前言：本篇系列文章參照高翔老師《視覺SLAM十四講從理論到實踐》的第四講，講解李群與李代數。寫此篇的目的是為了補足《十四講》中的數學類基礎知識，原書内容有點像直譯文獻，是以筆者根據自己了解，對整體内容重新劃分，更改了目錄章節，改寫部分公式，同時擴充了部分内容，有書的同學可以參照閱讀，達到事半功倍的效果。

1. 整體誤差最小化引出求導問題

使用李代數的一大動機是進行優化，而在優化過程中導數是非常必要的資訊。下面來讨論一個帶有李代數的函數，以及關于該李代數求導的問題，該問題有很強的實際應用背景。在SLAM中，我們使用 S O ( 3 ) SO(3) SO(3)上的旋轉矩陣或 S E ( 3 ) SE(3) SE(3)上的變換矩陣來描述如何估計一個相機的位置和姿态（位姿）。不妨設某個時刻機器人的位姿為 T \boldsymbol{T} T，它觀察到一個世界坐标位于 p p p的點，産生了一個觀測資料 z z z。那麼，由坐标變換關系知： z = T p + w . (1.1) z=\boldsymbol{T}p+w.\tag{1.1} z=Tp+w.(1.1)其中， w w w為随機噪聲。由于它的存在， z z z往往不可能精确地滿足 z = T p z=Tp z=Tp的關系。是以，我們通常會計算理想的觀測與實際資料的誤差： e = z − T p . (1.2) e=z-\boldsymbol{T}p.\tag{1.2} e=z−Tp.(1.2)假設一共有 N N N個這樣的路标點和觀測，于是就有 N N N個上式。那麼，對機器人進行位姿估計，相當于找到一個最優的 T \boldsymbol{T} T，使得整體誤差最小化： min ⁡ T J ( T ) = ∑ i = 1 N ∥ z i − T p i ∥ 2 . (1.3) \min_{\boldsymbol{T}}J(\boldsymbol{T})=\sum_{i=1}^{N}\left \| z_{i}-\boldsymbol{T}p_{i} \right \|^{2}.\tag{1.3} TminJ(T)=i=1∑N∥zi−Tpi∥2.(1.3)求解此問題，需要計算目标函數 J J J關于變換矩陣 T \boldsymbol{T} T的導數。我們把具體的算法留到後面講（精力有限，本系列部落格不再涉及，感興趣的同學去讀原書吧）。這裡的重點是，我們經常會建構與位姿有關的函數，然後讨論該函數關于位姿的導數，以調整目前的估計值。然而， S O ( 3 ) , S E ( 3 ) SO(3),SE(3) SO(3),SE(3)上并沒有良好定義的加法，它們隻是群，如果我們把 T \boldsymbol{T} T當成一個普通矩陣來處理優化，就必須對它加以限制，就會比較繁瑣。

而從李代數角度來說，由于李代數由向量組成，具有良好的加法運算，是以，可以使用李代數解決求導問題，求導思路分為兩種：

用李代數表示姿态，然後根據李代數加法對李代數求導。
對李群左乘或右乘微小擾動，然後對該擾動對應的李代數求導，稱為李群左擾動或右擾動模型。

第一種方式對應到李代數的求導，而第二種方式則對應到擾動模型。在讨論這兩種思路的異同之前，我們先看一下BCH線性近似。

2. BCH公式與近似形式

2.1 BCH公式

下面來考慮一個問題。雖然我們已經清楚了 S O ( 3 ) SO(3) SO(3)和 S E ( 3 ) SE(3) SE(3)上的李群和李代數關系，但是，擋在 S O ( 3 ) SO(3) SO(3)中完成兩個矩陣乘法時，李代數中 s e ( 3 ) \mathfrak{se}(3) se(3)上發生了什麼改變呢？反過來說，當 s o ( 3 ) \mathfrak{so}(3) so(3)做兩個李代數的加法時， S O ( 3 ) SO(3) SO(3)上是否對應着兩個矩陣的乘積？如果成立，相當于： exp ⁡ ( ϕ 1 ∧ ) exp ⁡ ( ϕ 2 ∧ ) = exp ⁡ ( ( ϕ 1 + ϕ 2 ) ∧ ) \exp(\phi^{\wedge}_{1})\exp(\phi^{\wedge}_{2})=\exp((\phi_{1}+\phi_{2})^{\wedge}) exp(ϕ1∧)exp(ϕ2∧)=exp((ϕ1+ϕ2)∧)如果 ϕ 1 , ϕ 2 \phi_{1},\phi_{2} ϕ1,ϕ2為标量，那麼顯然該式成立；但此處我們計算的是矩陣的指數函數，而非标量的指數。換言之，我們在研究下式是否成立： ln ⁡ ( exp ⁡ ( A ) exp ⁡ ( B ) ) = A + B ? \ln(\exp(A)\exp(B))=A+B? ln(exp(A)exp(B))=A+B?很遺憾，該式在矩陣時并不成立。兩個李代數指數映射乘積的完整形式，由Baker-Cambell-Hausdorff公式（BCH公式）1給出。由于其完整形式較複雜，我們隻給出其展開式的前幾項： ln ⁡ ( exp ⁡ ( A ) exp ⁡ ( B ) ) = A + B + 1 2 [ A , B ] + 1 12 [ A , [ A , B ] ] − 1 12 [ B , [ A , B ] ] − 1 24 [ B , [ A , [ A , B ] ] ] + ⋯ = A + B + 1 2 ( A B − B A ) + 1 12 ( A 2 B + A B 2 − 2 A B A + B 2 A + B A 2 − 2 B A B ) + 1 24 ( A 2 B 2 − 2 A B A B − B 2 A 2 + 2 B A B A ) . (2.1) \begin{aligned} \ln(\exp(A)\exp(B)) &= A+B+\frac{1}{2}[A,B]+\frac{1}{12}[A,[A,B]]-\frac{1}{12}[B,[A,B]]-\frac{1}{24}[B,[A,[A,B]]]+\cdots \\&= A+B+\frac{1}{2}(AB-BA)+\frac{1}{12}(A^{2}B+AB^{2}-2ABA+B^{2}A+BA^{2}-2BAB) + \\& \quad\quad\frac{1}{24}(A^{2}B^{2}-2ABAB-B^{2}A^{2}+2BABA) \end{aligned}.\tag{2.1} ln(exp(A)exp(B))=A+B+21[A,B]+121[A,[A,B]]−121[B,[A,B]]−241[B,[A,[A,B]]]+⋯=A+B+21(AB−BA)+121(A2B+AB2−2ABA+B2A+BA2−2BAB)+241(A2B2−2ABAB−B2A2+2BABA).(2.1)其中[]為李括号。BCH公式告訴我們，當處理兩個矩陣指數之積時，它們會産生一些由李括号組成的餘項。

2.2 BCH線性近似

特别的，考慮 S O ( 3 ) SO(3) SO(3)上的李代數 ln ⁡ ( exp ⁡ ( ϕ 1 ∧ ) exp ⁡ ( ϕ 2 ∧ ) ) ∨ \ln(\exp(\phi^{\wedge}_{1})\exp(\phi^{\wedge}_{2}))^{\vee} ln(exp(ϕ1∧)exp(ϕ2∧))∨，當 ϕ 1 \phi_{1} ϕ1或 ϕ 2 \phi_{2} ϕ2為小量時，小量二次以上的項可以被忽略，而旋轉或變換更新的基本都是小量，是以BCH具有實際應用意義。此時，BCH擁有線性近似表達2

ln ⁡ ( exp ⁡ ( ϕ 1 ∧ ) exp ⁡ ( ϕ 2 ∧ ) ) ∨ ≈ { J l ( ϕ 2 ) − 1 ϕ 1 + ϕ 2 當 ϕ 1 為小量 , J r ( ϕ 1 ) − 1 ϕ 2 + ϕ 1 當 ϕ 2 為小量 . (2.2) \ln(\exp(\phi^{\wedge}_{1})\exp(\phi^{\wedge}_{2}))^{\vee}\approx \left\{\begin{matrix} \boldsymbol{J}_{l}(\phi_{2})^{-1}\phi_{1}+\phi_{2} \quad\quad 當\phi_{1}為小量,\\ \boldsymbol{J}_{r}(\phi_{1})^{-1}\phi_{2}+\phi_{1} \quad\quad 當\phi_{2}為小量. \end{matrix}\right.\tag{2.2} ln(exp(ϕ1∧)exp(ϕ2∧))∨≈{Jl(ϕ2)−1ϕ1+ϕ2當ϕ1為小量,Jr(ϕ1)−1ϕ2+ϕ1當ϕ2為小量.(2.2)

以第一個近似為例。該式告訴我們，當對一個旋轉矩陣 R 2 R_{2} R2（李代數為 ϕ 2 \phi_{2} ϕ2）左乘一個微小旋轉矩陣 R 1 R_{1} R1（李代數為 ϕ 1 \phi_{1} ϕ1）時，可以近似地看作，在原有的李代數 ϕ 2 \phi_{2} ϕ2上加了一項 J l ( ϕ 2 ) − 1 ϕ 1 \boldsymbol{J}_{l}(\phi_{2})^{-1}\phi_{1} Jl(ϕ2)−1ϕ1。同理，第二個近似描述了右乘一個微小位移的情況。于是，李代數在BCH近似下，分成了左乘近似和右乘近似兩種，在使用時我們須注意使用的是左乘模型還是右乘模型。

本部落格以左乘為例。左乘BCH近似雅克比 J l \boldsymbol{J}_{l} Jl事實上就是《李群與李代數1》中的式 ( 4.6 ) (4.6) (4.6)的内容： J l = J = sin ⁡ θ θ I + ( 1 − sin ⁡ θ θ ) a a T + 1 − cos ⁡ θ θ a ∧ . (2.3) \boldsymbol{J}_{l}=\boldsymbol{J}=\frac{\sin\theta}{\theta}\boldsymbol{I}+(1-\frac{\sin\theta}{\theta})\boldsymbol{a}\boldsymbol{a}^{T}+\frac{1-\cos\theta}{\theta}\boldsymbol{a}^{\wedge}.\tag{2.3} Jl=J=θsinθI+(1−θsinθ)aaT+θ1−cosθa∧.(2.3)它的逆為： J l − 1 = θ 2 cot ⁡ θ 2 I + ( 1 − θ 2 cot ⁡ θ 2 ) a a T + θ 2 a ∧ . (2.4) \boldsymbol{J}_{l}^{-1}=\frac{\theta}{2}\cot\frac{\theta}{2}\boldsymbol{I}+(1-\frac{\theta}{2}\cot\frac{\theta}{2})\boldsymbol{a}\boldsymbol{a}^{T}+\frac{\theta}{2}\boldsymbol{a}^{\wedge}.\tag{2.4} Jl−1=2θcot2θI+(1−2θcot2θ)aaT+2θa∧.(2.4)而右乘雅克比僅需要對自變量取負号即可： J r ( ϕ ) = J l ( − ϕ ) . (2.5) \boldsymbol{J}_{r}(\phi)=\boldsymbol{J}_{l}(-\phi).\tag{2.5} Jr(ϕ)=Jl(−ϕ).(2.5)這樣，我們就可以談論李群乘法與李代數加法的關系了。

2.3 BCH近似的意義

為了友善讀者了解，我們重新叙述BCH近似的意義。假定對某個旋轉 R R R，對應的李代數為 ϕ \phi ϕ，我們給它左乘一個微小旋轉，記作 Δ R \Delta R ΔR，對應的李代數為 Δ ϕ \Delta \phi Δϕ。那麼，在李群上，得到的結果就是 Δ R ⋅ R \Delta R \cdot R ΔR⋅R，而在李代數上，根據BCH近似表達公式 ( 2.2 ) (2.2) (2.2)，為： J l ( ϕ ) − 1 Δ ϕ + ϕ \boldsymbol{J}_{l}(\phi)^{-1}\Delta\phi+\phi Jl(ϕ)−1Δϕ+ϕ。合并起來，可以簡單的寫成： exp ⁡ ( Δ ϕ ∧ ) exp ⁡ ( ϕ ∧ ) = exp ⁡ ( ( J l ( ϕ ) − 1 Δ ϕ ) ∧ + ϕ ) . (2.6) \exp(\Delta\phi^{\wedge})\exp(\phi^{\wedge}) = \exp\left ( (\boldsymbol{J}_{l}(\phi)^{-1}\Delta\phi)^{\wedge}+\phi \right ).\tag{2.6} exp(Δϕ∧)exp(ϕ∧)=exp((Jl(ϕ)−1Δϕ)∧+ϕ).(2.6)同理可得右乘擾動 R ⋅ Δ R R \cdot \Delta R R⋅ΔR對應的表達式為： exp ⁡ ( ϕ ∧ ) exp ⁡ ( Δ ϕ ∧ ) = exp ⁡ ( ( J r ( ϕ ) − 1 Δ ϕ ) ∧ + ϕ ) . (2.7) \exp(\phi^{\wedge})\exp(\Delta\phi^{\wedge}) = \exp\left ( (\boldsymbol{J}_{r}(\phi)^{-1}\Delta\phi)^{\wedge}+\phi \right ).\tag{2.7} exp(ϕ∧)exp(Δϕ∧)=exp((Jr(ϕ)−1Δϕ)∧+ϕ).(2.7)反之，如果我們在李代數上進行加法，讓一個 ϕ \phi ϕ加上 Δ ϕ \Delta \phi Δϕ，那麼可以近似為李群上帶左右雅克比的乘法： exp ⁡ ( ( ϕ + Δ ϕ ) ∧ ) = exp ⁡ ( ( J l ( ϕ ) Δ ϕ ) ∧ ) exp ⁡ ( ϕ ∧ ) = exp ⁡ ( ϕ ∧ ) exp ⁡ ( ( J r ( ϕ ) Δ ϕ ) ∧ ) . (2.8) \exp\left ( (\phi+\Delta\phi)^{\wedge} \right )=\exp\left ( (\boldsymbol{J}_{l}(\phi)\Delta\phi)^{\wedge} \right )\exp(\phi^{\wedge})=\exp(\phi^{\wedge})\exp\left ( (\boldsymbol{J}_{r}(\phi)\Delta\phi)^{\wedge} \right ).\tag{2.8} exp((ϕ+Δϕ)∧)=exp((Jl(ϕ)Δϕ)∧)exp(ϕ∧)=exp(ϕ∧)exp((Jr(ϕ)Δϕ)∧).(2.8)這就為之後李代數上做微分提供了理論基礎。

同樣地，對于 S E ( 3 ) SE(3) SE(3)，也有類似的BCH近似： exp ⁡ ( Δ ξ ∧ ) exp ⁡ ( ξ ∧ ) ≈ exp ⁡ ( ( ȷ l − 1 Δ ξ + ξ ) ∧ ) . (2.9) \exp(\Delta\xi^{\wedge})\exp(\xi^{\wedge})\approx \exp\left ( (\mathbf{\jmath} ^{-1}_{l}\Delta\xi+\xi )^{\wedge}\right ).\tag{2.9} exp(Δξ∧)exp(ξ∧)≈exp((ȷl−1Δξ+ξ)∧).(2.9) exp ⁡ ( ξ ∧ ) exp ⁡ ( Δ ξ ∧ ) ≈ exp ⁡ ( ( ȷ r − 1 Δ ξ + ξ ) ∧ ) . (2.10) \exp(\xi^{\wedge})\exp(\Delta\xi^{\wedge})\approx \exp\left ( (\mathbf{\jmath} ^{-1}_{r}\Delta\xi+\xi )^{\wedge}\right ).\tag{2.10} exp(ξ∧)exp(Δξ∧)≈exp((ȷr−1Δξ+ξ)∧).(2.10)這裡 ȷ \jmath ȷ形勢比較複雜，它是一個 6 × 6 6\times6 6×6的矩陣，讀者可以參考文獻[2]中式 ( 7.82 ) (7.82) (7.82)和 ( 7.83 ) (7.83) (7.83)的内容。

反之，如果我們在李代數上進行加法，讓一個 ξ \xi ξ加上 Δ ξ \Delta \xi Δξ，那麼可以近似為李群上帶左右雅克比的乘法： exp ⁡ ( ( ξ + Δ ξ ) ∧ ) ≈ exp ⁡ ( ( ȷ l ( ξ ) Δ ξ ) ∧ ) exp ⁡ ( ξ ∧ ) = exp ⁡ ( ξ ∧ ) exp ⁡ ( ( ȷ r ( ξ ) Δ ξ ) ∧ ) . (2.11) \exp\left ( (\xi+\Delta\xi)^{\wedge} \right )\approx\exp\left ( (\boldsymbol{\jmath}_{l}(\xi)\Delta\xi)^{\wedge} \right )\exp(\xi^{\wedge})=\exp(\xi^{\wedge})\exp\left ( (\boldsymbol{\jmath}_{r}(\xi)\Delta\xi)^{\wedge} \right ).\tag{2.11} exp((ξ+Δξ)∧)≈exp((ȷl(ξ)Δξ)∧)exp(ξ∧)=exp(ξ∧)exp((ȷr(ξ)Δξ)∧).(2.11)

下面我們正式開始讨論李代數的求導和李群擾動對應李代數的擾動模型求導兩種思路的異同。

3. 微分模型——李代數求導

首先，考慮SO(3)上的情況。假設我們對一個空間點 p p p進行了旋轉，得到 R p Rp Rp。現在，計算旋轉之後點的坐标相對于旋轉的導數，我們非正式地記為： ∂ ( R p ) ∂ R . (3.1) \frac{\partial (Rp)}{\partial R}.\tag{3.1} ∂R∂(Rp).(3.1)由于 S O ( 3 ) SO(3) SO(3)沒有加法，是以該導數無法按照導數的定義進行計算。設 R R R對應的李代數為 ϕ \phi ϕ，我們轉而計算3

∂ ( exp ⁡ ( ϕ ∧ ) p ) ∂ ϕ \frac{\partial (\exp(\phi^{\wedge })p)}{\partial \phi} ∂ϕ∂(exp(ϕ∧)p)按照導數的定義有： ∂ ( exp ⁡ ( ϕ ∧ ) p ) ∂ ϕ = lim ⁡ δ ϕ → 0 exp ⁡ ( ( ϕ + δ ϕ ) ∧ ) p − exp ⁡ ( ϕ ∧ ) p δ ϕ = lim ⁡ δ ϕ → 0 exp ⁡ ( ( J l δ ϕ ) ∧ ) exp ⁡ ( ϕ ∧ ) p − exp ⁡ ( ϕ ∧ ) p δ ϕ = lim ⁡ δ ϕ → 0 ( I + ( J l δ ϕ ) ∧ ) exp ⁡ ( ϕ ∧ ) p − exp ⁡ ( ϕ ∧ ) p δ ϕ = lim ⁡ δ ϕ → 0 ( J l δ ϕ ) ∧ ( exp ⁡ ( ϕ ∧ ) p ) δ ϕ = lim ⁡ δ ϕ → 0 − ( exp ⁡ ( ϕ ∧ ) p ) ∧ J l δ ϕ δ ϕ = − ( R p ) ∧ J l . \begin{aligned} \frac{\partial (\exp(\phi^{\wedge })p)}{\partial \phi} &= \lim_{\delta \phi\rightarrow 0}\frac{\exp((\phi+\delta\phi)^{\wedge})p-\exp(\phi^{\wedge})p}{\delta\phi} \\&= \lim_{\delta \phi\rightarrow 0}\frac{\exp((\boldsymbol{J_{l}}\delta\phi)^{\wedge})\exp(\phi^{\wedge})p-\exp(\phi^{\wedge})p}{\delta\phi} \\&= \lim_{\delta \phi\rightarrow 0}\frac{(\boldsymbol{I}+(\boldsymbol{J_{l}}\delta\phi)^{\wedge})\exp(\phi^{\wedge})p-\exp(\phi^{\wedge})p}{\delta\phi} \\&= \lim_{\delta \phi\rightarrow 0}\frac{(\boldsymbol{J_{l}}\delta\phi)^{\wedge}(\exp(\phi^{\wedge})p)}{\delta\phi} \\&= \lim_{\delta \phi\rightarrow 0}\frac{-(\exp(\phi^{\wedge})p)^{\wedge}\boldsymbol{J_{l}}\delta\phi}{\delta\phi} \\&= -(Rp)^{\wedge}\boldsymbol{J_{l}}. \end{aligned} ∂ϕ∂(exp(ϕ∧)p)=δϕ→0limδϕexp((ϕ+δϕ)∧)p−exp(ϕ∧)p=δϕ→0limδϕexp((Jlδϕ)∧)exp(ϕ∧)p−exp(ϕ∧)p=δϕ→0limδϕ(I+(Jlδϕ)∧)exp(ϕ∧)p−exp(ϕ∧)p=δϕ→0limδϕ(Jlδϕ)∧(exp(ϕ∧)p)=δϕ→0limδϕ−(exp(ϕ∧)p)∧Jlδϕ=−(Rp)∧Jl.推導說明：第二行的變換為BCH線性近似，第三行為泰勒展開舍去高階項後的近似（由于取了極限，可以寫等号），第四行至第五行将反對稱符号看做叉積，交換之後變号，即 a ∧ b = − b ∧ a a^{\wedge}b=-b^{\wedge}a a∧b=−b∧a。于是，我們推導出了旋轉後的點相對于李代數的導數： ∂ R p ∂ ϕ = ( − R p ) ∧ J l . (3.2) \frac{\partial Rp}{\partial \phi}=(-Rp)^{\wedge}\boldsymbol{J_{l}}.\tag{3.2} ∂ϕ∂Rp=(−Rp)∧Jl.(3.2)不過，由于這裡仍然含有形勢比較複雜的 J l \boldsymbol{J_{l}} Jl，我們不希望計算它。而下面要講的擾動模型則提供了更簡單的導數計算公式。

4. 擾動模型（左乘）

4.1 SO(3)上的擾動模型

另一種求導方式是對R進行一次擾動 Δ R \Delta R ΔR，然後求結果相對于擾動的變化率。這個擾動可以乘在左邊也可以乘在右邊，最後結果會有一點兒微小的差異，我們以左擾動為例。設 Δ R \Delta R ΔR對應的李代數為 φ \varphi φ，對 φ \varphi φ求導得： ∂ Δ R p ∂ φ = lim ⁡ φ → 0 exp ⁡ ( φ ∧ ) exp ⁡ ( ϕ ∧ ) p − exp ⁡ ( ϕ ∧ ) p φ = lim ⁡ φ → 0 ( I + φ ∧ ) exp ⁡ ( ϕ ∧ ) p − exp ⁡ ( ϕ ∧ ) p φ = lim ⁡ φ → 0 φ ∧ exp ⁡ ( ϕ ∧ ) p φ = lim ⁡ φ → 0 − ( R p ) ∧ φ φ = − ( R p ) ∧ . (3.3) \begin{aligned} \frac{\partial \Delta Rp}{\partial \varphi} &= \lim_{\varphi\rightarrow 0}\frac{\exp(\varphi^{\wedge})\exp(\phi^{\wedge})p-\exp(\phi^{\wedge})p}{\varphi} \\&= \lim_{\varphi\rightarrow 0}\frac{(\boldsymbol{I}+\varphi^{\wedge})\exp(\phi^{\wedge})p-\exp(\phi^{\wedge})p}{\varphi} \\&= \lim_{\varphi \rightarrow 0}\frac{\varphi^{\wedge}\exp(\phi^{\wedge})p}{\varphi } \\&= \lim_{\varphi \rightarrow 0}\frac{-(Rp)^{\wedge}\varphi }{\varphi } = -(Rp)^{\wedge}. \end{aligned}\tag{3.3} ∂φ∂ΔRp=φ→0limφexp(φ∧)exp(ϕ∧)p−exp(ϕ∧)p=φ→0limφ(I+φ∧)exp(ϕ∧)p−exp(ϕ∧)p=φ→0limφφ∧exp(ϕ∧)p=φ→0limφ−(Rp)∧φ=−(Rp)∧.(3.3)可見，相比于直接對李代數求導，省去了一個雅克比 J l \boldsymbol{J_{l}} Jl的計算。這使得擾動模型更為實用。請讀者務必了解這裡的求導運算，這在位姿估計中具有重要的意義。

4.2 SE(3)上的擾動模型

下邊，我們給出 S E ( 3 ) SE(3) SE(3)上的擾動模型。假設某空間點p經過一次變換 T \boldsymbol{T} T（對應李代數為 ξ \xi ξ），得到 T p \boldsymbol{T}p Tp4。現在給 T \boldsymbol{T} T左乘一個擾動 Δ T = exp ⁡ ( χ ∧ ) \Delta T=\exp(\chi^{\wedge}) ΔT=exp(χ∧)，我們設擾動項的李代數為 χ = [ Δ ρ , Δ ϕ ] T \chi=[\Delta \rho,\Delta\phi]^{T} χ=[Δρ,Δϕ]T，那麼：

∂ T p ∂ χ = lim ⁡ χ → 0 exp ⁡ ( χ ∧ ) exp ⁡ ( ξ ∧ ) p − exp ⁡ ( ξ ∧ ) p χ = lim ⁡ χ → 0 ( I + χ ∧ ) exp ⁡ ( ξ ∧ ) p − exp ⁡ ( ξ ∧ ) p χ = lim ⁡ χ → 0 χ ∧ exp ⁡ ( ξ ∧ ) p χ = lim ⁡ χ → 0 [ Δ ϕ ∧ Δ ρ 0 T 0 ] [ R p + t 1 ] χ = lim ⁡ χ → 0 [ Δ ϕ ∧ ( R p + t ) + Δ p 0 T ] [ Δ ρ , Δ ϕ ] T = [ I − ( R p + t ) ∧ 0 T 0 T ] = d e f ( T p ) ⊙ . (3.4) \begin{aligned} \frac{\partial \boldsymbol{T}p}{\partial \chi} &= \lim_{\chi\rightarrow 0} \frac{\exp(\chi^{\wedge})\exp(\xi^{\wedge})p-\exp(\xi^{\wedge})p}{\chi} \\&= \lim_{\chi\rightarrow 0} \frac{(\boldsymbol{I+\chi^{\wedge}})\exp(\xi^{\wedge})p-\exp(\xi^{\wedge})p}{\chi} \\&= \lim_{\chi\rightarrow 0} \frac{\chi^{\wedge}\exp(\xi^{\wedge})p}{\chi}\\&= \lim_{\chi\rightarrow 0} \frac{\begin{bmatrix} \Delta\phi^{\wedge} & \Delta \rho\\ \boldsymbol{0}^{T} & 0 \end{bmatrix}\begin{bmatrix} Rp+t\\ 1 \end{bmatrix}}{\chi} \\&= \lim_{\chi\rightarrow 0} \frac{\begin{bmatrix} \Delta\phi^{\wedge}(Rp+t)+\Delta p\\ \boldsymbol{0}^{T} \end{bmatrix}}{[\Delta \rho,\Delta\phi]^{T}} \\&= \begin{bmatrix} \boldsymbol{I} & -(Rp+t)^{\wedge}\\ \boldsymbol{0}^{T} & 0^{T} \end{bmatrix} \overset{\underset{\mathrm{def}}{}}{=}(Tp)^{\odot }. \end{aligned}\tag{3.4} ∂χ∂Tp=χ→0limχexp(χ∧)exp(ξ∧)p−exp(ξ∧)p=χ→0limχ(I+χ∧)exp(ξ∧)p−exp(ξ∧)p=χ→0limχχ∧exp(ξ∧)p=χ→0limχ[Δϕ∧0TΔρ0][Rp+t1]=χ→0lim[Δρ,Δϕ]T[Δϕ∧(Rp+t)+Δp0T]=[I0T−(Rp+t)∧0T]=def(Tp)⊙.(3.4)我們把最後的結果定義成一個算符 ⊙ \odot ⊙5，它把一個齊次坐标的空間點變換成一個 4 × 6 4\times 6 4×6的矩陣。

上式推導中，需要稍微解釋的是第五行的矩陣求導順序，假設 a , b , x , y \boldsymbol{a,b,x,y} a,b,x,y都是列向量，那麼在我們的符号寫法下，有如下規則： d [ a b ] d [ x y ] = ( d [ a , b ] T d [ x y ] ) T = [ d a d x d b d x d a d y d b d y ] T = [ d a d x d a d y d b d x d b d y . ] (3.5) \frac{d\begin{bmatrix} a\\ b \end{bmatrix}}{d\begin{bmatrix} x\\ y \end{bmatrix}}=\left ( \frac{d[a,b]^{T}}{d\begin{bmatrix} x\\ y \end{bmatrix}} \right )^{T}=\begin{bmatrix} \frac{da}{dx} & \frac{db}{dx}\\ \frac{da}{dy} & \frac{db}{dy} \end{bmatrix}^{T}=\begin{bmatrix} \frac{da}{dx} & \frac{da}{dy}\\ \frac{db}{dx} & \frac{db}{dy}. \end{bmatrix}\tag{3.5} d[xy]d[ab]=⎝⎜⎜⎛d[xy]d[a,b]T⎠⎟⎟⎞T=[dxdadydadxdbdydb]T=[dxdadxdbdydadydb.](3.5)

至此，我們已經介紹了李群與李代數上的微分運算。下面再補充一點其它性質。

4.3 伴随性質

對于公式： R p ∧ R T = ( R p ) ∧ . (3.6) Rp^{\wedge}R^{T}=(Rp)^{\wedge}.\tag{3.6} Rp∧RT=(Rp)∧.(3.6)有類似的公式： R exp ⁡ ( p ∧ ) R T = exp ⁡ ( ( R p ) ∧ ) . (3.7) R\exp(p^{\wedge})R^{T}=\exp((Rp)^{\wedge}).\tag{3.7} Rexp(p∧)RT=exp((Rp)∧).(3.7)該式稱為 S O ( 3 ) SO(3) SO(3)上的伴随性質。同樣地，在 S E ( 3 ) SE(3) SE(3)上也有伴随性質： T exp ⁡ ( ξ ∧ ) T − 1 = exp ⁡ ( ( A d ( T ) ξ ) ∧ ) , (3.8) T\exp(\xi^{\wedge})T^{-1}=\exp((Ad(T)\xi)^{\wedge}),\tag{3.8} Texp(ξ∧)T−1=exp((Ad(T)ξ)∧),(3.8)其中： A d ( T ) = [ R t ∧ R 0 R ] . Ad(T)=\begin{bmatrix} R & t^{\wedge}R \\ 0 & R \end{bmatrix}. Ad(T)=[R0t∧RR].本性質是書中習題，感興趣的同學可以研究下。

5. 相似變換群相關

最後，我們要提一下在單目視覺中使用的相似變換群 S i m ( 3 ) Sim(3) Sim(3)，它對應的李代數，以及他們之間的指數/對數映射關系，最後看一下它對應的擾動模型。如果你隻對雙目SLAM或RGB-D SLAM感興趣，可以跳過本節。因仿射變換和射影變換不再具備指數/對數映射性質和擾動模型，是以不再考慮它們。

5.1 相似變換群 S i m ( 3 ) Sim(3) Sim(3)

我們已經介紹過單目的尺度不确定性。如果在單目SLAM中使用 S E ( 3 ) SE(3) SE(3)表示位姿，那麼由于尺度不确定性與尺度漂移，整個SLAM過程中的尺度會發生變化，這在 S E ( 3 ) SE(3) SE(3)中未能展現出來。是以，在單目情況下，我們一般會顯示地把尺度因子表達出來。用數學語言來說，對于位于空間的點 p p p，在相機坐标系下要經過一個相似變換，而非歐氏變換： p ′ = [ s R t 0 T 1 ] p = s R p + t . (5.1) p'=\begin{bmatrix} s\boldsymbol{R} & \boldsymbol{t}\\ \boldsymbol{0}^{T} & 1 \end{bmatrix}p=s\boldsymbol{R}\boldsymbol{p}+\boldsymbol{t}.\tag{5.1} p′=[sR0Tt1]p=sRp+t.(5.1)在相似變換中，我們把尺度 s s s表達出來了，它同時作用在 p p p的3個坐标之上，對 p p p進行了一次縮放。

與 S O ( 3 ) 、 S E ( 3 ) SO(3)、SE(3) SO(3)、SE(3)類似，相似變換也對矩陣乘法構成群，稱為相似變換群 S i m ( 3 ) Sim(3) Sim(3)： S i m ( 3 ) = { S = [ s R t 0 T 1 ] ∈ R 4 × 4 } . (5.2) Sim(3)=\left \{ \boldsymbol{S}=\begin{bmatrix} s\boldsymbol{R} & \boldsymbol{t}\\ \boldsymbol{0}^{T} & 1 \end{bmatrix} \in \mathbb{R}^{4\times 4}\right \}.\tag{5.2} Sim(3)={S=[sR0Tt1]∈R4×4}.(5.2)同樣地， S i m ( 3 ) Sim(3) Sim(3)也有對應的李代數、指數映射、對數映射、擾動模型等，下邊分别介紹。

5.2 李代數 s i m ( 3 ) \mathfrak{sim}(3) sim(3)及指數/對數映射

5.2.1 李代數 s i m ( 3 ) \mathfrak{sim}(3) sim(3)

李代數 s i m ( 3 ) \mathfrak{sim}(3) sim(3)是一個7維向量 ζ \zeta ζ，它的前6維與 s e ( 3 ) \mathfrak{se}(3) se(3)相同，最後多了一項 σ \sigma σ： s i m ( 3 ) = { ζ = [ ρ ϕ σ ] ∈ R 7 , ρ ∈ R 3 , ϕ ∈ s o ( 3 ) , ζ ∧ = [ σ I + ϕ ∧ ρ 0 T 0 ] ∈ R 4 × 4 } . (5.3) \mathfrak{sim}(3)=\left \{ \boldsymbol{\zeta}=\begin{bmatrix} \boldsymbol{\rho} \\ \boldsymbol{\phi} \\ \sigma \end{bmatrix}\in \mathbb{R}^{7},\boldsymbol{\rho}\in\mathbb{R}^{3},\boldsymbol{\phi} \in \mathfrak{so}(3),\boldsymbol{\zeta}^{\wedge}=\begin{bmatrix} \sigma\boldsymbol{I}+\boldsymbol{\phi}^{\wedge} & \boldsymbol{\rho}\\ \boldsymbol{0}^{T} & 0 \end{bmatrix}\in \mathbb{R}^{4\times4} \right \}.\tag{5.3} sim(3)=⎩⎨⎧ζ=⎣⎡ρϕσ⎦⎤∈R7,ρ∈R3,ϕ∈so(3),ζ∧=[σI+ϕ∧0Tρ0]∈R4×4⎭⎬⎫.(5.3)

5.2.2 李代數到李群的指數映射

關聯 S i m ( 30 ) Sim(30) Sim(30)和 s i m ( 3 ) \mathfrak{sim}(3) sim(3)的仍是指數映射和對數映射。先看李代數 s i m ( 3 ) \mathfrak{sim}(3) sim(3)到李群 S i m ( 30 ) Sim(30) Sim(30)的指數映射： exp ⁡ ( ζ ∧ ) = [ e σ exp ⁡ ( ϕ ∧ ) J s ρ 0 T 1 ] . (5.4) \exp(\boldsymbol{\zeta}^{\wedge})=\begin{bmatrix} e^{\sigma}\exp(\boldsymbol{\phi}^{\wedge}) & \boldsymbol{J}_{s}\boldsymbol{\rho}\\ \boldsymbol{0}^{T} & 1 \end{bmatrix}.\tag{5.4} exp(ζ∧)=[eσexp(ϕ∧)0TJsρ1].(5.4)其中： J s = e σ − 1 σ I + σ e σ sin ⁡ θ + ( 1 − e σ cos ⁡ θ ) θ σ 2 + θ 2 a ∧ + ( e σ − 1 σ − ( e σ cos ⁡ θ − 1 ) σ + ( e σ sin ⁡ θ ) θ σ 2 + θ 2 ) a ∧ a ∧ \begin{aligned} \boldsymbol{J}_{s} &= \frac{e^{\sigma}-1}{\sigma}\boldsymbol{I}+\frac{\sigma e^{\sigma}\sin\theta+(1-e^{\sigma}\cos\theta)\theta}{\sigma^{2}+\theta^{2}}\boldsymbol{a}^{\wedge}+ \\& \quad\quad \left (\frac{e^{\sigma}-1}{\sigma}-\frac{(e^{\sigma}\cos\theta-1)\sigma+(e^{\sigma}\sin\theta)\theta}{\sigma^{2}+\theta^{2}} \right )\boldsymbol{a}^{\wedge}\boldsymbol{a}^{\wedge} \end{aligned} Js=σeσ−1I+σ2+θ2σeσsinθ+(1−eσcosθ)θa∧+(σeσ−1−σ2+θ2(eσcosθ−1)σ+(eσsinθ)θ)a∧a∧通過上式可知，已知李代數 ζ \zeta ζ，它與李群中元素的對應關系為： s = e σ , R = exp ⁡ ( ϕ ∧ ) , t = J s ρ . (5.5) s=e^{\sigma},\boldsymbol{R}=\exp(\boldsymbol{\phi}^{\wedge}),\boldsymbol{t}=\boldsymbol{J}_{s}\boldsymbol{\rho}.\tag{5.5} s=eσ,R=exp(ϕ∧),t=Jsρ.(5.5)說明：李群中的尺度因子 s s s即為李代數中 σ \sigma σ的指數函數；旋轉部分與 S O ( 3 ) SO(3) SO(3)中一緻；平移部分，在 m a t h f r a k s e ( 3 ) mathfrak{se}(3) mathfrakse(3)中需要乘一個雅克比 J \boldsymbol{J} J，而相似變換中是更為複雜的 J s \boldsymbol{J}_{s} Js。

5.2.3 李群到李代數的對數映射

已知李群 S i m ( 3 ) Sim(3) Sim(3)時，先将矩陣簡化為如下形式：左下角最後一行前6個元素為0，最後一個元素為1。求解各元素的步驟叙述如下：

σ \sigma σ。對于矩陣左上角 3 × 3 3\times 3 3×3的行列式，由于 ∥ R ∥ = 1 \left \| \boldsymbol{R}\right \|=1 ∥R∥=1，故行列式的值就是尺度因子 s s s，此時 σ = l n ( s ) \sigma=ln(s) σ=ln(s)；
ϕ \boldsymbol{\phi} ϕ。簡化後的李群 S i m ( 3 ) Sim(3) Sim(3)的左上角 3 × 3 3\times 3 3×3矩陣除以尺度因子 s s s，得到旋轉矩陣 R \boldsymbol{R} R，根據 R \boldsymbol{R} R和羅德裡格斯公式可求得 a \boldsymbol{a} a和 θ \theta θ，進而由 ϕ = a θ \boldsymbol{\phi}=\boldsymbol{a}\theta ϕ=aθ求得 ϕ \boldsymbol{\phi} ϕ；
ρ \boldsymbol{\rho} ρ。由 σ 、 a 、 θ \sigma、\boldsymbol{a}、\theta σ、a、θ可求得 J s \boldsymbol{J}_{s} Js，簡化後的李群 S i m ( 3 ) Sim(3) Sim(3)右上角一列為 t \boldsymbol{t} t，此時 ρ = t / J s \boldsymbol{\rho}=\boldsymbol{t}/\boldsymbol{J}_{s} ρ=t/Js。

5.3 擾動模型

S i m ( 3 ) Sim(3) Sim(3)的BCH近似與 S E ( 3 ) SE(3) SE(3)是類似的，我們可以讨論一個點 p p p經過相似變換 S p Sp Sp後，相對于 S S S的導數。同樣地，存在微分模型和擾動模型兩種方式，而擾動模型不用計算雅克比，較為簡單。是以我們省略推導過程，直接給出擾動模型的結果。

設給予 S p Sp Sp左側一個小擾動 exp ⁡ ( η ∧ ) \exp(\eta ^{\wedge}) exp(η∧)，并求 S p Sp Sp對于擾動的導數。因為 S p Sp Sp是4維的齊次坐标， η \eta η是7維向量，是以該導數應該是 4 × 7 4\times7 4×7的雅克比。友善起見，記 S p Sp Sp得前3維組成向量為 q q q，那麼： ∂ S p ∂ η = [ I − q ∧ q 0 T 0 T 0 ] . (5.6) \frac{\partial \boldsymbol{Sp}}{\partial \boldsymbol{\eta}}=\begin{bmatrix} \boldsymbol{I} & -\boldsymbol{q}^{\wedge} & \boldsymbol{q}\\ \boldsymbol{0}^{T} & \boldsymbol{0}^{T} & 0 \end{bmatrix}.\tag{5.6} ∂η∂Sp=[I0T−q∧0Tq0].(5.6)此式可對比公式 ( 3.4 ) (3.4) (3.4)了解記憶。關于 S i m ( 3 ) Sim(3) Sim(3)，我們就介紹到這裡。關于其更詳細的資料，建議讀者參見文獻[3]。

5.4 總結

本講《李群與李代數》引入了李群 S O ( 3 ) SO(3) SO(3)和 S E ( 3 ) SE(3) SE(3)，以及它們對應的李代數 s o ( 3 ) \mathfrak{so}(3) so(3)和 s e ( 3 ) \mathfrak{se}(3) se(3)。我們介紹了位姿在他們上面的表達和轉換，然後通過BCH的線性近似，就可以對位子進行擾動并求導。這給之後講解位姿的優化打下了理論基礎，因為我們需要經常對某一個位姿的估計值進行調整，使它對應的誤差減小，是以隻有弄清楚如何對位姿進行調整和更新之後，才能繼續下一步的内容。

本講内容比較偏理論，講的過程也非常精簡，速度也相對快，但本講内容是解決後續許多問題的基礎，請讀者務必了解，特别是位姿估計部分。值得一提的是，除了李代數，同樣也可以用四元數、歐拉角等方式表示旋轉，隻是後續處理要麻煩一些。在實際應用中，也可以使用 S O ( 3 ) SO(3) SO(3)加上平移的方式來代替 S E ( 3 ) SE(3) SE(3)，進而回避一些雅克比的計算。

為友善了解，下面來看兩個具體的例子。第一個例子是李群和李代數的基本操作，第二個例子是關于最小誤差的計算。

6. 編碼實戰

6.1 李群與李代數的運算庫：Sophus

我們已經介紹了李代數的入門知識，現在是通過實踐演練并鞏固所學知識的時候了。第3講中的Eigen提供了幾何子產品，但沒有提供李代數的支援。一個較好的李代數庫Strasdat維護的Sophu庫6，它支援本章讨論的 S O ( 3 ) SO(3) SO(3)和 S E ( 3 ) SE(3) SE(3)，此外，還有二維運動 S O ( 2 ) SO(2) SO(2)和 S E ( 2 ) SE(2) SE(2)及相似變換 S i m ( 3 ) Sim(3) Sim(3)的内容。它是直接在Eigen基礎上開發的，是以我們不需要安裝額外的依賴庫。讀者可以直接從GitHub上擷取Sophus，原書目錄slambook/3rdparty也提供Sophus源代碼。

由于曆史原因，Sophus早期版本隻提供雙精度的李群/李代數類，後續版本改寫成了模闆類。模闆類的Sophus可以使用不同精度的李群/李代數，但同時也增加了使用難度。本書中使用帶模闆類的Sophus庫，Sophus本身也是一個cmake工程，它隻需編譯即可，無需安裝。

下面來示範Sophus庫中的 S O ( 3 ) SO(3) SO(3)和 S E ( 3 ) SE(3) SE(3)運算：

#include<iostream>
#include<cmath>
#include<eigen3/Eigen/Core>
#include<eigen3/Eigen/Geometry>
#include "sophus/se3.hpp"

using namespace std;
using namespace Eigen;
using namespace Sophus;

int main(int argc, char **argv) {
    Matrix3d R = AngleAxisd(M_PI/2, Vector3d(0, 0, 1)).toRotationMatrix();
    Quaterniond q(R);
    Sophus::SO3d SO3_R(R);
    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;
    cout<<"so3 hat = \n"<<Sophus::SO3d::hat(so3)<<endl;
    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" <<endl<<SO3_updated.matrix()<<endl;
    
    cout<<"******************************"<<endl;
    
    Vector3d t(1, 0, 0);
    Sophus::SE3d SE3_Rt(R, t);
    Sophus::SE3d SE3_qt(q, t);
    cout<<"SE(3) from R, t:\n"<<SE3_Rt.matrix()<<endl;
    cout<<"SE(3) from q, t:\n"<<SE3_qt.matrix()<<endl;
    cout<<"they are equal"<<endl;
    
    typedef Eigen::Matrix<double, 6, 1> Vector6d;
    Vector6d se3 = SE3_Rt.log();
    cout<<"se3 = "<<se3.transpose()<<endl;
    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;
    cout<<"update_se3 = \n" <<endl<<update_se3.matrix()<<endl;
    Sophus::SE3d SE3_updated = Sophus::SE3d::exp(update_se3) * SE3_Rt;
    cout<<"SE3 updated = \n" <<endl<<SE3_updated.matrix()<<endl;
    
    return 0;
}

該示範程式分為兩部分：前半部分介紹 S O ( 3 ) SO(3) SO(3)上的操作，後半部分為 S E ( 3 ) SE(3) SE(3)。我們示範如何構造 S O ( 3 ) , S E ( 3 ) SO(3),SE(3) SO(3),SE(3)對象，對它們進行指數、對數映射，以及當知道更新量後，如何對李群元素進行更新。為了編譯它，請在CMakeLists.txt添加以下幾行代碼：

cmake_minimum_required(VERSION 3.0)

project(useSophus)

# 為使用 sophus，需要使用find_package指令找到它
find_package(Sophus REQUIRED)
find_package(Pangolin REQUIRED)
include_directories(${Sophus_INCLUDE_DIRS})
include_directories(${Pangolin_INCLUDE_DIRS})

add_executable(trajectoryError trajectoryError.cpp)
target_link_libraries(trajectoryError ${Pangolin_LIBRARIES})

include_directories("/usr/include/eigen3")
include_directories("/home/frank/slam14/slambook2/3rdparty/Sophus")

add_executable(useSophus useSophus.cpp)

# 把工程調為Debug編譯模式
set(CMAKE_BUILD_TYPE "Debug")

find_package指令是cmake提供的尋找某個庫的頭檔案和庫檔案的指令。如果cmake能找到它，就會提供頭檔案和庫檔案所在目錄的變量，在這個例子中，就是Sophus_INCLUDE_DIRS。基于模闆的Sophu庫和Eigen庫一樣，是僅含頭檔案而沒有源檔案的，根據它們，我們就能将Sophus庫引入自己的cmake工程。程式運作結果如下：

SO(3) from matrix:
2.22045e-16          -1           0
          1 2.22045e-16           0
          0           0           1
SO(3) from quaternion:
2.22045e-16          -1           0
          1 2.22045e-16           0
          0           0           1
they are equal
so3 =      0      0 1.5708
so3 hat = 
      0 -1.5708       0
 1.5708       0      -0
     -0       0       0
so3 hat vee =      0      0 1.5708
SO3 updated = 

          0          -1           0
          1           0     -0.0001
     0.0001 2.03288e-20           1
******************************
SE(3) from R, t:
2.22045e-16          -1           0           1
          1 2.22045e-16           0           0
          0           0           1           0
          0           0           0           1
SE(3) from q, t:
2.22045e-16          -1           0           1
          1 2.22045e-16           0           0
          0           0           1           0
          0           0           0           1
they are equal
se3 =  0.785398 -0.785398         0         0         0    1.5708
se3 hat = 
        0   -1.5708         0  0.785398
   1.5708         0        -0 -0.785398
       -0         0         0         0
        0         0         0         0
se3 hat vee =  0.785398 -0.785398         0         0         0    1.5708
update_se3 = 

0.0001
     0
     0
     0
     0
     0
SE3 updated = 

2.22045e-16          -1           0      1.0001
          1 2.22045e-16           0           0
          0           0           1           0
          0           0           0           1

6.2 評估軌迹的誤差

在實際工程中，我們經常需要評估一個算法的估計軌迹與真實軌迹的差異來評價算法的精度。真實軌迹往往通過某些更高精度的系統獲得，而估計軌迹則是由待評價的算法計算得到的。在三維剛體運動中，我們示範了如何顯示存儲在檔案中的某條軌迹，本節我們考慮如何計算兩條軌迹的誤差。

考慮一條估計軌迹 T e s t i , i \boldsymbol{T}_{esti,i} Testi,i和真實軌迹（ground truth） T g t , i \boldsymbol{T}_{gt,i} Tgt,i，其中 i = 1 , ⋯ , N i=1,\cdots,N i=1,⋯,N，那麼我們可以定義一些誤差名額來描述它們之間的差别。常見的誤差名額有很多種，介紹如下：

絕對軌迹誤差（Absolute Trajectory Error, ATE），計算公式如下： A T E a l l = 1 N ∑ i = 1 N ∥ log ⁡ ( T g t , i − 1 T e s t i , i ) ∨ ∥ 2 . (6.1) ATE_{all}=\sqrt{\frac{1}{N}\sum_{i=1}^{N}\left \| \log(\boldsymbol{T}^{-1}_{gt,i}\boldsymbol{T}_{esti,i})^{\vee} \right \|^{2}}.\tag{6.1} ATEall=N1i=1∑N∥∥log(Tgt,i−1Testi,i)∨∥∥2

.(6.1)這實際上是每個位姿李代數的均方根誤差（Root-Mean-Squared Error，RMSE），它可以刻畫兩條軌迹的旋轉和平移誤差。
平均平移誤差（Average Translational Error），也有的文獻僅考慮平移誤差，計算公式如下： A T E t r a n s = 1 N ∑ i = 1 N ∥ t r a n s ( T g t , i − 1 T e s t i , i ) ∥ 2 . (6.2) ATE_{trans}=\sqrt{\frac{1}{N}\sum_{i=1}^{N}\left \| trans(\boldsymbol{T}^{-1}_{gt,i}\boldsymbol{T}_{esti,i}) \right \|^{2}}.\tag{6.2} ATEtrans=N1i=1∑N∥∥trans(Tgt,i−1Testi,i)∥∥2

.(6.2)其中trans表示取括号内部變量的平移部分。因為從整條軌迹來看，旋轉出現誤差後，随後也會在平移上出現誤差，是以以上兩種名額在實際中都适用。
相對位姿誤差（Relative Pose Error，RPE），除此之外，還可以定義相對誤差。例如，考慮 i i i時刻到 i + Δ t i+\Delta t i+Δt時刻的運動，那麼RPE可定義為： R P E a l l = 1 N ∑ i = 1 N ∥ log ⁡ ( ( T g t , i − 1 T g t , i + Δ t ) − 1 ( T e s t i , i − 1 T e s t i , i + Δ t ) ) ∨ ∥ 2 . (6.3) RPE_{all}=\sqrt{\frac{1}{N}\sum_{i=1}^{N}\left \| \log\left ((\boldsymbol{T}^{-1}_{gt,i}\boldsymbol{T}_{gt,i+\Delta t})^{-1}(\boldsymbol{T}^{-1}_{esti,i}\boldsymbol{T}_{esti,i+\Delta t} )\right )^{\vee} \right \|^{2}}.\tag{6.3} RPEall=N1i=1∑N∥∥∥log((Tgt,i−1Tgt,i+Δt)−1(Testi,i−1Testi,i+Δt))∨∥∥∥2

.(6.3)同樣地，也可隻取平移部分： R P E t r a n s = 1 N ∑ i = 1 N ∥ t r a n s ( ( T g t , i − 1 T g t , i + Δ t ) − 1 ( T e s t i , i − 1 T e s t i , i + Δ t ) ) ∨ ∥ 2 RPE_{trans}=\sqrt{\frac{1}{N}\sum_{i=1}^{N}\left \| trans \left ((\boldsymbol{T}^{-1}_{gt,i}\boldsymbol{T}_{gt,i+\Delta t})^{-1}(\boldsymbol{T}^{-1}_{esti,i}\boldsymbol{T}_{esti,i+\Delta t} )\right )^{\vee} \right \|^{2}} RPEtrans=N1i=1∑N∥∥∥trans((Tgt,i−1Tgt,i+Δt)−1(Testi,i−1Testi,i+Δt))∨∥∥∥2

利用Sophus庫，很容易實作這部分計算。下面我們示範絕對軌迹誤差的計算。在這個例子中，我們有groundtruth.txt和estimated.txt兩條軌迹，下面的代碼江都區這兩條軌迹，計算RMSE然後顯示到3D視窗中：

#include<iostream>
#include<fstream>
#include<unistd.h>
#include<pangolin/pangolin.h>
#include<sophus/se3.hpp>

using namespace Sophus;
using namespace std;

string groundtruth_file = "../groundtruth.txt";
string estimated_file = "../estimated.txt";

typedef vector<Sophus::SE3d, Eigen::aligned_allocator<Sophus::SE3d>> TrajectoryType;

void DrawTrajectory(const TrajectoryType &gt, const TrajectoryType &esti);

TrajectoryType ReadTrajectory(const string &path);

int main(int argc, char **argv) {
    TrajectoryType groundtruth = ReadTrajectory(groundtruth_file);
    TrajectoryType estimated = ReadTrajectory(estimated_file);
    cout<<groundtruth_file<<endl;
    assert(!groundtruth.empty() && !estimated.empty());
    assert(groundtruth.size() == estimated.size());
    
    double rmse = 0;
    for (size_t i=0; i<estimated.size(); i++) {
        Sophus::SE3d p1 = estimated[1], p2 = groundtruth[i];
        double error = (p2.inverse() * p1).log().norm();
        rmse += error * error;
    }
    rmse = rmse/double(estimated.size());
    rmse = sqrt(rmse);
    cout<<"RMSE = "<<rmse<<endl;
    
    DrawTrajectory(groundtruth, estimated);
    return 0;
}

TrajectoryType ReadTrajectory(const string& path) {
    ifstream fin(path);
    TrajectoryType trajectory;
    if (!fin) {
        cerr<<"trajectory: "<<path<<" not found."<<endl;
        return trajectory;
    }
    
    while (!fin.eof()) {
        double time, tx, ty, tz, qx, qy, qz, qw;
        fin>>time>>tx>>ty>>tz>>qx>>qy>>qz>>qw;
        Sophus::SE3d p1(Eigen::Quaterniond(qx, qy, qz, qw), Eigen::Vector3d(tx, ty, tz));
        trajectory.push_back(p1);
    }
    
    return trajectory;
}

void DrawTrajectory(const TrajectoryType &gt, const TrajectoryType &esti) {
  // create pangolin window and plot the trajectory
  pangolin::CreateWindowAndBind("Trajectory Viewer", 1024, 768);
  glEnable(GL_DEPTH_TEST);
  glEnable(GL_BLEND);
  glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);

  pangolin::OpenGlRenderState s_cam(
      pangolin::ProjectionMatrix(1024, 768, 500, 500, 512, 389, 0.1, 1000),
      pangolin::ModelViewLookAt(0, -0.1, -1.8, 0, 0, 0, 0.0, -1.0, 0.0)
  );

  pangolin::View &d_cam = pangolin::CreateDisplay()
      .SetBounds(0.0, 1.0, pangolin::Attach::Pix(175), 1.0, -1024.0f / 768.0f)
      .SetHandler(new pangolin::Handler3D(s_cam));


  while (pangolin::ShouldQuit() == false) {
    glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);

    d_cam.Activate(s_cam);
    glClearColor(1.0f, 1.0f, 1.0f, 1.0f);

    glLineWidth(2);
    for (size_t i = 0; i < gt.size() - 1; i++) {
      glColor3f(0.0f, 0.0f, 1.0f);  // blue for ground truth
      glBegin(GL_LINES);
      auto p1 = gt[i], p2 = gt[i + 1];
      glVertex3d(p1.translation()[0], p1.translation()[1], p1.translation()[2]);
      glVertex3d(p2.translation()[0], p2.translation()[1], p2.translation()[2]);
      glEnd();
    }

    for (size_t i = 0; i < esti.size() - 1; i++) {
      glColor3f(1.0f, 0.0f, 0.0f);  // red for estimated
      glBegin(GL_LINES);
      auto p1 = esti[i], p2 = esti[i + 1];
      glVertex3d(p1.translation()[0], p1.translation()[1], p1.translation()[2]);
      glVertex3d(p2.translation()[0], p2.translation()[1], p2.translation()[2]);
      glEnd();
    }
    pangolin::FinishFrame();
    usleep(5000);   // sleep 5 ms
  }

}

代碼中文本檔案groundtruth.txt和estimated.txt請從部落格資源部分下載下傳。

該程式輸出結果為RMSE = 3.09706。讀者也可以嘗試将旋轉部分去掉，僅計算平移部分的誤差。這個例子中的資料已經經過預處理，包括軌迹的時間對齊、外參預估等，請讀者知悉。程式運作後的繪制圖如下：

李群與李代數2：李代數求導和李群擾動模型1. 整體誤差最小化引出求導問題2. BCH公式與近似形式3. 微分模型——李代數求導4. 擾動模型（左乘）5. 相似變換群相關6. 編碼實戰後記：參考文獻：

後記：

SLAM系列到此完結。筆者認為數學部分難以了解的部分已經補充完畢，剩下的部分，高翔老師的書講解的沒那麼混亂了，仔細閱讀可以了解。後邊十講的部分，筆者會以思維導圖的形式繼續發部落格總結，喜歡的童鞋記得關注我吧。

參考文獻：

《視覺SLAM十四講：從理論到實踐》，高翔、張濤等著，中國工信出版社。
T.Barfoot, State estimation for robotics: A matrix lie group approach, 2016.
H. Strasdat, Local accuracy and global consistency for efficient visual slam. PhD thesis, Citeseer, 2012.

參見https://groupprops.subwiki.org/wiki/Baker-Campbell-Hausdorff_formula。 ↩︎
對BCH具體形式和近似表達的具體推導，本文不作具體讨論，請參考文獻[2]。 ↩︎
嚴格來說，在矩陣微分中，隻能求行向量關于列向量的導數，所得結果是一個矩陣。但本文寫成列向量對列向量的導數，讀者可以認為先對分子進行轉置，再對最後結果進行轉置，這使得式子變得簡潔。 ↩︎
請注意，為了使乘法成立， p p p必須使用齊次坐标。 ↩︎
作者将其讀作“咚”，像一個石子掉在井裡的聲音。 ↩︎
最早提出李代數的是Sophus Lie，是以庫以他的名字命名。 ↩︎