拆分基础slam（3）-后端优化：KF and BA

后端优化

我写文章喜欢一个板块写完，如果觉得累看不完没关系，休息会下次再来，我积累不深，有错误欢迎大家指出。前端的视觉里程计使我们能够粗略估算相机和机器人的移动，但必定会有误差的产生和累积。所以我们需要在相机进行姿态估计后将得到的数据放到后端进行优化，降低观察点批的误差。

运动、观测方程与贝叶斯

对于一个小车，运动方程为：
$$x_k=f\left(x_{k-1},u_k\right)+w_k$$
观测方程为：
$$z_{k,j}=h\left(y_j,x_k\right)+v_{k,j}$$
在上式中，x代表相机的位姿，y代表路标，z代表观测数据。运动方程的意思是这一刻的运动状态等于上一时刻运动状态和控制输入的共同作用结果，再加上一个误差。观测方程的意思是相机的观测数据等于机器人的位姿和路标点（特征点）的共同作用结果（相机观测方程h），再加上一个误差。在后端优化中，比如BA算法就是用最小二乘法尽力降低这个误差项。
此时我要讲到一个条件概率公式：
$$P\left(x_k|x_{0,u_{1:k},z_{1:k}}\right)$$
这个公式的意思是k时刻小车的状态是由0时刻的状态和一系列的输入信号u和观测信号z决定的。设想如果这个公式的值为1会发生什么？就是说如果k-1时刻之前按照公式里给的走的话，那么小车状态一定会变成xk。所以很多时候用贝叶斯法则优化问题时，就是让这个公式的值越大越好，这个方法叫最大后验估计。下面我讲怎么优化。
由贝叶斯法则：
$$P\left(x_k|x_{0,u_{1:k},z_{1:k}}\right)\propto P\left(z_k|x_k\right)P\left(x_k|x_0,u_{1:k},z_{1:k-1}\right)$$
这个式子就是贝叶斯法则的核心应用，右边省略了分母，一个常数无关项，因为我们想优化为最大值，所以具体多少不重要，就变成了正比于。第一项叫似然指的是在发生某事的情况下观察到它的概率，这其实就是观测关系。第二项叫先验，指的是还没有观测到这件事发生的情况下这件事发生的概率（相比原式少了一个zk，就是没观测到k时刻状态）。
我们刚才把目标式子拆分为了先验和似然，似然已经可以获得了，我们现在讲讲先验有何意义，如何获得。
$$P\left(x_k|x_0,u_{1:k},z_{1:k-1}\right)=\int P\left(x_k|x_{k-1},x_0,u_{1:k},z_{1:k-1}\right)P\left(x_{k-1}|x_0,u_{1:k},z_{1:k-1}\right)d{x}_{k-1}$$
这个式子的意思是：发展为xk状态的概率是无数种x(k-1)状态的概率乘以该状态发展为xk状态的概率再求和。就是说有很多种发展为xk的路，那么发展为xk的概率就等于各种路走过来的概率求和，够具象吧。此时上式的右边就又等于原式往前递推一个状态了，就成一个递归关系了，我们能够回溯到x0状态。
从此，我们就可以将一个状态递推回上一个状态了。我们对这一个过程假设马尔科夫性，即该状态只和上一个状态相关，这种假设下的优化办法主要是卡尔曼滤波。

卡尔曼滤波

我们看上面那个带积分号的式子，是这个式子连接了状态递推。刚才我们说了xk状态只与x(k-1)有关，我现在会依据这个规则简化上式右边：
$$P\left(x_k|x_{k-1},x_0,u_{1:k},z_{1:k-1}\right)=P\left(x_k|x_{k-1},u_k\right)$$
这个式子原先意思是由上一个状态发展为这个状态的概率跟之前很多状态的观测输入等都有关，现在简化为只跟上个状态和它的输入有关。我们看第二部分：
$$P\left(x_{k-1}|x_0,u_{1:k},z_{1:k-1}\right)=P\left(x_{k-1}|x_0,u_{1:k-1},z_{1:k-1}\right)$$
这个式子意思是k时刻的输入与k-1时刻的状态无关，自然就拿掉了。
我着重讲清楚贝叶斯公式怎么添加马尔可夫性，卡尔曼滤波的前提就是马尔科夫性，卡尔曼滤波不推了，我再讲下卡尔曼滤波怎么应用。
第一步是计算先验：
$${\overline{x}}_k=A_k{\hat{x}}_{k-1}+u_k$$
上式中左边叫做先验，上划线表示，右边是上一次的后验，用上尖号表示。上式是运动方程，表示状态从k-1时刻通过输入控制信号作用转移到了k时刻的状态。用上一时刻的后验状态（因为上一时刻已经获得了叫后验）来计算这一时刻的先验状态。
第二步我们需要估算下一时刻的先验概率：
$${\overline{P}}_k=A_k{\hat{P}}_{k-1}{A}_k^T+R$$
要理解上式中Pk和Pk-1要看我这节的第一二个式子，第一个式子表示的是先验，因为z还停留在k-1时刻，它就在说k时刻的概率了；第二个式子表示的是后验，x的状态是k-1时刻的，但k-1时刻z已经观测了x的状态了。所以上式就是当前状态后验概率推下一个状态先验概率。R是运动方程中包含误差的标准差。
第三步是计算卡尔曼增益：
$$K={\overline{P}}_k{C}_K^T(C_k{\overline{P}}_k{C}_k^T+Q{)}^{-1}$$
上式计算卡尔曼增益用的是上一步中得到的先验概率计算，Q表示的是观测方程中高斯分布的标准差。
第四步使用卡尔曼增益更新先验为后验：
$$\begin{gathered} {\hat{x}}_k={\overline{x}}_k+K(z_k-C_k{\overline{x}}_k) \\ {\hat{P}}_k=(I-K{C}_k){\overline{P}}_k \end{gathered}$$
我们在实际的相机模型，观测方程经常是非线性的，这时需要使用扩展卡尔曼滤波，基本的思想是在某个点附近采用一阶线性化，思想是非常类似的。
卡尔曼滤波的方法需要的计算资源小，原理简单，但是存在一些局限性：1、开始的时候我们就做了假设，当前状态只与上一次的状态相关，但事实上肯定会与更早的数据有所关联，比如我看见了很久之前见过的东西（回环检测）。2、我们在使用EKF的时候，进行了一阶近似，但是这一步同样会带来误差。
所以更大型的工程中使用更多的是非线性优化的方法。

BA

在之前我讲解前端的算法中提到了BA优化，可以同时优化相机的姿态和空间点坐标，优化的目标函数如下：
$$\begin{gathered} e=z-h(\xi ,p) \\ \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({\xi }_i,p_j)|{|}^2 \end{gathered}$$
对上式不了解的同学可以看：
前端算法
这个BA算法其实贯穿前后端，既用在前端来计算相机位姿，同时带批优化的功能。在上一章我主要讲了这个算法怎么引出，这张着重讲一些数学方面的内容，如何利用稀疏性和边缘化解这个问题。
我们先引出这个问题和它的难点。因为我们知道观测模型h是非线性过程，我们不能用一般的解方程思想来求解，这些东西在数值分析中会学到。求解大型方程组或者非线性模型，我们首先初始化一个解，然后寻找能够使残差下降的方向来更新x。我们来看看BA优化需要优化的变量：
$$x=[{\xi }_1,...,{\xi }_m,p_1,...,p_n{]}^T$$
我们在上一章也讲到了，BA优化的项目是姿态和空间点。用我刚才说到的思想，给初始化的x一个下降方向来优化解，所以目标函数变为：
$$\frac{1}{2}||f(x+△x)|{|}^2=\frac{1}{2}\sum _{i=1}^m\sum _{j=1}^n||e_{ij}+F_{ij}△{\xi }_i+E_{ij}△p_j|{|}^2$$
上式中表示的是给x一个差分，会引起关于位姿和空间点的一个误差，F代表的是误差函数对相机姿态的偏导，E代表的是误差函数对空间点的偏导，我们要最小化这个函数。取最速下降法（需要学习高等数值分析和高斯牛顿法）来优化这个方程，最速下降法的意思是x每次都往使代价函数下降最快的方向修正，那么为了求解这个deltax，我们需要求解以下方程组：
$$J(x{)}^{T}J(x)△x=-J(x{)}^{T}f(x)$$
上式中J是雅各比矩阵，意思是代价函数关于自变量的导数。在很多资料中这个式子会写成：
$$H△x=g$$
我们回到要求解的问题上面来，我们要优化姿态和空间点两个变量，我之前已经写了代价函数关于这两个变量的偏导分别为E和F，那么：
$$J=[FE]$$
则：
$$H=J^{T}J=\left[\begin{array}{c}{F}^{T}F{F}^{T}E\\ E^{T}F{E}^{T}E\end{array}\right]$$
我们现在要求解这个deltax就需要求解H矩阵，这时一个数学问题，H矩阵的维度会非常大，因为我们能看见非常多的路标点特征点，如果要求逆难度就会更高，所以我们提出稀疏化和边缘化来加快这个计算。下一节会是数学解释，但是会加深Slam过程的理解。BA的主要原理以及解释完毕了。

稀疏化和边缘化

首先要说一个基本的性质，当小车在某个姿态的时候，它不一定能看见所有点，所以此时代价函数在这个姿态下关于这个路标的导数就该是0。这样的结果就是虽然点非常多，但在我们处理起来的时候仍然会有很多0.
我们关于在i姿态下得到的雅各比矩阵就会有以下形式：
$$J_{ij}(x)=(0_{2\ast 6},...,0_{2\ast 6},\frac{\partial e_{ij}}{\partial {\xi }_i},0_{2\ast 6},...0_{2\ast 3},\frac{\partial e_{ij}}{\partial p_j},0_{2\ast 3},...,0_{2\ast 3})$$
在状态为i观测点为j的情况下的雅各比矩阵只有两个非零项，只描述了一个观察状态。我认为有两行的原因可能是因为有姿态和空间点两个变量，偏导事实上会包含两个组成成分（我思考的结果，仅作参考欢迎指正）。位姿的六个维度分别是罗德里格斯向量三位加上t三位，空间点的三个维度好理解。由此雅各比矩阵的分析结束。由此我们可以描述形态关系如下，取高翔老师的图：

我们就可以知道H的形态，我们最终的H计算式如下：
$$H=\sum _{i,j}{J}_{ij}^T{J}_{ij}$$
H就是我们在所有姿态下观测到的所有路标点组合在一起，我们进一步给一个更全面的图：

左侧的雅各比矩阵代表着在某些姿态下我们观测到了某些点，计算出的H矩阵形态如右图所示。这个矩阵我们可以把它分为四块：

上图中B矩阵是J中的姿态对应姿态，当然只能对焦对应，C矩阵是点对点，同理。E矩阵就是姿态对点的观测。我们求解的关键问题就出现了如下转换：
$$H△x=g\gg \left[\begin{array}{c}BE\\ E^{T}C\end{array}\right]\left[\begin{array}{c}△x_c\\ △x_p\end{array}\right]=\left[\begin{array}{c}v\\ w\end{array}\right]$$
为什么要理解H矩阵的型式？关键在于我们知道了BC是两个对角矩阵，这种矩阵求逆非常容易，比直接对H矩阵求逆快很多，这就是意识到稀疏性和边缘化的意义所在，看消元过程，消去右上角E：
$$\left[\begin{array}{c}B-E{C}^{-1}{E}^{T}0\\ E^{T}C\end{array}\right]\left[\begin{array}{c}△x_c\\ △x_p\end{array}\right]=\left[\begin{array}{c}v-E{C}^{-1}w\\ w\end{array}\right]$$
我们利用第一行解出xc，代入第二行接触xp问题就解决了。先消去右上角我们叫做边缘化或者舒尔消元。好处我也说了，对角阵利于求逆。

鲁棒核函数

这里提一句使用鲁邦核函数的原因是防止出现过于离谱的误差干扰我们的代价函数。因为代价函数是二范数形式的，也就是说如果出现了一个很大的误差，那么在优化中对问题准确度的影响会非常大，所以修改二次误差为如下形式：
$$H(e)=\{\begin{array}{r}\frac{1}{2}{e}^{2}if|e|\le \delta \\ \delta (|e|-\frac{1}{2}\delta )otherwise\end{array}$$
使误差过大的项从二次变为一次。

位姿图优化

后端中除了可以使用BA，也可以使用位姿图优化的方法。下节再讲吧。