视觉SLAM笔记（27） 非线性最小二乘

---

1. 最小二乘

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

这里自变量 $x$ ∈ $R$ⁿ， $f$ 是任意一个非线性函数，设它有 m 维： $f(x)$ ∈ $R$^m

下面讨论如何求解这样一个优化问题
如果 $f$ 是个数学形式上很简单的函数，那问题也许可以用解析形式来求
令目标函数的导数为零，然后求解 $x$ 的最优值，就和一个求二元函数的极值一样：

解此方程，就得到了导数为零处的极值
它们可能是极大、极小或鞍点处的值，只要挨个儿比较它们的函数值大小即可

但是，这个方程是否容易求解呢？这取决于 f 导函数的形式
在 SLAM 中，使用李代数来表示机器人的旋转和位移
尽管在之前讨论了它的导数形式，但这不代表就能够顺利求解上式这样一个复杂的非线性方程

对于不方便直接求解的最小二乘问题，可以用迭代的方式
从一个初始值出发，不断地更新当前的优化变量，使目标函数下降
具体步骤可列写如下：

这让求解导函数为零的问题，变成了一个不断寻找梯度并下降的过程
直到某个时刻增量非常小，无法再使函数下降
此时算法收敛，目标达到了一个极小，完成了寻找极小值的过程
在这个过程中，只要找到迭代点的梯度方向即可，而无需寻找全局导函数为零的情况

接下来的问题是，增量 $∆x$_k 如何确定？
实际上，研究者们已经花费了大量精力探索增量的求解方式
将介绍两类办法，它们用不同的手段来寻找这个增量
目前这两种方法在视觉 SLAM 的优化问题上也被广泛采用，大多数优化库都可以使用它们

---

2. 一阶和二阶梯度法

求解增量最直观的方式是将目标函数在 $x$ 附近进行泰勒展开：

这里 $J$ 是 $∥f(x)∥$² 关于 $x$ 的导数（雅可比矩阵），而 $H$ 则是二阶导数（海塞（Hessian）矩阵）
可以选择保留泰勒展开的一阶或二阶项，对应的求解方法则为一阶梯度或二阶梯度法
如果保留一阶梯度，那么增量的方向为：

它的直观意义非常简单，只要沿着反向梯度方向前进即可
当然，还需要该方向上取一个步长 $λ$，求得最快的下降方式，这种方法被称为最速下降法
另一方面，如果保留二阶梯度信息，那么增量方程为：

求右侧等式关于 $∆x$ 的导数并令它为零，就得到了增量的解：

该方法称又为牛顿法
看到，一阶和二阶梯度法都十分直观，只要把函数在迭代点附近进行泰勒展开，并针对更新量作最小化即可
由于泰勒展开之后函数变成了多项式，所以求解增量时只需解线性方程即可
避免了直接求导函数为零这样的非线性方程的困难

不过，这两种方法也存在它们自身的问题
最速下降法过于贪心，容易走出锯齿路线，反而增加了迭代次数

而牛顿法则需要计算目标函数的 H 矩阵
这在问题规模较大时非常困难，通常倾向于避免 $H$ 的计算

---

3. 高斯-牛顿迭代法

高斯-牛顿迭代法 （Gauss Newton） 是最优化算法里面最简单的方法之一
它的思想是将 $f(x)$ 进行一阶的泰勒展开（请注意不是目标函数 $f(x)$²）：

这里 $J(x)$ 为 $f(x)$ 关于 $x$ 的导数，实际上是一个 m × n 的矩阵，也是一个雅可比矩阵
根据前面的框架，当前的目标是为了寻找下降矢量 $∆x$，使得 $∥f (x + ∆x)∥$² 达到最小
为了求 $∆x$，需要解一个线性的最小二乘问题：

这个方程与之前有什么不一样呢？
根据极值条件，将上述目标函数对 ∆x 求导，并令导数为零
由于这里考虑的是 $∆x$ 的导数（而不是 $x$），最后将得到一个线性的方程
为此，先展开目标函数的平方项：

求上式关于 $∆x$ 的导数，并令其为零：

可以得到如下方程组：

注意，要求解的变量是 $∆x$，因此这是一个线性方程组，称它为增量方程
也可以称为高斯牛顿方程 (Gauss Newton equations) 或者正规方程 (Normal equations)

把左边的系数定义为 $H$，右边定义为 $g$，那么上式变为：

这里把左侧记作 H 是有意义的
对比牛顿法可见， Gauss-Newton 用 $J$^T $J$ 作为牛顿法中二阶 Hessian 矩阵的近似，从而省略了计算 $H$ 的过程
求解增量方程是整个优化问题的核心所在
如果能够顺利解出该方程，那么 Gauss-Newton 的算法步骤可以写成：

从算法步骤中可以看到，增量方程的求解占据着主要地位
原则上，它要求所用的近似 H 矩阵是可逆的（而且是正定的），但实际数据中计算得到的 $J$^T $J$ 却只有半正定性
也就是说，在使用 Gauss Newton 方法时，可能出现 $J$^T $J$ 为奇异矩阵或者病态 (illcondition) 的情况
此时增量的稳定性较差，导致算法不收敛
更严重的是，就算假设 H 非奇异也非病态，如果求出来的步长 $∆x$ 太大，也会导致采用的局部近似
不够准确
这样一来，甚至都无法保证它的迭代收敛，哪怕是让目标函数变得更大都是有可能的

尽管 Gauss Newton 法有这些缺点，但是它依然值得去学习
因为在非线性优化里，相当多的算法都可以归结为 Gauss Newton 法的变种
这些算法都借助了 Gauss Newton 法的思想并且通过自己的改进修正 Gauss Newton 法的缺点

例如一些线搜索方法 (line search method)，这类改进就是加入了一个标量 $α$，在确定了 $∆x$ 进一步找到 $α$
使得 $∥f(x + α∆x)∥$² 达到最小，而不是像 Gauss Newton 法那样简单地令 $α$ = 1

---

4. 阻尼牛顿法

阻尼牛顿法（Levenberg-Marquadt）在一定程度上修正了这些问题，一般认为它比 Gauss Newton 更为鲁棒
尽管它的收敛速度可能会比 Gauss Newton 更慢，但是在 SLAM 里面却被大量应用

由于 Gauss-Newton 方法中采用的近似二阶泰勒展开只能在展开点附近有较好的近似效
所以很自然地想到应该给 $∆x$ 添加一个信赖区域（Trust Region），不能让它太大而使得近似不准确
非线性优化种有一系列这类方法，这类方法也被称之为信赖区域方法 (Trust Region Method)

在信赖区域里边，认为近似是有效的
出了这个区域，近似可能会出问题

一个比较好的方法是根据近似模型跟实际函数之间的差异来确定这个范围：
如果差异小，就让范围尽可能大；如果差异大，就缩小这个近似范围
因此，考虑使用：

来判断泰勒近似是否够好
$ρ$ 的分子是实际函数下降的值，分母是近似模型下降的值
如果 $ρ$ 接近于 1，则近似是好的
如果 $ρ$ 太小，说明实际减小的值远少于近似减小的值，则认为近似比较差，需要缩小近似范围
如果 $ρ$ 比较大，则说明实际下降的比预计的更大，可以放大近似范围

于是，构建一个改良版的非线性优化框架，该框架会比 Gauss Newton 有更好的效果：

这里近似范围扩大的倍数和阈值都是经验值，可以替换成别的数值
在上式中，把增量限定于一个半径为 $µ$ 的球中，认为只在这个球内才是有效的
带上 $D$ 之后，这个球可以看成一个椭球
在 Levenberg 提出的优化方法中，把 $D$ 取成单位阵 $I$，相当于直接把 $∆x$ 约束在一个球中
随后， Marqaurdt 提出将 $D$ 取成非负数对角阵
实际中通常用 $J$^T $J$ 的对角元素平方根，使得在梯度小的维度上约束范围更大一些

不论如何，在 L-M 优化中，我们都需要解式那样一个子问题来获得梯度
这个子问题是带不等式约束的优化问题，用 Lagrange 乘子将它转化为一个无约束优化问题：

这里 $λ$ 为 Lagrange 乘子
类似于 Gauss-Newton 中的做法，把它展开后，发现该问题的核心仍是计算增量的线性方程：

可以看到，增量方程相比于 Gauss-Newton，多了一项 $λD$^T $D$
如果考虑它的简化形式，即 $D = I$，那么相当于求解：

看到，当参数 $λ$ 比较小时， $H$ 占主要地位
这说明二次近似模型在该范围内是比较好的，L-M 方法更接近于 G-N 法

另一方面，当 $λ$ 比较大时， $λI$ 占据主要地位， L-M 更接近于一阶梯度下降法（即最速下降）
这说明附近的二次近似不够好

L-M 的求解方式，可在一定程度上避免线性方程组的系数矩阵的非奇异和病态问题，提供更稳定更准确的增量 $∆x$
在实际中，还存在许多其它的方式来求解函数的增量，例如 Dog-Leg 等方法
在这里所介绍的，只是最常见而且最基本的方式，也是视觉 SLAM 中用的最多的方式
总而言之，非线性优化问题的框架，分为 Line Search 和 Trust Region 两类
Line Search 先固定搜索方向，然后在该方向寻找步长，以最速下降法和 Gauss-Newton 法为代表
TrustRegion 则先固定搜索区域，再考虑找该区域内的最优点
此类方法以 L-M 为代表

实际问题中，通常选择 G-N 或 L-M 之一作为梯度下降策略

---

参考：

《视觉SLAM十四讲》

---

相关推荐：

视觉SLAM笔记（26） 状态估计问题
视觉SLAM笔记（25） 拼接点云
视觉SLAM笔记（24） 图像基础操作
视觉SLAM笔记（23） 图像
视觉SLAM笔记（22） RGB-D相机模型

---

谢谢！

转载：https://blog.csdn.net/qq_32618327/article/details/102403099