李群与李代数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，因此库以他的名字命名。 ↩︎