解读《视觉SLAM十四讲》，带你一步一步入门视觉SLAM—— 第 10 讲 后端1

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

概述

状态估计和概率解释

我们再来看一下SLAM过程的运动方程和观测方程：
{xk=f(xk−1,uk)+wk　　　　（运动方程）zk=h(xk)+vk，k=1,…,N（观测方程）(0) \left\{ \begin{aligned} &x_k = f(x_{k-1},u_k)+w_k 　　　　（运动方程）\\ &z_{k}=h(x_k)+v_{k} ，k=1,\dots,N（观测方程） \end{aligned} \right. \tag 0 {​xk​=f(xk−1​,uk​)+wk​　　　　（运动方程）zk​=h(xk​)+vk​，k=1,…,N（观测方程）​(0)这两个方程我们已经多次见过了，依然有必要再仔细看看它：
　　系统状态：xxx
　　输入：　　uuu（也可以理解为控制量，这里和书中的说法不一样！！）
　　过程噪声：www
　　测量：　　zzz
　　测量噪声：vvv

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

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

根据贝叶斯公式：
P(xk∣x0,u1:k,z1:k)∝P(zk∣xk)P(xk∣x0,u1:k,z1:k−1)(1) 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}) \tag 1 P(xk​∣x0​,u1:k​,z1:k​)∝P(zk​∣xk​)P(xk​∣x0​,u1:k​,z1:k−1​)(1)对于上式不用过多解释了，非常简单的贝叶斯公式。
　　P(zk∣xk)P(z_k|x_k)P(zk​∣xk​)是似然，也就是xkx_kxk​条件下，zkz_kzk​的概率分布，对应到SAMM问题中，也就是某一个状态下的测量值，这个可以通过测量方程给出。
　　P(xk∣x0,u1:k,z1:k−1)P(x_k|x_0,u_{1:k},z_{1:k-1})P(xk​∣x0​,u1:k​,z1:k−1​)是先验，它的求法和我们选取什么样的状态有关，可以选择上一个时刻的状态，也可以选取以前所有的状态。如果按照xk−1x_{k-1}xk−1​时刻的状态为条件：
P(xk∣x0,u1:k,z1:k−1)=∫P(xk∣xk−1,x0,u1:k,z1:k−1)P(xk−1∣x0,u1:k,z1:k−1)dxk−1(2) 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} \tag 2 P(xk​∣x0​,u1:k​,z1:k−1​)=∫P(xk​∣xk−1​,x0​,u1:k​,z1:k−1​)P(xk−1​∣x0​,u1:k​,z1:k−1​)dxk−1​(2)上式子（2）的操作方法很多，选择不同的操作，就对应着不同的理论，如果认为只和上一时刻有关，那么就会得到以卡尔曼滤波器为代表的滤波器方法。如果认为和所有状态都有关，那么得到的就是非线性优化的方法。

线性系统和KF

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

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

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

非线性系统和EKF

上面讲的是线性的系统，对于（０）式这样的非线性的系统，我们可以将其通过泰勒展开保留一阶项，也就可以将非线性系统线性化，然后同样的思路推导出扩展卡尔曼方程：
　　①预测：
x‾k=f(x^k−1,uk),　　P‾k=FP^k−1FT+Rk(6) \overline x_k = f(\hat x_{k-1},u_k), 　　\overline P_k=F\hat P_{k-1}F^{T} +R_k \tag 6 xk​=f(x^k−1​,uk​),　　Pk​=FP^k−1​FT+Rk​(6)　　②更新：先计算卡尔曼增益KKK：
Kk=P‾kHT(HP‾kHT+Qk)−1(7) K_k=\overline P_kH^{T}(H \overline P_kH^{T}+Q_k)^{-1} \tag 7 Kk​=Pk​HT(HPk​HT+Qk​)−1(7) 　　然后计算后验概率分布
x^k=x‾k+Kk(zk−h(x‾k))(8) \hat x_k = \overline x_k + K_k(z_k-h(\overline x_k)) \tag 8 x^k​=xk​+Kk​(zk​−h(xk​))(8)P^k=(I−KkH)P‾k \hat P_k = (I-K_kH) \overline P_k P^k​=(I−Kk​H)Pk​
其中FFF是f(x)f(x)f(x)关于xxx的偏导数，HHH是h(x)h(x)h(x)关于xxx的偏导数。

EKF的讨论

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

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

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

BA和图优化

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

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

投影模型和BA代价函数

1.首先将一个世界坐标下的空间点ppp转换到相机坐标下：
P′=Rp+t=[X′,Y′,Z′]T P'=Rp+t=[X',Y',Z']^{T} P′=Rp+t=[X′,Y′,Z′]T2.然后P′P'P′投影到归一化平面，得到归一化坐标：
Pc=[uc,vc,1]T=[X′Z′,Y′Z′,1]T P_c=[u_c,v_c,1]^{T}=[\frac{X'}{Z'},\frac{Y'}{Z'},1]^{T} Pc​=[uc​,vc​,1]T=[Z′X′​,Z′Y′​,1]T
3.考虑归一化坐标的畸变情况，得到去畸变（只考虑径向畸变）后的归一化坐标：
{uc′=uc(1+k1rc2+k2rc4)vc′=vc(1+k1rc2+k2rc4) \left\{ \begin{aligned} u_c'=u_c(1+k_1r_c^{2}+k_2r_c^4) \\ v_c'=v_c(1+k_1r_c^2+k_2r_c^4) \end{aligned} \right. {uc′​=uc​(1+k1​rc2​+k2​rc4​)vc′​=vc​(1+k1​rc2​+k2​rc4​)​
4.最后根据相机内参模型，将去畸变后归一化平面的点投影到像素平面：
{us=fxuc′+cxvs=fyvc′+cy \left\{ \begin{aligned} u_s=f_xu_c'+c_x \\ v_s=f_yv_c'+c_y \end{aligned} \right. {us​=fx​uc′​+cx​vs​=fy​vc′​+cy​​
上面四步也就是前面讲的观测方程，在前面我们的观测方程被抽象成了：
z=h(x,y) z=h(x,y) z=h(x,y)那么xxx就相当于是这里的相机外参数R,tR,tR,t，对应的李代数为ξ\xiξ，yyy就相当于是这里的三维点ppp，zzz就是这里的观测数据[us,vs][u_s,v_s][us​,vs​]。那么观测误差就是：
e=z−h(ξ,p)(9) e = z-h(\xi,p) \tag 9 e=z−h(ξ,p)(9)很多人不明白这个式子，在这里我解释一下：空间中的点已经投影到了像素平面上，这个已经是一个事实了，谁也改变不了了。现在我们有一个相机姿态ξ\xiξ和空间的不准确值，按说如果我们的ξ,p\xi,pξ,p完全准确的话（9）式就会严格成立，但是并不准，所以我们按照这两个不准确的值进行投影的时候得到的值，和我实际测量的像素值就会有差异，上面的（9）式没有体现出这样的逻辑关系，如果你还是不懂的话，请你停下来再仔细整理一下思路！！

我们将所有的误差叠加到一起，具体说就是所有位姿下观测到的所有路标点，然后写成最小二乘的形式：
12∑i=1m∑j=1n∥eij∥2=12∑i=1m∑j=1n∥zij−h(ξi,pj)∥2(10) \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 \tag{10} 21​i=1∑m​j=1∑n​∥eij​∥2=21​i=1∑m​j=1∑n​∥zij​−h(ξi​,pj​)∥2(10)上式的zijz_{ij}zij​表示的是在ξi\xi_iξi​处观察到的路标点pjp_jpj​.

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

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

BA的求解

（10）中待优化的变量是位姿和路标，我们将它们写在一起，组成一个整体的待优化变量：
x=[ξ1,ξ2,⋯ ,ξm,p1,p2,⋯ ,pn]T x=[\xi_1,\xi_2,\cdots,\xi_m,p_1,p_2,\cdots,p_n]^{T} x=[ξ1​,ξ2​,⋯,ξm​,p1​,p2​,⋯,pn​]T相应的，（10）式可以简化为下式：
12∥f(x)∥2=12∑i=1m∑j=1n∥zij−h(ξi,pj)∥2(11) \frac{1}{2} \| f(x)\|^2 = \frac{1}{2} \sum_{i=1}^{m} \sum_{j=1}^{n}\| z_{ij}-h(\xi_i,p_j) \|^2 \tag{11} 21​∥f(x)∥2=21​i=1∑m​j=1∑n​∥zij​−h(ξi​,pj​)∥2(11)那么对（11）进行泰勒展开，保留一阶项：
12∥f(x)∥2≈12∑i=1m∑j=1n∥eij+FijΔξi+EijΔpj∥2(12) \frac{1}{2} \| f(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 \tag{12} 21​∥f(x)∥2≈21​i=1∑m​j=1∑n​∥eij​+Fij​Δξi​+Eij​Δpj​∥2(12)其中Fij,EijF_{ij},E_{ij}Fij​,Eij​分别是代价函数对相机姿态和路标点的偏导数.

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

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

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

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

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

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

鲁棒核函数

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

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

实践

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

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

• 点赞
• 收藏
• 分享
• 文章举报