从这一讲开始我们要开始学习SLAM的后端，前面我们学习了视觉里程计（VO），通过视觉里程计，我们可以构建一个短时间内效果还不错的轨迹和地图，但是长时间的误差的累计，还是会迅速的降低我们的轨迹和地图的精度，所以维护一个更大的地图和轨迹，使其达到最优状态，在实际中还是非常需要的。而这项任务的完成就离不开——后端。

概述

状态估计和概率解释

我们再来看一下SLAM过程的运动方程和观测方程：
$$\{\begin{array}{rl}& x_k=f(x_{k-1},u_k)+w_k（运动方程）\\ & z_k=h(x_k)+v_k，k=1,\dots ,N（观测方程）\end{array}\text{\hspace{1em}(0)}$$这两个方程我们已经多次见过了，依然有必要再仔细看看它：
　　系统状态：$x$
　　输入：　　$u$（也可以理解为控制量，这里和书中的说法不一样！！）
　　过程噪声：$w$
　　测量：　　$z$
　　测量噪声：$v$

我们不妨通过描述的方法去认识一下（0）式：
　　首先看运动方程：$k$时刻的系统状态$x_k$，是由$k-1$时刻的状态$x_{k-1}$，$k$时刻的控制量$u_k$，再加上过程噪声共同决定的。
　　测量方程：$z_k$时刻的测量值，是由在$x_k$状态下看到的观测，在加上测量噪声决定的。
　　
　　这个式子是一个很抽象的式子，它表示了一大类运动过程，如果不根据实际的情况分析，可能你不太能明白这其中的具体过程。

现在我们想获得$k$时刻的系统状态分布，我们希望这个分布是从过去0到$k$中的所有数据估计而来的：
$$P(x_k|x_0,u_{1:k},z_{1:k})$$注意从0到$k$时刻，我们能得到的数据就是0时刻的系统状态$x_0$、控制量$u$，测量值$z$。

根据贝叶斯公式：
$$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})\text{\hspace{1em}(1)}$$对于上式不用过多解释了，非常简单的贝叶斯公式。
　　$P(z_k|x_k)$是似然，也就是$x_k$条件下，$z_k$的概率分布，对应到SAMM问题中，也就是某一个状态下的测量值，这个可以通过测量方程给出。
　　$P(x_k|x_0,u_{1:k},z_{1: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})d{x}_{k-1}\text{\hspace{1em}(2)}$$上式子（2）的操作方法很多，选择不同的操作，就对应着不同的理论，如果认为只和上一时刻有关，那么就会得到以卡尔曼滤波器为代表的滤波器方法。如果认为和所有状态都有关，那么得到的就是非线性优化的方法。

线性系统和KF

首先我们看一下认为只和上一个时刻有关的马儿可夫理论，也就是基于滤波器模型。

在这里由于推导过程比较复杂（如果你觉得推导看着费劲，那就先记住结论吧），所以我就直接总结出线性卡尔曼滤波器的公式：
　　①预测：
$${\overline{x}}_k=A_k{\hat{x}}_{k-1}+u_k,{\overline{P}}_k=A_k{\hat{P}}_{k-1}{A}_k^T+R\text{\hspace{1em}(3)}$$　　②更新：先计算卡尔曼增益$K$
$$K_k={\overline{P}}_k{C}_k^T(C_k{\overline{P}}_k{C}_k^T+Q_k{)}^{-1}\text{\hspace{1em}(4)}$$　　　然后计算后验概率分布
$${\hat{x}}_k={\overline{x}}_k+K_k(z_k-C_k{\overline{x}}_k)\text{\hspace{1em}(5)}$$$${\hat{P}}_k=(I-K_k{C}_k){\overline{P}}_k$$（３）（４）（５）就是伟大的卡尔曼滤波器公式，你不要小瞧它，它的用处远比你想象的还要大，你可以不会推导，但是一定要学会怎么使用，这一点非常重要，因为它的用处实在太大了！！让我们看一下每一个变量的意义吧：
　　　状态转移矩阵：　　　　$A$
　　　过程噪声：　　　　　　$u$
　　　过程噪声的协方差矩阵：$Ｑ$
　　　观测矩阵：　　　　　　$C$
　　　观测噪声协方差：　　　$R$
卡尔曼滤波器的更直白的描述是：现在我们有一个当前时刻的测量值，和当前状态的预测值，我们觉得两个值都不是最准确的，但是我们不能丢弃任何一个，而是通过对这两个值取一个折中值（不是平均值，不然也太low了），将这个折中值就作为当前状态的最优估计。（４）式的卡尔曼增益，就是告诉我们了，应该相信测量值更多一点儿，还是应该相信预测值更多一点儿。

关于卡尔曼滤波器更细致的理解和使用方法，我推荐大家看以下博客
《详解卡尔曼滤波原理》
《卡尔曼滤波应用及其matlab实现》

非线性系统和EKF

上面讲的是线性的系统，对于（０）式这样的非线性的系统，我们可以将其通过泰勒展开保留一阶项，也就可以将非线性系统线性化，然后同样的思路推导出扩展卡尔曼方程：
　　①预测：
$${\overline{x}}_k=f({\hat{x}}_{k-1},u_k),{\overline{P}}_k=F{\hat{P}}_{k-1}{F}^T+R_k\text{\hspace{1em}(6)}$$　　②更新：先计算卡尔曼增益$K$：
$$K_k={\overline{P}}_k{H}^T(H{\overline{P}}_k{H}^T+Q_k{)}^{-1}\text{\hspace{1em}(7)}$$ 　　然后计算后验概率分布
$${\hat{x}}_k={\overline{x}}_k+K_k(z_k-h({\overline{x}}_k))\text{\hspace{1em}(8)}$$$${\hat{P}}_k=(I-K_{k}H){\overline{P}}_k$$
其中$F$是$f(x)$关于$x$的偏导数，$H$是$h(x)$关于$x$的偏导数。

EKF的讨论

KF也好，EKF也好，总之基于卡尔曼滤波器发展起来的各种滤波器种类繁多，在不同的应用方向上发挥着巨大的作用。不管现今的SLAM是否还在使用滤波器，但是对于计算资源受限的硬件来说，选择卡尔曼滤波器是不错的选择。

随着计算机硬件的提升，主流的SLAM系统基本都已经摒弃了基于滤波的方法。主要原因是因为以下方面：

• 1.滤波器的方法是基于前后两个时刻的状态进行计算的，实际上我们知道，我们当前的状态不是任何一个状态导致的，而是很多状态累积出来的结果，如果只认为与前一状态有关，势必会丢掉很多有用的信息。
• 2.EKF在非线性强烈的情况下，线性近似就不在成立，导致误差会很大。
• 3.EKF需要存储的信息量很多，不能适应大型场景。

BA和图优化

既然基于滤波器的方法不能适应现在的需求，那么我们就能来看看更好的方法。

关于BA我在第7讲的视觉里程计中给了一个还算直观的解释，在这里我就不在赘述了。

投影模型和BA代价函数

1.首先将一个世界坐标下的空间点$p$转换到相机坐标下：
$$P^{\prime }=Rp+t=[X^{\prime },Y^{\prime },Z^{\prime }{]}^T$$2.然后$P^{\prime }$投影到归一化平面，得到归一化坐标：
$$P_c=[u_c,v_c,1{]}^T=[\frac{X^{\prime }}{Z^{\prime }},\frac{Y^{\prime }}{Z^{\prime }},1{]}^T$$
3.考虑归一化坐标的畸变情况，得到去畸变（只考虑径向畸变）后的归一化坐标：
$$\{\begin{array}{r}{u}_c^{\prime }=u_c(1+k_1{r}_c^2+k_2{r}_c^4)\\ v_c^{\prime }=v_c(1+k_1{r}_c^2+k_2{r}_c^4)\end{array}$$
4.最后根据相机内参模型，将去畸变后归一化平面的点投影到像素平面：
$$\{\begin{array}{r}{u}_s=f_x{u}_c^{\prime }+c_x\\ v_s=f_y{v}_c^{\prime }+c_y\end{array}$$
上面四步也就是前面讲的观测方程，在前面我们的观测方程被抽象成了：
$$z=h(x,y)$$那么$x$就相当于是这里的相机外参数$R,t$，对应的李代数为$\xi$，$y$就相当于是这里的三维点$p$，$z$就是这里的观测数据$[u_s,v_s]$。那么观测误差就是：
$$e=z-h(\xi ,p)\text{\hspace{1em}(9)}$$很多人不明白这个式子，在这里我解释一下：空间中的点已经投影到了像素平面上，这个已经是一个事实了，谁也改变不了了。现在我们有一个相机姿态$\xi$和空间点位置的不准确值，按说如果我们的$\xi ,p$完全准确的话（9）式就会严格成立，$e=0$。但是实际上算出来的$\xi ,p$并不准，所以我们按照这两个不准确的值进行投影的时候得到的值，和我实际测量的像素值就会有差异，上面的（9）式没有体现出这样的逻辑关系，如果你还是不懂的话，请你停下来再仔细整理一下思路！！

我们将所有的误差叠加到一起，具体说就是所有位姿下观测到的所有路标点，然后写成最小二乘的形式：
$$\frac{1}{2}\sum _{i=1}^m\sum _{j=1}^n\Vert e_{ij}{\Vert }^2=\frac{1}{2}\sum _{i=1}^m\sum _{j=1}^n\Vert z_{ij}-h({\xi }_i,p_j){\Vert }^2\text{\hspace{1em}(10)}$$上式的$z_{ij}$表示的是在${\xi }_i$处观察到的路标点$p_j$.

BA优化最终做的就是优化上面这个式子的位姿和路标点，使得整体误差最小。我不知道你是否清楚这个过程，我还是愿意多费几句口舌跟你重复一下这个过程：对于优化上式获得误差和最小时的位姿和路标点，其实就是在不断地寻找每一个位姿、每一个路标点的值，让它不断地接近误差和最小。

至于怎么去寻找，下面我们就讲讲BA的求解

BA的求解

（10）中待优化的变量是位姿和路标，我们将它们写在一起，组成一个整体的待优化变量：
$$x=[{\xi }_1,{\xi }_2,\cdots ,{\xi }_m,p_1,p_2,\cdots ,p_n{]}^T$$相应的，（10）式可以简化为下式：
$$\frac{1}{2}\Vert f(x){\Vert }^2=\frac{1}{2}\sum _{i=1}^m\sum _{j=1}^n\Vert z_{ij}-h({\xi }_i,p_j){\Vert }^2\text{\hspace{1em}(11)}$$那么对（11）进行泰勒展开，保留一阶项：
$$\frac{1}{2}\Vert f(x){\Vert }^2\approx \frac{1}{2}\sum _{i=1}^m\sum _{j=1}^n\Vert e_{ij}+F_{ij}\Delta {\xi }_i+E_{ij}\Delta p_j{\Vert }^2\text{\hspace{1em}(12)}$$其中$F_{ij},E_{ij}$分别是代价函数对相机姿态和路标点的偏导数.

将（12）式右边写成向量的形式：
$$\frac{1}{2}\Vert f(x){\Vert }^2\approx \frac{1}{2}\Vert e+F\Delta x_c+E\Delta x_p{\Vert }^2\text{\hspace{1em}(12)}$$实际上，上面的步骤不知道你注意到没有，我们已经由整体的代价函数，变成了在代价函数某一点处进行一阶近似了，希望你别迷糊了。

对于（12）式的优化，不论是GN还是LM方法，它们的雅克比矩阵可以写成如下形式：
$$J=[FE]$$海森矩阵可以写成：$$H=J^{T}J=\left[\begin{array}{cc}{F}^{T}F& F^{T}E\\ E^{T}F& E^{T}E\end{array}\right]\text{\hspace{1em}(13)}$$
如果你还记得非线性优化那一讲的内容，那么你应该知道对于非线性优化增量的线性方程：
$$H\Delta x=g$$我们要想解出$\Delta x$，就得对$H$矩阵求逆，实际上是一个非常大的矩阵，因为包含了数百个位姿，几十上百万的路标点，所以对于它的求逆能不能实时就决定了，SLAM整个系统能不能实时运行，这一块之前的研究者研究了很多，才找到了$H$的特别之处。

$H$矩阵的特别之处在于，它是稀疏的！！这一点很重要！我们来看一个特别大型的实际例子下$H$矩阵的结构，如下图：

上图就是$H$矩阵的结构，小黑块就代表是有数据的，别的地方都是０，也就是说B和C是对角矩阵块，E是一个普通的矩阵，我们对对角矩阵求逆是很容易的，所以按照分块求逆可以很容易解出$\Delta x$。

我并没有仔细去讲述怎么得出$H$的矩阵形式，这部分内容书中介绍的很详细，所以我就只是告诉你结论。

求出了$\Delta x$之后，对于代价函数的优化就很容易了。实际上我们并不需要自己去设计整个算法，g2o我们已经用过好几次了，我们只要按照要求构造优化变量，程序就会为我们自动去优化和计算。

鲁棒核函数

我们想一想如果某一个测量值误差特别大，那么它的梯度也会很大，那么优化的时候，整体梯度就会被它给拉偏了，这个时候可能就会导致别的边得不到合适的值，所以就必须对边加入鲁棒核函数，抑制某些边过大，我们常常采用Huber核：
$$H(e)\{\begin{array}{rl}& \frac{1}{2}{e}^2当|e|<\delta \\ & \delta (|e|-\frac{1}{2}\delta )其他\end{array}$$

非线性优化是非常重要的，光靠看书我觉得永远都不能真正理解它，如果你有时间可以挑战一下手写一个优化器，完成优化任务，可能你没有g2o或者ceres写得好，但是写完一遍你就真正的理解了非线性优化的真谛了！

实践

实践部分的两个不同库编写的代码，都不能直接运行在比较新的库上，所以需要做一些修改，这部分大家自己改吧，留给大家一个挑战（主要是我不想写了，哈哈）

如果自己都在偷懒，命运又怎么会认可你。别再虚度光阴，叫醒那个沉睡的自己。记住，只要开始，就永远不晚。