视觉SLAM⑨后端Ⅰ(KF、EKF、非线性优化)

9.0 本章内容

9.1 概述

9.1.1 状态估计的概率解释

9.1.2 线性系统和KF

9.1.3 非线性系统和EKF

9.1.4 EKF的讨论

9.2 BA与图优化

9.2.1 投影模型和BA代价函数

9.2.2 BA的求解

9.2.3 稀疏性和边缘化

9.2.4 鲁棒核函数

9.3 小结

9.0 本章内容

1.理解后端的概念
2.理解以EKF为代表的滤波器后端的工作原理
3.理解非线性优化的后端，明白稀疏性是如何利用的

为啥需要后端里程计？(只要前端里程计不行吗？)(前端里程计有什么局限性呢？)

前端视觉里程计能给出一个短时间内的轨迹和地图，但由于不可避免的误差累积，这个地图在长时间内是不准确的。

所以，在视觉里程计的基础上，我们还希望构建一个尺度、规模更大的优化问题，以考虑长时间内的最优轨迹和地图。

9.1 概述

9.1.1 状态估计的概率解释

1.为什么仅用前端视觉里程计不行

视觉里程计只有短暂的记忆(比如上一章我们讨论二帧模型)，而我们希望整个运动轨迹在较长时间内都能保持最优的状态。

我们会用最新的知识，更新较久远的状态———站在“久远的状态”的角度上看，仿佛是未来的信息告诉它“你应该在哪里”。

2.后端优化中两种处理方式（批量、渐进）

在后端优化中通常考虑一段更长时间内（或所有时间内）的状态估计问题，而且不仅使用过去的信息更新自己的状态，也会用未来的信息来更新，这种处理方式称为“批量的”( Batch )。

否则，如果当前的状态只由过去的时刻决定，甚至只由前一个时刻决定，则称为“渐进的”(Incremental)。

3.深入理解观测方程

我们已经知道SLAM过程可以由运动方程和观测方程来描述。那么假设在 $t=0$ 到 $t=N$ 的时间内，有位姿 $x_{0}$ 到 $x_{N}$ ，并且有路标 $y_{1},y_{2},.....,y_{M}$ 。运动和观测方程为：

$\left\{\begin{matrix} x_{k}=f(x_{k-1},u_{k})+w_{k}\\ z_{k,j}=h(y_{j},x_{k})+v_{k,j} \end{matrix}\right. \ \ \ \ \ k=1,2,.....N;j=1,2,.....,M$

式9-1 观测方程

在观测方程中：

①观测方程中，只有当 $x_{k}$ 看到了 $y_{j}$ 时，才会产生观测数据，否则就没有。事实上，在一个位置通常只能看到一小部分路标。而且，由于视觉SLAM特征点数量众多、所以实际中观测方程的数量会远远大于运动方程。
②可能没有测量运动的装置，也可能没有运动方程。在这个情况下，有若干种处理方式：认为确实没有运动方程，或假设相机不动，或假设相机匀速运动。这几种方式都是可行的。在没有运动方程的情况下，整个优化问题就只由许多个观测方程组成。这就非常类似于SfM问题，相当于我们通过一组图像来恢复运动和结构。不同的是，SLAM中的图像有时间上的先后顺序，而SfM中允许使用完全无关的图像。

4.将问题定性

每个方程都受噪声影响，所以要把这里的位姿 $x$ 和路标 $y$ 看成服从某种概率分布的随机变量，而不是单独的一个数。

因此，我们关心的问题就变成：当我们拥有某些运动数据 $u$ 和观测数据 $z$ 时，如何确定状态量 $x,y$ 的分布？进而，如果得到了新时刻的数据，它们的分布又将发生怎样的变化？

在比较常见且合理的情况下，我们假设状态量和噪声项服从高斯分布——这意味着在程序中只需要存储它们的均值和协方差矩阵即可。均值可看作对变量最优值的估计，而协方差矩阵则度量了它的不确定性。

那么，问题就转变为：当存在一些运动数据和观测数据时，我们如何估计状态量的高斯分布？
只有运动方程时，相当于我们蒙着眼睛在一个未知的地方走路。尽管我们知道自己每一步走了多远，但是随着时间流逝，我们越来越不确定自己的位置——内心也就越不安。这说明当输人数据受噪声影响时，误差是逐渐累积的，我们对位置方差的估计将越来越大。但若能不断地观测到外部场景，就会使得位置估计的不确定性变小。

如果用椭圆或椭球直观地表达协方差阵，那么这个过程有点像是在手机地图软件中走路的感觉。

以下图为例，当没有观测数据时，这个圆会随着运动越来越大；而如果有正确的观测数据，圆就会缩小至一定的大小，保持稳定。

但事先知道参考点是很重要的，在SLAM中强调未知的环境，地图是自己通过视觉建立起来的，因此未知环境的SLAM更麻烦一点。

图9-1不确定性的直观描述左侧:只有运动方程时，由于下一时刻的位姿是在上一
时刻基础上添加了噪声，所以不确定性越来越大。右侧:存在路标点时，不确定性会明显减小，但这只是一个直观的示意图，并非实际数据。

5.将问题定量

Ⅰ.知识回顾：非线性优化我的非线性优化博客

Ⅱ.改进观测方程与运动方程

在第6讲中，介绍了最大似然估计，提到批量状态估计问题可以转化为最大似然估计问题，并使用最小二乘法进行求解。在本节，我们将探讨如何将该结论应用于渐进式问题。同时，在视觉SLAM 里，最小二乘法又有何特殊的结构。

首先，由于位姿和路标点都是待估计的变量，我们改变记号，令 $x_{k}$ 为 $k$ 时刻的所有未知量。它包含了当前时刻的相机位姿与 $m$ 个路标点。在这种记号的意义下(虽然与之前稍有不同，但含义是清楚的)，写成：

$x_{k} \underset{=}{def}{x_{k},y_{1},y_{2},....,y_{m}}$

式9-2 重新定义x

同时，把 $k$ 时刻的所有观测记作 $z$ 。于是，运动方程与观测方程的形式可写得更简洁。这里不会出现 $y$ ，但我们心里要明白这时心中已经包含了之前的 $y$ ：

$\left\{\begin{matrix} x_{k}=f(x_{k-1},u_{k})+w_{k}\\ z_{k,j}=h(x_{k})+v_{k} \end{matrix}\right. \ \ \ \ \ k=1,2,.....N$

式9-3 改进后的运动方程和观测方程

Ⅲ.如何通过过去的状态估计当前状态

现在考虑第 $k$ 时刻的情况。我们希望用过去 $0$ 到 $k$ 中的数据来估计现在的状态分布：

$P(x_{k}|x_{0},u_{1:k},z_{1:k})$

式9-4 用过去的数据来估计现在的状态分布

下标 $0:k$ 表示从0时刻到k时刻的所有数据。请注意， $z_{k}$ 表示所有在 $k$ 时刻的观测数据，它可能不止一个。

同时， $x_{k}$ 实际上和 $x_{k-1},x_{k-2}$ 这些量都有关，但是此式没有显式地将它们写出来。

下面我们来看如何对状态进行估计。按照贝叶斯法则，把 $z_{k}$ 与 $x_{k}$ 交换位置，有：

$P(x_{k}|x_{0},u_{1:k},z_{1:k}) = P(x_{k}|x_{0},u_{1:k},z_{1:k-1},z_{k}) \propto P(z_{k}|x_{k})P(x_{k}|x_{0},u_{1:k},z_{1:k-1})$

式9-5 利用贝叶斯公式将后验概率转化为似然概率与先验概率

第一项称为似然，第二项称为先验。似然由观测方程给定，而先验部分，我们要明白当前状态 $x_{k}$ 是基于过去所有的状态估计得来的。至少，它会受 $x_{k-1}$ 影响，于是以 $x_{k-1}$ 时刻为条件概率展开：

$P(x_{k}|x_{0},u_{1:k},z_{1:k-1})=\int P(x_{k}|x_{k-1},x_{0},u_{1:k},z_{1:k-1}) P(x_{k-1}|x_{0},u_{1:k},z_{1:k-1})dx_{k-1}$

式9-6 先验概率展开

如果考虑更久之前的状态，也可以继续对此式进行展开。（二阶先验、三阶先验..）

但现在我们只关心 $k$ 时刻和 $k-1$ 时刻的情况。至此，我们给出了贝叶斯估计，因为上式还没有具体的概率分布形式，所以没法实际操作它。对这一步的后续处理，方法上产生了一些分歧。

式子分为两部分：k时刻受先前状态的影响与k-1时刻的状态估计

Ⅳ.扩展卡尔曼滤波器与卡尔曼滤波器

大体上讲，存在若干种选择：

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

另一种方法是依然考虑 $k$ 时刻状态与之前所有状态的关系，此时将得到非线性优化为主体的优化框架。

目前，视觉SLAM的主流为非线性优化方法。不过，为了让本书更全面，我们要先介绍卡尔曼滤波器(KF)和 EKF的原理。

附：考研时的全概率公式和贝叶斯公式

$P(B)=\sum_{i=1}^{N}P(A_{i})P(B|A_{i})$

$P(A_{j}|B)= \frac{P(A_{j}B)}{P(B)} = \frac{P(A_{j})P(B|A_{j})}{\sum_{i=1}^{N}P(A_{i})P(B|A_{i})}$

9.1.2 线性系统和KF

1.根据马尔可夫性修改先验概率展开式

先看滤波器模型。当我们假设了马尔可夫性，从数学角度会发生哪些变化呢？

首先，当前时刻状态只和上一个时刻有关，式(9.6)中右侧第一部分可进一步简化：

$P(x_{k}|x_{k-1},x_{0},u_{1:k},z_{1:k-1}) = P(x_{k}|x_{k-1},u_{k})$

式9-7-1 根据马尔代夫性简化先验的条件概率展开的第一部分

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

$P(x_{k-1}|x_{0},u_{1:k-1},z_{1:k-1})$

式9-7-2 根据马尔代夫性简化先验的条件概率展开的第二部分

考虑到 $k$ 时刻的输入量 $u_{k}$ 与 $k-1$ 时刻的状态无关，所以我们把 $u_{k}$ 拿掉。

可以看到，这一项实际上是 $k-1$ 时刻的状态分布。

于是，这一系列方程说明，我们实际在做的是“如何把 $k-1$ 时刻的状态分布推导至 $k$ 时刻”这样一件事。

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

如小萝卜上的定位系统一直在向外输出两个定位信息：一是自己的位姿，二是自己的不确定性。实际中往往也是如此。

2.线性高斯系统到卡尔曼滤波器

①预备知识及推导

Ⅰ.预备知识

①：高斯分布：假设 $x\sim N(u,\Sigma )$ ， $y = Ax+b$ ，则 $y$ 的分布为 $N(Au+b,A\Sigma A^{T})$

②：后验与先验：

先验在拉丁文中指“来自先前的东西”，或稍稍引申指“在经验之前”。近代西方传统中，认为先验指无需经验或先于经验获得的知识。

后验意指“在经验之后”，需要经验。这一区分来自于中世纪逻辑所区分的两种论证，从原因到结果的论证称为“先验的”，而从结果到原因的论证称为“后验的”。

先验概率是指根据以往经验和分析得到的概率，如全概率公式，它往往作为“由因求果”问题中的“因”出现。后验概率是指在得到“结果”的信息后重新修正的概率，是“执果寻因”问题中的“因” 。

后验概率是基于新的信息，修正原来的先验概率后所获得的更接近实际情况的概率估计。先验概率和后验概率是相对的。如果以后还有新的信息引入，更新了现在所谓的后验概率，得到了新的概率值，那么这个新的概率值被称为后验概率。

后验概率是指通过调查或其它方式获取新的附加信息，利用贝叶斯公式对先验概率进行修正，而后得到的概率。先验概率和后验概率的区别：先验概率不是根据有关自然状态的全部资料测定的，而只是利用现有的材料(主要是历史资料)计算的；后验概率使用了有关自然状态更加全面的资料，既有先验概率资料，也有补充资料。

Ⅱ.初始条件

线性高斯系统是指，运动方程和观测方程可以由线性方程来描述：

$\left\{\begin{matrix} x_{k}=A_{k}x_{k-1}+u_{k}+w_{k} \\ z_{k} = C_{k}x_{k}+v_{k} \end{matrix}\right. \ \ \ \ \ k=1,2,...,N$

式9-8 通过线性方程描述运动方程和观测方程

假设所有的状态和噪声均满足高斯分布，记这里的噪声服从零均值高斯分布：

$w_{k}\sim N(0,R_{k})\ \ \ \ \ \ \ v_{k}\sim N(0,Q_{k})$

式9-9 假设噪声服从高斯分布

Ⅲ.要解决的问题

利用马尔可夫性，假设我们知道了 $k-1$ 时刻的后验（在 $k-1$ 时刻看来，得到“结果”的信息后重新修正的概率）状态估计 $\hat{x_{k-1}}$ 及其协方差 $\hat{P_{k-1}}$ ，现在要根据 $k$ 时刻的输入和观测数据，确定 $x_{k}$ 的后验分布。

Ⅳ.推导

为区分推导中的先验和后验，我们在记号上做一点区别：以上帽子 $\hat{x_{k}}$ 表示后验，以下帽子 $\check{x_{k}}$ 表示先验分布。

卡尔曼滤波器的第一步，通过运动方程确定 $x_{k}$ 的先验分布(纯属根据递推式9-8，按上一时刻的后验机器计算出)。这一步是线性的，而高斯分布的线性变换仍是高斯分布[Ⅰ.预备知识]。有：

$P(x_{k-1})=N(\hat{x_{k-1}},\hat{P_{k-1}})$

式9-10 k-1时刻的后验分布--通过k-1时刻的先验和改正（基于新的信息，修正原来的先验概率后所获得的更接近实际情况的概率估计）得到的

$P(x_{k}|x_{0},u_{1:k},z_{1:k-1}) = N(A_{k}\hat{x_{k-1}}+u_{k},A_{k}\hat(P_{k-1})A_{k}^{T}+R)$

式9-11 通过k-1的后验计算出k时刻的先验概率

这一步称为预测(Predict)。它显示了如何从上一个时刻的状态，根据输入信息（但是有噪声 $R$ )推断当前时刻的状态分布。这个分布也就是先验。记：

$\check{x_{k}} = A_{k}\hat{x_{k-1}}+u_{k},\check{P_{k}} =A_{k}\hat(P_{k-1})A_{k}^{T}+R$

式9-12 k时刻的先验表达式

$P(x_{k}|z_{k}) = N(C_{k}x_{k},Q)$

式9-13 k时刻的似然表达式

为了得到后验概率，我们想要计算它们的乘积，也就是由式(9.5)给出的贝叶斯公式。然而，虽然我们知道最后会得到一个关于 $x_{k}$ 的高斯分布，我们先把结果设为 $x_{k}\sim N(\hat x_{k},\hat P_{k})$ ，那么：

$N(\hat x_{k},\hat P_{k}) = \eta N(C_{k}x_{k},Q)\cdot N(\hat x_{k},\hat P_{k})$

式9-14 k时刻的后验分布是似然乘先验

既然我们已经知道等式两侧都是高斯分布，那就只需比较指数部分，无须理会高斯分布前面的因子部分。指数部分很像是一个二次型的配方，我们来推导一下。首先把指数部分展开，有：

$(x_{k}-\hat{x_{k}})^{T}\hat{P_{k}^{-1}}(x_{k}-\hat{x_{k}}) = (z_{k}-C_{k}x_{k})^TQ^{-1}(z_{k}-C_{k}x_{k}) + (x_{k}-\check{x_{k}})^TP_{k}^{-1}(x_{k}-\check{x_{k}})$

(9-15)

为了求左侧的 $\hat{P_{k}}$ 和 $\hat{x_{k}}$ ,我们把两边展开，并比较 $x_{k}$ 的二次和一次系数。对于二次系数，有：

$\hat{P_{k}^{-1}}= C_{k}^{T}Q^{-1}C_{k}+\check{P_{k}^{-1}}$ (9-16)

定义一个中间变量：

$K =\hat{P_{k}}C_{k}^{T}Q^{-1}$ (9-17)

在9-16左右乘以 $\hat{P_{k}}$ ，有：

$\hat{P_{k}}= (I-KC_{k}) \check {P_{k}}$ (9-18)

比较一次项的系数，有： $-2\hat {x_{k}^{T}}\hat{P_{k}^{-1}}x_{k}=-2z_{k}^{T}Q^{-1}C_{k}x_{k}-2\check{x_{k}^{T}}\check{P_{k}^{-1}}x_{k}$ (9-19)

整理(取系数并转置)得：

$P_{k}^{-1}\hat{x_{k}}=C_{k}^{T}Q^{-1}z_{k}+\check {P_{k}^{-1}}\check{x_{k}}$ (9-20)

两侧乘以 $\hat {P_{k}}$ 并代入式(9.17)，得

$\check{x_{k}} = \check{x_{k}}+K(z_{k}-C_{k}\check{x_{k}})$ (9-21)

于是我们又得到了后验均值的表达。

②结论

总而言之，上面的两个步骤可以归纳为“预测”和“更新”(Update)两个步骤：

Ⅰ.预测

$\check{x_{k}} = A_{k}\hat{x_{k-1}}+u_{k},\check{P_{k}} =A_{k}\hat{(P_{k-1})}A_{k}^{T}+R$

Ⅱ.计算卡尔曼增益

$K =\check{P_{k}}C_{k}^{T}(C_{k}\check{P_{k}}C_{k}^{T}+Q_{k})^{-1}$

Ⅲ.更新后验概率的分布

$\hat{x_{k}} = \check{x_{k}}+K(z_{k}-C_{k}\check{x_{k}}) ,\hat{p_{k}} =(I-KC_{k})\check{P_{k}}$

至此，已经推导了经典的卡尔曼滤波器的整个过程。我们使用从概率角度出发的最大后验概率估计的方式。

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

9.1.3 非线性系统和EKF

1.线性滤波器（卡尔曼滤波器）的问题

在理解了卡尔曼滤波之后，我们必须要澄清一点：SLAM中的运动方程和观测方程通常是非线性函数，尤其是视觉SLAM中的相机模型，需要使用相机内参模型及李代数表示的位姿，更不可能是一个线性系统。

一个高斯分布，经过非线性变换后，往往不再是高斯分布。所以在非线性系统中，我们必须取一定的近似，将一个非高斯分布近似成高斯分布。

2.扩展卡尔曼滤波器（非线性）

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

令 $k-1$ 时刻的均值与协方差矩阵为 $\hat{x_{k-1}},\hat{P_{k-1}}$ 。在 $k$ 时刻，我们把运动方程和观测方程在 $\hat{x_{k-1}},\hat{P_{k-1}}$ 处进行线性化(相当于一阶泰勒展开)，有：

$x_{k}\approx f( \hat{x_{k-1}} ,u_{k})+\frac{\vartheta f}{\vartheta x_{k-1}}|_{\hat{x_{k-1}}}(x_{k-1}-\hat{x_{k-1} })+w_{k}$ (9.22)

记这里的偏导数为：

                                                 $F=\frac{\vartheta f}{\vartheta x_{k-1}|_{\hat{x_{k-1}}}$ (9-23)

同样，对于观测方程，亦有：

$z_{k} \approx h(\check{x_{k}})+ \frac{\vartheta h}{\vartheta x_{k}}|_{\check x_{k }} (x_{k}-\check{x_{k}})+n_{k}$ (9-24)

记这里偏导数为：

                                                         $H=\frac{\vartheta h}{\vartheta x_{k}}|_{\check x_{k }}$ (9-25)

那么，在预测步骤中，根据运动方程有：

                 $P(x_{k}|x_{0},u_{1:k},z_{0:k-1}) = N(f(\hat{x_{k-1}},u_{k}),F\hat{P_{k-1}}F^{T}+R_{k})$ (9-26)

这些推导和卡尔曼滤波是十分相似的。为方便表述，记这里的先验和协方差的均值为
                                         $\check{x_{k}}=f(\check{x_{k-1}},u_{k}),\check{P_{k}}=F\hat{P_{k-1}}F^{T}+R_{k}$ (9-27)

然后，考虑在观测中我们有：

$P(z_{k}|x_{k})=N(h(\check{x_{k}})+H(x_{k}-\check{x_{k}}),Q_{k})$ (9-28)

最后，根据最开始的贝叶斯展开式，可以推导出 $x_{k}$ 的后验概率形式。我们略去中间的推导过程，只介绍其结果。读者可以仿照卡尔曼滤波器的方式，推导EKF的预测与更新方程。简而言之，我们会先定义一个卡尔曼增益 $K_{k}$ ：

$K_{k} = \hat{P_{k}}H^{T}(H\hat{P_{k}}H^{T}+Q_{k})^{-1}$

式9-29 卡尔曼增益

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

$\hat{x_{k}} = \check{x_{k}}+K_{k}(z_{k}-h\check{(x_{k})}),\hat{P_{k}}=(I-K_{k}H)\check{P_{k}}$

式9-30 扩展卡尔曼滤波器的后验概率

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

9.1.4 EKF的讨论

EKF形式简洁、应用广泛著称。当想要在某段时间内估计某个不确定量时，我们首先想到的就是EKF。在早期的SLAM中，EKF占据了很长一段时间的主导地位，研究者们讨论了各种各样的滤波器在SLAM中的应用，如IF(信息滤波器)、IKF(Iterated KF)、UKF(Unscented KF)和粒子滤波器、SWF(Sliding Window Filter)等等，或者用分治法等思路改进EKF的效率。时至今日，尽管我们认识到非线性优化比滤波器占有明显的优势，但是在计算资源受限，或待估计量比较简单的场合，EKF仍不失为一种有效的方式。

EKF有哪些局限呢?
1.滤波器方法在一定程度上假设了马尔可夫性，也就是 $k$ 时刻的状态只与 $k-1$ 时刻相关，而与 $k-1$ 之前的状态和观测都无关(或者和前几个有限时刻的状态相关)。这有点像是在视觉里程计中只考虑相邻两帧的关系。如果当前帧确实与很久之前的数据有关(例如回环)，那么滤波器会难以处理。

而非线性优化方法则倾向于使用所有的历史数据。它不光考虑邻近时刻的特征点与轨迹关系，更会把很久之前的状态也考虑进来，称为全体时间上的SLAM(Full-SLAM)。在这种意义下，非线性优化方法使用了更多信息，当然也需要更多的计算。
2.与第6讲介绍的优化方法相比，EKF滤波器仅在 $\hat{x_{k-1}}$ 处做了一次线性化，就直接根据这次线性化的结果，把后验概率给算了出来。这相当于在说，我们认为该点处的线性化近似在后验概率处仍然是有效的。而实际上，当我们离工作点较远时，一阶泰勒展开并不一定能够近似整个函数，这取决于运动模型和观测模型的非线性情况。如果它们有强烈的非线性，那么线性近似就只在很小范围内成立，不能认为在很远的地方仍能用线性来近似。这就是EKF的非线性误差，也是它的主要问题所在。在优化问题中，尽管我们也做一阶(最速下降)或二阶(高斯牛顿法或列文伯格一马夸尔特方法)的近似，但每迭代一次，状态估计发生改变之后，我们会重新对新的估计点做泰勒展开，而不像EKF那样只在固定点上做一次泰勒展开。这就使得优化的方法适用范围更广，在状态变化较大时也能适用。所以大体来说，可以粗略地认为EKF仅是优化中的一次迭代R。

3.从程序实现上来说，EKF需要存储状态量的均值和万差，并对它们进行维护和更新。如
果把路标也放进状态，由于视觉SLAM 中路标数量很大，则这个存储重是相当可观的，且与状态量呈平方增长(因为要存储协方差矩阵)。因此，普遍认为EKF SLAM个适用于大型场景。
4.EKF等滤波器方法没有异常检测机制，导致系统在存在异常值的时候很容易发散。而在视觉SLAM 中，异常值却是很常见的：无论特征匹配还是光流法，都容易追踪或匹配到错误的点。没有异常值检测机制会让系统在实用中非常不稳定

由于EKF存在这些明显的缺点，我们通常认为，在同等计算量的情况下，非线性优化能取得更好的效果。这里“更好”是指精度和鲁棒性同时达到更好的意思。下面我们来讨论以非线性优化为主的后端。

9.2 BA与图优化

所谓的Bundle Adjustment(BA)，是指从视觉图像中提炼出最优的3D模型和相机参数(内参数和外参数)。考虑从任意特征点发射出来的几束光线(bundles of light rays)，它们会在几个相机的成像平面上变成像素或是检测到的特征点。如果我们调整(adjustment)各相机姿态和各特征点的空间位置，使得这些光线最终收束到相机的光心，就称为BA。
我们在第5讲和第7讲已经简单介绍过BA的原理，本节的重点是介绍它对应的图模型结构的特点，然后介绍一些通用的快速求解方法。

9.2.1 投影模型和BA代价函数

首先，我们复习整个投影的过程。从一个世界坐标系中的点 $p$ 出发，把相机的内外参数和畸变都考虑进来，最后投影成像素坐标，需要如下步骤：

Ⅰ.把世界坐标转换到相机坐标，这里将用到相机外参数 $(R,t)$ ：

$P'=Rp+t=[X',Y',Z']^{T}$

Ⅱ.将 $P'$ 投至归一化平面，得到归一化坐标：

$P_{c}=[u_{c},v_{c},1]^{T}=[X'/Z',Y'/Z',1]^{T}$

Ⅲ.考虑归一化坐标的畸变情况，得到去畸变前的原始像素坐标。这里暂时只考虑径向畸变：

$\left\{\begin{matrix} u_{c}' = u_{c}(1+k_{1}r_{c}^{2}+k_{2}r_{c}^{4}) \\ v_{c}' = v_{c}(1+k_{1}r_{c}^{2}+k_{2}r_{c}^{4}) \end{matrix}\right.$

Ⅳ.根据内参模型，计算像素坐标：

$\left\{\begin{matrix} u_{s}=f_{x}u_{c}'+c_{x} \\ v_{s}=f_{y}v_{c}'+c_{y} \end{matrix}\right.$

这一系列计算流程看似复杂。我们用流程图9-3表示整个过程。读者应该能领会到，这个过程也就是前面讲的观测方程，之前我们把它抽象地记成：

$z=h(x,y)$

图9-3 计算流程示意图

现在，我们给出了它的详细参数化过程。具体地说，这里的 $x$ 指代此时相机的位姿，即外参 $R,t$ ，它对应的李群为 $T$ ，李代数为 $\xi$ 。路标 $y$ 即这里的三维点 $p$ ，而观测数据则是像素坐标 $z \ \ \underline{def}\ \ \ [u_s,v_s]^T$ 。以最小二乘的角度来考虑，那么可以列写关于此次观测的误差：

$e=z-h(T,p)$

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

$\frac{1}{2}\sum_{i=1}^{m}\sum_{j=1}^{n}||e_{ij}||^2 = \frac{1}{2}\sum_{i=1}^{m}\sum_{j=1}^{n}||z_{ij}-h(T_{i},p_{j}))||^2$

对这个最小二乘进行求解，相当于对位姿和路标同时做了调整，也就是所谓的BA。接下来，我们会根据该目标函数和第6讲介绍的非线性优化内容，逐步深入地探讨该模型的求解。

9.2.2 BA的求解

1.回顾高斯牛顿法

高斯牛顿法是最优化算法中最简单的方法之一。它的思想是将 $f(x)$ 进行一阶的泰勒展开。请注意这里不是目标函数 $F(x)$ 而是 $f(x)$ ，否则就变成牛顿法了。

$f(x+\Delta x) \approx f(x)+J(x)^{T}\Delta x$

这里 $J(x)^{T}$ 为 $f(x)$ 关于 $x$ 的导数，为 $n\times 1$ 的列向量。根据前面的框架，当前的目标是寻找增量 $\Delta x$ ，使得 $||f(x+\Delta x)||^{2}$ 达到最小。为了求 $\Delta x$ ，我们需要解一个线性的最小二乘问题：

$\Delta x^{*} = arg \ \underset{ \Delta x}{\min}\frac{1}{2}\begin{Vmatrix} f(x)+J(x)^{T}\Delta x \end{Vmatrix}^{2}$

这个方程与之前的有什么不一样呢？根据极值条件，将上述目标函数对 $\Delta x$ 求导，并令导数为零。为此，先展开目标函数的平方项：

求上式关于 $\Delta x$ 的导数,并令其为零：

可以得到以下方程组：

                                 $\underbrace{J(x)J(x)^{T}}\Delta x=\underbrace{-J(x)f(x)}$

$H(x)$                        $g(x)$

这个方程是关于变量 $\Delta x$ 的线性方程组,我们称它为增量方程，也可以称为高斯牛顿方程
(Gauss-Newton equation)或者正规方程(Normal equation)。我们把左边的系数定义为 $H$ 、右边定义为 $g$ 、那么上式变为

                                                          $H\Delta x=g$

这里把左侧记作 $H$ 是有意义的。对比牛顿法可见，高斯牛顿法用 $JJ^{T}$ 作为牛顿法中二阶Hessian矩阵的近似，从而省略了计算H的过程。求解增量方程是整个优化问题的核心所在。如果我们能够顺利解出该方程，那么高斯牛顿法的算法步骤可以写成：

        从算法步骤中可以看到，增量方程的求解占据着主要地位。只要我们能够顺利解出增量，就能保证目标函数能够正确地下降。
        为了求解增量方程，我们需要求解 $H^{-1}$ ，这需要 $H$ 矩阵可逆，但实际数据中计算得到
$JJ^{T}$ 却只有半正定性。（为什么要算 $H^{-1}$ ， $\Delta x_{k}=H^{-1}g$ ）

也就是说，在使用高斯牛顿法时，可能出现 $JJ^{T}$ 为奇异矩阵或者病态（ill-condition）的情况，此时增量的稳定性较差，导致算法不收敛。直观地说，原函数在这个点的局部近似不像一个二次函数。

更严重的是，就算我们假设 $H$ 非奇异也非病态，如果我们求出来的步长 $\Delta x$ 太大，也会导致我们采用的局部近似式不够准确，这样一来我们甚至无法保证它的迭代收敛，哪怕是让目标函数变得更大都是有可能的。
        尽管高斯牛顿法有这些缺点，但它依然算是非线性优化方面一种简单有效的方法，值得我们学习。在非线性优化领域，相当多的算法都可以归结为高斯牛顿法的变种。这些算法都借助了高斯牛顿法的思想并且通过自己的改进修正其缺点。例如，一些线搜索方法 (Line Search Method )，加入了一个步长 $\alpha$ ，在确定了 $\Delta x$ 后进一步找到 $\alpha$ 使得 $\begin{Vmatrix} f(x+\alpha \Delta x) \end{Vmatrix}^{2}$ 达到最小，而不是简单地令 $\alpha =1$ 。

2.回顾最小化重投影误差求解PnP

除了使用线性方法，我们还可以把PnP问题构建成一个关于重投影误差的非线性最小二乘问题。

线性方法，往往是先求相机位姿，再求空间点位置，而非线性优化则是把它们都看成优化变量，放在一起优化。

这是一种非常通用的求解方式，我们可以用它对PnP或ICP给出的结果进行优化。这一类把相机和三维点放在一起进行最小化的问题，统称为Bundle Adjustment。

我们完全可以在 PnP中构建一个Bundle Adjustment问题对相机位姿进行优化。如果相机是连续运动的(比如大多数SLAM过程)，也可以直接用BA求解相机位姿。我们将在本节给出此问题在两个视图下的基本形式，然后在第9讲讨论较大规模的BA问题。
        考虑 $n$ 个三维空间点 $P$ 及其投影 $p$ ，我们希望计算相机的位姿 $R,t$ ，它的李群表示为 $T$ 假设某空间点坐标为 $P_{i}= [X_{i}, Y_{i},Z_{i}]^T$ ，其投影的像素坐标为 $\mathbf{u_{i}}=[u_{i},v_{i}]^T$ 。根据第5讲的内容，像素位置与空间点位置的关系如下：

$s_{i}\begin{bmatrix} u_{i}\\v_{i} \\ 1 \end{bmatrix} =KT\begin{bmatrix} X_{i}\\ Y_{i} \\ Z_{i} \\ 1 \end{bmatrix}$

写成矩阵形式是： $s_{i}\mathbf{u_{i}}=KTP_{i}$

这个式子隐含了一次从齐次坐标到非齐次的转换，否则按矩阵的乘法来说，维度是不对的。现在，由于相机位姿未知及观测点的噪声，该等式存在一个误差。因此，我们把误差求和，构建最小二乘问题，然后寻找最好的相机位姿，使它最小化：

$\xi^{\ast } = arg \ \underset{\xi}{min } \frac{1}{2 }\sum_{i=1}^{n}||u_{i}-\frac{1}{s_{i}}Kexp(\xi^{\wedge })P_{i}||_{2}^{2}$

该问题的误差项，是将3D点的投影位置与观测位置作差，所以称为重投影误差。使用齐次坐标时，这个误差有3维。不过，由于 $u$ 最后一维为1，该维度的误差一直为零，因而我们更多时候使用非齐次坐标，于是误差就只有2维了。

如下图所示，我们通过特征匹配知道了 $p_{1}$ 和 $p_{2}$ 是同一个空间点 $P$ 的投影，但是不知道相机的位姿。在初始值中， $P$ 的投影 $\hat{p_{2}}$ 与实际的 $p_{2}$ 之间有一定的距离。于是我们调整相机的位姿，使得这个距离变小。不过，由于这个调整需要考虑很多个点，所以最后的效果是整体误差的缩小，而每个点的误差通常都不会精确为零。

使用李代数，可以构建无约束的优化问题，很方便地通过高斯牛顿法、列文伯格—马夸尔特方法等优化算法进行求解。不过，在使用高斯牛顿法和列文伯格一马夸尔特方法之前，我们需要知道每个误差项关于优化变量的导数，也就是线性化：

$e(x+\Delta x)\approx e(x)+J^{T}\Delta x$

这里的 $J^{T}$ 的形式是值得讨论甚至可以说是关键所在。

(通过高斯牛顿法去解非线性优化问题主要是解决 $J(x)J(x)^{T}\Delta x=-J(x)f(x)$ 中的 $\Delta x$ )

我们固然可以使用数值导数，但如果能够推导出解析形式，则优先考虑解析导数。现在，当 $e$ 为像素坐标误差(2维)， $x$ 为相机位姿(6维)时， $J^{T}$ 将是一个2×6的矩阵。我们来推导 $J^{T}$ 的形式。

回忆李代数的内容，我们介绍了如何使用扰动模型来求李代数的导数。首先，记变换到相机坐标系下的空间点坐标为 $P'$ ，并且将其前3维取出来： $P'=(TP)_{1:3}=[X',Y',Z']^T$

那么，相机投影模型相对于 $P'$ 为：   $su=KP'$

展开：

利用第3行消去 $s$ (实际上就是 $P'$ 的距离)，得

这与第5讲的相机模型是一致的。当我们求误差时，可以把这里的 $u,v$ 与实际的测量值比较，求差。在定义了中间变量后，我们对 $T$ 左乘扰动量 $\delta \xi$ ，然后考虑 $e$ 的变化关于扰动量的导数。利用链式法则，可以列写如下：

这里的 $\oplus$ 指李代数上的左乘扰动。第一项是误差关于投影点的导数：

而第二项为变换后的点关于李代数的导数，根据4.3.5节中的推导，得

而在 $P'$ 的定义中，我们取出了前3维

将这两项相乘,就得到了2×6的雅可比矩阵：

这个雅可比矩阵描述了重投影误差关于相机位姿李代数的一阶变化关系。我们保留了前面的负号，这是因为误差是由观测值减预测值定义的。当然也可以反过来将它定义成“预测值减观测值”的形式。在那种情况下，只要去掉前面的负号即可。此外，如果 $\textbf{se}(3)$ 的定义方式是旋转在前，平移在后，则只要把这个矩阵的前3列与后3列对调即可。
        除了优化位姿，我们还希望优化特征点的空间位置。因此，需要讨论 $e$ 关于空间点 $P$ 的导数。。仍利用链式法则，有

$\frac{\vartheta e }{\vartheta P }=\frac{\vartheta e }{\vartheta P' }\frac{\vartheta P' }{\vartheta P }$

第一项在前面已推导，关于第二项，按照定义 $P'=(TP)_{1:3} =RP+t$

我们发现 $P'$ 对 $P$ 求导后将只剩下 $R$ 。于是：

于是，我们推导出了观测相机方程关于相机位姿与特征点的两个导数矩阵。它们十分重要，能够在优化过程中提供重要的梯度方向，指导优化的迭代。

3. 利用非线性方法优化

①利用高斯牛顿法解决矩阵H的运算

观察9.2.1节中的观测模型 $h(T, p)$ ，很容易判断该函数不是线性函数。所以我们希望使用6.2节介绍的一些非线性优化手段来优化它。

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

$x = [T_{1},T_{2},...,T_{m},p_{1},p_{2},...,p_{n}]$

式9-31 将待优化的变量记为x

本节用高斯牛顿法进行优化，高斯牛顿法的核心就是求解 $J$ 矩阵，即代价函数对每个待优化变量求偏导数。

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

$\frac{1}{2}||f(x+\Delta x)||^{2} \approx \frac{1}{2} \sum_{i=1}^{m} \cdot \sum_{j=1}^{n} ||e_{ij}+F_{ij}\Delta \xi _{i}+E_{ij}\Delta p_{i}||^2$

式9-32 用高斯牛顿法解决非线性优化问题

其中， $F$ 表示整个代价函数在当前状态下对相机姿态的偏导数，而 $E_{ij}$ 表示该函数对路标点位置的偏导。现在，把相机位姿变量放在一起：

$x_{c} = [\xi_{1},\xi_{2},...,\xi_{m}]^T \ \epsilon \ \ R^{6m}$

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

$x_{p} = [p_{1},p_{2},...,p_{n}]^T \ \epsilon \ \ R^{3n}$

那么，上式可简化为：

$\frac{1}{2}||f(x+\Delta x)||^{2} \approx \frac{1}{2} \sum_{i=1}^{m} \cdot \sum_{j=1}^{n} ||e+F\Delta x_{c}+E\Delta x_{p}||^2$

式9-33 用高斯牛顿法解决非线性优化问题

需要注意的是，该式从一个由很多个小型二次项之和，变成矩阵形式。这里的雅可比矩阵 $E$ 和 $F$ 必须是整体目标函数对整体变量的导数，它将是一个很大块的矩阵，而里头每个小分块，需要由每个误差项的导数 $F_{ij}$ 和 $E_{ij}$ 拼凑”起来。然后，无论我们使用高斯牛顿法还是列文伯格一马夸尔特方法，最后都将面对增量线性方程：

$H\Delta x=g$

根据第6讲的知识，我们知道高斯牛顿法和列文伯格—马夸尔特方法的主要差别在于，这里的 $H$ 是取 $J^TJ$ 还是 $J^TJ + \lambda I$ 的形式。由于我们把变量归类成了位姿和空间点两种，所以雅可比矩阵可以分块为：

$J=[F\ E]$

那么，以高斯牛顿法为例，则 $H$ 矩阵为：

$H = J^{T}J=\begin{bmatrix} F^TF & F^TE\\ E^TF & E^TE \end{bmatrix}$

式9-34 高斯牛顿法的H矩阵，是不是维数太大了？求逆很困难。

当然，在列文伯格一马夸尔特方法中我们也需要计算这个矩阵。不难发现，因为考虑了所有的优化变量，所以这个线性方程的维度将非常大，包含了所有的相机位姿和路标点。尤其是在视觉SLAM中，一幅图像会提出数百个特征点，大大增加了这个线性方程的规模。如果直接对H求逆来计算增量方程，由于矩阵求逆是复杂度为 $O(n^3)$ 的操作，那么消耗的计算资源会非常多。但这里的 $H$ 矩阵是有一定的特殊结构的。利用这个特殊结构，我们可以加速求解过程。

②H矩阵存在的问题

Ⅰ. 用图的知识来描述这个最小二乘问题

图9-4 利用图论的知识解释为什么不对H进行优化会导致H求逆很困难

相邻的x之间会有一个运动方程描述，构成运动误差。同时每个 $x_{i}$ 又看到一部分路标 $p_{j}$ ，通过重投影过程最小化重投影误差。

Ⅱ.特点

每个观测只关系两个变量，其中一个是相机（ $v_{1},v_{2},v_{3},v_{4}$ ），一个是路标（ $v_{5},v_{6}$ ）
纯视觉BA中，不存在相机与相机/路标与路标之间的关联 $(v_{1},v_{1})(v_{5},v_{5})$
整个误差函数由许多个这样小的项组成：考虑在位姿 $i$ 处对路标 $j$ 的一次观测，误差为 $e_{ij} = z_{ij}-h(x_{i},y_{j})$
Ⅲ.量化问题

计算 $H\Delta x = -b$ ，因此要计算 $H^{-1}$ ，H可能比较大，这里的 $H$ 可以看成协方差 $P^{-1}$

图9-5 x_{i}处对y_{j}处进行观测

其矩阵计算如下式： $H = \sum_{ij}^{}J_{ij}^TJ_{ij}$

对每一个 $x_{i}$ 观测到 $y_{j}$ 路标点进行处理，形成四处矩阵块，具有特殊结构，求逆十分复杂，于是引出了 $H$ 矩阵的稀疏性和边缘化问题。

9.2.3 稀疏性和边缘化

1.雅可比矩阵的形式

$H$ 矩阵的稀疏性是由雅可比矩阵 $J(x)$ 引起的。考虑这些代价函数当中的其中一个 $e_{ij}$ 。注意，这个误差项只描述了在 $T_{i}$ 看到 $P_{j}$ 这件事，只涉及第 $i$ 个相机位姿和第 $j$ 个路标点，对其余部分的变量的导数都为0。所以该误差项对应的雅可比矩阵有下面的形式：

$J_{ij}(x) = [\frac{\vartheta e}{\vartheta x_{j}},\frac{\vartheta e}{\vartheta p_{i}}]$

式9-35 不去考虑运动模型，每个观测方程关联p_j和x_i，其对大雅可比的贡献

$J_{ij}(x) = (0_{2\ast 6},0_{2\ast 6},\frac{\vartheta e_{ij}}{\vartheta T_{i}},0_{2\ast 6},....,0_{2\ast 3},...,0_{2\ast 3},\frac{\vartheta e_{ij}}{\vartheta p_{j}},0_{2\ast 3},...,0_{2\ast 3})$

式9-36 e对于其余变量求导都为0，只存在两个非零项

这个误差项的雅可比矩阵，除了这两处为非零块，其余地方都为零。这体现了该误差项与其他路标和轨迹无关的特性。从图优化角度来说，这条观测边只和两个顶点有关。那么，它对增量方程有何影响？ $H$ 矩阵为什么会产生稀疏性呢？

以图9-5为例，我们设 $J_{ij}$ 只在 $i,j$ 处有非零块，那么它对 $H$ 的贡献为 $J_{ij}^TJ_{ij}$ ，具有图上所画的稀疏形式。这个 $J_{ij}^TJ_{ij}$ 矩阵也仅有4个非零块，位于 $(i,i),(i.j),(j,i),(j,j)$ 。对于整体的 $H$ ，有：

$H = \sum_{ij}^{}J_{ij}^TJ_{ij}$

式9-37 整体雅可比矩阵的求法

2.雅可比矩阵分块并了解其性质

请注意， $i$ 在所有相机位姿中取值， $j$ 在所有路标点中取值。我们把 $H$ 进行分块：

$H = \begin{bmatrix} H_{11} & H_{12} \\ H_{21} & H_{22} \end{bmatrix} = H \begin{bmatrix} B & E \\E^T & C\end{bmatrix}$

式9-38 将整体雅可比矩阵分块

这里， $H_{11}$ 只和相机位姿有关而 $H_{22}$ 只和路标点有关。当我们遍历 $i,j$ 时，以下事实总是成立的：
1.不管 $i,j$ 怎么变， $H_{11}$ 都是对角阵，只在 $H_{i,i}$ 处有非零块。

（ $x_{i}$ 对于某个 $y_{j}$ 观测形成的雅可比矩阵 $J_{ij}$ 进行 $J^TJ$ 运算得到 $H_{ij}$ 矩阵会产生一个坐标 $(i,i)$ 处的值）

2.同理， $H_{22}$ 也是对角阵，只在 $H_{j,j}$ 处有非零块。

（ $x_{i}$ 对于某个 $y_{j}$ 观测形成的雅可比矩阵 $J_{ij}$ 进行 $J^TJ$ 运算得到 $H_{ij}$ 矩阵会产生一个坐标 $(j,j)$ 处的值）
3.对于 $H_{12}$ 和 $H_{21}$ ，它们可能是稀疏的，也可能是稠密的，视具体的观测数据而定。

这表明了 $H$ 的稀疏结构。之后对线性方程的求解中，也正需要利用它的稀疏结构。

举一个实例来直观地说明它的情况。假设一个场景内有2个相机位姿 $(C_{1},C_2)$ 和6个路标点 $(P_{1},P_{2},P_{3},...,P_{6})$ 。这些相机和点云所对应的变量为 $T_{i},i=1,2$ 及 $p_{j},j=1,2,...,6$ 。相机 $C_{1}$ 观测到路标点 $P_{1},P_{2},P_{3},P_{4}$ ，相机 $C_{2}$ 观测到路标点 $P_{5},P_{6},P_{7},P_{8}$ 。我们把这个过程画成示意图，如图9-6所示。相机和路标以圆形节点表示。如果 $i$ 相机能够观测到 $j$ 点，我们就在它们对应的节点连上一条边。

图9-6 点和边组成的示意图。该图显示相机C1观测到了路标点P1,P2,P3,P4，相机C2看到了路标点P3,P4,P5,P6

可以推出，场景下的BA目标函数应该为：

$\frac{1}{2}(||e_{11}||^{2}+||e_{12}||^{2}+||e_{13}||^{2}+||e_{14}||^{2}+||e_{23}||^{2}+||e_{24}||^{2}+||e_{25}||^{2}+||e_{26}||^{2})$

式9-39 BA表达式

这里的 $e_{ij}$ 使用之前定义过的代价函数，即：

$\frac{1}{2}\sum_{i=1}^{m}\sum_{j=1}^{n}||e_{ij}||^2 = \frac{1}{2}\sum_{i=1}^{m}\sum_{j=1}^{n}||z_{ij}-h(T_{i},p_{j}))||^2$

式9-40 BA表达式的e_ij

以 $e_{11}$ 为例，它描述了在 $C_{1}$ 看到了 $P_{1}$ 这件事，与其他的相机位姿和路标无关。令 $J_{11}$ 为 $e_{11}$ 所对应的雅可比矩阵，不难看出 $e_{11}$ 对相机变量 $\xi _{2}$ 和路标点 $p_{2},...p_{6}$ 的偏导数都为0。我们把所有变量 $x= {\xi_{1},\xi_{2},p_{1},...p_{2}}$ 的顺序摆放，则有：

$J_{11} = \frac{\vartheta e_{11}}{\vartheta x}= (\frac{\vartheta e_{11}}{\vartheta \xi _{1}},0_{2*6},\frac{\vartheta e_{11}}{\vartheta p_{1}},0_{2*3},0_{2*3},0_{2*3},0_{2*3},0_{2*3})$

式9-41 C1对p1进行观测得到的雅可比矩阵

为了方便表示稀疏性，我们用带有颜色的方块表示矩阵在该方块内有数值，其余没有颜色的区域表示矩阵在该处数值都为0。那么上面的 $J_{11}$ 则可以表示成图9-7所示的图案。

图9-7 J11矩阵的非零块分布图。上方的标记表示矩阵该列所对应的变量。由于相机参数维数比点云参数维数大，所以C对应的矩阵块要比P对应的矩阵块宽

为了得到该目标函数对应的雅可比矩阵，我们可以将这些 $J_{ij}$ 按照一定顺序列为向量，那么整体雅可比矩阵及相应的 $H$ 矩阵的稀疏情况就如图9-8所示。

图9-8 雅可比矩阵的稀疏性（左）和H矩阵的稀疏性（右），填色的方块表示矩阵在对应的矩阵块处有数值，其余没有颜色的部分表示矩阵在该处的数值始终为0

图9-6右对应的邻接矩阵（Adjacency Matrix）和图9-8中的 $H$ 矩阵，除了对角元素外的其余部分有着完全一致的结构。事实上的确如此。上面的 $H$ 矩阵一共有8×8个矩阵块，对于 $H$ 矩阵中处于非对角线的矩阵块来说，如果该矩阵块非零，则其位置所对应的变量之间在图中会存在一条边，我们可以从图9-6中清晰地看到这一点。所以， $H$ 矩阵中的非对角部分的非零矩阵块可以理解为其对应的两个变量之间存在联系，或者可以称之为约束。于是，我们发现图优化结构与增量方程的稀疏性存在着明显的联系。

现在考虑更一般的情况，假如我们有m个相机位姿，n个路标点。
        由于通常路标的数量远远多于相机，于是有 $n \gg m$ 。由上面的推理可知，一般情况下的H矩阵如图9-9所示。它的左上角块显得非常小，而右下角的对角块占据了大量地方。除此之外，非对角部分则分布着散乱的观测数据。由于它的形状很像箭头，又称为箭头形（Arrow-like）矩阵

图9-9 一般形式的H矩阵

对于具有这种稀疏结构的 $H$ ，线性方程 $H\Delta x = g$ 的求解会有什么不同呢？现实当中存在着若干种利用 $H$ 的稀疏性加速计算的方法，本节介绍视觉SLAM里一种最常用的手段：Schur消元。在SLAM研究中也称为Marginalization(边缘化)。
        仔细观察图9-9，我们不难发现这个矩阵可以分成4个块，和式9-38一致。左上角为对角块矩阵，每个对角块元素的维度与相机位姿的维度相同，且是一个对角块矩阵。右下角也是对角块矩阵，每个对角块的维度是路标的维度。非对角块的结构与具体观测数据相关。我们首先将这个矩阵按照图9-9所示的方式做区域划分，读者不难发现，这4个区域正好对应了公式(9.50) 中的4个矩阵块。为了后续分析方便，我们记这4个块为 $B,E,E^{T},C$ 。

于是，对应的线性方程组也可以由 $H\Delta x=g$ 变为如下形式：

$\begin{bmatrix} B & E \\E^T & C\end{bmatrix} \begin{bmatrix} \Delta x_{c}\\ \Delta x_{p} \end{bmatrix} = \begin{bmatrix} v\\ w \end{bmatrix}$

式9-42 高斯牛顿法解决最小而成问题的解

其中 $B$ 是对角块矩阵，每个对角块的维度和相机参数的维度相同，对角块的个数是相机变量的个数。由于路标数量会远远大于相机变量个数，所以 $C$ 往往也远大于 $B$ 。三维空间中每个路标点为三维，于是 $C$ 矩阵为对角块矩阵，每个块为3×3矩阵。

对角块矩阵求逆的难度远小于对一般矩阵的求逆难度，因为我们只需要对那些对角线矩阵块分别求逆即可。考虑到这个特性，我们对线性方程组进行高斯消元，目标是消去右上角的非对角部分 $E$ （三角消元），得：

$\begin{bmatrix} I & -EC^{-1}\\ 0&I \end{bmatrix}\begin{bmatrix} B & E \\E^T & C\end{bmatrix} \begin{bmatrix} \Delta x_{c}\\ \Delta x_{p} \end{bmatrix} =\begin{bmatrix} I &-EC^{-1} \\ 0& I \end{bmatrix} \begin{bmatrix} v\\w \end{bmatrix}$

式9-42-1 对高斯牛顿法解决最小而成问题的解进行高斯消元

整理，得：

$\begin{bmatrix} B-EC^{-1}E^{T} & 0\\ E^{T} & C \end{bmatrix} \begin{bmatrix} \Delta x_{c}\\ \Delta x_{p} \end{bmatrix}= \begin{bmatrix} v-EC^{-1}w\\ w \end{bmatrix}$

式9-42-2 对高斯牛顿法解决最小而成问题的解进行高斯消元

消元之后，方程组第一行变成和 $\Delta x_{p}$ 无关的项。单独把它拿出来，得到关于位姿部分的增量方程：

$[B-EC^{-1}E^{T} ]\Delta x_{c}=v-EC^{-1}w$

式9-42-3 式9-42-2的第一行

然后把解得的 $\Delta x_{c}$ 代这个线性方程的维度和 $B$ 矩阵一样。我们的做法是先求解这个方程
入原方程，求解 $\Delta x_{p}$ 。这个过程称为Marginalization，或者Schur消元（Schur Elimination）。相比于直接解线性方程的做法，它的优势在于：

Ⅰ.在消元过程中，由于 $C$ 是对角块，因此 $C^{-1}$ 容易解出。

Ⅱ.求解了 $\Delta x_{c}$ 之后，路标部分的增量方程由 $\Delta x_{p} = C^{-1}(w-E^{T}\Delta x_{c})$ 给出。

于是，边缘化的主要计算量在于求解式9-42-3。我们将此方程的系数记为 $S$ ，它的稀疏性如何呢？图9-10显示了对H矩阵进行Schur消元后的一个 $S$ 实例，可以看到它的稀疏性是不规则的。

图9-10 对H矩阵进行Schur消元后的S矩阵的稀疏状态

前面说到， $H$ 矩阵的非对角块处的非零元素对应着相机和路标的关联。那么，进行了Schur消元后 $S$ 的稀疏性是否具有物理意义呢？

$S$ 矩阵的非对角线上的非零矩阵块，表示了该处对应的两个相机变量之间存在着共同观测的路标点，有时称为共视（Co-visibility）。反之，如果该块为零，则表示这两个相机没有共同观测。例如，图9-10所示的稀疏矩阵，左上角前4×4个矩阵块可以表示对应的相机变量 $C_{1},C_{2},C_{3},C_{4}$ 之间有共同观测。

于是， $S$ 矩阵的稀疏性结构当取决于实际观测的结果，我们无法提前预知。在实践中，例如ORB-SLAM中的Local Mapping 环节，在做BA的时候刻意选择那些具有共同观测的帧作为关键帧，在这种情况下，Schur消元后得到的 $S$ 就是稠密矩阵。不过，由于这个模块并不是实时执行，所以这种做法也是可以接受的。但是有另一些方法，例如DSo、OKVIS等，它们采用了滑动窗口(Sliding Window)方法。这类方法对每一帧都要求做一次BA来防止误差的累积.因此它们也必须采用一些技巧来保持S矩阵的稀疏性。

从概率角度来看,我们称这一步为边缘化，是因为我们实际上把求 $(\Delta x_{c},\Delta x_{p})$ 的问题，转化成了先固定 $\Delta x_{p}$ ，求出 $\Delta x_{c}$ ，再求 $\Delta x_{p}$ 的过程。这一步相当于做了条件概率展开：

                                         $P(x_{c},x_{p})=P(x_{c}|x_{p})P(x_{p})$ (9.43)

结果是求出了关于 $x_{p}$ 的边缘分布，故称边缘化。在前面介绍的边缘化过程中，实际上我们把所有的路标点都给边缘化了。根据实际情况，我们也能选择—部分进行边缘化。同时，Schur消元只是实现边缘化的其中一种方式，同样可以使用Cholesky分解进行边缘化。

在进行了Schur消元后，我们还需要求解线性方程组9-42-3，对它的求解是否有什么技巧呢？无，通常是用分解来计算的。不管采用哪种求解办法都建议利用 $H$ 的稀疏性进行Schur消元。不光是因为这样可以提高速度，也因为消元后的S矩阵的条件数往往比之前的 $H$ 矩阵要小。Schur消元也并不意味着将所有路标消元，将相机变量消元也是SLAM中采用的手段。

几个问题需要强调：

Ⅰ.为什么要进行Schur消元？意义在于什么？

由于SLAM中观测方程数量要远远大于运动方程，因此 $x_{c}$ 的维数远小于 $x_{p}$ 的维数，求解规模较小。

Ⅱ.边缘化是什么？

①联合分布=边缘分布乘条件分布： $P(x_{c},x_{p})=P(x_{c}|x_{p})P(x_{p})$ ，先求上面的再求下面的。

②上述的做法边缘化掉了所有的路标点，让概率全跑到相机位姿中，这是为了加速求解 $Hx=-b$ 。

③但通常情况下的边缘化操作会给H矩阵带来填充（fill-in），使其不再具有稀疏结构。

④所以，要么刻意选择特定的边缘化策略，要么仅使用稠密的 $H$ 求解线性方程，但这样效率会降低。

图9-11 边缘化图解

Ⅲ.共视：上文已讲述

Ⅳ.EKF和BA有什么关系？

EKF假设了马尔可夫性，BA没有假设马尔可夫性，因此得到整体的优化（Full SLAM），还能发现共视关系。

EKF可以计算卡尔曼增益，通过卡尔曼增益去更新当前的状态。在BA里面状态也在不断更新，不过问题是迭代进行的，每次迭代重新算雅可比矩阵看状态怎么更新，每迭代一次也可看作EKF进行的一次更新。

9.2.4 鲁棒核函数

在前面的BA问题中，我们将最小化误差项的二范数平方和作为目标函数。这种做法虽然很直观，但存在一个严重的问题：如果出于误匹配等原因，某个误差项给的数据是错误的，会发生什么呢？如果把一条原本不应该加到图中的边给加进去了，然而优化算法并不能辨别出这是个错误数据，它会把所有的数据都当作误差来处理。在算法看来，这相当于我们突然观测到了一次很不可能产生的数据。这时，在图优化中会有一条误差很大的边，它的梯度也很大，意味着调整与它相关的变量会使目标函数下降更多。所以，算法将试图优先调整这条边所连接的节点的估计值，使它们顺应这条边的无理要求。由于这条边的误差真的很大，往往会抹平其他正确边的影响，使优化算法专注于调整一个错误的值。这显然不是我们希望看到的。
出现这种问题的原因是，当误差很大时，二范数增长得太快。于是就有了核函数的存在。核函数保证每条边的误差不会大得没边而掩盖其他的边。具体的方式是，把原先误差的二范数度量替换成一个增长没有那么快的函数，同时保证自己的光滑性质(不然无法求导)。因为它们使得整个优化结果更为稳健，所以又叫它们鲁棒核函数(Robust Kernel)。
鲁棒核函数有许多种，例如最常用的Huber核：
$H(e) = \left\{\begin{matrix} \frac{1}{2}e^{2}\\ \delta (|e|-\frac{1}{2}\delta) \end{matrix}\right.$ 前者用于 $|e|\leqslant \delta$ ，后者用于其他情况

当误差 $e$ 大于某个阈值 $\delta$ 后，函数增长由二次形式变成了一次形式，相当于限制了梯度的最大值。同时，Huber核函数又是光滑的，可以很方便地求导。图9-12显示了Huber核函数与二次函数的对比，可见在误差较大时Huber核函数增长明显低于二次函数。

图9-12 Huber核函数与二次函数的对比

9.3 小结

本节我们重点介绍了BA中的稀疏性问题。不过，实践中，多数软件库已经为我们实现了细节操作，而我们需要做的主要是构造BA问题，设置Schur消元，然后调用稠密或者稀疏矩阵求解器对变量进行优化。

视觉SLAM⑨后端Ⅰ(KF、EKF、非线性优化)相关推荐

vins中imu融合_基于非线性优化算法—当视觉SLAM遇到VINS会碰撞出怎样的火花？
今天来给大家分享一个视觉SLAM中比较综合且复杂的系统-VINS.VINS旨在通过融合两个传感器测量数据获得移动机器人的位姿和特征点在空间中的位置,在现代控制理论学科中属于最优估计问题.在移动智能机器 ...
SLAM | 视觉SLAM中的后端：后端优化算法与建图模板
点击上方"AI算法修炼营",选择加星标或"置顶" 标题以下,全是干货前面的话前面系列一中我们介绍了,VSLAM 是利用多视图几何理论,根据相机拍摄的图像信息 ...
视觉SLAM⑥---非线性优化
目录 6.0 主要目标 6.1 SLAM问题的数学表达 6.2 状态估计问题 6.2.1 批量状态估计与最大后验估计 6.1.2 最小二乘的引出 6.1.3 例子:批量状态估计 6.2 非线性最小二乘 ...
视觉SLAM十四讲学习笔记-第六讲-非线性优化的状态估计问题
专栏系列文章如下: 视觉SLAM十四讲学习笔记-第一讲_goldqiu的博客-CSDN博客视觉SLAM十四讲学习笔记-第二讲-初识SLAM_goldqiu的博客-CSDN博客视觉SLAM十四讲学习 ...
视觉SLAM ch6 非线性优化
一.首先理解运动方程和观测方程 (1)运动方程: x表示相机的位姿,每个时刻位姿记为x1, x2...... xk,构成了运动的轨迹.uk 是传感器的输入:wk 是噪声,假设噪声服从高斯分布. 运动方 ...
视觉SLAM总结——后端总结
视觉SLAM总结--后端总结视觉SLAM总结--后端总结 (1)扰动模型 (2)Bundle Adjustment推导及稀疏性分析 Step 1:Bundle Adjustment推导 Step 2 ...
视觉SLAM十四讲学习笔记-第六讲学习笔记总结（1）---非线性优化原理
第六讲学习笔记如下: 视觉SLAM十四讲学习笔记-第六讲-非线性优化的状态估计问题_goldqiu的博客-CSDN博客视觉SLAM十四讲学习笔记-第六讲-非线性优化的非线性最小二乘问题_ ...
视觉SLAM十四讲学习笔记-第六讲-非线性优化的实践-高斯牛顿法和曲线拟合
专栏系列文章如下: 视觉SLAM十四讲学习笔记-第一讲_goldqiu的博客-CSDN博客视觉SLAM十四讲学习笔记-第二讲-初识SLAM_goldqiu的博客-CSDN博客视觉SLAM十四讲学习 ...
视觉SLAM十四讲学习笔记-第六讲-非线性优化的非线性最小二乘问题
专栏系列文章如下: 视觉SLAM十四讲学习笔记-第一讲_goldqiu的博客-CSDN博客视觉SLAM十四讲学习笔记-第二讲-初识SLAM_goldqiu的博客-CSDN博客视觉SLAM十四讲学习 ...

视觉SLAM⑨后端Ⅰ(KF、EKF、非线性优化)

9.0 本章内容

9.1 概述

9.1.1 状态估计的概率解释

9.1.2 线性系统和KF

9.1.3 非线性系统和EKF

9.1.4 EKF的讨论

9.2 BA与图优化

9.2.1 投影模型和BA代价函数

9.2.2 BA的求解

9.2.3 稀疏性和边缘化

9.2.4 鲁棒核函数

9.3 小结

视觉SLAM⑨后端Ⅰ(KF、EKF、非线性优化)相关推荐

最新文章

热门文章