自动驾驶（四十）---------视觉SLAM（非线性优化）

       相机与图像我自己相对理解比较深，有不懂的参考这篇文章：https://blog.csdn.net/LWHGMAN/article/details/88976116

       现在我们已经知道，方程中的位姿可以由变换矩阵来描述，然后用李代数进行优化。观测方程由相机成像模型给出。然而，由于噪声的存在，运动方程和观测方程的等式必定不是精确成立的。所以，与其假设数据必须符合方程，不如来讨论，如何在有噪声的数据中进行准确的状态估计。这就是我们本章讨论的问题。由于在 SLAM 问题中，同一个点往往会被一个相机在不同的时间内多次观测，同一个相机在每个时刻观测到的点也不止一个。这些因素交织在一起，使我们拥有了更多的约束，最终能够较好地从噪声数据中恢复出我们需要的东西。

1. 最大后验与最大似然

        SLAM模型由两个方程构成:      (与kalman filter类似)  Xk 乃是相机的位姿。我们可以使用变换矩阵或李代数表示它（由 Tk 或 exp(ξ∧ k )表达）。

        至于观测方程，即针孔相机模型。比如：假设在 xk 处对路标 yj 进行了一次观测，对应到 图像上的像素位置 zk,j，那么，观测方程可以表示成：   这里 K 为相机内参，s 为像素点的距离。同时这里 的 zk,j 和 yj 都必须以齐次坐标来描述。

        在运动和观测方程中，我们通常假设两个噪声项 wk,vk,j 满足零均值的高斯分布。我们希望通过带噪声的数据 z 和 u，推断位姿 x 和地图 y（以 及它们的概率分布），这构成了一个状态估计问题。

       很长一段时间内，研究者们使用滤波器，尤其是扩展卡尔曼滤波器（EKF）求解它。但是卡尔曼滤波器关心当前时刻的状态估计 xk，即只有相邻帧之间的传递，而对之前的状态则不多考虑；相对的，近年来普遍使用的非线性优化方法，使用所有时刻采集到的数据进行状态估计，并被认为优于传统的滤波器，成为当前视觉 SLAM 的主流方法。

      概率论上：对机器人状态的估计，就是求已知输入数据 u 和观测数据 z 的条件下，计算状态 x 的条件概率分布：当我们没有测量运动的传感器（u就是IMU的读数）， 只有一张张的图像时，即只考虑观测方程带来的数据时，相当于估计 P(x|z) 的条件概率分布。利用贝叶斯法则，有：

      贝叶斯法则左侧通常称为后验概率。它右侧的 P(z|x) 称为似然，另一部分 P(x) 称 为先验。直接求后验分布是困难的，但是求一个状态最优估计，使得在该状态下，后验概 率最大化（Maximize a Posterior，MAP），则是可行的：

                                     

2. 最小二乘法

      对于某一次观测：假设了噪声项为了计算使它最大化的 xk, yj，我们往往使用最小化负对数的方 式，来求一个高斯分布的最大似然。（这是在 求出似然函数概率 的最大概率值-->等价于求  后验函数概率 的最大概率值）取它的负对数，则变为：。       

       对原分布求最大化相当于对负对数求最小化。第一项与 x 无 关，可以略去。于是，只要最小化右侧的二次型项，就得到了对状态的最大似然估计。代 入 SLAM 的观测模型，相当于我们在求：

       该式等价于最小化噪声项（即误差）的平方（Σ 范数意义下）。我们现在将运动方程和观测方程同时考虑，不再只单单考虑观测方程。我们定义数据与估计值之间的误差： 。

       并求该误差的平方之和：        这就得到了一个总体意义下的最小二乘问题（Least Square Problem）。我们明白它的最优 解等价于状态的最大似然估计。直观来讲，由于噪声的存在，当我们把估计的轨迹与地图 代入 SLAM 的运动、观测方程中时，它们并不会完美的成立。这时候怎么办呢？我们把状态的估计值（函数中的参数）进行微调，使得整体的误差下降一些。当然这个下降也有限度，它一般会到达 一个极小值。这就是一个典型非线性优化的过程。