视觉SLAM十四讲（6）：非线性优化原理

视觉SLAM十四讲（6）：非线性优化原理

总结：

经典SLAM模型的位姿可以由变换矩阵来描述，然后用李代数进行优化。由于噪声的存在，运动方程和观测方程的等式必定不是精确成立的。得到的数据通常是受各种未知噪声影响的。如何在有噪声的数据中进行准确的状态估计，这需要非线性优化的相关知识。

第一步：建模

第二步：概率最大化问题转化为非线性最小二乘问题：

因为噪声项满足：

它的概率密度函数展开形式为：

对其取负对数，则变为：

因为对数函数是单调递增的，所以对原函数求最大化相当于对负对数求最小化。在最小化上式的x时，第一项与x无关，可以略去。于是，只要最小化右侧的二次型项，就得到了对状态的最大似然估计。代入SLAM的观测模型，相当于在求：

该式等价于最小化噪声项（即误差）的一个二次型。这个二次型称为马哈拉诺比斯距离,又叫马氏距离。

现在考虑批量时刻的数据。通常假设各个时刻的输入和观测是相互独立的，这意味着各个输入之间是独立的，各个观测之间是独立的，并且输入和观测也是独立的。于是可以对联合概率分布进行因式分解：

这说明可以独立地处理各时刻的运动和观测。定义各次输入和观测数据与模型之间的误差：

那么，最小化所有时刻估计值与真实值之间的马氏距离，等价于求最大似然估计。负对数允许把乘积变成求和：

这样就得到了一个非线性最小二乘问题，它的解等价于状态的最大似然估计，在因子图中就是所有因子的乘积最大化。直观上看，由于噪声的存在，当我们把估计出的轨迹与地图代入SLAM的运动、观测方程中时，它们并不会完美地成立。这时对状态的估计值进行微调，使得整体的误差下降一些，一般会到达一个极小值。这就是一个典型非线性优化的过程。

第三步：求解非线性最小二乘问题

求解非线性最小二乘优化问题的方法很多，而高斯牛顿法是其中最简单和最常用的方法。由前面可以假设因子的函数形式是：

1. 给定初始值x0。

2. 对于第k次迭代，求出当前的雅可比矩阵

和误差函数

3. 求解增量方程：

4. 若

足够小，则停止。否则，令

返回第2步。