视觉SLAM十四讲学习笔记-第六讲-非线性优化的非线性最小二乘问题

 专栏系列文章如下：

 视觉SLAM十四讲学习笔记-第一讲_goldqiu的博客-CSDN博客

视觉SLAM十四讲学习笔记-第二讲-初识SLAM_goldqiu的博客-CSDN博客

视觉SLAM十四讲学习笔记-第二讲-开发环境搭建_goldqiu的博客-CSDN博客

视觉SLAM十四讲学习笔记-第三讲-旋转矩阵和Eigen库_goldqiu的博客-CSDN博客

视觉SLAM十四讲学习笔记-第三讲-旋转向量、欧拉角、四元数_goldqiu的博客-CSDN博客

视觉SLAM十四讲学习笔记-第三讲-相似、仿射、射影变换和eigen程序、可视化演示_goldqiu的博客-CSDN博客

视觉SLAM十四讲学习笔记-第四讲-李群与李代数基础和定义、指数和对数映射_goldqiu的博客-CSDN博客

视觉SLAM十四讲学习笔记-第四讲-李代数求导与扰动模型_goldqiu的博客-CSDN博客

视觉SLAM十四讲学习笔记-第四讲-Sophus实践、相似变换群与李代数_goldqiu的博客-CSDN博客

视觉SLAM十四讲学习笔记-第五讲-相机模型_goldqiu的博客-CSDN博客

视觉SLAM十四讲学习笔记-第五讲-图像和实践_goldqiu的博客-CSDN博客_global shutter

视觉SLAM十四讲学习笔记-第六讲-非线性优化的状态估计问题_goldqiu的博客-CSDN博客

6.2 非线性最小二乘

考虑一个最小二乘问题：

其中，自变量x ∈ Rn，f是任意标量非线性函数 f(x) : Rn→ R。注意这里的系数1/2是无关紧要的。如何求解这样一个优化问题：如果 f 是个数学形式上很简单的函数，那么该问题可以用解析形式来求。令目标函数的导数为零，然后求解x的最优值，就和求二元函数的极值一样：

解此方程，就得到了导数为零处的极值。可能是极大、极小或鞍点处的值，只要逐个比较函数值大小即可。如果 f 为简单的线性函数，那么这个问题就是简单的线性最小二乘问题，但是有些导函数形式复杂，使得该方程不容易求解。求解这个方程需要知道关于目标函数的全局性质，而通常这是不大可能的。对于不方便直接求解的最小二乘问题，可以用迭代的方式，从一个初始值出发，不断地更新当前的优化变量，使目标函数下降。具体步骤可列写如下：

1. 给定某个初始值x0。
2. 对于第 k 次迭代，寻找一个增量 ∆xk，使得

达到极小值。

3. 若 ∆xk 足够小，则停止。

4. 否则，令x(k+1) = xk + ∆xk，返回第 2 步。

这让求解导函数为零的问题变成了一个不断寻找下降增量 ∆xk 的问题。由于可以对 f 进行线性化，增量的计算将简单很多。当函数下降直到增量非常小的时候，就认为算法收敛，目标函数达到了一个极小值。在这个过程中，问题在于如何找到每次迭代点的增量，而这是一个局部的问题，只需要关心 *f* 在迭代值处的局部性质而非全局性质。这类方法在最优化、机器学习等领域应用非常广泛。

下面是如何寻找这个增量 ∆xk的方法。

6.2.1 一阶和二阶梯度法

考虑第 k 次迭代，假设在xk处，想要找到增量 ∆xk，最直观的方式是将目标函数在xk附近进行泰勒展开：

其中J(xk) 是 F(x)关于x的一阶导数（也叫梯度、雅可比矩阵﹝Jacobian﹞），H 则是二阶导数（海塞﹝Hessian﹞矩阵），它们都在xk 处取值。可以选择保留泰勒展开的一阶或二阶项，那么对应的求解方法则称为一阶梯度或二阶梯度法。如果保留一阶梯度，取增量为反向的梯度，即可保证函数下降：∆x∗= −J(xk)

这只是个方向，通常还要再指定一个步长 λ。步长可以根据一定的条件来计算，在机器学习当中也有一些经验性质的方法。这种方法被称为最速下降法。它的直观意义是：只要沿着反向梯度方向前进，在一阶（线性）的近似下，目标函数必定会下降。

以上都是在第k次迭代时进行的，并不涉及其他的迭代信息。为了简化符号，省略下标k，并认为这些对任意一次迭代都成立。

选择保留二阶梯度信息，此时增量方程为：

右侧只含 ∆x的零次、一次和二次项。求右侧等式关于 ∆x的导数并令它为零，得到：

求解这个线性方程就得到了增量。此类方法又称为牛顿法。

一阶和二阶梯度法都十分直观，只要把函数在迭代点附近进行泰勒展开，并针对更新量做最小化即可。用一个一次或二次的函数近似原函数，然后用近似函数的最小值来估计原函数的极小值。只要原目标函数局部看起来像一次或二次函数，这类算法就是成立的。不过这两种方法也存在问题：最速下降法过于贪心，容易走出锯齿路线，反而增加了迭代次数。而牛顿法则需要计算目标函数的H矩阵，这在问题规模较大时非常困难，通常倾向于避免H的计算。所以引出了下面的一些方法。

6.2.2 高斯牛顿法

牛顿法是对目标函数 F(x) 进行一阶泰勒展开，而高斯牛顿法是对f(x)进行一阶泰勒展开：f (x + ∆x) ≈ f(x) + J(x)T∆x.

注：把 J(x) 写成列向量，那么它可以和 ∆x 进行内积，得到一个标量。

这里J(x)T为f(x)关于x的导数，为n×1的列向量。目标是寻找增量∆x，使得∥f(x+∆x)∥2达到最小。为了求 ∆x，需要解一个线性的最小二乘问题：

根据极值条件，将上述目标函数对 ∆x求导，并令导数为零。可以得到如下方程组：

这个方程是关于变量∆x的线性方程组，称它为增量方程，也可以称为高斯牛顿方程（GaussNewton equation）或者正规方程（Normal equation）。把左边的系数定义为H，右边定义为g，那么上式变为：H∆x = g.

对比牛顿法可见，高斯牛顿法用JJT作为牛顿法中二阶Hessian矩阵的近似，从而省略了计算H的过程。求解增量方程是整个优化问题的核心所在。高斯牛顿法的算法步骤可以写成：

1. 给定初始值x0。

2. 对于第 k 次迭代，求出当前的雅可比矩阵J(xk) 和误差f(xk)。

3. 求解增量方程：H∆xk = g。

4. 若 ∆xk 足够小，则停止。否则，令x(k+1) = xk+ ∆xk，返回第 2 步。

从算法步骤中，主要还是增量的求解。只要能够顺利解出增量，就能保证目标函数能够正确地下降。

为了求解增量方程，需要求解H−1，这需要H矩阵可逆，但实际数据中计算得到的JJT只有半正定性。也就是说，在使用高斯牛顿法时，可能出现JJT为奇异矩阵或者病态（ill-condition）的情况，此时增量的稳定性较差，导致算法不收敛。直观地说，原函数在这个点的局部近似不像一个二次函数。假设H非奇异也非病态，如果求出来的步长∆x太大，也会导致采用的局部近似式：f (x + ∆x) ≈ f(x) + J(x)T∆x.不够准确，这样一来无法保证它的迭代收敛。

在非线性优化领域，相当多的算法都可以归结为高斯牛顿法的变种。这些算法都借助了高斯牛顿法的思想并且通过改进修正其缺点。例如一些线搜索方法 (line search method) 加入了一个步长α，在确定了∆x后进一步找到 α 使得 ∥f(x + α∆x)∥2 达到最小，而不是简单地令 α = 1。

列文伯格—马夸尔特方法在一定程度上修正了这些问题，比高斯牛顿法更为鲁棒，但收敛速度会比高斯牛顿法更慢，被称为阻尼牛顿法（Damped Newton Method）。

6.2.3 列文伯格—马夸尔特方法

高斯牛顿法中采用的近似二阶泰勒展开只能在展开点附近有较好的近似效果，所以应该给∆x添加一个范围，称为信赖区域（Trust Region）。这个范围定义了在什么情况下二阶近似是有效的，这类方法也称为信赖区域方法（Trust Region Method）。在信赖区域里边，近似是有效的；出了这个区域，近似可能会出问题。

那么如何确定这个信赖区域的范围呢？一个比较好的方法是根据近似模型跟实际函数之间的差异来确定：如果差异小，说明近似效果好，扩大近似的范围；反之，如果差异大，就缩小近似的范围。我们定义一个指标 ρ 来刻画近似的好坏程度：

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

于是构建一个改良版的非线性优化框架，该框架会比高斯牛顿法有更好的效果：

1. 给定初始值 x0，以及初始优化半径 µ。
2. 对于第 k 次迭代，在高斯牛顿法的基础上加上信赖区域，求解：

1. 其中 µ 是信赖区域的半径，D 为系数矩阵。
2. 计算 ρ。
3. 若 ρ > 3/4，则设置 µ = 2µ。
4. 若 ρ < 1/4，则设置 µ = 0.5µ。
5. 如果 ρ 大于某阈值，则认为近似可行。令 xk+1 = xk + ∆xk。
6. 判断算法是否收敛。如不收敛则返回第 2 步，否则结束。

这里近似范围扩大的倍数和阈值都是经验值，可以替换成别的数值。

这个式子中，把增量限定于一个半径为 µ 的球中，认为只在这个球内才是有效的。带上D之后，这个球可以看成一个椭球。在列文伯格提出的优化方法中，把D取成单位阵I，相当于直接把 ∆xk约束在一个球中。随后，马夸尔特提出将D取成非负数对角阵——实际中通常用JTJ 的对角元素平方根，使得在梯度小的维度上约束范围更大一些。

在列文伯格—马夸尔特优化中，需要求解这个子问题来获得梯度。

这个子问题是带不等式约束的优化问题，用拉格朗日乘子把约束项放到目标函数中，构成拉格朗日函数：

这里 λ 为拉格朗日乘子。类似于高斯牛顿法中的做法，令该拉格朗日函数关于∆x的导数为零，它的核心仍是计算增量的线性方程：

可以看到，增量方程相比于高斯牛顿法，多了一项 λDTD。考虑它的简化形式，即D = I，那么相当于求解：

(H + λI)∆xk = g.

当参数λ比较小时，H占主要地位，这说明二次近似模型在该范围内是比较好的，列文伯格—马夸尔特方法更接近于高斯牛顿法。另一方面，当λ比较大时，λI占据主要地位，列文伯格—马夸尔特方法更接近于一阶梯度下降法（即最速下降），这说明附近的二次近似不够好。列文伯格—马夸尔特方法的求解方式，可在一定程度上避免线性方程组的系数矩阵的非奇异和病态问题，提供更稳定、更准确的增量 ∆x。

在实际中，还存在许多其他的方式来求解增量，例如

[31] J. Nocedal and S. Wright, Numerical Optimization. Springer Science & Business Media, 2006.

实际问题中，通常选择高斯牛顿法或列文伯格—马夸尔特方法其中之一作为梯度下降策略。当问题性质较好时，用高斯牛顿。如果问题接近病态，则用列文伯格—马夸尔特方法。

小结

专门介绍数值优化的书籍

[31] J. Nocedal and S. Wright, Numerical Optimization. Springer Science & Business Media, 2006.

以高斯牛顿法和列文伯格—马夸尔特方法为代表的优化方法，在很多开源的优化库中都已经实现并提供给用户。最优化是处理许多实际问题的基本数学工具，不光在视觉 SLAM 中起着核心作用，在类似于深度学习等其他领域，它也是求解问题的核心方法之一（深度学习数据量很大，以一阶方法为主）。

无论是高斯牛顿法还是列文伯格—马夸尔特方法，在做最优化计算时，都需要提供变量的初始值。这个初始值不能随意设置。实际上非线性优化的所有迭代求解方案，都需要用户来提供一个良好的初始值。由于目标函数太复杂，导致在求解空间上的变化难以预测，对问题提供不同的初始值往往会导致不同的计算结果。这种情况是非线性优化的通病：大多数算法都容易陷入局部极小值。因此，无论是哪类科学问题，提供初始值都应该有科学依据，例如视觉 SLAM 问题中，会用 ICP、PnP 之类的算法提供优化初始值（视觉前端）。一个良好的初始值对最优化问题非常重要。

如何求解线性增量方程组呢？在视觉SLAM算法里，经常遇到∆x的维度很大，如果是要做大规模的视觉三维重建，就会经常发现这个维度可以轻易达到几十万甚至更高的级别。要对那么大个矩阵进行求逆是大多数处理器无法负担的，因此存在着许多针对线性方程组的数值求解方法。在不同的领域有不同的求解方式，但几乎没有一种方式是直接求系数矩阵的逆，会采用矩阵分解的方法来解线性方程，例如 QR、Cholesky 等分解方法。(工程数学)

视觉SLAM里这个矩阵往往有特定的稀疏形式，这为实时求解优化问题提供了可能性。利用稀疏形式的消元、分解，最后再进行求解增量，会让求解的效率大大提高。在很多开源的优化库上，比如GTSAM，维度为一万多的变量在一般的PC上就可以在几秒甚至更短的时间内被求解出来，其原因也是用了更加高级的数学工具（因子图增量平滑优化）。视觉SLAM算法现在能够实时地实现，多亏了系数矩阵是稀疏的，如果矩阵是稠密的，优化这类视觉SLAM算法不会被学界广泛采纳了。