视觉SLAM十四讲笔记-6-2

视觉SLAM十四讲笔记-6-2

6.2 非线性最小二乘

先来考虑一个简单的最小二乘问题：
$$\underset{\mathbf{x}}{\min }F(\mathbf{x})=\frac{1}{2}\Vert f(\mathbf{x}){\Vert }_2^2$$
其中，自变量 $x\in R^n$，$f$ 是任意标量非线性函数 $f(x):R^n\mapsto R$．这里的系数 $1/2$ 是无关紧要的，下面讨论如何求解这样一个优化问题．
如果 $f$ 是一个数学形式上很简单的函数，那么该问题可以用解析形式来．令目标函数的导数为零，然后求解 $x$ 的最优解．就和求二元函数的极值是一样的:
$$\frac{dF}{dx}=0$$
解此方程，就得到了导数为零处的极值．它们可能是极大，极小，或者鞍点处的值，只要逐个比较它们的函数值大小即可．
但是，这个方程是否容易求解尼?这取决于 $f$ 导函数的形式．如果 $f$ 为简单的线性函数，那么这个问题就是简单的线性最小二乘问题．但是有些导函数可能形式上很复杂，使得该方程可能不容易求解．求解这个方程需要知道关于目标函数的全局性质，而通常这是不大可能的．对于不方便直接求解的最小二乘问题，可以使用迭代的方式，从一个初始值出发，不断地更新当前的优化变量，使目标函数下降．　具体的步骤可列写如下：

1.给定某个初始值 $x_0$;
2.对于第 $k$ 次迭代，寻找一个增量 $\Delta x_k$，使得 $\left | f(x_k + \Delta x_k) \right |_2^2 $ 达到极小值；
3.若 $\Delta x_k$ 足够小，则停止;
4.否则，令 $x_{k+1}=x_k+\Delta x_k$，返回第 $2$ 步．

这让求解导函数为零的问题变成了一个不断寻找下降增量 $\Delta x_k$ 的问题．后面会看到，由于可以对 $f$ 进行线性化，增量的计算会简单很多．当函数下降直到增量非常小的时候，就认为算法收敛，目标就达到了一个极小值．在这个过程中，问题在于如何找到每次迭代的增量，而这是一个局部的问题，只需关心 $f$ 在迭代值处的局部性质而非全局性质．这类方法在最优化，机器学习等领域应用非常广泛．
接下来，将考察如何寻找这个增量 $\Delta x_k$，这部分知识实际属于数值优化的领域，接下来介绍一些广泛使用的结果．

6.2.1 一阶和二阶梯度法

现在考虑第 $k$ 次迭代，假设在 $x_k$ 处，想要寻找增量 $\Delta x_k$, 那么最直观的方式是将目标函数在 $x_k$ 附近进行泰勒展开：
$${\Vert F(x_k+\Delta x_k)\Vert }_2^2\approx {\Vert F(x_k)\Vert }_2^2+J(x_k)\Delta x_k+\frac{1}{2}\Delta x_k^{T}H(x_k)\Delta x_k$$
其中 $J(x_k)$ 是 $F(x)$ 关于 $x$ 在 $x_k$ 处的一阶导数(也叫梯度，或者雅克比矩阵, Jacobian)， $H$ 则是二阶导数(也成为海塞矩阵，Hessian)．
可以选择保留泰勒展开的一阶或者二阶项，对应的求解方法则称为一阶梯度或者二阶梯度法．
1.如果保留一阶梯度，那么取增量为反向的梯度，即可保证函数下降：
$$\Delta x^{\ast }=-J(x_k)$$
当然这只是个方向，通常还需要再指定一个步长 $\lambda$，步长可以根据一定的条件来计算．按照这种方法进行选取 $\Delta x_k$ 的方法称为最速下降法，直观意思是沿着反向梯度方向前进，在一阶(线性)的近似下，目标函数必定会下降．
上式中下标代表第 $k$ 次迭代，可以将 $k$ 去掉，代表每一次迭代都成立．
2.如果选择保留二阶梯度，此时增量方程为：
$$\Delta x^{\ast }=argmin(F(x)+J(x{)}^T\Delta x+\frac{1}{2}\Delta x^{T}H\Delta x)$$
右侧只含 $\Delta x$ 的零次，一次，二次项．求右侧等式等于关于 $\Delta x$ 的导数并令其等于零，得到：
$$J+H\Delta x=0⟹H\Delta x=-J$$
求解这个线性方程，就得到了增量．该方法称为牛顿法．
一阶和二阶梯度法都十分直观，只要把函数在迭代点附近进行泰勒展开，并针对更新量做最小化即可．实际上是使用了一个一次函数或者二次函数近似了原函数，然后用近似函数的最小值来猜测原函数的极小值．只要原目标函数局部看起来像一次或二次函数，这类算法就是成立的．
不过，这两种方法也存在自身的问题．最速下降法过于贪心，容易走出锯齿路线，反而增加了迭代次数．而牛顿法则需要计算目标函数的 $H$ 矩阵(海塞矩阵*，Hessian，二阶导数)，这在问题规模较大时非常困难，通常倾向于避免 $H$ 的计算．
对于一般的问题，一些拟牛顿法可以得到很好的结果，而对于最小二乘问题，还有几类更实用的方法：高斯牛顿法和列文伯格-马夸尔特方法．

6.2.2 高斯牛顿法

高斯牛顿法是最优化算法中最简单的方法之一．它的思想是将 $f(x)$ 进行一阶的泰勒展开，注意，这里的目标函数不是 $F(x)$ 而是 $f(x)$，否则就变成牛顿法了．
$$f(x+\Delta x)\approx f(x)+J(x{)}^T\Delta x$$
这里 $J(x{)}^T$ 为 $f(x)$ 关于 $x$ 的导数，为 $n\ast 1$ 的列向量．根据前面的框架，当前的目标是寻找增量 $\Delta x$ ，使得 ${\Vert f(x+\Delta x)\Vert }^2$ 达到最小．为了求 $\Delta x$，需要解一个线性的最小二乘问题：
$$\Delta x^{\ast }=arg\underset{\mathbf{\Delta }\mathbf{x}}{\min }\frac{1}{2}{\Vert f(x)+J(x{)}^T\Delta x\Vert }^2$$
这个方程和之前的一阶二阶梯度法有什么区别吗?根据极值条件，将上述目标函数对 $\Delta x$ 求导，并令导数等于0．为此，先展开目标函数的平方项：
$$\begin{gathered} \frac{1}{2}{\Vert f(x)+J(x{)}^T\Delta x\Vert }^2=\frac{1}{2}(f(x)+J(x{)}^T\Delta x{)}^T(f(x)+J(x{)}^T\Delta x) \\ =\frac{1}{2}({\Vert f(x)\Vert }^2+2f(x)J(x{)}^T\Delta x+\Delta x^{T}J(x)J(x{)}^T\Delta x) \end{gathered}$$
求上式关于 $\Delta x$ 的导数，并令其为零：
$$J(x)f(x)+J(x)J(x{)}^T\Delta x=0$$
(这里求导没看明白，后续再看)
可以得到如下方程组：
$$\underset{H(x)}{\underbrace{J(x)J(x{)}^T}}\Delta x=\underset{g(x)}{\underbrace{-J(x)f(x)}}$$
这个方程是关于变量 $\Delta x $ 的线性方程组，称之为**增量方程，也可以称为高斯牛顿方程**(Gauss-Newton equation)或者正规方程(Normal equation)．把左边的系数定义为 $H$，右边的定义为 $g$，那么上式变为：
$$H\Delta x=g$$
这里把左侧记作 $H$ 是有意义的．对比牛顿法可见，高斯牛顿法用 $J{J}^T$ 作为牛顿法中二阶 $Hessian$ 矩阵的近似，从而省略了计算 $H$ 的过程．求解增量方程是整个优化问题的核心所在．如果能顺利解出该方程，那么高斯牛顿法的算法步骤可以写做：

1.给定初始值 $x_0$
2.对于第 $k$ 次迭代，求出当前的雅克比矩阵 $J(x_k)$ 和误差 $f(x_k)$
3.求增量方程:$H\Delta x_k=g$
4.若 $\Delta x_k$ 足够小，则停止．否则，令 $x_{k+1}=x_k+\Delta x_k$，返回第2步

从算法步骤可以看出，增量方程的求解占据着主导地位．只要能够顺利地解出增量，就能保证目标函数能够正确地下降．
但是在求解 $H\Delta x_k=g$ 时，需要求解 $H^{-1}$ ，这时需要 $H$ 矩阵可逆，但在实际的数据中计算得到的 $J{J}^T$ 却只有半正定的性质，不一定可逆．这时，求出来的增量稳定性较差．尽管高斯牛顿法存在这些缺点，但它依然是非线性优化方面一种简单有效的方法．
在非线性优化领域，相当多的算法都可以归结于高斯牛顿法的变种．例如，一些线搜索方法，加入了一个步长 $\alpha$，在确定了 $\Delta x$ 后进一步找到 $\alpha$ 使得 ${\Vert f(x+\alpha \Delta x)\Vert }^2$达到最小，而不是简单地令 $\alpha =1$．

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

高斯牛顿法中采用的近似二阶泰勒展开只能在展开点附近有较好的近似效果，所以很自然得想到应该给 $\Delta x$ 添加一个范围，称为信赖区域(Trust Region)．这个范围定义了在什么情况下二阶近似是有效的，这类方法也称为信赖区域方法(Trust Region Method).在信赖区域里，可以认为是有效的；出了这个区域，近似可能会出问题．
那么，如何确定这个信赖区域的范围尼? 一个比较好的方法是根据近似模型跟实际函数之间的差异来确定：如果差异小，说明近似效果好，可以扩大近似的范围；反之，如果差异大，就缩小近似的范围．定义一个指标 $\rho$ 来刻画近似的好坏程度：
$$\rho =\frac{f(x+\Delta x)-f(x)}{J(x{)}^T\Delta x}$$
$\rho$ 的分子是实际函数下降的值，分母是近似模型下降的值．如果 $\rho$ 接近于 1，则近似是好的．如果 $\rho$ 太小，说明实际减小的值远小于近似减小的值，则认为近似比较差，需要缩小近似范围．反之，如果 $\rho$ 比较大，则说明实际下降的比预计的更大，可以放大近似范围．
于是，构建一个改良版的非线性优化框架，该框架会比高斯牛顿法有更好的效果：

1.给定初始值 $x_0$，以及初始优化半径 $\mu$
2.对于第 $k$ 次迭代，在高斯牛顿法的基础上加上信赖区域，求解：
$$\underset{\mathbf{\Delta }{\mathbf{x}}_{\mathbf{k}}}{\min }\frac{1}{2}{\Vert f(x_k)+J(x_k{)}^T\Delta x_k\Vert }^2，s.t.{\Vert D\Delta x_k\Vert }^2\le \mu$$
其中， $\mu$ 是信赖区域的半径， $D$ 为系数矩阵，将在后文说明．
3.按照
$$\rho =\frac{f(x+\Delta x)-f(x)}{J(x{)}^T\Delta x}$$
计算 $\rho$
4.若 $\rho$ > $\frac{3}{4}$，则设置$\mu =2\mu$;
5.若 $\rho$ < $\frac{1}{4}$，则设置$\mu =0.5\mu$;
6.如果 $\rho$ 大于某阈值，则认为近似可行．令 $x_{k+1}=x_k+\Delta x_k$;
7.判断算法是否收敛．如果不收敛则返回第2步，否则结束．

这里近似范围和扩大缩小的倍数都是近似值，可以替换为别的数值．在 ${\Vert D\Delta x_k\Vert }^2\le \mu$ 中将增量限制在一个半径为 $\mu$ 的球中，认为只在这个球内才是有效的．带上 $D$ 以后，这个球可以看成一个椭球．随后，在列文伯格提出的优化方法中，把 $D$ 取成单位阵 $I$，相当于直接把 $\Delta x$ 约束在一个球中．随后，马尔夸特提出将 $D$ 取成非负数对角阵------实际上通常用 $J^{T}J$ 的对角元素平方根，使得在梯度小的维度上约束范围更大一些．
无论如何，在列文伯格－马夸尔特优化中，都需要解${\min }_{\mathbf{\Delta }{\mathbf{x}}_{\mathbf{k}}}\frac{1}{2}{\Vert f(x_k)+J(x_k{)}^T\Delta x_k\Vert }^2，s.t.{\Vert D\Delta x_k\Vert }^2\le \mu$ 这样的子问题来获得梯度．这个子问题是带不等式约束的优化问题，用拉格朗日乘子把约束项放到目标函数中，构成拉格朗日乘子把约束项放到目标函数中，构成拉格朗日函数：
$$\mathcal{L}(\Delta x_k,\lambda )=\frac{1}{2}{\Vert f\left({\mathbf{x}}_k\right)+\mathbf{J}{\left({\mathbf{x}}_k\right)}^T\Delta {\mathbf{x}}_k\Vert }^2+\frac{\lambda }{2}\Vert (\mathbf{D}\Delta {\mathbf{x}}_{\mathbf{k}}{\Vert }^2-\mu )$$
它的核心仍是计算增量的线性方程，和高斯牛顿法中处理一样，对上式展开然后关于 $\Delta x$ 求导，并令其等于零：
$$(H+\lambda D^{T}D)\Delta x_k=g$$
可以看到，相比于高斯牛顿法，该增量方程多了一项 $\lambda D^{T}D$，如果考虑它的简化形式，即 $D=I$，则相当于求解：
$$(H+\lambda I)\Delta x_k=g$$
一方面，当参数 $\lambda$ 比较小时， $H$ 占主要地位，这说明二次近似模型在该范围内是比较好的，列文伯格－马夸尔特方法更接近高斯牛顿法；另一方面，当 参数 $\lambda$ 比较大时，$\lambda I$ 占主要地位，列文伯格－马夸尔特方法更接近一阶梯度下降法(即最速下降)，这说明附近的二次近似不够好．
列文伯格-马夸尔特方法的求解方式，可在一定程度上避免线性方程组的系数矩阵的非奇异和病态问题，提供更稳定，更准确的增量 $\Delta x$．
在实际中，存在许多其他方式来求解增量，例如 $Dog-Leg$ 等方法．在这里只介绍了最常见且最基本的方法，也是视觉SLAM中用的最多的方法．实际问题中，通常选择高斯牛顿法或列文伯格－马夸尔特方法中的一种作为梯度下降策略．当问题性质较好时，用高斯牛顿；如果问题接近病态，则用列文伯格－马夸尔特方法．

小结

1.以高斯牛顿法和列文伯格－马夸尔特方法为代表的优化方法，在很多开源的优化库中都已经实现并提供给用户．
2.无论是高斯牛顿法还是列文伯格－马夸尔特方法，在做最优化计算时，都需要提供变量的初始值．这个初始值能随意设置吗?当然不能．实际上，非线性优化的所有迭代求解方案，都需要用户提供一个良好的初始值．由于目标函数太复杂，导致在求解空间上的变化难以预测，对问题提供不同的初始值往往会导致不同的计算结果．这种情况是非线性优化的通病：大多数算法都容易陷入局部极小值．因此，无论是哪类科学问题，提供初始值都应该有科学依据，例如在视觉SLAM问题中，会用ICP，PnP之类的算法提供优化初始值，总之一个良好的初始值对最优化问题非常重要!
3.如何求解线性增量方程组尼?一般不会对矩阵进行求逆运算，而是采用矩阵分解的方法来求解线性方程，例如QR，Cholesky方程．
4.视觉SLAM里这个矩阵往往有特定的稀疏形式，这为实时分解优化问题提供了可能性．