【视觉SLAM十四讲】第10-11讲 后端

10.1 概述

视觉里程计只有短暂的记忆，如果希望获得长久的效果最优的状态，就需要后端的优化。在后端优化中，我们通常考虑一个更长的时间内的状态估计问题，不仅使用过去的信息，也会使用未来的信息来更新自己，这种方式被称为批量的，与之相对，如果当前的状态只由过去的时刻决定，就称为渐进的。
前面提到过，SLAM的过程可以由运动方程加一个观测方程来确定，假设在t=0到t=N的时间段内，有x0-xN一共N个姿态，并且观测到了y1-yM一共M个路标，那么按照之前的写法，应该有：

对于这两个方程，有下面几点需要注意的地方：
①观测方程中，只有在某个位姿下观测到了某个路标点才会产生观测数据，否则没有数据。
②可能没有测量运动的装置，所以可能没有运动方程，在这种情况下，有多种处理方式：认为确实没有运动方程，或假设相机不动，或假设相机匀速运动。

由于噪声的存在，这里的位姿x和路标点y都不是一个准确的值，应该看成服从某种概率分布的随机变量。现在问题就变成了有一些观测数据z和运动数据u的情况下，如何确定状态量xy的分布。在常见的情况下，我们假设状态量和噪声都服从高斯分布，均值看作是对变量最优值的估计，协方差矩阵则度量了它的不确定性。
运动方程可以看作是利用运动学公式计算出的下一个时刻的位置信息，但是这个信息完全是自己算出来的，而不依赖观测的数据，在实际情况下，只使用运动方程显然是不行的，只有运动方程的情况下（下图左侧部分），由于噪声的存在，每次计算都是在加入了当前时刻误差的情况下计算的新值，所以误差会越来越大，最后偏得离谱。所以我们的做法都是将运动方程与观测方程结合起来（下图右侧部分），彼此互相校正，从而得到更加准确的信息。

下面讨论一下具体的优化问题，由于位姿和路标点都是待估计的变量，所以这里我们改变一下符号，让xk表示k时刻所有的未知量，这里面包含了当前时刻的相机位姿和这个时刻观测到的m个路标点，于是写成：

此外，将k时刻的所有观测记作zk，于是运动方程和观测方程可以简化为下面的形式，在这个形式中y已经不存在了，x已经包含之前的y了：

现在考虑k时刻的情况，我们的目标是用过去0-k的数据，来估计现在的状态分布，即：

按照贝叶斯公式，这个条件概率公式可以转换为先验和似然的乘积：

这个式子中，似然的部分由观测方程给出，而先验部分，是根据过去时刻得到的，于是将先验部分展开：

如果我们考虑更久之前的状态，也可以继续对此式进行展开，但现在我们只关心k时刻和k−1时刻的情况。对这一步的后续处理，方法上出现了分歧，存在若干种选择：其一是假设马尔可夫性（当一个随机过程在给定现在状态及所有过去状态情况下，其未来状态的条件概率分布仅依赖于当前状态；换句话说，在给定现在状态时，它与过去状态是条件独立的，那么此随机过程即具有马尔可夫性质。具有马尔可夫性质的过程通常称之为马尔可夫过程），简单的一阶马氏性认为，k时刻状态只与k−1时刻状态有关，而与再之前的无关。如果做出这样的假设，我们就会得到以扩展卡尔曼滤波（EKF）为代表的滤波器方法。在滤波方法中，我们会从某时刻的状态估计，推导到下一个时刻。另外一种方法是依然考虑k时刻状态与之前所有状态的关系，此时将得到非线性优化为主体的优化框架。非线性优化的基本知识已经在前文介绍过了，目前视觉SLAM 主流为非线性优化方法。

不过这里我们也简单介绍一下卡尔曼滤波器和扩展卡尔曼滤波的原理。这部分十四讲上是用的先验概率去解释的卡尔曼滤波，无人驾驶原理与实践上是用运动公式去解释的，此外还参考了一个知乎帖子，这个帖子是用高斯分布叠加的思路去解释的，卡尔曼滤波有很多种解释思路，但无论哪一种，其核心都是对预测和观测的整合，每次都根据一定比例去将上一时刻对这一时刻的预测和这一时刻的测量值做一个加权求和，将结果作为真实的状态参与下次计算。

先看一下十四讲上的思路。对于一般的卡尔曼滤波，当我们假设了马尔可夫性后，由于当前时刻的状态只与上一个时刻有关，所以10.6式中右侧第一部分可以进一步简化：

由于k时刻状态与k−1之前的无关，所以就简化成只与xk−1和uk有关的形式，与k时刻的运动方程对应。第二部分可简化为：

这是考虑到k时刻的输入量uk与k−1时刻的状态无关，所以我们把uk拿掉。可以看到，这一项实际是k−1时刻的状态分布。于是，这一系列方程说明了，我们实际在做的是“如何把k−1时刻的状态分布推导至k时刻”这样一件事。这意味着我们不需要存储许多状态量，而是对一个状态量不断进行维护和更新，进一步如果假设状态量符合高斯分布，那么就只需要维护状态量的均值和协方差即可。

对于形式最简单的线性高斯系统，我们有下面的式子：

对这个式子，其中xk-1为上一个时刻的状态，当前时刻的状态量是由三部分来的：上一个时刻状态量的转换、这一个时刻的新的输入以及噪声。所谓线性高斯系统，就是在状态量转换的过程中，使用线性转换就可以完成，而不需要复杂的转换。这里其实在无人驾驶原理与实践中讲的更清楚，线性变换就是最简单的运动学公式，而非线性转换对应着复杂的运动学公式。正因为运动的过程描述变复杂了，所以原本的线性变换已经没法表述了，所以必须要用更复杂的非线性转换。

现在利用马尔可夫性，假设知道了k-1时刻的状态和它的协方差，现在要根据k时刻的输入和观测数据去确定k时刻的数据。卡尔曼滤波主要是两步：预测和更新。预测阶段，根据运动方程确定一个预测的状态和协方差，根据高斯分布的性质，应该有：

对上式做如下的简化：

也就是说当前时刻的位置不是一个确定的位置，而是一个位置的分布，这个分布描述了在哪里最可能出现以及偏差情况。而最可能出现的位置即高斯分布的μ，就对应着我们根据运动方程推测出来的位置，偏差情况即高斯分布的б对应的就是根据运动方程计算的协方差。
另一方面，由观测方程，我们可以计算在某个状态下，应该产生怎样的观测数据：

根据贝叶斯公式，我们要求的预测实际上就是一个乘积：

后面的部分就完全看不懂了，概率论学的不好是硬伤，不过勉强能看出来是在解释卡尔曼滤波的更新过程：

其实这里的解释完全不如知乎的帖子解释的清楚：https://zhuanlan.zhihu.com/p/36745755
这个帖子中用两个高斯分布的叠加去解释，我们的预测和测量都存在误差，这使得我们的预测结果和测量结果都不是一个准确值，而应该是一个分布，假设噪声都是高斯分布，那么测量结果和预测结果就都是高斯分布，我们的最终结果，应该是两个结果的融合，不能完全依靠测量也不能完全依靠预测，类似于一个加权求和。而权重不应该是一成不变的，应该根据二者的可靠性来变化，这就设计卡尔曼增益和协方差矩阵，这部分无人驾驶原理与实践上讲的更清楚，卡尔曼滤波可以看作一个置信度，我该相信谁更多一些，而它的计算就要涉及协方差矩阵，协方差矩阵在高斯分布中作为离散程度的代表，显然描述了准确性，所以可以用协方差矩阵计算一个卡尔曼增益，再根据这个增益值，去结合预测值和测量值，同时更新协方差矩阵，用于下一个时刻的计算。

总之，卡尔曼滤波的解释方法很多，无外乎两个步骤：预测和更新。

预测阶段根据上一个时刻的状态，通过运动方程转换到当前的时刻，得到当前时刻的状态的预测值和预测的准确率（协方差），之后计算卡尔曼增益，确定该相信谁更多一些，之后用这个卡尔曼增益去结合测量值去计算真正的状态量和准确率。

前面也提到了，SLAM中的运动方程并没有那么简单，一般都是非线性函数，尤其是视觉SLAM中的相机模型，使用内参模型和李代数，必然不可能是一个线性系统。如果要将卡尔曼滤波的结果扩展到非线性系统中，就变成了扩展卡尔曼滤波。通常的做法是在某个点附近，用运动方程和观测方程的一阶泰勒展开，保留一阶项，然后按照线性系统进行推导。具体的推导就不写了，反正看不懂。

扩展卡尔曼滤波形式简洁、应用广泛。当我们想要在某段时间内估计某个不确定量时，首先想到的就是扩展卡尔曼滤波。但是这种方法也存在不足。首先滤波器假设了马尔可夫性，认为当前时刻的信息只与上一个时刻有关，而忽略了前面的全部关系，在视觉SLAM中，像回环这样的存在显然是不合适的。其次扩展卡尔曼滤波用一阶泰勒展开去计算，显然在一些情况下，一阶泰勒展开是不能近似整个函数的，这种情况下会引入非线性误差。最后，扩展卡尔曼滤波需要存储状态量的均值和方差，并对它们进行维护和更新，如果将路标点也纳入其中的话，存储量很大，因此一般不用在大型场景中。

10.2 BA和图优化

所谓的 Bundle Adjustment（光束法平差），是指从视觉重建中提炼出最优的3D模型和相机参数（内参数和外参数）。从每一个特征点反射出来的几束光线，在我们把相机姿态和特征点空间位置做出最优的调整之后，最后收束到相机光心的这个过程，简称为BA。在以图优化框架的视觉SLAM算法中，BA起到了核心作用。

首先扯一下前面出现过的旧知识：从一个世界坐标系中的点p到像素坐标系的转换。这个过程中，首先是坐标系的转换，将世界坐标系中的点根据相机的外参数（R和t）转换到相机坐标系下，之后将相机坐标系下的点转换到归一化平面，去畸变之后，根据相机模型转换到像素坐标系上，最终得到像素坐标。

不难发现，这个过程实际上就是前面讲的观测方程，之前我们的写法是：

现在我们可以给它一个详细化的参数过程，这里x指的是此时相机的位姿，即外参数R和t，对应李代数为ξ，而路标点y指的就是三维点p，而观测的结果就是像素坐标。因为有误差的噪声的存在，所以我们的观测值和真实值之间存在偏差，也就是对于一个三维点p，我们根据相机外参投影到像素坐标上的结果和实际拍摄得到的像素坐标是存在偏差的，从最小二乘的角度出发，可以写出这次观测的误差：

然后，把其他时刻的观测量也考虑进来，我们可以给误差添加一个下标。设zij为在位姿ξi处观察路标pj产生的数据，那么整体的代价函数为：

将这个问题看作最小二乘问题去求一个最优解，相当于对位姿和路标同时做了优化，也就是所谓的BA优化。

现在BA优化是什么已经明确了，但问题是如何去求解。根据非线性优化的思想，我们应该从某个初始值开始，不断地寻找下降方向，从而得到一个最优解。尽管误差项都是针对单个位姿和路标点的，但在整体BA目标函数上，我们必须把自变量定义成所有待优化的变量：

可以看出这时候优化向量是包含了所有的待优化的量，即相机位姿和空间点的位置。在这个意义下，我们给自变量一个增量时，目标函数就变为：

其中Fij表示整个代价函数在当前状态下对相机位姿的偏导数，而Eij则表示该函数对路标点位置的偏导数，我们将变量放在一起，上式就可以变成：

该式从一个由很多个小型二次项之和，变成了一个更整体的样子。这里的雅可比矩阵E和F必须是整体目标函数对整体变量的导数，它将是一个很大块的矩阵，而里头每个小分块，需要由每个误差项的导数Fij和Eij“拼凑”起来。之后无论使用哪一种求解方法，都会涉及到增量线性方程（具体看第六讲）。
根据第六讲的内容，主要的两种方法GN和LM方法，区别在于H也就是海森矩阵的形式。由于我们把变量归类成了位姿和空间点两种，那么雅可比矩阵就可以拆成：

那么对于高斯牛顿法，海森矩阵就是下面的形式：

两种方法都跑不了计算海森矩阵，因为考虑了所有的优化变量，所以这个线性方程的维度会很大，直接对海森矩阵求逆来计算增量方程，会十分消耗计算资源。好在H有一定的特殊结构，可以利用这个结构去简化计算。

这里我们说的海森矩阵的稀疏性，本质上是由于雅可比矩阵的稀疏性导致的。首先，海森矩阵和雅可比矩阵本身就是矩阵形式下的二阶导数和一阶导数。而对于代价函数中的一个eij，这个误差项只描述了在位姿ξi处观察路标pj的这件事，所以这个误差项相当于只是ξi和pj的事情，和别人无关，所以在求导数的时候，对其余部分的导数应该都是0。也就是说雅可比矩阵应该有下面的形式：

这个雅可比矩阵是用矩阵块的形式去表示的，可见只有对位姿i和路标j的偏导不是零，其余都是全0块，这体现了该误差项与其他路标和
轨迹无关的特性。我们用有颜色的块来代表这个块不是零，那么有下图：

在高斯牛顿法中，我们用JTJ来近似表示海森矩阵，所以在计算海森矩阵时，Jij对H的贡献为JijTJij，对于整体的海森矩阵，应该有：

为了方便后续的讨论，我们将H分块：

这里H11只和相机位姿有关，而H22只和路标点有关。当我们遍历i,j时，以下事实总是成立的：
1、不管i, j怎么变，H11都是对角阵，只在Hi,i处有非零块；
2、同理，H22也是对角阵，只在Hj,j处有非零块；
3、对于H12和H21，它们可能是稀疏的，也可能是稠密的，视具体的观测数据而定。

我们用下面的示意图去进一步解释分块后的海森矩阵：

根据前面的推理，这个例子中的BA目标函数应该是：

其中每个e都表示一个误差函数，那么对于e11而言，其对应的偏导数应该为：

对应色块表示如下：

为了得到该目标函数对应的雅可比矩阵，我们可以将这些Jij按照一定顺序列为向量，那么整体雅可比矩阵以及相应的H矩阵的稀疏情况就是下图中所展示的那样。

不难发现，如果将海森矩阵看做邻接矩阵，这个图中非对角线的部分刚好与原图中的邻接关系是对应的。对于H矩阵当中处于非对角线的矩阵块来说，如果该矩阵块非零，则其位置所对应的变量之间会在图中存在一条边。H矩阵当中的非对角部分的非零矩阵块可以理解为它对应的两个变量之间存在联系，或者可以称之为约束。于是，我们发现图优化结构与增量方程的稀疏性存在着明显的联系。也就是说，分块后的海森矩阵，左上角表示相机位姿，右下角表示路标点，左下和右上对应的才是观测信息的约束。

一般情况下路标点的数目会远大于位姿数目，这会让左上角的部分显得很小，右下角会显得特别大，剩下的两部分会十分混乱，它的形状很像稿子，这里作者将其戏称为镐形矩阵。

仔细观察一下上图，我们不难发现这个矩阵可以分成4个块，和前面的划分一致。左上角为对角块矩阵，每个对角块元素的维度与相机位姿的维度相等，且是一个对角块矩阵。右下角也是对角块矩阵，每个对角块的维度是路标的维度。非对角块的结构与具体观测数据相关。我们首先将这个矩阵划分为四个区域，不难发现，这四个区域正是对应了公式中的四个矩阵块。为了后续分析地方便，我们称这四个块为B，E，C。
海森矩阵划分后，前面的线性方程组也可以改写为：

由于对角矩阵求逆的难度远小于一般矩阵，所以这里做一下的化简：

整理可得：

这个矩阵乘法其实可以拆成两个式子，这两个式子组成一个方程组，其中第一行对应的方程是一个与△xp无关的项，这样可以得到一个只关于位姿的方程：

这个式子中只有一个变量△xc，解出来之后再带入第二行对应的式子，就可以解出来位姿和位置的增量。这个分两部分解方程组的过程被称为边缘化。边缘化后矩阵的稀疏性依然具有原来的物理意义。

在前面的 BA 问题中，我们最小化误差项的二范数平方和，作为目标函数。这种做法虽然很直观，但存在一个严重的问题：如果出于误匹配等原因，某个误差项给的数据是错误的，会发生什么呢？我们把一条原本不应该加到图中的边给加进去了，然而优化算法并不能辨别出这是个错误数据，它会把所有的数据都当作误差来处理。这时，算法会看到一条误差很大的边，它的梯度也很大，意味着调整与它相关的变量会使目标函数下降更多。所以，算法将试图调整这条边所连接的节点的估计值，使它们顺应这条边的无理要求。由于这个边的误差真的很大，往往会抹平了其他正确边的影响，使优化算法专注于调整一个错误的值。

也就是说如果不加处理，出现错得离谱的数据我们不会认为错误，而是会认为这个错误待优化，而牺牲别的数据去给这个大误差最优化。这里使用了一种鲁棒核函数来做处理，这个在vins的论文里也出现过，形式和卷积神经网络里面的核函数类似：

这是一种最普通的鲁棒核函数，使用后效果如下：

可见误差在较小的时候没有太大的影响，而到了很大的时候，鲁棒核函数的用处就显现出来，相当于对误差过大时起了一个抑制作用。

11.1 位姿图

前面提到的BA优化，是对带有相机位姿和空间点的图进行的优化，而在实际情况中，随着时间增加，机器人运动轨迹会越来越长，地图规模也会越来越大，继续采用BA优化的策略，会因为优化数据特别多导致效率下降。实际上在经过若干次测量之后，前面收敛的特征点的空间位置估计会保持在一个值附近不动，所以这时继续对这些点优化，就多少有点费力不讨好。因此我们选择固定特征点，只把他们看做位姿的约束，而不去继续优化他们的位置估计。

按照这个思路，我们的图完全可以进行一定的简化，也就是删掉图中的特征点，只保留图中的位姿。这样我们就可以构建一种叫做位姿图的结构，在这个结构中，节点表示相机的位姿，而边表示的是两个节点之间相对运动的估计。这里一定要理顺清楚，两帧之间的相对运动估计，就是前端用特征点法或者直接法计算出来的相对运动，这里我们认为这个相对运动是比较准确的，实际上确实是相对准确的，而位姿是存在偏差的，主要是误差累积导致的，误差累积导致位姿越来越偏，但是相对运动依然保持在一个比较准确的水平，位姿图优化就是利用相对运动和位姿差来进行的优化。

在没有误差的情况下，边与点的关系应该满足：

但是因为误差的存在，等式不会成立，因此我们设立一个最小二乘问题，优化这个问题，得到一个最优化的相机位姿。即最小化误差：

这是对于一个边的误差，而总体误差应该为：

所有的位姿顶点和位姿——位姿边构成了一个图优化，本质上是一个最小二乘问题，优化变量为各个顶点的位姿，边来自于位姿观测约束，ε为所有边的集合。所以问题就变成了寻找最优的位姿，让所有点与点之间相对运动的值与位姿变换尽可能接近。采用这种方式，让误差被每个边均摊，从而达到一个全局优化的过程。

参考链接1：https://www.zhihu.com/question/291275949
参考链接2：https://blog.51cto.com/u_14411234/3223690