《第10讲 后端1》 读书笔记

本文是《视觉SLAM十四讲》第10讲的个人读书笔记，为防止后期记忆遗忘写的。

本节知识脉络

前面前端的内容，只能求出短时间或者说相邻两帧的转换关系，并建立局部意义的地图。然而当时间延长或者规模变大之后，那么将会出现累积误差。而后端就是要解决这个长时间、大规模建图的问题。

对于问题的解决，后端分为批量和渐进两种解决方法。其中渐进是以卡尔曼滤波为代表，而批量是以非线性优化（BA、图优化等）为代表。

所以，先介绍了卡尔曼的原理（当前状态只和上一时刻有关），并结合概率论的知识，进行推理研算。并从卡尔曼通过泰勒近似推导出了扩展卡尔曼滤波器的知识。

对于BA的的接收，前期已经有所涉及。本讲将深入解释BA的原理以及详细的推导求解过程。

---

前端和后端的作用：

前端视觉里程计能给出一个短时间内的轨迹和地图，但由于不可避免的误 差累积，这个地图在长时间内是不准确的。所以，在视觉里程计的基础上，我们需要后端。后端可以构建一个尺度、规模更大的优化问题，以考虑长时间内的最优轨迹和地图。

10.1 概述

10.1.1 状态估计的概率解释

后端优化的作用

SLAM是由两个方程的实现为线索的，分别是运动方程和观测方程。

每个方程都受噪声影响，假设状态量和噪声项服从高斯分布——意味着在程序中，只需要储存它们的均值和协方差矩阵即可。均值可看作是对变量最优值的估计，而协方差矩阵则度量了它的不确定性。

要注意两个函数的问题：

• 观测方程中，在 一个位置通常只能看到一小部分路标。而且，由于视觉 SLAM 特征点数量众多，所 以实际当中观测方程数量会远远大于运动方程的数量。
• 我们可能没有测量运动的装置，所以也可能没有运动方程。在这个情况下，有若干种 处理方式：认为确实没有运动方程，或假设相机不动，或假设相机匀速运动。这几种 方式都是可行的。在没有运动方程的情况下，整个优化问题就只由许多个观测方程组成。

这两个方程是相辅相成的。少一个，都将会有巨大的影响。我们举个例子来说明两个方程的作用。

只有运动方程时（没有观测方程），相当于我们蒙着眼睛在一个 未知的地方走路。尽管我们知道自己每一步走了多远，但是随着时间增长，我们将对自己 的位置越来越不确定，我们对位 置方差的估计将越来越大。

当我们睁开眼睛时（同时有观测方程和观测），由于能够不断地观测到外部场景，使得位置估计的不确定性变小了。如果用椭圆或椭球直观地表达协方 差阵，当没有观测数据时，这个圆会随着运动越来越大；而如果有正确观测的话，圆就会缩 小至一定的大小，保持稳定。

所以，在后端优化中，就是实现这个行走的过程，使得误差从大椭圆到一定圆的过程。

 

后端优化考虑一个更长时间内（或所有时间内）的状态估计问题。根据用到的数据信息性质不同，处理方式分为批量和渐进。

1. 批量：不仅使用过去的信息更新自己的状态，也会用未来的信息来更新自己。（非线性优化为代表）
2.  渐进：如果当前的状态只由过去的时刻决定，甚至只由前一个时刻决定。（扩展卡尔曼滤波EKF 为代表）

---

后端优化的实现的思路

因为后端是状态估计问题，我们关心的问题就变成了：（1.1）当我拥有某些 运动数据 u 和观测数据 z 时，如何来确定状态量 x, y 的分布？（1.2）进而，如果得到了新来时 刻的数据之后，那么它们的分布又将发生怎样的变化？

--------------加上条件--------------》（条件）假设状态量和噪声项服从高斯分布，

---------那么问题转变为-----------》(2)当存在一些运动数据和观测数据时，我们如何去估计状态量的高斯分布？

 

以下内容参考联系第6讲

后端优化是一个状态估计问题。首先，由于位姿和路标点都是待估计的变量，令 xk 为 k 时刻的所有未知量，它包含了当前时刻的相机位姿与 m 个路标 点。

于是，运动方程与观测方程的形式可写得更加简洁。（这时 x 中已经包含了之前的 y 了）

但是此时运动方程只是描述了前后两帧之间的联系关系。我们希望用过去 0 到 k 中的数据，来估计现在的状态分布。

于是有：             

  按照贝叶斯法则，交换Zk和Xk位置     （似然产生Zk补充了先验）

这里第一项称为似然，第二项称为先验。似然由观测方程给定，而先验部分，我们要明白当前状态 xk 是基于过

去所有的状态估计得来的。至少，它 会受 xk−1 影响。于是按照 xk−1 时刻为条件概率展开：

（后面之于前面的目的在于消去Xk-1）

考虑更久之前的状态，也可以继续对此式进行展开。我们给出了贝叶斯估计，虽然上式还没有具体的概率分布 形式，所以我还没法实际地操作它。对这一步的后续处理，方法上产生了一些分歧。

其一是假设马尔可夫性，简单的一阶马氏性认为，k 时刻状态只 与 k −1 时刻状态有关，而与再之前的无关。如果做出这样的假设，我们就会得到以扩展卡 尔曼滤波（EKF）为代表的滤波器方法。在滤波方法中，我们会从某时刻的状态估计，推导到下一个时刻。（渐进的）

另外一种方法是依然考虑 k 时刻状态与之前所有状态的关系，此时将得 到非线性优化为主体的优化框架。目前视觉 SLAM 主流为非线性优化方法。（批量的）

---

---

---

 

后端优化可以用渐进的方法，也可以用批量的方法。我们先看渐进的方法。

---

 

10.1.2 线性系统（线性高斯系统形式）和 KF（卡尔曼滤波器）

因为                       （公式1）

第一项称为似然由观测方程给定，而第二项先验按照xk−1 时刻为条件概率展开。并再继续讨论，但又分成了两种分歧。

于是，我们先讨论马尔可夫性（当前时刻状态只和上一个时刻有关）。所以上式子第一部分简化为(只和上一个时刻关)：(只由上一时刻的过去状态推断现在时刻的状态）。

第二部分简化为：

也就是说，在程序运行期间，我们只要维护一个状态量，对它由上一状态进行不断地迭代和更新即可。如果假设 状态量服从高斯分布，那我们只需考虑维护状态量的均值和协方差即可。

---

现在，我们来维护这个状态量的均值和协方差，我们假设其以线性高斯系统形式的关系存在，最终推导出卡尔曼滤波器。

  （公式2） 噪声也服从零均值高斯分布     

问题现在归结为：我们知道了 k − 1 时刻的后验（在 k − 1 时刻看来）状态估计：xˆk−1 和它的协方差 Pˆ k−1，现在要根据 k 时 刻的输入和观测数据，确定 xk 的后验分布。。

卡尔曼滤波的第一步：预测

也就是：通过运动方程确定 xk 的先验分布。（公式1）中的  第二部分先验  根据（公式2的第一个方程）（可理解为先前的经验觉得可能关系）。它显示了如何从上一个时刻的状态，根据输入信息（但是有噪声），推断当前时刻的状态分布。这个分布也就是先验。

（公式1）中的  第一部分似然（观测方程）  根据（公式2的第二个方程），我们可以计算在某个状态下，应该产生怎样的 观测数据：       

然而，我们的目的是得到后验公式，也就是（公式1）的左侧表达式：我们假设  也就有

。问题转换为怎么求出X^k和P^k的过程。

卡尔曼滤波的第二步，更新：计算 K（卡尔曼增益），并求出求出X^k和P^k

等式两侧都是高斯分布，那就只需 比较指数部分即可。首先把指数部分展开

比较 xk 的二次和一次系数。

对于二次 系数有：，定义一个中间变量那么上式子变成：----------》

对于一次 系数有：，整理得到：     ----》 

 

至此，我们推导了经典的卡尔曼滤波器的整个过程。   在线性 高斯系统中，卡尔曼滤波器构成了该系统中的最大后验概率估计。而且，由于高斯分布经 过线性变换后仍服从高斯分布，所以整个过程中我们没有进行任何的近似。可以说，卡尔 曼滤波器构成了线性系统的最优无偏估计。

---

10.1.3 非线性系统和 EKF   

SLAM 中的运动方程和观测方程通常 是非线性函数，尤其是视觉 SLAM 中的相机模型。一个高斯分布，经过非线性变换后，往往不再是高斯 分布，所以在非线性系统中，我们必须取一定的近似，将一个非高斯的分布近似成一个高 斯分布。这就是EKF的重要推导思想。

把卡尔曼滤波器的结果拓展到非线性系统中来，称为扩展卡尔曼滤波器EKF。EKF做法：在某个点附近考虑运动方程以及观测方程的 一阶泰勒展开，只保留一阶项，即线性的部分，然后按照线性系统进行推导。

令 k−1 时刻的 均值与协方差矩阵为  在 k 时刻，我们把运动方程和观测方程，在处进行线性化（相当于一阶泰勒展开）。运动方程：

运动方程：

取：       

 

根据，第一步，预测：

通过运动方程确定 xk 的先验分布：

先验和协方差的均值为   

第一部分似然（观测方程）有：         

第二步，计算K，更新：

 卡尔曼增益 Kk：  

在卡尔曼增益的基础上，后验概率的形式为：

卡尔曼滤波器和扩展卡尔曼滤波区别：

卡尔曼滤波器给出了在线性化之后，状态变量分布的变化过程。在线性系统和高斯噪 声下，卡尔曼滤波器给出了无偏最优估计。而在 SLAM 这种非线性的情况下，它给出了单次线性近似下最大后验估计（MAP）。

---

10.1.4 EKF 的讨论

1. 首先，滤波器方法在一定程度上假设了马尔可夫性，也就是 k 时刻的状态只与 k − 1 时刻相关，而与 k − 1 之前的状态和观测都无关。当前帧确实与很 久之前的数据有关（例如回环），那么滤波器就会难以处理这种情况。非线性优化方法则倾向于使用所有的历史数据，称为全体时间上的 SLAM。

2. 与第六章介绍的优化方法相比，EKF 滤波器仅在 xˆk−1 处做了一次线性化，然后就直接根据这次线性化结果，把后验概率给算了出来。在优化问题中，尽管我们也做一阶（最速下降）或二阶（G-N 或 L-M）的近似，但 每迭代一次，状态估计发生改变之后，我们会重新对新的估计点做泰勒展开，而不像 EKF 那样只在固定点上做一次泰勒展开。这就导致优化方法适用范围更广，则在状 态变化较大时亦能适用。

3. 从程序实现上来说，EKF 需要存储状态量的均值和方差，并对它们进行维护和更新。 如果把路标也放进状态的话，由于视觉 SLAM 中路标数量很大，这个存储量是相当可观的。

所以我们通常认为，在同等计算量的情况下，非线性优 化能取得更好的效果。

---

10.2 BA 与图优化

前面接触了BA，特别是第七讲和第9讲，但是都没有深入讲BA的原理和求解过程。这一节，将解决这个问题。

BA的原理

 

                           

Bundle Adjustment的作用：从视觉重建中提炼出最优的 3D 模型和相机参数（内参数和外参数）。形象的理解：从每一个特征点反射出来的几束光线（bundles of light rays），在我们把相机姿态和特征点 空间位置做出最优的调整 (adjustment) 之后，最后收束到相机光心的这个过程 [26]，简称 为 BA。

BA 算法不仅具有很高的精度，也开始具备良好的实时性。实时性上，是源于充分利用了H矩阵的稀疏性进行求解。

回忆观测函数，也就是整个坐标之间的转换和投影过程。

这就是观测函数z = h(x, y).的详细过程。这里的 x 指代此时相机的位姿， 即外参 R, t，它对应的李代数为 ξ。路标 y 即这里的三维点 p，以最小二乘的角度来考虑，那么可以列写关于此次观测的误差：

设 zij 为在 位姿 ξi 处观察路标 pj 产生的数据，那么整体的代价函数（Cost Function）（同时优化李代数 ξ以及观测点p）为：对这个最小二乘进行求解，相当于对位姿和路标同时作了调整，也就是所谓的 BA。

---

BA的求解

根据非线性优化的思想，我们应该从某 个的初始值开始，不断地寻找下降方向 ∆x 来找到目标函数的最优解，即不断地求解增量 方程（6.22）中的增量 ∆x。尽管误差项都是针对单个位姿和路标点的，但在整体 BA 目 标函数上，我们必须把自变量定义成所有待优化的变量：

相应的，增量方程中的 ∆x 则是对整体自变量的增量。在这个意义下，当我们给自变 量一个增量时，目标函数变为：

其中 Fij 表示整个儿代价函数在当前状态下对相机姿态的偏导数（十四讲书中公式 7.41），而 Eij 表示该函数 对路标点位置的偏导（十四讲书中公式 7.46）。

把相机位姿变量放在一起：

把空间点的变量也放在一起：

目标函数可以简化为：

联系第6讲，现在问题不再是对相邻两次观测进行优化了，区别在于对整体内外参数进行优化。该式从一个由很多个小型二次项之和，变成了一个更整体的样子。这 里的雅可比矩阵 E 和 F 必须是整体目标函数对整体变量的导数，它将是一个很大块的矩 阵，而里头每个小分块，需要由每个误差项的导数 Fij 和 Eij“拼凑”起来。然后，无论 我们使用 G-N 还是 L-M 方法，最后都将面对增量线性方程：

其实，最终的问题就变成了对H矩阵的求解。通过对H分解，求出R和t。因为考虑了所有的优化变量，这 个线性方程的维度将非常大，包含了所有的相机位姿和路标点。如果直接对 H 求逆 来计算增量方程，由于矩阵求逆是复杂度为 O(n 3 ) 的操作 ，这是非常消耗计算资源的。

但是很幸运的是，发现BA求解的目标矩阵H具有稀疏性。所以，后续只需要对其进行稀疏化和边缘化的处理，并结合线性代数的知识（消元）来求解出H。

因为具体的数学求解的推导过程稍有难度，书上讲解深入浅出。所以，从10.2.3--10.2.4,最好照书上推算一遍。这里就不赘述（复制）书上的内容。