SLAM④----李群与李代数

APS2023

已于 2022-05-21 13:16:59 修改

阅读量2.4k

点赞数 2

分类专栏： SLAM 十四讲读书笔记文章标签：线性代数 c++ 自动驾驶计算机视觉

于 2022-05-04 19:45:32 首次发布

本文链接：https://blog.csdn.net/qq_41694024/article/details/124542837

版权

SLAM 十四讲读书笔记专栏收录该内容

12 篇文章 51 订阅

订阅专栏

4.0 本章任务

主要目标
1.理解李群与李代数的概念，掌握 $SO(3)$ 、 $SE(3)$ 与对应李代数的表示方式

2.理解BCH近似的意义

3.学会在李代数上的扰动模型

4.使用Sophus对李代数进行运算

       之前介绍了三维世界中刚体运动的描述方式，包括旋转矩阵、旋转向量、欧拉角、四元数等若干种方式。

        我们介绍了旋转的表示，但是在SLAM中，除了表示，我们还要对它们进行估计和优化。

        因为在SLAM 中位姿是未知的，而我们需要解决形如“什么样的相机位姿最符合当前观测数据”这样的问题。一种典型的方式是把它构建成一个优化问题，求解最优的 $R,t$ ，使得误差最小化。
        如前所言，旋转矩阵自身是带有约束的(正交且行列式为1)。它们作为优化变量时，会引入额外的约束，使优化变得困难。通过李群-----李代数间的转换关系，我们希望把位姿估计变成无约束的优化问题，简化求解方式。

4.1 李群和李代数基础

        第3讲中，我们介绍了旋转矩阵和变换矩阵的定义。当时，我们说三维旋转矩阵构成了特殊正交群SO(3)，而变换矩阵构成了特殊欧氏群SE(3)：



式4-1 特殊正交群

式4-2 特殊欧式群

旋转矩阵也好，变换矩阵也好，它们对加法是不封闭的。换句话说，对于任意两个旋转矩阵，按照矩阵加法的定义，和不再是一个旋转矩阵：

$R_{1}+R_{2} \ not \ belong SO(3),T_{1}+T_{2} \ not \ belong SE(3)$
式4-3 旋转与变换矩阵对加法是不封闭的

        也可以说两种矩阵并没有良好定义的加法，或者通常矩阵加法对这两个集合不封闭。相对地，它们只有一种较好的运算：乘法SO(3)和 SE(3)关于乘法是封闭的：

$R_{1}R_{2}\epsilon SO(3),T_{1}T_{2}\epsilon SE(3)$
式4-4 旋转矩阵、变换矩阵对乘法是封闭的

        同时，我们也可以对任何一个旋转或变换矩阵(在乘法的意义上)求逆。乘法对应着旋转或变换的复合，两个旋转矩阵相乘表示做了两次旋转。

        对于这种只有一个(良好的)运算的集合，我们称之为群。

4.1.1 群

1.群的定义

群(Group)是一种集合加上一种运算的代数结构。我们把集合记作 $A$ ，运算记作 $\cdot$ ，那么群可以记作 $G=(A,.)$ 。群要求这个运算满足以下几个条件:

（个人理解，群是一个集合上有约束的运算关系）

式4-5 群的要求

图4-1 群的要求举例

2. 旋转矩阵、变换矩阵与群

可以将其记作“封结幺逆”

        旋转矩阵集合和矩阵乘法构成群、变换矩阵和矩阵乘法也构成群（因此才能称它们为旋转矩阵群和变换矩阵群)。

        其他常见的群包括整数的加法(Z,+)，去掉0后的有理数的乘法（幺元为1)(Q\0,.)，等等。矩阵中常见的群有：

        群结构保证了在群上的运算具有良好的性质，群论则是研究群的各种结构和性质的理论。

        李群是指具有连续（光滑）性质的群。像整数群Z那样离散的群没有连续性质，所以不是李群。而 SO(n)和 SE(n)在实数空间上是连续的。我们能够直观地想象一个刚体能够连续地在空间中运动，所以它们都是李群。

        由于SO(3)和SE(3)对于相机姿态估计尤其重要，所以主要讨论这两个李群。

4.1.2 李代数的引出

1.李代数的引出--推导

        考虑任意旋转矩阵R，满足：

$RR^T=I$
式4-6 旋转矩阵（正交阵）的性质

        现在，我们说 $R$ (是某个相机的旋转，它会随时间连续地变化，即为时间的函数：R(t)。由于它仍是旋转矩阵，有

$R(t)R(t)^T=I$
式4-7 相机的旋转时间函数仍是旋转矩阵

在等式两边对时间求导，得到



式4-8 式4-7对两边求导

整理，得

式4-9 红字是结论

        可以看出 ${\color{Blue} }{\color{Blue} }R(t)^{'}R(t)^{T}$ 是一个反对称矩阵。回忆一下。介绍叉积时，引入了^符号，将一个向量变成了反对称矩阵。同理，对于任意反对称矩阵，我们也能找到唯一与之对应的向量。把这个运算用符号∨表示：

式4-10 ∨符号与^符号

        于是,由于 $R(t)^{'}R(t)^{T}$ 是一个反对称矩阵．我们可以找到一个三维向量Φ(t) ∈ $R^{3}$ 与之对应:

式4-11 与R(t)'R(t)T矩阵对应的三维向量Φ(t)

         等式两边右乘R(t)，由于R为正交阵、有



式4-12 观察规律

        结论：可以看到，每对旋转矩阵求一次导数。只需左乘一个Φ(t) 矩阵即可。考虑 $t _{0}=0$ 时，设此时旋转矩阵为 $R(0)=I$ 。按照导数定义，可以把R(t)在 $t=0$ 附近进行一阶泰勒展开：(其中 $R'(t_{0})=\phi (t_{0})^{\wedge }R(t_{0})$ )

式4-13 R(t)在t =0附近进行一阶泰勒展开

        我们看到 $\phi$ 反映了R的导数性质，故称它在SO(3)原点附近的正切空间(Tangent Space)
上。同时在 $a_{0}$ =0 附近。设 $\phi$ 保持为常数 $\phi \bigl(\begin{smallmatrix} t_{0} \end{smallmatrix}\bigr) = \phi _{0}$ 。那么根据式(4-12)，有

式4-14 函数连续，带入数据

         上式是一个关于R的微分方程,而且有初始值 $R(0)= I$ ，解得

$R(t)=exp(\phi _{0}^{\wedge }t)$
式4-15 旋转矩阵与反对称矩阵的关系

2.李代数的引出--关系

这说明在 $t =0$ 附近，旋转矩阵可以由 $exp\hat{(\phi _{0})}t$ 计算出来

旋转矩阵R与另一个反对称矩阵 $exp\hat{(\phi _{0})}t$ 通过指数关系发生了关系，但是矩阵的指数是什么呢？

1．给定某时刻的 $R$ ，我们就能求得一个 $\phi$ 。它描述了 $R$ 在局部的导数关系。与 $R$ 对应的 $\phi$
有什么含义呢？我们说， $\phi$ 正是对应到 $SO(3)$ 的李代数 $so(3)$ ；
2．其次，给定某个向量时，矩阵指数 $exp\hat{(\phi _{0})}$ 如何计算？反之，给定 $R$ 时，能否有相反
的运算来计算 $\phi$ ？事实上，这正是李群和李代数间的指数/对数映射。
下面，我们来解决这两个问题。

4.1.3 李代数的定义

        每个李群都有与之对应的李代数。李代数描述了李群的局部性质，准确地说，是单位元附近的正切空间。一般的李代数的定义如下:
        李代数由一个集合V、一个数域F和一个元运算[,]组成。如果它们满足以下几条性质,则称(V,F,[,])为一个李代数，记作 $g$

图4-2 李代数要满足的性质

        其中二元运算被称为李括号。从表面上看，李代数所需要的性质还是挺多的。相比于群中的较为简单的二元运算，李括号表达了两个元素的差异。它不要求结合律，而要求元素和自己做李括号之后为零的性质。作为例子，三维向量 $\mathbb{R}^{3}$ 上定义的叉积×是一种李括号，

        因此 $g$ =( $\mathbb{R}^{3}$ ， $\mathbb{R}$ ，×)构成了一个李代数。三维空间向量+叉积运算构成李代数。

图4-3 三维空间向量+叉积运算构成李代数

4.1.4 李代数 so(3)

        之前提到的 $\phi$ ，事实上是一种李代数。

        SO(3)对应的李代数是定义在 $\mathbb{R}^{3}$ 上的向量，我们记作 $\phi$ 。根据前面的推导，每个 $\phi$ 都可以生成一个反对称矩阵：

式4-16 SO(3)的李代数

在此定义下，两个向量中 $\varphi _{1}\varphi _{2}$ 的李括号为

式4-17 SO(3)的李代数对应的李括号

        定义下的李括号满足上面的四条性质。由于向量 $\varphi$ 与反对称矩阵是一一对应的，在不引起歧义的情况下，就说 $so\begin{pmatrix} 3 \end{pmatrix}$ 的元素是三维向量或者三维反对称矩阵，不加区别:

式4-18 李代数so(3)

        至此，我们已清楚了 $so\begin{pmatrix} 3 \end{pmatrix}$ 的内容。它们是一个由三维向量组成的集合，每个向量对应一个反对称矩阵，可以用于表达旋转矩阵的导数。它与SO(3)的关系由指数映射给定：

$R=exp(\phi ^\wedge )$
式4-19 李代数与旋转矩阵的关系

4.1.5 李代数 se(3)

         同理，SE(3)亦有李代数 $se\begin{pmatrix} 3 \end{pmatrix}$ ：与 $so\begin{pmatrix} 3 \end{pmatrix}$ 相似， $se\begin{pmatrix} 3 \end{pmatrix}$ 位于 $\mathbb{R}^{6}$ 空间中：

式4-20 李代数se(3)

         我们把每个 $se\begin{pmatrix} 3 \end{pmatrix}$ 元素记作 $\xi$ ，它是一个六维向量。

前三维为平移（但含义与变换矩阵中的平移不同，分析见后)，记作 $\rho$ ；后三维为旋转，记作 $\o$ ，实质上是 $so\begin{pmatrix} 3 \end{pmatrix}$ 元素。同时，我们拓展了 ^ 符号的含义。在 $se\begin{pmatrix} 3 \end{pmatrix}$ 中，同样使用 ^ 符号，将一个六维向量转换成四维矩阵，但这里不再表示反对称。

        我们仍使用 ^ 和∨符号指代“从向量到矩阵”和“从矩阵到向量”的关系，以保持和 $so\begin{pmatrix} 3 \end{pmatrix}$ 上
的一致性。它们依旧是一一对应的。

        可以简单地把 $se\begin{pmatrix} 3 \end{pmatrix}$ 理解成“由一个平移加上一个 $so\begin{pmatrix} 3 \end{pmatrix}$ 元素构成的向量”(尽管这里的 $\rho$ 还不直接是平移)。同样，李代数 $se\begin{pmatrix} 3 \end{pmatrix}$ 也有类似于 $so\begin{pmatrix} 3 \end{pmatrix}$ 的李括号：

式4-21 李代数se(3)的李括号

         不同书籍对 $se(3)$ 的平移/旋转分量的先后顺序定义不同。这里使用平移在前的方式，也有地方是旋转在前的。把李代数理解成向量形式或矩阵形式都是可以的。向量形式更加自然一些。

4.2 指数与对数映射

4.2.1 SO(3)上的指数映射

1.指数映射的推导--如何计算 $exp(\phi ^{\wedge })$

如何计算 $exp\hat{(\phi _{0})}$ ，显然它是一个矩阵的指数，在李群和李代数中，称为指数映射（Exponential Map)。同样，我们会先讨论 $so\begin{pmatrix} 3 \end{pmatrix}$ 的指数映射，再讨论 $se\begin{pmatrix} 3 \end{pmatrix}$ 的情形。

        任意矩阵的指数映射可以写成一个泰勒展开，但是只有在收敛的情况下才会有结果，其结果仍是一个矩阵：

$exp(A)=\sum_{n=0}^{\infty }\frac{1}{n!}A^n$
式4-22 任意矩阵泰勒 展开

         同样地，对 $so\begin{pmatrix} 3 \end{pmatrix}$ 中的任意元素 $\varphi$ ，我们也可按此方式定义它的指数映射：

$exp(\phi ^\wedge )=\sum_{n=0}^{\infty }\frac{1}{n!}(\phi ^\wedge )^n$
式4-23 so(3)矩阵泰勒展开

        但这个定义没法直接计算，因为我们不想计算矩阵的无穷次幂。

        下面我们推导一种计算指数映射的简便方法。由于是三维向量，我们可以定义它的模长和方向，分别记作 $\theta$ 和 $a$ ，于是有：

$\phi=\theta a$
式4-24 三维向量的另一种表示方法

         这里 $a$ 是一个长度为1的方向向量，即 $\begin{vmatrix} a \end{vmatrix}=1$ 。首先，对于 $a^{\wedge }$ ，有以下两条性质：

$a^{\wedge }a^{\wedge }=aa^T-I ,a^{\wedge }a^{\wedge }a^{\wedge }=-a^{\wedge }$
式4-25 a^的性质

于是：

式4-26 通过Φ分解出的θ和a计算出旋转矩阵

        它和罗德里格斯公式如出一辙。

2.指数映射的结论与性质

        这表明， $so\begin{pmatrix} 3 \end{pmatrix}$ 实际上就是由所谓的旋转向量组成的空间，而指数映射即罗德里格斯公式。通过它们，我们把 $so\begin{pmatrix} 3 \end{pmatrix}$ 中任意一个向量对应到了一个位于SO(3)中的旋转矩阵。反之，如果定义对数映射，也能把SO(3)中的元素对应到 $so\begin{pmatrix} 3 \end{pmatrix}$ 中:



式4-27 SO(3)中的元素对应到so(3)中

         但实际当中没必要这样求，在旋转向量小节已经介绍了矩阵到向量的转换关系：

$\theta =arccos(\frac{tr(R)-1}{2}) , \ Rn=n$
式4-28 旋转矩阵到向量的转化关系

        指数映射有何性质呢?

        是否对于任意的R都能找到一个唯一的?

        指数映射只是一个满射，并不是单射。

        这意味着每个SO(3)中的元素，都可以找到一个 $so\begin{pmatrix} 3 \end{pmatrix}$ 元素与之对应；但是可能存在多个 $so\begin{pmatrix} 3 \end{pmatrix}$ 中的元素，对应到同一个SO(3)。至少对于旋转角0，我们知道多转360°和没有转是一样的——它具有周期性。但是，如果我们把旋转角度固定在 $\pm \pi$ 之间，那么李群和李代数元素是一一对应的。
        SO(3)与 $so\begin{pmatrix} 3 \end{pmatrix}$ 的结论似乎在我们的意料之中。它和我们前面讲的旋转向量与旋转矩阵很相似，而指数映射即罗德里格斯公式。旋转矩阵的导数可以由旋转向量指定，指导着如何在旋转矩阵中进行微积分运算。

图4-4 单射，满射，全射

4.2.2 SE(3)上的指数映射

         $se\begin{pmatrix} 3 \end{pmatrix}$ 上的指数映射形式为

式4-29 se(3)到SE(3)的指数映射

         其中 $J$ 为雅可比矩阵

式4-30 雅可比矩阵

        从结果上看， $\xi$ 的指数映射左上角的R是SO(3)中的元素，与 $se\begin{pmatrix} 3 \end{pmatrix}$ 中的旋转部分 $\phi$ 对应。而右上角的J就是4-30式：

        该式与罗德里格斯公式有些相似，但不完全一样。我们看到，平移部分经过指数映射之后发生了一次以 $J$ 为系数矩阵的线性变换。
         同样地，虽然我们也可以类比推得对数映射，不过根据变换矩阵 $T$ 求 $so\begin{pmatrix} 3 \end{pmatrix}$ 上的对应向量也有更省事的方式：从左上角的R计算旋转向量，而右上角的 $t$ 满足：

$t=J\rho$
式4-31 t的式子

        由于 $J$ 可以由 $\phi$ 得到，所以这里的 $\rho$ 也可由此线性方程解得。现在，我们已经弄清了李群、李代数的定义与相互的转换关系。

图4-5 李群、李代数的定义与相互的转换关系

4.3 李代数求导与扰动模型

4.3.1 BCH公式与近似形式

1.矩阵运算与代数运算是否等同？

        使用李代数的一大动机是进行优化，而在优化过程中导数是非常必要的信息。下面来考虑一个问题。

        虽然我们已经清楚了 $SO(3)$ 和 $SE(3)$ 上的李群与李代数关系。但是，当在SO(3)中完成两个矩阵乘法时，李代数中 $so\begin{pmatrix} 3 \end{pmatrix}$ 上发生了什么改变呢？反过来说，当 $so\begin{pmatrix} 3 \end{pmatrix}$ 上做两个李代数的加法时，SO(3)上是否对应着两个矩阵的乘积？如果成立，相当于：

$exp(\phi _{1}^{\wedge })exp(\phi _{2}^{\wedge }) = exp((\phi_{1}+\phi_{2})^\wedge ))$
式4-32-1 当在李代数中做加法时，是否等价于在李群上做乘法



如果 $\phi _{1}$ , $\phi _{2}$ 为标量，那么显然该式成立；但此处我们计算的是矩阵的指数函数，而非标量的指数。换言之，我们在研究下式是否成立：



$ln(exp(A)exp(B))?=A+B$
式4-32-2 矩阵运算是否与代数运算相同

        然而，该式在矩阵不成立！

2. BCH公式

式4-32 BCH公式

其中[ ]为李括号。BCH公式告诉我们，当处理两个矩阵指数之积时，它们会产生一些由李括号组成的余项。特别地，考虑 $SO(3)$ 上的李代数 $ln(exp(\phi _{1}^\wedge )exp(\phi _{2}^\wedge ))^\vee$ ，当如 $\phi _{1}$ 或 $\phi _{2}$ 为小量时，小量二次以上的项都可以被忽略。此时，BCH拥有线性近似表达：

$ln(exp(\phi _{1}^{\wedge })exp(\phi _{2}^{\wedge }))^{\vee }\approx \left\{\begin{matrix} J_{l}(\phi _{2})^{-1}\phi _{1}+\phi_{2} \ \ \ \ if \ \phi _{1}\ is\ small \\ J_{r}(\phi _{1})^{-1}\phi _{2}+\phi_{1} \ \ \ \ if \ \phi _{2}\ is\ small \end{matrix}\right.$
式4-33 BCH线性近似表达

其中：

式4-34 左雅可比与右雅可比

以第一个近似为例。该式告诉我们，当对个旋转矩阵 $R_{2}$ （李代数为中 $\phi _{2}$ ）左乘一个微小旋转矩阵 $R_{1}$ (李代数为如 $\phi _{1}$ )时，可以近似地看作，在原有的李代数上加上了一项 $J_{l}(\phi _{2})^{-1}\phi _{1}$ 。同理，第二个近似描述了右乘一个微小位移的情况。于是，李代数在BCH近似下，分成了左乘近似和右乘近似两种，在使用时我们须注意使用的是左乘模型还是右乘模型。

3. BCH近似的意义

        假定对某个旋转 $R$ ，对应的李代数为 $\phi$ 。我们给它左乘一个微小旋转，记作 $\Delta R$ ，对应的李代数为 $\Delta \phi$ 。

        那么，在李群上,得到的结果就是 $\Delta R\cdot R$ ，而在李代数上，根据BHP相似，为 $J_{l}^{-1}\begin{pmatrix} \phi \end{pmatrix}\Delta \phi+ \phi$ ，合并起来，可以写成：

$exp(\Delta \phi ^{\wedge })exp(\phi ^{\wedge })=exp((\phi + J_{l}^{-1}(\phi )\Delta \phi)^{\wedge })$
式4-35 左乘微小位移

         反之，如果我们在李代数上进行加法，让一个 $\phi$ 加上 $\Delta \phi$ ，那么可以近似为李群上带左右雅可比的乘法：

由式 $ln(exp(\phi _{1}^{\wedge })exp(\phi _{2}^{\wedge }))^{\vee }\approx \left\{\begin{matrix} J_{l}(\phi _{2})^{-1}\phi _{1}+\phi_{2} \ \ \ \ if \ \phi _{1}\ is\ small \\ J_{r}(\phi _{1})^{-1}\phi _{2}+\phi_{1} \ \ \ \ if \ \phi _{2}\ is\ small \end{matrix}\right.$ ，两边取指数

$exp((\phi +\Delta \phi )^{\wedge }) = exp((J_{l}\Delta \phi )^\wedge )exp(\phi ^\wedge ) = exp(\phi ^\wedge )exp((J_{r}\Delta \phi )^\wedge )$
式4-35 核心：李代数加法与李代数乘法关系

         这就为之后李代数上做微积分提供了理论基础。同样地，对于 $SE(3)$ ，也有类似的BCH近似:

式4-36 SE(3)的BCH近似

4.3.2 SO(3)上的李代数求导

        在SLAM 中，我们要估计一个相机的位置和姿态，该位姿是由SO(3)上的旋转矩阵或SE(3)上的变换矩阵描述的。不妨设某个时刻小萝卜的位姿为 $T$ 。它观察到了一个世界坐标位于 $p$ 的点，产生了一个观测数据 $z$ 。那么，由坐标变换关系知:

$z = Tp + w$
式4-37 slam观测方程低配版

  其中 $w$ 为随机噪声。由于它的存在， $z$ 往往不可能精确地满足 $z = Tp$ 的关系。所以，我们通常会计算理想的观测与实际数据的误差:



$e = z - Tp$
式4-38 实际值与观测值的误差e

        假设一共有N个这样的路标点和观测，于是就有N个上式。那么，对小萝卜进行位姿估计，相当于寻找一个最优的 $T$ ，使得整体误差最小化：（寻找一个最好的位姿观察点，使在该处的观察和实际位姿相差最小，就求一个这样的 $T$ ）

$\underset{T}{min}J(T) = \sum_{i=1}^{N}\begin{Vmatrix} z_{i}-Tp_{i} \end{Vmatrix}_{2}^{2}$
式4-38 寻找一个最好的位姿T，使得观测误差最小

         求解此问题，需要计算目标函数 $J$ 关于变换矩阵 $T$ 的导数。我们把具体的算法留到后面再讲。

        这里的重点是，我们经常会构建与位姿有关的函数，然后讨论该函数关于位姿的导数，以调整当前的估计值。然而， $SO(3), SE(3)$ 上并没有良好定义的加法，它们只是群。如果我们把 $T$ 当成一个普通矩阵来处理优化，就必须对它加以约束。而从李代数角度来说，由于李代数由向量组成，具有良好的加法运算。因此，使用李代数解决求导问题的思路分为两种:

        1．用李代数表示姿态,然后根据李代数加法对李代数求导。（对R对应的李代数加上小量，求相对于小量的变化率（导数模型)）
        2．对李群左乘或右乘微小扰动,然后对该扰动求导，称为左扰动和右扰动模型。（对R左乘或右乘一个小量，求相对于小量的李代数的变化率（扰动模型)）
        第一种方式对应到李代数的求导模型，而第二种方式则对应到扰动模型。下面讨论这两种思路的异同。

4.3.3 李代数求导

        首先，考虑 $SO(3)$ 上的情况。假设我们对一个空间点 $p$ 进行了旋转，得到了 $Rp$ 。现在，要计算旋转之后点的坐标相对于旋转的导数，我们非正式地记为：

$\frac{\vartheta Rp}{\vartheta R}$
式4-39 旋转之后点的坐标相对于旋转的导数

         由于SO(3)没有加法，所以该导数无法按照导数的定义进行计算。设 $R$ 对应的李代数为 $\phi$ ，我们转而计算：

$\frac{\vartheta (exp(\phi ^{\wedge }p)}{\vartheta \phi }$
式4-40 旋转之后点的坐标相对于旋转的导数由李代数表示

按照导数的定义：

式4-41 推导

第2行的近似为BCH线性近似，第3行为泰勒展开舍去高阶项后的近似(由于取了极限，可以写等号)，第4行至第5行将反对称符号看作叉积，交换之后变号。于是，我们推导出了旋转后的点相对于李代数的导数：

$\frac{\vartheta Rp}{\vartheta \phi } =-(Rp)^{\wedge }J_{l}$
式4-41 旋转后的点相对于李代数的导数

         不过，由于这里仍然含有形式比较复杂的 $J$ ，我们不太希望计算它。

4.3.4 扰动模型（左乘)

另一种求导方式是对 $R$ 进行一次扰动 $\Delta R$ ，看结果相对于扰动的变化率。这个扰动可以乘在左边也可以乘在右边，最后结果会有差异，以左扰动为例。设左扰动 $\Delta R$ 对应的李代数为 $\phi$ 。然后，对 $\phi$ 求导，即：

式4-42 对左扰动模型求导

可见，相比于直接对李代数求导，省去了一个雅可比 $J_{l}$ 的计算。这使得扰动模型更为实用。请读者务必理解这里的求导运算，这在位姿估计中具有重要的意义。

式子解释：第一行中 $exp(\phi ^{\wedge })p = Rp$ ，左乘一个 $\varphi ^{\wedge }$ 并使其趋于0相当于让其左乘一个微小的位移变换(R的几何意义是物体的位姿)，则第一行抽象化，可写为下式形式再计算就好理解了：

$\underset{\Delta x\rightarrow 0}{lim} \frac{ f(x+\Delta x) - f(x)}{\Delta x}$
式4-43 对左扰动模型抽象化

此外，这个式子的意义是结果相对于扰动的变化率。

4.3.5 SE(3)上的李代数求导

        假设某空间点 $p$ 经过一次变换 $T$ (对应李代数为 $\xi$ )，得到 $Tp$ 。

        现在，给 $T$ 左乘一个扰动 $\Delta T = exp (\delta \xi ^{\wedge })$ ，我们设扰动项的李代数为 $\delta \xi =[\delta \rho ,\delta \phi ]^{T}$ ，那么：

式4-44 李代数求导的推导

        我们把最后的结果定义成一个算符⊙，它把一个齐次坐标的空间点变换成一个4×6的矩阵。此式稍微需要解释的是矩阵求导方面的顺序，假设 $a,b,x,y$ 都是列向量，有如下规则：



$\frac{d\begin{bmatrix} a\\b \end{bmatrix}}{d\begin{bmatrix} x\\y \end{bmatrix}}=(\frac{d[a,b]^T}{d\begin{bmatrix} x\\y \end{bmatrix}})^{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}$
式4-45 矩阵求导

4.4 实践 Sophus

4.4.1 Sophus的基本使用方法

#include <iostream>
#include <cmath>
#include <Eigen/Core>
#include <Eigen/Geometry>
#include "sophus/se3.hpp"
using namespace std;
using namespace Eigen;

/// 本程序演示sophus的基本用法

int main(int argc, char **argv) {

  // 沿Z轴转90度的旋转矩阵
  Matrix3d R = AngleAxisd(M_PI / 2, Vector3d(0, 0, 1)).toRotationMatrix();
  // 或者四元数
  Quaterniond q(R);
  Sophus::SO3d SO3_R(R);              // Sophus::SO3d可以直接从旋转矩阵构造
  Sophus::SO3d SO3_q(q);              // 也可以通过四元数构造
  // 二者是等价的
  cout << "SO(3) from matrix:\n" << SO3_R.matrix() << endl;
  cout << "SO(3) from quaternion:\n" << SO3_q.matrix() << endl;
  cout << "they are equal" << endl;

  // 使用对数映射获得它的李代数
  Vector3d so3 = SO3_R.log();
  cout << "so3 = " << so3.transpose() << endl;
  // hat 为向量到反对称矩阵
  cout << "so3 hat=\n" << Sophus::SO3d::hat(so3) << endl;
  // 相对的，vee为反对称到向量
  cout << "so3 hat vee= " << Sophus::SO3d::vee(Sophus::SO3d::hat(so3)).transpose() << endl;

  // 增量扰动模型的更新
  Vector3d update_so3(1e-4, 0, 0); //假设更新量为这么多
  Sophus::SO3d SO3_updated = Sophus::SO3d::exp(update_so3) * SO3_R;
  cout << "SO3 updated = \n" << SO3_updated.matrix() << endl;

  cout << "*******************************" << endl;
  // 对SE(3)操作大同小异
  Vector3d t(1, 0, 0);           // 沿X轴平移1
  Sophus::SE3d SE3_Rt(R, t);           // 从R,t构造SE(3)
  Sophus::SE3d SE3_qt(q, t);            // 从q,t构造SE(3)
  cout << "SE3 from R,t= \n" << SE3_Rt.matrix() << endl;
  cout << "SE3 from q,t= \n" << SE3_qt.matrix() << endl;
  // 李代数se(3) 是一个六维向量，方便起见先typedef一下
  typedef Eigen::Matrix<double, 6, 1> Vector6d;
  Vector6d se3 = SE3_Rt.log();
  cout << "se3 = " << se3.transpose() << endl;
  // 观察输出，会发现在Sophus中，se(3)的平移在前，旋转在后.
  // 同样的，有hat和vee两个算符
  cout << "se3 hat = \n" << Sophus::SE3d::hat(se3) << endl;
  cout << "se3 hat vee = " << Sophus::SE3d::vee(Sophus::SE3d::hat(se3)).transpose() << endl;

  // 最后，演示一下更新
  Vector6d update_se3; //更新量
  update_se3.setZero();
  update_se3(0, 0) = 1e-4;
  Sophus::SE3d SE3_updated = Sophus::SE3d::exp(update_se3) * SE3_Rt;
  cout << "SE3 updated = " << endl << SE3_updated.matrix() << endl;

  return 0;
}

执行结果

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
*******************************
SE3 from R,t= 
2.22045e-16          -1           0           1
          1 2.22045e-16           0           0
          0           0           1           0
          0           0           0           1
SE3 from q,t= 
2.22045e-16          -1           0           1
          1 2.22045e-16           0           0
          0           0           1           0
          0           0           0           1
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
SE3 updated = 
2.22045e-16          -1           0      1.0001
          1 2.22045e-16           0           0
          0           0           1           0
          0           0           0           1

该演示程序分为两部分。前半部分介绍SO(3)上的操作，后半部分则为SE(3)。我们演示了如何构造SO(3), SE(3)对象，对它们进行指数、对数映射，以及当知道更新量后，如何对李群元素进行更新。

4.4.2 例子:评估轨迹的误差

        在实际工程中，我们经常需要评估一个算法的估计轨迹与真实轨迹的差异来评价算法的精度。真实轨迹往往通过某些更高精度的系统获得，而估计轨迹则是由待评价的算法计算得到的。
第3讲我们演示了如何显示存储在文件中的某条轨迹，本节我们考虑如何计算两条轨迹的误差。考虑一条估计轨迹 $T_{esti}$ ,和真实轨迹 $T_{gt}$ .，其中i= 1,…,N，那么我们可以定义一些误差指标来描述它们之间的差别。

        误差指标可以有很多种，常见的有绝对轨迹误差（Absolute Trajectory Error，ATE)，形如:



绝对轨迹误差

        这实际上是每个位姿李代数的均方根误差（Root-Mean-Squared Error，RMSE)。这种误差可以刻画两条轨迹的旋转和平移误差。同时，也有的文献仅考虑平移误差,从而可以定义绝对平移误差（Average Translational Error)：

式4-46 绝对平移误差

         其中 trans表示取括号内部变量的平移部分。因为从整条轨迹上看，旋转出现误差后，随后的轨迹在平移上也会出现误差,所以两种指标在实际中都适用。

        除此之外，也可以定义相对的误差。例如，考虑i时刻到 $i + \Delta t$ 时刻的运动,那么相对位姿误差（Relative Pose Error，RPE）可定义为


式4-47 相对位姿误差

         同样地,也可只取平移部分:

式4-48 只取平移部分的相对位姿误差

         利用Sophus库，很容易实现这部分计算。下面我们演示绝对轨迹误差的计算。在这个例子中，我们有groundtruth.txt和 estimated.txt两条轨迹，下面的代码将读取这两条轨迹，计算误差,然后显示到3D窗口中。为简洁起见，省略了画轨迹部分的代码，在第3讲中我们已经做过类似的工作。

代码如下：
#include <iostream>
#include <fstream>
#include <unistd.h>
#include <pangolin/pangolin.h>
#include "sophus/se3.hpp"

using namespace Sophus;
using namespace std;

string groundtruth_file = "/home/liuhongwei/桌面/slambook2/ch4/example/groundtruth.txt";
string estimated_file = "/home/liuhongwei/桌面/slambook2/ch4/example/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);
  assert(!groundtruth.empty() && !estimated.empty());
  assert(groundtruth.size() == estimated.size());

  // compute rmse
  double rmse = 0;
  for (size_t i = 0; i < estimated.size(); i++) {
    Sophus::SE3d p1 = estimated[i], 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(qw, qx, qy, qz), 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
  }

}
结果：计算结果和真实结果的误差

图4-6 实验数据

4.5 小结

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