ORB-SLAM2 ---- 非线性优化在SLAM中的应用（二）

一、非线性优化的在SLAM中的应用简介

学到这里我们应该清楚的知道，SLAM的重中之重是优化，在大学中学过最优化或者优化理论的同学知道，优化方法有很多种，而非线性优化的特点是迭代次数少，且精度高，对于不同的精度可以使用不同的优化方法。例如对计算速度要求高，但对精度要求相对不是很高的场合，可以使用高斯-牛顿的方法来进行优化，如果对精度要求高的场合，使用列文伯格-马夸尔方法更加合适。在学习ORBSLAM2比较仔细的会发现，使用的优化全都是列文伯格-马夸尔方法。故这里的讲解重点也放在列文伯格-马夸尔方法上。

二、几种非线性优化方法

本人还是那个观点，先要知道目的再去学习方法，才能真正的弄明白原理。在前面讲到，我们优化的目标是一个误差（前面已经讲述了如何构造误差函数），可以将这个误差当作目标函数，那么我们要做的就是让这个误差尽可能的小，误差最小时对应的那个相机位姿，就是我们优化后的相机位姿。故我们的目的时求：

最小时x的值（及相机的位姿）

1. 高斯牛顿法(Gauss-Newton)

高斯-牛顿法时最优化算法中最简单的方法之一。他的原理是将目标函数进行一阶泰勒展开

为了求增量Δx，将问题转换为最小二乘问题：

我们使用求导的方法来得到极值：




最后面的这个带海森矩阵的函数就是我们高斯-牛顿方程。高斯牛顿法用雅可比矩阵和他的转置的乘积来近似海森矩阵（H），从而忽略计算H的过程。求解增量方程是整个优化问题的核心所在。得到这个方程后，高斯牛顿法的算法步骤可以写成：

2. 列文伯格-马夸尔方法（LM）

列文伯格-马夸尔方法是高斯-牛顿法的进阶版本，这个方法认为GN方法不够稳定（也就是常说的鲁棒性不好）。认为增量超出一定范围就近似得就不准确，应该加一个区间，让增量不能超过这个区间，而这个区间被叫做信赖区间，用一个ρ来刻画近似得好坏程度：

我们可以很直观得看出，分子为实际得函数下降值，分母为模型预测得下降值。很显然ρ接近于，则说明近似很好，如果ρ得值太小，说明近似效果差，就要缩小信赖区间，如果ρ比较大，则说明实际下降比预期得更大，我们可以放大信赖区间。
下面是改良的非线性优化框架：

这里的D，列文伯格提出的方法是D取单位矩阵，马夸尔的方法提出取成非负对角阵—通常用
在前面的框架中给函数多了一个约束条件，我们用考研高数中的方法来求解（拉格朗日乘除法）：

同理，对增量Δx求导来求极值：

这个公式就是列文伯格-马夸尔公式，很显然其比GN公式多了一项。这个公式的优点从公式本身分析就能发现，当参数λ较小时，海森矩阵H站主导，这时更接近二次模型，当参数λ较大时，后面一项站主导，这时更接近一阶梯度法。也就是说这个函数更加的稳定，这也是ORBSLAM2使用LM方法的原因。

3. 总结

ORB-SLAM2 ---- 非线性优化在SLAM中的应用的第一篇文章，主要是让大家将代码和数学联系起来，这一篇的内容，主要是讲非线性优化在代码中的应用。接下来的一张，会从整个优化的角度来讲数学和代码联系起来。会讲到图优化和扩展的卡尔曼滤波器，SLAM的就业目前主要是无人机和自动驾驶两个大方向，无人驾驶更偏向使用图优化，而无人机更多的使用的是扩展的卡尔曼滤波器。希望这三篇文章能对你有帮助，如果有问题，欢迎随时交流。