第十讲：后端 1

本节目标

1. 理解后端的概念。
2. 理解以 EKF 为代表的滤波器后端工作原理。
3. 理解非线性优化的后端，明白稀疏性是如何被利用的。
4. 使用 g2o 和 Ceres 实际操作后端优化。
   知识点整理

本讲主要是针对长时间内最优轨迹和地图的优化，减小累积误差对长时间运动带来的影响。

1、批量的处理方式：考虑在很长一段时间内的状态估计问题，而且不仅使用过去的信息更新自己的状态，也会用未来的信息来更新自己。
2、渐进的处理方式：当前状态只由过去的时刻决定，甚至只由前一时刻决定。
3、当没有运动方程时的处理方式：认为确实没有运动方程，或假设相机不动，或假设相机匀速运动。此时非常类似于SfM（即通过一组图像来恢复运动和结构，但是SfM中允许使用完全无关的图像），SLAM中的图像有时间上的先后顺序。
4、由于每个方程都受噪声的影响，所以把位姿x和路标y看成服从某种概率分布的随机变量，会随着运动和观测数据的到来进行更新。
5、当将状态量和噪声项以高斯分布来模拟时，均值可看作是对变量最优值的估计，而协方差矩阵则度量了它的不确定性。
6、对状态进行估计，可以得到当前状态的估计=似然*先验，其中似然由观测方程给定，先验表示当前状态是基于过去所有的状态估计得到的。当前状态估计至少会受到前一时刻状态的影响，所以，对此引出两种不同的方法。
1）马尔科夫性：即认为当前时刻状态仅与前一时刻状态有关，与之前时刻的状态无关。此时就可以得到以扩展卡尔曼滤波为代表的滤波器方法
2）当前时刻与之前所有状态都有关：非线性优化方法
7、线性系统的最优无偏估计——卡尔曼滤波：做的工作是“如何把k-1时刻的状态分布推导至k时刻”，即只需要维护一个状态量，对它不断的进行迭代和更新即可。根据贝叶斯定律可以得到下述等式，前一项表示似然函数，后一项表示根据k-1时刻的状态构成的先验

将步骤分为预测和更新两个阶段。其中，预测阶段显示了如何从上一时刻的状态，根据输入信息来推断当前时刻的状态分布

推导过程中使用到的一个小技巧是根据高斯分布的特点，针对指数部分比较其一次项和二次项的系数。
然后注意到，此时的卡尔曼增益，使用方差的先验分布计算得到，然后再使用该增益即可得到方差的后验估计。
8、非线性系统和扩展卡尔曼滤波器：在某点附近考虑运动方程及观测方程的一阶展开，只保留一阶项，即可将非线性系统转化为线性系统。此时给出的是单次线性近似下的最大后验估计。
9、当想在某段时间内估计某个不确定量时，可以尝试用EKF。
10、EKF的局限：
假设了马尔科夫性，当出现回环时，滤波器将会难以处理
在推导过程中，只在固定点上做一次Taylor展开，当具有强烈非线性时就会存在加大的非线性误差。而非线性优化会在迭代中，状态估计发生改变之后，会重新对新的估计点进行Taylor展开
由于EKF将非线性系统转化为线性系统，所以需要存储状态量的均值和方差，并对它们进行维护和更新，当路标数也放进去时存储量会很大。
11、BA：从视觉重建中提炼出最优的3D模型和相机参数。从每一个特征点反射出来的几束光线，在对相机姿态和特征点空间位置做出最优调整之后，最后收束到相机光心的过程。它类似于求解只有观测方程的SLAM问题。目前，BA不仅具有很高的精度，也开始具有良好的实时性，这一点借助于BA的稀疏性
从世界坐标系中的一个点P出发，把相机的内外参数和畸变都考虑进来，最后投影成像坐标。在这里，路标即为这里的三维点P，观测数据则像是像素坐标。如下图所示

当时用非线性优化思想时，需要不断求解增量方程的增量△x

注意，此时的E和F均为整体目标函数对整体变量的导数，将是由很多个小分块构成的大块矩阵，可以使用高斯牛顿法或列文伯格-马夸尔特方法求解，其中使用具体哪一种方法的差别为H是取JTJ还是JTJ+λI。F表示整个代价函数在当前状态下对相机姿态的偏导数，E表示该函数对路标点位置的偏导数

13、矩阵H的稀疏性可以使用图优化方法来实现运算的加速。误差项eij仅描述在位姿i状态下看到路标j这一件事，对其余部分的变量的导数为0.这体现了该误差项与其他路标和轨迹无关的特性。下图表示了图优化结构与增量方程的稀疏性之间的关系

14、Schur消元：将H△x=g变成下述形式，其中B和C都是对角块矩阵，而E是非对角。

所以，需要使用高斯消元，去掉右上角的矩阵E。即可得

如此一来，根据第一行的值，只需要求解C的逆矩阵，可以得到△xc，然后再带入△xc，即可得△xp。这一步也称为边缘化，即做条件概率展开
15、物理意义：S矩阵的非对角线上的非零矩阵快，表示了该出对应的两个相机变量之间存在着共同观测的路标点，称为共视（co-visibility）
16、鲁棒核函数：核函数保证每条边的误差不会大的没边而掩盖掉其他的边，即将原先的二范数度量变成一个光滑的、增长没有那么快的函数，这可以使得整个优化结果更为稳健

第四章 李群与李代数

主要目标

1. 理解李群与李代数的概念，掌握 SO(3), SE(3) 与对应李代数的表示方式。
2. 理解 BCH 近似的意义。
3. 学会在李代数上的扰动模型。
4. 使用 Sophus 对李代数进行运算。
   旋转矩阵自身是带有约束的（正交且行列式为 1）。它们作为优化变量时，会引入额外的约束，使优化变得困难。通过李群—李代数间的转换关系，我们希望把位姿估计变成无约束的优化问题，简化求解方式。
   一、李群与李代数基础
   说三维旋转矩阵构成了特殊正交群 SO(3)，而变换矩阵构成了特殊欧氏群 SE(3)。它们写起来像这样：
   
   1）群
   群（Group）是一种集合加上一种运算的代数结构。我们把集合记作 A，运算记作 ·，那么群可以记作 G = (A, ·)。群要求这个运算满足以下几个条件：
   1.封闭性： ∀a1, a2 ∈ A, a1 · a2 ∈ A.
   2.结合律： ∀a1, a2, a3 ∈ A, (a1 · a2) · a3 = a1 · (a2 · a3).
   3.幺元： ∃a0 ∈ A, s.t. ∀a ∈ A, a0 · a = a · a0 = a.
   4.逆： ∀a ∈ A, ∃a 1 ∈ A, s.t. a · a 1 = a0.
   李群是指具有连续（光滑）性质的群。
   2）李代数的引入