自动驾驶（四十二）---------视觉SLAM（后端优化）

       前面前端的内容，只能求出短时间或者说相邻两帧的转换关系，并建立局部意义的地图。然而当时间延长或者规模变大之后，那么将会出现累积误差。而后端就是要解决这个长时间、大规模建图的问题。

1. 状态估计

       SLAM是由两个方程的实现为线索的，分别是运动方程和观测方程。每个方程都受噪声影响，假设状态量和噪声项服从高斯分布——意味着在程序中，只需要储存它们的均值和协方差矩阵即可。均值可看作是对变量最优值的估计，而协方差矩阵则度量了它的不确定性。

                                               

       观测方程中，在 一个位置通常只能看到一小部分路标。而且，由于视觉 SLAM 特征点数量众多，所 以实际当中观测方程数量会远远大于运动方程的数量。我们可能没有测量运动的装置，所以也可能没有运动方程。在这个情况下，有若干种 处理方式：认为确实没有运动方程，或假设相机不动，或假设相机匀速运动。这几种 方式都是可行的。在没有运动方程的情况下，整个优化问题就只由许多个观测方程组成。这两个方程是相辅相成的。少一个，都将会有巨大的影响。我们举个例子来说明两个方程的作用。

        只有运动方程时（没有观测方程），相当于我们蒙着眼睛在一个 未知的地方走路。尽管我们知道自己每一步走了多远，但是随着时间增长，我们将对自己 的位置越来越不确定，我们对位 置方差的估计将越来越大。

       当我们睁开眼睛时（同时有观测方程和观测），由于能够不断地观测到外部场景，使得位置估计的不确定性变小了。如果用椭圆或椭球直观地表达协方 差阵，当没有观测数据时，这个圆会随着运动越来越大；而如果有正确观测的话，圆就会缩 小至一定的大小，保持稳定。

                                            

      

       所以，在后端优化中，就是实现这个行走的过程，使得误差从大椭圆到一定圆的过程。后端优化考虑一个更长时间内（或所有时间内）的状态估计问题。根据用到的数据信息性质不同，处理方式分为批量和渐进。

        批量：不仅使用过去的信息更新自己的状态，也会用未来的信息来更新自己。（非线性优化为代表）
        渐进：如果当前的状态只由过去的时刻决定，甚至只由前一个时刻决定。（扩展卡尔曼滤波EKF 为代表）

       后端优化是一个状态估计问题。首先，由于位姿和路标点都是待估计的变量，令 xk 为 k 时刻的所有未知量，它包含了当前时刻的相机位姿与 m 个路标点。  于是，运动方程与观测方程的形式可写得更加简洁。（这时 x 中已经包含了之前的 y 了）

       但是此时运动方程只是描述了前后两帧之间的联系关系。我们希望用过去 0 到 k 中的数据，来估计现在的状态分布。
于是有：   按照贝叶斯法则，交换Zk和Xk位置     （似然产生Zk补充了先验）

       这里第一项称为似然，第二项称为先验。似然由观测方程给定，而先验部分，我们要明白当前状态 xk 是基于过去所有的状态估计得来的。至少，它 会受 xk−1 影响。于是按照 xk−1 时刻为条件概率展开：（后面之于前面的目的在于消去Xk-1）

       考虑更久之前的状态，也可以继续对此式进行展开。我们给出了贝叶斯估计，虽然上式还没有具体的概率分布 形式，所以我还没法实际地操作它。对这一步的后续处理，方法上产生了一些分歧。

        其一是假设马尔可夫性，简单的一阶马氏性认为，k 时刻状态只 与 k −1 时刻状态有关，而与再之前的无关。如果做出这样的假设，我们就会得到以扩展卡 尔曼滤波（EKF）为代表的滤波器方法。在滤波方法中，我们会从某时刻的状态估计，推导到下一个时刻。（渐进的）

        另外一种方法是依然考虑 k 时刻状态与之前所有状态的关系，此时将得 到非线性优化为主体的优化框架。目前视觉 SLAM 主流为非线性优化方法。（批量的）