视觉SLAM14讲笔记-第10讲-后端1

我们可以看到，前端视觉里程计能给出一个短时间内的轨迹和地图，但由于不可避免的误差累积，这个地图在长时间内是不准确的。所以怎么办？我们人类自己在现实世界中会记录自己走了多少米，还有就是使用路面的标志物来辅助定位，和机器人的方式也是相同的，即同时存在2种数学上的解释：运动方程，观测方程。

因此，我们关心的问题就变成：当我拥有某些运动数据u和观测数据z时，如何来确定状态量位姿x，路标y的分布？进而，如果得到了新时刻的数据，那么他们的分布又将发生怎样的变化？在比较常见且合理的情况下，我们假设状态量和噪声项服从高斯分布，这意味着在程序中只需要存储他们的均值和协方差矩阵即可。均值可看作是对变量最优值的估计，而协方差矩阵则度量了它的不确定性。那么，问题就转变为：当存在一些运动数据和观测数据时，我们如何去估计状态量的高斯分布？

马尔可夫性的引出:
人一辈子可能就是一个公式或概念，开玩笑，马尔可夫应该不止如此。简单的一阶马氏性认为，k时刻状态只与k-1时刻状态有关，而与再之前的无关。如果做出这样的假设，我们就会得到以扩展卡尔曼滤波为代表的滤波器方法。
而另外一种方法是依然考虑k时刻状态与之前所有状态的关系，此时将得到非线性优化为主体的优化框架。

重要的事情说3遍：
卡尔曼滤波的重要前提是运动与观测方程只与前一时刻有关，即满足马尔可夫性。
卡尔曼滤波的重要前提是运动与观测方程只与前一时刻有关，即满足马尔可夫性。
卡尔曼滤波的重要前提是运动与观测方程只与前一时刻有关，即满足马尔可夫性。

卡尔曼滤波的公式很简洁，只要直接带入就可以求解。不过卡尔曼的推导却不是那么直接，如果你连一眼也不看，卡尔曼博士可能会不开心。

贝叶斯公式的引出
$$P(B\mid A)=\frac{P(A\mid B)P(B)}{P(A)}$$
语言描述为：
当A发生的情况下，B发生的概率 等于 B发生的情况下，A发生的概率 乘以B发生的概率，再除以A发生的概率。

先验分布和后验分布的引出
先验分布指的是机器人经过运动方程后的机器人状态的估计分布，后验分布指的是机器人在观测方程后的机器人状态的估计分布

这里停止住高博的思路，因为我实在看不下去了，我之前看过《学习OpenCV》一书里讲解的卡尔曼滤波，我觉的他的方式更容易理解，而且通篇不提贝叶斯，我们知道两个独立事件同时发生的概率等于两个独立事件概率的乘积，即：
$$P(AB)=P(A)P(B)$$
虽然高博也是用的这个公式，但是说是通过贝叶斯公式推导出来的，但是这里这样理解岂不更加简单、直接。

这里A为后验，B为先验，可以理解为
$$P(运动方程+观测方程)=P（观测方程）\ast P（运动方程）$$

后面的证明同高博一致，这里就不一一叙述了。但是我还是建议大家看看《学习OpenCV》的一维卡尔曼证明，相当简介明了。
与OpenCV介绍不同的是，观测方程里最终是关于观测量（路标）的高斯分布，运动方程里最终是关于机器人位姿的高斯分布。我们不禁想问，对于2种不同变量的分布，是否可以相乘呢？为什么不行，因为他们是独立事件，另外再看看高斯分布的概率密度函数，如下：
如果一个随机变量x服从高斯分布$N(\mu ,\sigma )$，那么它的概率密度函数为
$$p(x)=\frac{1}{\sqrt{2\pi }\sigma }exp(-\frac{1}{2}\frac{(x-\mu {)}^2}{{\sigma }^2})$$
其高维形式为：这里x是高维形式。
$$p(x)=\frac{1}{\sqrt{(2\pi {)}^{N}det(\sum )}}exp(-\frac{1}{2}(x-\mu {)}^T{\sum }^{-1}(x-\mu ))$$
我们会发现概率密度函数本身就是一个单纯的数（例如0,2 、0,6等）。

EKF的引出：
我们必须澄清一点：SLAM中的运动方程和观测方程通常是非线性函数，尤其是视觉SLAM中的相机模型，需要使用相机内参数模型和李代数表示的位姿，更不可能是一个线性系统。一个高斯分布，经过非线性变换后，往往不再是高斯分布，所以在非线性系统中，我们必须取一定的近似，将一个非高斯分布近似成高斯分布。
通常的做法是，在某个点附近考虑运动方程及观测方程的一阶泰勒展开，只保留一阶项，即线性的部分，然后按照线性系统进行推导。
细节不再一一复述。
但是EKF也存在很多的缺点，我们通常认为，在同等计算量的情况下，非线性优化取得更好的效果。但是在导航中EKF的性价比又是一个很高的选择。

BA与图优化
在21世纪早期，虽然计算机视觉领域的研究者们已经开始利用BA进行重构，但SLAM的研究者们通常认为包含大量特征点和相机位姿的BA计算量过大，不适合实时计算。直到近十年，人们逐渐认识到SLAM问题中BA的稀疏特性，才使它能够在实时的场景中应用。因此，掌握好BA，深入研究其中的理论和实践细节，是做好视觉SLAM的关键。

稀疏性和边缘化的引出
因为残差函数对位姿和路标的导数存在稀疏性，所以H矩阵是一个镐形矩阵（由于雅克比矩阵的稀疏性，导致H也为稀疏性），由于H矩阵的特殊结构，可以使用Schur消元法（也称为Marginalization(边缘化)）来进行求解。由于是先求处理关于$x_c$的边缘分布，所以称为边缘化。

鲁棒核函数的引出
如果出现了误匹配，当误差很大时，二范数平方和作为目标函数会使误差增长太快，于是就有了核函数的存在。具体的方式是，把原先误差的二范数度量替换成一个增长没有那么快的函数，同时保证自己的光滑性质。因为他们使得整个优化结果更为稳健，所以又叫它们鲁棒核函数（Robust Kernel）。

虽然表面上很复杂，但是许多软件库为我们提供了细节操作，而我们需要做的主要是构造Bundle Adjustment问题。