第九讲后端1（backend）-优快云博客

1、理解后端概念；

2、理解以EKF为代表的滤波器后端的工作原理；

3、理解非线性优化的后端，明白稀疏性是如何利用的；

4、使用g2o个Ceres实际操作后端优化。

前端视觉里程计能给出一个短时间内的轨迹和地图，由于不可避免的误差积累，长时间是不准确的。所以，还希望构建一个尺度规模更大的优化问题，以考虑长时间内的最优轨迹和地图。

1、概述

后端优化
渐进式（Incremental）：当前状态只由过去的时刻决定（滤波）
批量式（Batch）：给定一定规模的数据，计算该数据下的最优估计（优化）

1.1 状态估计的概率解释

SLAM过程可以由运动方程和观测方程来描述：
- 运动方程： $x_k=f(x_{k-1} ,u_k,w_k)$
- 观测方程： $z_{k,j}=h(x_k,y_j,v_{k,j})$
- 由于视觉SLAM特征点数量众多，因此实际中观测方程数量会远远大于运动方程。
- 若没有运动方程，整个优化问题就只由许多个观测方程组成，类似于SfM问题，相当于通过一组图像来恢复运动和结构。不同的是，SLAM中的图像有时间序列，SfM允许使用完全无关的图像。
状态估计问题如何求解？
- 位姿x和路标y服从某种概率分布的随机变量，我们关心的问题：当我们拥有某些运动数据u和观测数据z时，如何确定状态量x,y的分布。假设状态量和噪声服从高斯分布，问题转为：当存在一些运动数据和观测数据时，我们如何估计状态量的高斯分布？
- 用带噪声的数据估计内在状态（Estimated the inner state from noisy data）——状态估计问题。下图形象解释了状态估计中的问题。
- 批量状态估计问题可以转化为最大似然估计问题，并使用最小二乘法求解。（第6讲）。本节探讨如何将该结论应用于渐进式问题。
状态估计问题求解方法
- $x_k$ 为k时刻的所有未知量，它包含了当前时刻的相机位姿与m个路标点。
- $x_k\overset{def}{=}\{x_k,y_1,...,y_m\}$
- k时刻所有观测，记为 $z_k$ 。k=1,...N
- 运动方程： $x_k=f(x_{k-1},u_k)+w_k$
- 观测方程： $z_k=h(x_k)+v_k$
- 现在考虑第k时刻的情况。用过去0-k中的数据来估计现在的状态分布： $P(x_k|x_0,u_{1:k},z_{1:k})$
- 按照贝叶斯法则 $P(A|B)=\frac{P(B|A)P(A)}{P(B)}$ ，把 $z_k$ 和 $x_k$ 交换位置（分子与 $x_k$ 无关，可拿掉），有： $P(x_k|x_0,u_{1:k},z_{1:k})\propto P(z_k|x_k)P(x_k|x_0,u_{1:k},z_{1:k-1})$ 。
- 其中 $P(x_k|x_0,u_{1:k},z_{1:k})$ ：在给定初始状态 $x_0$ 、一些列控制输入 $u_{1:k}$ 和一系列观测 $z_{1:k}$ 的条件下，系统状态在时间k的概率分布。
- 第一项似然由观测方程给定；第二项先验部分，当前状态的 $x_k$ 是基于过去所有状态估计得来的。至少会受 $x_{k-1}$ 影响，于是以 $x_{k-1}$ 时刻为条件概率展开（对 $x_{k-1}$ 进行积分，左边是 $x_{k-1}$ 多加个条件下的 $x_{k}$ 的分布，右边是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}$
- 至此，给出了贝叶斯估计。
但上式还没有具体的概率分布形式，后续处理有两种选择：
- 一种，假设马尔可夫性（k时刻状态只于k-1时刻状态有关），如此会得到扩展卡尔曼滤波（EKF）为代表的滤波器方法。
- 另一种，考虑k时刻与之前所有状态的关系，此时将得到非线性优化为主体的优化框架。目前视觉SLAM的主流为非线性优化。

1.2 线性系统和KF

我们假设了马尔可夫性，则
- 先验的第一部分可化简： $P(x_k|x_{k-1},x_0,u_{1:k},z_{1:k-1})=P(x_k|x_{k-1},u_k)$
- 先验的第二部分可化简为： $P(x_{k-1}|x_0,u_{1:k},z_{1:k-1})=P(x_{k-1}|x_0,u_{1:k-1},z_{1:k-1})dx_{k-1}$
- 该项实际上是k-1时刻的状态分布
- 我们实际在做的是“如何把k-1时刻的状态分布推导至k时刻”。
从形式最简单的线性高斯系统开始，最后得到卡尔曼滤波器：
- 运动方程和观测方程： $\left\{\begin{matrix} x_k=A_kx_{k-1}+u_k+w_k\\ z_k=C_kx_k+v_k \end{matrix}\right.$ ，k=1,...,N，假设所有状态和噪声满足高斯分布。 $w_k\sim N(0,R),v_k\sim N(0,Q)$
- 假设已知k-1时刻的后验状态估计 $\hat{x}_{k-1}$ 及其协方差 $\hat{P}_{k-1}$ ，现在要根据k时刻的输入和观测数据，确定k时刻的 $x_k$ 后验分布。 $\hat{x}_k$ 代表后验， $\check{x}_k$ 代表先验。
- 通过运动方程确定 $x_k$ 的先验分布，有 $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)$
- 这一步称为预测。由k-1时刻后验，通过运动方程推算k时刻的先验，这个分布也就是先验： $\check{x}_k=A_k\hat{x}_{k-1}+u_k$ , $\check{P}_k=A_k\hat{P}_{k-1}A_k^T+R$
- 由观测方程，我们可以计算在某个状态下应该产生怎样的观测数据： $P(z_k,x_k)=N(C_kx_k,Q)$ 。
- 更新：先计算K，即卡尔曼增益： $K=\check{P}_kC_k^T(C_k\check{P}_kC_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$
以上，推导了经典的卡尔曼滤波器的整个过程。这里是从概率角度出发的最大后验概率估计的方式。过程中没有用到任何近似，可以说，卡尔曼滤波器构成了线性系统的最优无偏估计。

1.3 非线性系统和EKF

SLAM中的运动方程和观测方程通常是非线性函数。一个高斯分布，经过非线性变换后，往往不再是高斯分布。所以在非线性系统中，我们必须取一定的近似，将一个非高斯分布近似成高斯分布。
我们希望把卡尔曼滤波器的结果拓展到非线性系统中，称为扩展卡尔曼滤波。通常的做法，在某个点附近考虑运动方程及观测方程的一阶泰勒展开，只保留一阶项，即线性的部分，然后按照线性系统进行推导。
预测，根据运动方程，有 $P(x_k|x_0,u_{1:k},z_{0:k-1})=N(f(\hat{x}_{k-1},u_k),F_k\hat{P}_{k-1}F_k^T+R_k)$ ，其中F为偏导数。记这里的先验和协方差的均值： $\check{x}_k=f(\hat{x}_{k-1},u_k)$ , $\check{P}_k=F_k\hat{P}_{k-1}F_k^T+R_k$
更新：卡尔曼增益 $K=\check{P}_kH_k^T(H_k\check{P}_kH_k^T+Q_k)^{-1}$ ，在卡尔曼增益的基础上，后验概率的形式： $\hat{x}_k=\check{x}_k+K_k(z_k-h(\check{x}_k))$ , $\hat{P}_k=(I-K_kH)\check{P}_k$ 。H为偏导数。

1.4 EKF的讨论

EKF以形式简洁，应用广泛著称。当想要在某段时间内估计某个不确定量时，首先想到的就是EKF。还有一些变种，例如IF（信息滤波器）、IKF（Iterated KF）、UKF(Unscented KF)和粒子滤波器、SWF(Sliding Window Filter)等。如今，非线性优化比滤波器更有优势，但在计算资源有限或待估计量比较简单的场合，EKF不失为一种有效方式。
EKF的局限：
- 1、滤波器方法在一定程度上假设了马尔可夫性，而非线性优化则倾向于使用所有历史数据。
- 2、EKF滤波器仅在 $\hat{x}_{k-1}$ 处做了一次线性化，就直接根据线性化结果计算后验概率。相当于，我们认为该点处的线性化近似在后验概率处仍然有效。当离工作点较远时，一阶泰勒展开不一定能够近似整个函数，若运动模型和观测模型有强烈的非线性，则线性近似只在很小范围内成立。这就是EKF的非线性误差。非线性优化中，尽管也做一阶、二阶近似，但每迭代一次，状态估计发生改变时，会重新对新的估计点做泰勒展开，使得优化方法适用范围更广。大体来说，可以粗略地认为EKF仅是优化中的一次迭代。
EKF需存储状态量的均值和方差，并维护和更新，数据量很大，因此普遍认为EKF SLAM不适用于大型场景。
EKF等滤波器方法美欧异常检测机制，导致系统在存在异常值的时候很容易发散。
因此，在同等计算量的情况下，非线性优化能取得更好的效果（即精度和鲁棒性同时更好）。

2、BA和图优化

所谓BA（Bundle Ajustment）是指从视觉图像中提炼最优的3D模型和相机参数（内外参）。考虑从任意特征点发射出来的几束光线，它们会在几个相机的成像平面上变成像素或是检测到的特征点。如果我们调整各相机姿态和各特征点的空间位置，使得这些光束最终都收束到相机的光心，称为BA。

2.1 投影模型和BA代价函数

从一个世界坐标系中的点p出发，把相机内外参和畸变都考虑，最后投影成像素坐标，这个过程就是观测方程，之前将它抽象记成： $z=h(x,y)$ 。
x指代此时的相机位姿，即外参R,t，对应李群T，李代数 $\xi$ 。
y是路标，即这里的三维点p。
观测数据则是像素坐标 $z\overset{def}{=}[u_s,v_s]^T$
关于此次观测的误差： $e=z-h(x,y)$ 。将其他时刻的观测量也考虑进来，设 $z_{i,j}$ 为在位姿 $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。

2.2 BA的求解

非线性化的思想：应该从某个初始值开始，不断寻找下降方向 $\Delta x$ ，来找到目标函数的最优解，即不断地求解增量方程中的增量 $\Delta x$ 。
将自变量定义成所有待优化的变量： $x=[T_1,...,T_M,p_1,...p_n]^T$ ，此时目标函数变为： $\frac{1}{2}||f(x+\Delta x)||^2\approx\frac{1}{2}\sum_{i=1}^{m}\sum_{j=1}^{n}||e_{ij}+F_{ij}\Delta \xi_i+E_{ij}\Delta p_j||^2$ 。
其中 $F_{ij}$ 表示整个代价函数在当前状态下对相机姿态的偏导数，而 $E_{ij}$ 表示该函数对路标位置的偏导。
位姿变量放在一起： $x_c=[\xi_1,\xi_2,...\xi_m]^T\in \mathbb{R}^{6m}$ ，空间点变量放在一起： $x_p=[p_1,p_2,...p_n]^T\in \mathbb{R}^{3n}$ 。如此，目标函数简化为： $\frac{1}{2}||f(x+\Delta x)||^2=\frac{1}{2}||e+F\Delta x_c+F\Delta x_p||^2$
需要注意的是，该式从一个由很多个小型二次项之和，变成矩阵形式。这里的雅可比矩阵E和F必须是整体目标对整体变量的导数，将是一个很大块的矩阵，而里头每个小分块，需要由每个误差项的导数 $F_{ij}$ 和 $E_{ij}$ “拼凑”起来。
然后，用高斯牛顿法或列文伯格-马夸尔特法，都将面对增量线性方程： $H\Delta x=g$
雅可比矩阵： $J=[F E]$ ，若使用高斯牛顿，则 $H=J^TJ=\begin{bmatrix} F^TF & F^TE\\ E^TF & E^TE \end{bmatrix}$
这个线性方程的维度非常大，包含了所有相机位姿和路标点。如果直接对H求逆计算增量方程，消耗计算资源非常多。幸运的是H矩阵由一定特殊结构，利用这个特殊结构，可以加速求解。

2.3 稀疏性和边缘化

矩阵H可以自然、显式地用图优化来表示。
H矩阵的稀疏性是由雅可比矩阵 $J(x)$ 引起的。考虑这些代价函数当中的其中一个 $e_{ij}$ 。注意，这个误差项只描述了在 $T_i$ 看到 $p_i$ 这件事，只涉及第I个相机位姿和第j个路标点，对其余部分的变量的导数都是0。
设 $J_{ij}$ 只在i,j处由非零块，那么它对H的贡献为 $J_{ij}^TJ_{ij}$ ，仅有4个非零块，如下图所示。
对于H矩阵中出于非对角线的矩阵块来说，如果该矩阵块非零，则其位置所对应的变量之间在图中会存在一条边。所以H矩阵中的飞对角线部分的非零矩阵块可以理解为其对应的两个变量之间存在联系，或者可以成为约束。因此我们发现图优化结构与增量方程的稀疏性存在明显关系。如下图所示。
假设有m个相机位姿，n个路标点。由于通常路标的数量远远多于相机，一般情况下H矩阵如下图所示。称为箭头形（Arrow-like）矩阵，或镐形矩阵。
对于这种稀疏结构的H，线性方程 $H\Delta x=g$ 求解，在SLAM中常用Schur消元，在SLAM研究中也称为Marginalization（边缘化）。
- $\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}$ ，其中B、C矩阵均为对角块矩阵。对角块矩阵求逆难度远小于一般矩阵，只需对对角线矩阵块分别求逆即可。结合该特性，对线性方程组进行高斯消元，目标是消去右上角的非对角部分的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}$ ，整理得 $\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}$
- 方程组第一行与无关的项，只与有关。求解后，代入原方程第二行再求解。这个过程称为Marginalization，或Schur消元。
  - $[B-EC^{-1}E^T]\Delta x_c=v-EC^{-1}w$
  - $\Delta x_p=C^{-1}(w-E^T\Delta x_c)$
- 将边缘化第一步的系数记为S，它是一个普通的线性方程。S矩阵的非对角线上的非零矩阵块，表示了该处对应的两个相机变量之间存在着共同观测的路标点，有时成为共视。
  - S矩阵的稀疏性结构取决于实际观测结果，我们无法提前预知。
  - 在ORB-SLAM中的Local Mapping环节，做BA时可以选择那些具有相同观测的帧作为关键帧，这种情况下消元后的S矩阵一般是稠密矩阵。
  - 但另外一些方法，例如DSO、OKVIS等，采用滑动窗口法，对每一帧都要求做一次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)$ 。结果是求出了 $\Delta x_p$ 的边缘分布，故称边缘化。
- 同样我们也可以使用Cholesky分解进行边缘化。

2.4 鲁棒核函数

在前面的BA问题中，我们将最小化误差的二范数平方和作为目标函数。这种做法有一个严重的问题：如果出现误匹配等原因，某个误差项给的数据是错误的，且误差很大，往往会抹平其他正确边的影响，使优化算法专注于调整一个错误的值。
这种问题原因是，当误差很大时，二范数增长太快。于是就有了核函数的存在。核函数保证每条边的误差不会大的没变而掩盖其他边。具体方式：把原先误差的二范数度量替换成一个增长没有那么开的函数，同时保证自己的光滑性质（不然没法求导）。以为它们使得整个优化更为稳健，所有又叫它们鲁棒核函数。
- 常用的Huber核： $H(e)=\left\{\begin{matrix} \frac{1}{2}e^2,when|e| \leqslant \delta \\ \delta (|e|-\frac{1}{2}\delta ),other \end{matrix}\right.$ 。当误差e大于某个与之 $\delta$ 时，函数增长由二次形变为一次形式，相当于限制了梯度的最大值。
- 除了Huber核，还有Cauchy核、Tukey核等。