视觉slam14讲之后端优化

     前端视觉里程计能给出一个短时间内的轨迹和地图，但不可避免的误差累积，这个地图在长时间内是不准确的。要想构建一个尺度，规模更大的优化问题，以考虑长时间内的最优轨迹和地图，这时后端优化就出现了。

1.滤波器模型

     a. 马尔可夫性，当前状态只和上一个时刻有关。 P(Xk| Xk-1,X0, U1:k,Z1:k-1) = P(Xk| Xk-1, Uk)。把K-1时刻的状态分布推导至k时刻，也就是说，在程序运行期间，我们只要维护一个状态量，对它不断地进行迭代和更新即可。
     b. 线性高斯系统，运动方程可以由线性方程来描述。

     利用马尔可夫性，假设我们知道了k-1时刻的后验状态估计及协方差，现在根据k时刻的输入和观测数据，确定Xk的后验分布。
     c. 卡尔曼滤波。其第一步是通过运动方程确定Xk的先验分布。这一步称为预测，它显示了如何从上一个时刻的状态，根据输入信息(但有噪音)推断当前时刻的状态分布，这个分布叫做先验。（后验在b中说过了，可以对比一下）。在线性高斯系统中，卡尔曼滤波器构成了该系统的最大后验概率估计，而且由于高斯分布经过线性变换后仍服从高斯分布，所以整个过程中我们没有进行任何的近似。卡尔曼滤波器构成了线性系统最大无偏估计。

2.非线性系统和EKF

     slam中的运动方程和观测方程通常是非线性函数，尤其是视觉slam中的相机模型，需要使用相机内参模型及李代数表示位姿，更不太可能是一个线性系统。我们往往希望把卡尔曼滤波器的结果扩展到非线性系统中，称为扩展卡尔曼滤波器（EKF）。通常在某个点附近考虑运动方程及观测方程的一阶泰勒展开，只保留一阶项，即线性的部分，然后按照线性系统进行推导。
     a. 运动方程与观测方程进行线性化（相当于一阶泰勒展开），再进行预测步骤，最后根据贝叶斯展开式，推导出Xk的后验概率形式。
      b. EKF的局限性
         首先，滤波器方法一定程度上假设了马尔可夫性，也就是k时刻的状态只与k-1时刻相关，而与k-1之前的状态和观测都无关。如果当前帧确实与很久之前的数据有关（例如回环），那么滤波器就会很难处理。
         其次，EKF滤波器仅在Xk-1处做了一次线性化，然后根据这次线性化结果，把后概率给算出来。这默认该点处的线性化近似在后概率处仍然是有效的。而实际上，当我们离工作点较远时，一阶泰勒展开与整个函数区别其实很大。
         最后，EKF需要对存储状态量的均值和方差，并对它们进行维护和更新。如果把路标也放进状态，由于视觉slam中路标数量很大，这个存储量相当可观，且与状态量呈平方增长，因此EKF SLAM普遍认为不适合大型场景。

3. BA与图优化

     a. Bundle Adjustment，是指从视觉重建中提取出最优的3D模型和相机参数（内参数和外参数）。从每一个特征点反射出来的几束光线，在我们把相机姿态和特征点空间位置做出最优的调整之后，最后收束到相机光心的这个过程。在图优化框架的视觉slam算法中，BA起核心作用。
     b. 投影模型和BA代价函数。

4.后端优化之位姿图与因子图

     a. BA为主的图优化，能精确的优化每个相机位姿与特征点位置。但缺陷却十分明显，大量的特征点的存在严重降低计算效率，导致计算量越来越大以至于无法实时化。
     b. 位姿图-完全不管路标而只管轨迹，只构建只有轨迹的图优化，而位姿节点之间的边由两个关键帧通过特征匹配之后得到的运动估计来给定初始值。一旦初始估计完成，我们就不再优化哪些路标点的位置，而只关心所有的相机位姿之间的联系。省去大量的特征点优化的计算，只保留了关键帧的轨迹，从而构建了所谓的位姿图(Pose Graph)。
      c. *因子图。从贝叶斯网络的角度来看，SLAM可以自然地表达成一个动态贝叶斯网路。它是一种无向图，由两种节点组成：表示优化变量大的变量节点，以及表示因子的因子节点。
用圆圈表示变量节点。与上述贝叶斯网络不同，这里的变量是SLAM中待优化的部分，即相机位姿x和路标L。