SLAM学习笔记-------------（六）非线性优化

由于我们得到的数据通常都是受到各种噪声的影响的，所以运动方程和观测方程都只能近似成立。

所以我们必须考虑如何在有噪声的数据中进行准确的状态估计。

6.1 状态估计问题

经典的SLAM模型由运动方程和观测方程组成。

经过前面几讲的学习，这两个抽象的方程我们已经可以具体的描述了。

首先运动方程，Xk是位姿，我们知道可以用变换矩阵来描述SE(3)

然后，观测方程，上一讲刚学过就是针孔相机模型。

但是呢，这都是理想状态，实际情况下，噪声的影响是我们不可忽略的因素。

处理这个状态估计问题的方法主要分为两种：

持有当前时刻的估计状态，用新的数据来估计它。这种方式称为滤波器。
把数据“攒”起来一并处理，这种方法称为批量

滤波器方法只关心当前时刻的状态。批量方法可以在更大范围达到最优化。所以批量是主流方法。

极端情况下，我们让机器人收集所有时刻的数据，带回计算中心统一处理。（这正是SfM（Structure from Motion 从运动恢复结构）的做法。但是这种做法，显然不是实时的。不能满足SLAM的要求。所以我们采用折中的办法：我们固定一些历史轨迹，仅对当前时刻附近的一些轨迹进行优化。（这就是滑动窗口估计法）

本节我们主要介绍以非线性优化为主的批量优化方法。首先做一些定义：

那么这个状态估计的问题就转换为了，求状态x,y的条件概率分布。

使用贝叶斯法则，可以转换为这个样子。（为什么把分母去掉呢？因为分母与待估计的x,y没关系，故可以看做对P(z,u|x,y)P(x,y)的求解

但是这个东西啊，很难解，P(x,y|z,u)不好求

但是呢，我们可以求它的一个估计，就是使得P(x,y|z,u)最大的一个估计。

这里的似然和先验，假如我们不知道当前位姿或路标，那么就进一步简化为只求似然 P(z,u | x,y) 就好了。

P(z,u | x,y) 的含义是在已知位置的情况下，可能产生什么样的观测数据？

那我们求他的最大似然估计，意思就是“什么样的位置下，最有可能产生现在观测到的数据？”

怎么求这个最大似然估计呢？（高斯分布就是正态分布）

为什么取负对数呢？由于对数函数是递增的，所以要求原函数的最大值，那么就是求这个负对数的最小值

求最小值呢，又发现，这两项相加，其中左边这项跟x没关系。所以，就是要求右边这项的最小值。

上面这块描述，我没看懂。。。。。所以先放这吧

定义各次输入和观测数据与模型之间的误差为：

虽然前边的没看懂，但是先略过不说。

只要知道，经过一系列数学的魔法，我们把最大似然估计问题转化为了一个最小二乘的问题。

接下来我们的目标就是求解这个最小二乘（通用的无约束非线性最小二乘问题）。

6.2 非线性最小二乘

考虑一个最简单的最小二乘问题

显然，要求这个最小值。我们都知道，求导就可以。令导数等于0，求出极值点，比较大小即可。

但是问题就是，如果这个函数比较复杂，没办法求导，或者求导之后，导数等于0的式子无法解出来，怎么办呢？

那种容易求导的，就叫线性最小二乘，复杂的，我们就叫非线性最小二乘问题。

我们通常用迭代的方法来解决

简单来说，就是找一个小的增量，让这个函数下降，然后一直找，来不断逼近我们要求的最小值。

直到函数下降到增量足够小的时候，我们就算法收敛了，目标函数达到了极小值。

那么问题的关键就是：如何找这个增量？

假设我们在 $x_{k}$ ，想找到增量 $\Delta x_{k}$ 。最直观的方法就是把目标函数在 $x_{k}$ 附近泰勒展开。

你可以选择保留泰勒展开的一阶项或二阶项，对应的求解方法就是一阶梯度下降和二阶梯度下降。

我们保留一阶梯度，取增量方向为反向的梯度，沿着反向梯度前进，目标函数必然下降，这叫最速下降法。
也可以保留二阶梯度信息，得到如下式子
右侧式子对 $\Delta x$ 求导并令其等于0，可得：
求解这个线性方程，解出来 $\Delta x$ 即可。该方法称为牛顿法。

这俩方法都有缺点：

最速法太贪心，容易走出锯齿路线，反而增加迭代次数

牛顿法的问题是，需要求H矩阵。这玩意不好求

故而，我们介绍两种更加实用的方法：

高斯牛顿法

这个式子的优点在于可以直接求导求最小值了

先把平方项展开，然后求导，令导数等于0

最终得到这个方程就是关于 $\Delta x$ 的线性方程组。我们称它为高斯牛顿方程（或增量方程，或正规方程）。

左边定义为H，右边定义为g

注意，这里把左边记作H是有意义的。我们用 $JJ^{T}$ 作为Hessian矩阵的近似，从而避免了对H矩阵的求解。

所以高斯牛顿法的步骤如下：

这个方法还存在一个问题就是，求解增量方程那一步。我们都知道，求解 $\Delta x$ 只需要式子左乘一个 $H^{-1}$ 就好了

但是关键就是 $JJ^{T}$ 不能保证一定可逆。所以说就可能会导致我们使用这个方法的时候出问题。

列文伯格-马夸尔特方法在一定程度上优化了这个问题。

（一般认为列文伯格-马夸尔特方法比高斯牛顿法更健壮，但是它比高斯牛顿收敛的更慢，所以也称他为阻尼牛顿法）

列文伯格-马夸尔特方法属于信赖区域的方法

使用信赖区域，我们把求解过程优化为如下框架：

注意：这里面的2和0.5都是经验值，可以自由设定。

另外6.35的这个公式，我们把增量限制在一个半径为 $\mu$ 的球里面，加上D以后，就是个椭球。

除了对D的优化，还有一个求解那个方程的问题

这个方程是一个带约束的优化问题。构建拉格朗日函数来求解（我的理解就是，这相当于高数中的条件极值解法，不知道对不对）

这里只介绍了两种非线性优化方法

你可以注意到，这两个方法都需要提供一个初始值。

事实上，非线性优化方法的所有的迭代求解方法都需要用户提供一个良好的初始值。（我们一般用ICP，PnP来提供初始值）

6.3 实践曲线拟合问题

这里有个手写高斯牛顿的程序，我就先跳过了

6.3.2 Ceres

Ceres是一个最小二乘问题求解库

这里我不写了，我安装了库，执行了下给的样例代码，也执行成功了

后边有机会了，再来分析代码

6.3.3 g2o

同上