李群与李代数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 相似变换群Sim(3)Sim(3)Sim(3)
- 5.2 李代数sim(3)\mathfrak{sim}(3)sim(3)及指数/对数映射
- - 5.2.1 李代数sim(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中，我们使用SO(3)SO(3)SO(3)上的旋转矩阵或SE(3)SE(3)SE(3)上的变换矩阵来描述如何估计一个相机的位置和姿态（位姿）。不妨设某个时刻机器人的位姿为T\boldsymbol{T}T，它观察到一个世界坐标位于ppp的点，产生了一个观测数据zzz。那么，由坐标变换关系知：z=Tp+w.(1.1)z=\boldsymbol{T}p+w.\tag{1.1}z=Tp+w.(1.1)其中，www为随机噪声。由于它的存在，zzz往往不可能精确地满足z=Tpz=Tpz=Tp的关系。所以，我们通常会计算理想的观测与实际数据的误差：e=z−Tp.(1.2)e=z-\boldsymbol{T}p.\tag{1.2}e=z−Tp.(1.2)假设一共有NNN个这样的路标点和观测，于是就有NNN个上式。那么，对机器人进行位姿估计，相当于找到一个最优的T\boldsymbol{T}T，使得整体误差最小化：min⁡TJ(T)=∑i=1N∥zi−Tpi∥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)求解此问题，需要计算目标函数JJJ关于变换矩阵T\boldsymbol{T}T的导数。我们把具体的算法留到后面讲（精力有限，本系列博客不再涉及，感兴趣的同学去读原书吧）。这里的重点是，我们经常会构建与位姿有关的函数，然后讨论该函数关于位姿的导数，以调整当前的估计值。然而，SO(3),SE(3)SO(3),SE(3)SO(3),SE(3)上并没有良好定义的加法，它们只是群，如果我们把T\boldsymbol{T}T当成一个普通矩阵来处理优化，就必须对它加以约束，就会比较繁琐。
而从李代数角度来说，由于李代数由向量组成，具有良好的加法运算，因此，可以使用李代数解决求导问题，求导思路分为两种：

用李代数表示姿态，然后根据李代数加法对李代数求导。
对李群左乘或右乘微小扰动，然后对该扰动对应的李代数求导，称为李群左扰动或右扰动模型。

第一种方式对应到李代数的求导，而第二种方式则对应到扰动模型。在讨论这两种思路的异同之前，我们先看一下BCH（Baker-Campbell-Hausdorff）线性近似。

2. BCH公式与近似形式

2.1 BCH公式

下面来考虑一个问题。虽然我们已经清楚了SO(3)SO(3)SO(3)和SE(3)SE(3)SE(3)上的李群和李代数关系，但是当在SO(3)SO(3)SO(3)中完成两个矩阵乘法时，李代数中se(3)\mathfrak{se}(3)se(3)上发生了什么改变呢？反过来说，当so(3)\mathfrak{so}(3)so(3)做两个李代数的加法时，SO(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公式）¹给出。由于其完整形式较复杂，我们只给出其展开式的前几项：ln⁡(exp⁡(A)exp⁡(B))=A+B+12[A,B]+112[A,[A,B]]−112[B,[A,B]]−124[B,[A,[A,B]]]+⋯=A+B+12(AB−BA)+112(A2B+AB2−2ABA+B2A+BA2−2BAB)+124(A2B2−2ABAB−B2A2+2BABA).(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线性近似

特别的，考虑SO(3)SO(3)SO(3)上的李代数(ln⁡exp⁡(ϕ1∧)exp⁡(ϕ2∧))∨(\ln\exp(\phi^{\wedge}_{1})\exp(\phi^{\wedge}_{2}))^{\vee}(lnexp(ϕ1∧)exp(ϕ2∧))∨，当ϕ1\phi_{1}ϕ1或ϕ2\phi_{2}ϕ2为小量时，小量二次以上的项可以被忽略，而旋转或变换更新的基本都是小量，因此BCH具有实际应用意义。此时，BCH拥有线性近似表达²

(ln⁡exp⁡(ϕ1∧)exp⁡(ϕ2∧))∨≈{Jl(ϕ2)−1ϕ1+ϕ2当ϕ1为小量,Jr(ϕ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}(lnexp(ϕ1∧)exp(ϕ2∧))∨≈{Jl(ϕ2)−1ϕ1+ϕ2当ϕ1为小量,Jr(ϕ1)−1ϕ2+ϕ1当ϕ2为小量.(2.2)
以第一个近似为例。该式告诉我们，当对一个旋转矩阵R2R_{2}R2（李代数为ϕ2\phi_{2}ϕ2）左乘一个微小旋转矩阵R1R_{1}R1（李代数为ϕ1\phi_{1}ϕ1）时，可以近似地看作，在原有的李代数ϕ2\phi_{2}ϕ2上加了一项Jl(ϕ2)−1ϕ1\boldsymbol{J}_{l}(\phi_{2})^{-1}\phi_{1}Jl(ϕ2)−1ϕ1。同理，第二个近似描述了右乘一个微小位移的情况。于是，李代数在BCH近似下，分成了左乘近似和右乘近似两种，在使用时我们须注意使用的是左乘模型还是右乘模型。
本文以左乘为例。左乘BCH近似雅克比Jl\boldsymbol{J}_{l}Jl事实上就是《李群与李代数1》中的式(4.6)(4.6)(4.6)的内容：Jl=J=sin⁡θθI+(1−sin⁡θθ)aaT+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)它的逆为：Jl−1=θ2cot⁡θ2I+(1−θ2cot⁡θ2)aaT+θ2a∧.(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)而右乘雅克比仅需要对自变量取负号即可：Jr(ϕ)=Jl(−ϕ).(2.5)\boldsymbol{J}_{r}(\phi)=\boldsymbol{J}_{l}(-\phi).\tag{2.5}Jr(ϕ)=Jl(−ϕ).(2.5)这样，我们就可以谈论李群乘法与李代数加法的关系了。

2.3 BCH近似的意义

为了方便读者理解，我们重新叙述BCH近似的意义。假定对某个旋转RRR，对应的李代数为ϕ\phiϕ，我们给它左乘一个微小旋转，记作ΔR\Delta RΔR，对应的李代数为Δϕ\Delta \phiΔϕ。那么，在李群上，得到的结果就是ΔR⋅R\Delta R \cdot RΔR⋅R，而在李代数上，根据BCH近似表达公式(2.2)(2.2)(2.2)，为：Jl(ϕ)−1Δϕ+ϕ\boldsymbol{J}_{l}(\phi)^{-1}\Delta\phi+\phiJl(ϕ)−1Δϕ+ϕ。合并起来，可以简单的写成：exp⁡(Δϕ∧)exp⁡(ϕ∧)=exp⁡((Jl(ϕ)−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⋅ΔRR \cdot \Delta RR⋅ΔR对应的表达式为：exp⁡(ϕ∧)exp⁡(Δϕ∧)=exp⁡((Jr(ϕ)−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⁡((Jl(ϕ)Δϕ)∧)exp⁡(ϕ∧)=exp⁡(ϕ∧)exp⁡((Jr(ϕ)Δϕ)∧).(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)这就为之后李代数上做微分提供了理论基础。

同样地，对于SE(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×66\times66×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)上的情况。假设我们对一个空间点ppp进行了旋转，得到RpRpRp。现在，计算旋转之后点的坐标相对于旋转的导数，我们非正式地记为：∂(Rp)∂R.(3.1)\frac{\partial (Rp)}{\partial R}.\tag{3.1}∂R∂(Rp).(3.1)由于SO(3)SO(3)SO(3)没有加法，所以该导数无法按照导数的定义进行计算。设RRR对应的李代数为ϕ\phiϕ，我们转而计算³：

∂(exp⁡(ϕ∧)p)∂ϕ\frac{\partial (\exp(\phi^{\wedge })p)}{\partial \phi}∂ϕ∂(exp(ϕ∧)p)按照导数的定义有：∂(exp⁡(ϕ∧)p)∂ϕ=lim⁡δϕ→0exp⁡((ϕ+δϕ)∧)p−exp⁡(ϕ∧)pδϕ=lim⁡δϕ→0exp⁡((Jlδϕ)∧)exp⁡(ϕ∧)p−exp⁡(ϕ∧)pδϕ=lim⁡δϕ→0(I+(Jlδϕ)∧)exp⁡(ϕ∧)p−exp⁡(ϕ∧)pδϕ=lim⁡δϕ→0(Jlδϕ)∧(exp⁡(ϕ∧)p)δϕ=lim⁡δϕ→0−(exp⁡(ϕ∧)p)∧Jlδϕδϕ=−(Rp)∧Jl.\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∧aa^{\wedge}b=-b^{\wedge}aa∧b=−b∧a。于是，我们推导出了旋转后的点相对于李代数的导数：∂Rp∂ϕ=(−Rp)∧Jl.(3.2)\frac{\partial Rp}{\partial \phi}=(-Rp)^{\wedge}\boldsymbol{J_{l}}.\tag{3.2}∂ϕ∂Rp=(−Rp)∧Jl.(3.2)不过，由于这里仍然含有形势比较复杂的Jl\boldsymbol{J_{l}}Jl，我们不希望计算它。而下面要讲的扰动模型则提供了更简单的导数计算公式。

4. 扰动模型求导（左乘）

4.1 SO(3)上的扰动模型求导

另一种求导方式是对R进行一次扰动ΔR\Delta RΔR，然后求结果相对于扰动的变化率。这个扰动可以乘在左边也可以乘在右边，最后结果会有一点儿微小的差异，我们以左扰动为例。设ΔR\Delta RΔR对应的李代数为φ\varphiφ，RRR左乘微小旋转ΔR\Delta RΔR后就是ΔR⋅R\Delta R\cdot RΔR⋅R。
这里需要着重强调的是，ΔR⋅R\Delta R\cdot RΔR⋅R非常重要，理解了它就等于理解了扰动，本章的扰动模型求导就是根据ΔR⋅R\Delta R\cdot RΔR⋅R进行的。
根据BCH近似，其对应的李代数就是φ+ϕ\varphi+\phiφ+ϕ，对φ\varphiφ求导得：∂(Rp)∂φ=lim⁡φ→0(ΔR⋅R)p−Rpφ=lim⁡φ→0exp⁡(φ∧)exp⁡(ϕ∧)p−exp⁡(ϕ∧)pφ=lim⁡φ→0(I+φ∧)exp⁡(ϕ∧)p−exp⁡(ϕ∧)pφ=lim⁡φ→0φ∧exp⁡(ϕ∧)pφ=lim⁡φ→0−(Rp)∧φφ=−(Rp)∧.(3.3)\begin{aligned} \frac{\partial (Rp)}{\partial \varphi} &= \lim_{\varphi\rightarrow 0}\frac{(\Delta R\cdot R)p-Rp}{\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φ(ΔR⋅R)p−Rp=φ→0limφexp(φ∧)exp(ϕ∧)p−exp(ϕ∧)p=φ→0limφ(I+φ∧)exp(ϕ∧)p−exp(ϕ∧)p=φ→0limφφ∧exp(ϕ∧)p=φ→0limφ−(Rp)∧φ=−(Rp)∧.(3.3)推导说明：第一行是求导公式的直接展开，第二行直接将李群替换为对应的李代数，第三行为泰勒展开舍去高阶小项后的近似（取极限后写等号），第四到第五行即a∧b=−b∧aa^{\wedge}b=-b^{\wedge}aa∧b=−b∧a。可见，相比于直接对李代数求导，省去了一个雅克比Jl\boldsymbol{J_{l}}Jl的计算。这使得扰动模型更为实用。请读者务必理解这里的求导运算，这在位姿估计中具有重要的意义。

4.2 SE(3)上的扰动模型求导

下边，我们给出SE(3)SE(3)SE(3)上的扰动模型。假设某空间点p经过一次变换T\boldsymbol{T}T（对应李代数为ξ\xiξ），得到Tp\boldsymbol{T}pTp⁴。现在给T\boldsymbol{T}T左乘一个扰动ΔT=exp⁡(χ∧)\Delta T=\exp(\chi^{\wedge})ΔT=exp(χ∧)，我们设扰动项的李代数为χ=[Δρ,Δϕ]T\chi=[\Delta \rho,\Delta\phi]^{T}χ=[Δρ,Δϕ]T，那么：

∂Tp∂χ=lim⁡χ→0(ΔT⋅T)p−Tpχ=lim⁡χ→0exp⁡(χ∧)exp⁡(ξ∧)p−exp⁡(ξ∧)pχ=lim⁡χ→0(I+χ∧)exp⁡(ξ∧)p−exp⁡(ξ∧)pχ=lim⁡χ→0χ∧exp⁡(ξ∧)pχ=lim⁡χ→0[Δϕ∧Δρ0T0][Rp+t1]χ=lim⁡χ→0[Δϕ∧(Rp+t)+Δp0T][Δρ,Δϕ]T=[I−(Rp+t)∧0T0T]=def(Tp)⊙.(3.4)\begin{aligned} \frac{\partial \boldsymbol{T}p}{\partial \chi} &= \lim_{\chi\rightarrow 0} \frac{(\Delta T\cdot T)p-Tp}{\chi} \\&= \lim_{\chi\rightarrow 0} \frac{\exp(\chi^{\wedge})\exp(\xi^{\wedge})p-\exp(\xi^{\wedge})p}{\chi} \\&= \lim_{\chi\rightarrow 0} \frac{(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χ(ΔT⋅T)p−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⊙⁵，它把一个齐次坐标的空间点变换成一个4×64\times 64×6的矩阵。
上式推导中，需要稍微解释的是第五行的矩阵求导顺序，假设a,b,x,y\boldsymbol{a,b,x,y}a,b,x,y都是列向量，那么在我们的符号写法下，有如下规则：d[ab]d[xy]=(d[a,b]Td[xy])T=[dadxdbdxdadydbdy]T=[dadxdadydbdxdbdy.](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 伴随性质

对于公式：Rp∧RT=(Rp)∧.(3.6)Rp^{\wedge}R^{T}=(Rp)^{\wedge}.\tag{3.6}Rp∧RT=(Rp)∧.(3.6)有类似的公式：Rexp⁡(p∧)RT=exp⁡((Rp)∧).(3.7)R\exp(p^{\wedge})R^{T}=\exp((Rp)^{\wedge}).\tag{3.7}Rexp(p∧)RT=exp((Rp)∧).(3.7)该式称为SO(3)SO(3)SO(3)上的伴随性质。同样地，在SE(3)SE(3)SE(3)上也有伴随性质：Texp⁡(ξ∧)T−1=exp⁡((Ad(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)其中：Ad(T)=[Rt∧R0R].Ad(T)=\begin{bmatrix} R & t^{\wedge}R \\ 0 & R \end{bmatrix}.Ad(T)=[R0t∧RR].本性质是书中习题，感兴趣的同学可以研究下。

5. 相似变换群相关

最后，我们要提一下在单目视觉中使用的相似变换群Sim(3)Sim(3)Sim(3)，它对应的李代数，以及他们之间的指数/对数映射关系，最后看一下它对应的扰动模型。如果你只对双目SLAM或RGB-D SLAM感兴趣，可以跳过本节。因仿射变换和射影变换不再具备指数/对数映射性质和扰动模型，所以不再考虑它们。

5.1 相似变换群Sim(3)Sim(3)Sim(3)

我们已经介绍过单目的尺度不确定性。如果在单目SLAM中使用SE(3)SE(3)SE(3)表示位姿，那么由于尺度不确定性与尺度漂移，整个SLAM过程中的尺度会发生变化，这在SE(3)SE(3)SE(3)中未能体现出来。因此，在单目情况下，我们一般会显示地把尺度因子表达出来。用数学语言来说，对于位于空间的点ppp，在相机坐标系下要经过一个相似变换，而非欧氏变换：p′=[sRt0T1]p=sRp+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)在相似变换中，我们把尺度sss表达出来了，它同时作用在ppp的3个坐标之上，对ppp进行了一次缩放。
与SO(3)、SE(3)SO(3)、SE(3)SO(3)、SE(3)类似，相似变换也对矩阵乘法构成群，称为相似变换群Sim(3)Sim(3)Sim(3)：Sim(3)={S=[sRt0T1]∈R4×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)同样地，Sim(3)Sim(3)Sim(3)也有对应的李代数、指数映射、对数映射、扰动模型等，下边分别介绍。

5.2 李代数sim(3)\mathfrak{sim}(3)sim(3)及指数/对数映射

5.2.1 李代数sim(3)\mathfrak{sim}(3)sim(3)

李代数sim(3)\mathfrak{sim}(3)sim(3)是一个7维向量ζ\zetaζ，它的前6维与se(3)\mathfrak{se}(3)se(3)相同，最后多了一项相似变换系数σ\sigmaσ：sim(3)={ζ=[ρϕσ]∈R7,ρ∈R3,ϕ∈so(3),ζ∧=[σI+ϕ∧ρ0T0]∈R4×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 李代数到李群的指数映射

关联Sim(3)Sim(3)Sim(3)和sim(3)\mathfrak{sim}(3)sim(3)的仍是指数映射和对数映射。先看李代数sim(3)\mathfrak{sim}(3)sim(3)到李群Sim(3)Sim(3)Sim(3)的指数映射：exp⁡(ζ∧)=[eσexp⁡(ϕ∧)Jsρ0T1].(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)其中：Js=eσ−1σI+σeσsin⁡θ+(1−eσcos⁡θ)θσ2+θ2a∧+(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=Jsρ.(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)说明：李群中的尺度因子sss即为李代数中σ\sigmaσ的指数函数；旋转部分与SO(3)SO(3)SO(3)中一致；平移部分，在se(3)\mathfrak{se}(3)se(3)中需要乘一个雅克比J\boldsymbol{J}J，而相似变换中是更为复杂的Js\boldsymbol{J}_{s}Js。

5.2.3 李群到李代数的对数映射

已知李群Sim(3)Sim(3)Sim(3)时，先将矩阵简化为如下形式：左下角最后一行前6个元素为0，最后一个元素为1。求解各元素的步骤叙述如下：

σ\sigmaσ。对于矩阵左上角3×33\times 33×3的行列式，由于∥R∥=1\left \| \boldsymbol{R}\right \|=1∥R∥=1，故行列式的值就是尺度因子sss，此时σ=ln(s)\sigma=ln(s)σ=ln(s)；
ϕ\boldsymbol{\phi}ϕ。简化后的李群Sim(3)Sim(3)Sim(3)的左上角3×33\times 33×3矩阵除以尺度因子sss，得到旋转矩阵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、θ可求得Js\boldsymbol{J}_{s}Js，简化后的李群Sim(3)Sim(3)Sim(3)右上角一列为t\boldsymbol{t}t，此时ρ=t/Js\boldsymbol{\rho}=\boldsymbol{t}/\boldsymbol{J}_{s}ρ=t/Js。

最后由σ\sigmaσ、ϕ\boldsymbol{\phi}ϕ和ρ\boldsymbol{\rho}ρ组合，得到对应的李代数ζ\boldsymbol{\zeta}ζ。

5.3 扰动模型

Sim(3)Sim(3)Sim(3)的BCH近似与SE(3)SE(3)SE(3)是类似的，我们可以讨论一个点ppp经过相似变换SpSpSp后，相对于SSS的导数。同样地，存在微分模型和扰动模型两种方式，而扰动模型不用计算雅克比，较为简单。因此我们省略推导过程，直接给出扰动模型的结果。
设给予SpSpSp左侧一个小扰动exp⁡(η∧)\exp(\eta ^{\wedge})exp(η∧)，并求SpSpSp对于扰动的导数。因为SpSpSp是4维的齐次坐标，η\etaη是7维向量，所以该导数应该是4×74\times74×7的雅克比。方便起见，记SpSpSp的前3维组成向量为qqq，那么：∂Sp∂η=[I−q∧q0T0T0].(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)理解记忆。关于Sim(3)Sim(3)Sim(3)，我们就介绍到这里。关于其更详细的资料，建议读者参见文献[3]。

5.4 总结

本讲《李群与李代数》引入了李群SO(3)SO(3)SO(3)和SE(3)SE(3)SE(3)，以及它们对应的李代数so(3)\mathfrak{so}(3)so(3)和se(3)\mathfrak{se}(3)se(3)。我们介绍了位姿在他们上面的表达和转换，然后通过BCH的线性近似，就可以对位子进行扰动并求导。这给之后讲解位姿的优化打下了理论基础，因为我们需要经常对某一个位姿的估计值进行调整，使它对应的误差减小，因此只有弄清楚如何对位姿进行调整和更新之后，才能继续下一步的内容。

本讲内容比较偏理论，讲的过程也非常精简，速度也相对快，但本讲内容是解决后续许多问题的基础，请读者务必理解，特别是位姿估计部分。值得一提的是，除了李代数，同样也可以用四元数、欧拉角等方式表示旋转，只是后续处理要麻烦一些。在实际应用中，也可以使用SO(3)SO(3)SO(3)加上平移的方式来代替SE(3)SE(3)SE(3)，从而回避一些雅克比的计算。

为方便理解，下面来看两个具体的例子。第一个例子是李群和李代数的基本操作，第二个例子是关于最小误差的计算。

6. 编码实战

6.1 李群与李代数的运算库：Sophus

我们已经介绍了李代数的入门知识，现在是通过实践演练并巩固所学知识的时候了。第3讲中的Eigen提供了几何模块，但没有提供李代数的支持。一个较好的李代数库Strasdat维护的Sophu库⁶，它支持本章讨论的SO(3)SO(3)SO(3)和SE(3)SE(3)SE(3)，此外，还有二维运动SO(2)SO(2)SO(2)和SE(2)SE(2)SE(2)及相似变换Sim(3)Sim(3)Sim(3)的内容。它是直接在Eigen基础上开发的，因此我们不需要安装额外的依赖库。读者可以直接从GitHub上获取Sophus，原书目录slambook/3rdparty也提供Sophus源代码。

由于历史原因，Sophus早期版本只提供双精度的李群/李代数类，后续版本改写成了模板类。模板类的Sophus可以使用不同精度的李群/李代数，但同时也增加了使用难度。本书中使用带模板类的Sophus库，Sophus本身也是一个cmake工程，它只需编译即可，无需安装。

下面来演示Sophus库中的SO(3)SO(3)SO(3)和SE(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;
}

该演示程序分为两部分：前半部分介绍SO(3)SO(3)SO(3)上的操作，后半部分为SE(3)SE(3)SE(3)。我们演示如何构造SO(3),SE(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           01 2.22045e-16           00           0           1
SO(3) from quaternion:
2.22045e-16          -1           01 2.22045e-16           00           0           1
they are equal
so3 =      0      0 1.5708
so3 hat = 0 -1.5708       01.5708       0      -0-0       0       0
so3 hat vee =      0      0 1.5708
SO3 updated = 0          -1           01           0     -0.00010.0001 2.03288e-20           1
******************************
SE(3) from R, t:
2.22045e-16          -1           0           11 2.22045e-16           0           00           0           1           00           0           0           1
SE(3) from q, t:
2.22045e-16          -1           0           11 2.22045e-16           0           00           0           1           00           0           0           1
they are equal
se3 =  0.785398 -0.785398         0         0         0    1.5708
se3 hat = 0   -1.5708         0  0.7853981.5708         0        -0 -0.785398-0         0         0         00         0         0         0
se3 hat vee =  0.785398 -0.785398         0         0         0    1.5708
update_se3 = 0.000100000
SE3 updated = 2.22045e-16          -1           0      1.00011 2.22045e-16           0           00           0           1           00           0           0           1

6.2 评估轨迹的误差

在实际工程中，我们经常需要评估一个算法的估计轨迹与真实轨迹的差异来评价算法的精度。真实轨迹往往通过某些更高精度的系统获得，而估计轨迹则是由待评价的算法计算得到的。在三维刚体运动中，我们演示了如何显示存储在文件中的某条轨迹，本节我们考虑如何计算两条轨迹的误差。

考虑一条估计轨迹Testi,i\boldsymbol{T}_{esti,i}Testi,i和真实轨迹（ground truth）Tgt,i\boldsymbol{T}_{gt,i}Tgt,i，其中i=1,⋯,Ni=1,\cdots,Ni=1,⋯,N，那么我们可以定义一些误差指标来描述它们之间的差别。常见的误差指标有很多种，介绍如下：

绝对轨迹误差（Absolute Trajectory Error, ATE），计算公式如下：ATEall=1N∑i=1N∥log⁡(Tgt,i−1Testi,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），也有的文献仅考虑平移误差，计算公式如下：ATEtrans=1N∑i=1N∥trans(Tgt,i−1Testi,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），除此之外，还可以定义相对误差。例如，考虑iii时刻到i+Δti+\Delta ti+Δt时刻的运动，那么RPE可定义为：RPEall=1N∑i=1N∥log⁡((Tgt,i−1Tgt,i+Δt)−1(Testi,i−1Testi,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)同样地，也可只取平移部分：RPEtrans=1N∑i=1N∥trans((Tgt,i−1Tgt,i+Δt)−1(Testi,i−1Testi,i+Δt))∨∥2RPE_{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 trajectorypangolin::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 truthglBegin(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 estimatedglBegin(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。读者也可以尝试将旋转部分去掉，仅计算平移部分的误差。这个例子中的数据已经经过预处理，包括轨迹的时间对齐、外参预估等，请读者知悉。程序运行后的绘制图如下：

后记：

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]。 ↩︎
严格来说，在矩阵微分中，只能求行向量关于列向量的导数，所得结果是一个矩阵。但本文写成列向量对列向量的导数，读者可以认为先对分子进行转置，再对最后结果进行转置，这使得式子变得简洁。在这种意义下，可以认为d(Ax)dx=A\frac{d(Ax)}{dx}=Adxd(Ax)=A ↩︎
请注意，为了使乘法成立，ppp必须使用齐次坐标。 ↩︎
作者将其读作“咚”，像一个石子掉在井里的声音。 ↩︎
最早提出李代数的是Sophus Lie，因此库以他的名字命名。 ↩︎

李群与李代数2：李代数求导和李群扰动模型相关推荐

VSLAM学习记录-求导：李群与李代数
旋转的求导过程 1.基础知识回顾:导数与微分 2.优化中的旋转量 2.1 背景 2.2 李群和李代数 2.2.1 李群 2.2.2 李代数与李群的关系:指数映射 2.2.3 如何求李代数 3.求导:左 ...
李代数的求导及扰动模型
1.引言李代数的求导以及扰动模型有什么用?当然就是求导的用,但在什么情况下需要求导,在机器人领域和3d视觉领域当然是函数的变量是旋转矩阵或变换矩阵,多发生在优化问题上! 举个例子,比如说在PnP问题 ...
视觉SLAM十四讲学习笔记-第四讲-李代数求导与扰动模型
专栏系列文章如下: 视觉SLAM十四讲学习笔记-第一讲_goldqiu的博客-CSDN博客视觉SLAM十四讲学习笔记-第二讲-初识SLAM_goldqiu的博客-CSDN博客视觉SLAM十四讲学习 ...
视觉SLAM笔记（17）李代数求导与扰动模型
视觉SLAM笔记(17) 李代数求导与扰动模型 1. BCH 公式与近似形式 2. SO(3) 李代数上的求导 3. 李代数求导 4. 扰动模型 5. SE(3) 上的李代数求导 1. BCH 公式与 ...
李代数求导与扰动模型
我们经常构建与位姿有关的函数,然后讨论该函数关于位姿的导数,以调整当前的估计值.表示姿态的李群与表示位姿的李群上并没有定义加法,而求导数是需要加法的.因此此时如何求导就成了一个需要解决的问题,它们的李 ...
SLAM总结(二)-数学基础之求导和线性方程求解
SLAM总结(二)-数学基础之求导和线性方程求解 1.求导:高数中常见的是一个函数对一个自变量求导,属于标量对标量求导.在SLAM问题中,函数是目标函数(残差项,约束项),一般包含多个函数,用多维列向 ...
李群和李代数以及李代数求导（扰动模型）
目录 1.李群和李代数李群 2.李代数的引出 3.为什么要用李代数 4.李群与李代数间的指数映射 5.李代数求导(扰动模型) 扰动模型(左扰动为例) 6.下节预告 7.==非常感谢您的阅读!== 1 ...
【杂文随笔1】学习笔记（含欧氏空间、范数、李群与李代数、单满双射、矩阵求导等）
文章目录前言一.欧几里得空间(欧氏空间) 二.范数 1. 定义1 2. 定义2 三.李群与李代数四.单.满.双射五.矩阵的求导六.齐次坐标七.尺寸不变性.旋转不变性八.句柄(Handle ...
李群李代数：李代数求导
雅可比雅可比分为左乘雅可比和右乘雅可比,分别记为JlJ_lJl和JrJ_rJr.左乘雅可比即为李代数se(3)中的JJJ Jl=J=sin⁡θθI+(1−sin⁡θθ)aaT+1−cos⁡θθa ...

李群与李代数2：李代数求导和李群扰动模型

李群与李代数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 相似变换群Sim(3)Sim(3)Sim(3)

5.2 李代数sim(3)\mathfrak{sim}(3)sim(3)及指数/对数映射

5.2.1 李代数sim(3)\mathfrak{sim}(3)sim(3)

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

5.2.3 李群到李代数的对数映射

5.3 扰动模型

5.4 总结

6. 编码实战

6.1 李群与李代数的运算库：Sophus

6.2 评估轨迹的误差

后记：

参考文献：

李群与李代数2：李代数求导和李群扰动模型相关推荐

最新文章

热门文章