【视觉SLAM入门】4.2 非线性最小二乘理论部分------线搜索，信赖域，最速/牛顿下降法，高斯牛顿，LM等原理推导

注意： 上一节得到的最小二乘问题，本节来讨论---- $求解非线性最小二乘问题$

0. 引入

求解这个简单的最小二乘问题:
$$\underset{x}{\min }\frac{1}{2}||f(x)|{|}_2^2(1)$$

• 最优条件法：当函数存在解析形式，能够通过最优性条件求解出显式最优解，类似求导之类的；
• 迭代法：从初始值出发，不断更新当前的优化变量，使目标函数下降。

一个简单的迭代法步骤：

很少有函数能满足存在解析形式，迭代法的研究是首选，后边要做的就是如何确定增量 $\Delta x_k$,也引出了一些方法。

1. 最速下降法

也叫一阶梯度法，首先将目标函数泰勒展开至一阶：
$$\begin{gathered} ||f(x+\Delta x)|{|}_2^2\approx ||f(x)|{|}_2^2+J(x)\Delta x \\ \Downarrow 其中J是||f(x)|{|}_2^2关于x的导数(雅克比矩阵) \\ \Downarrow 增量方向为 \\ \Delta x^{\ast }=-J^T(x) \end{gathered}$$

• 优点：直观，避免了求导的困难。
• 缺点：贪心，下降路线锯齿状，迭代次数多。

2. 牛顿法

也叫二阶梯度法，首先将目标函数泰勒展开至二阶：
$$\begin{gathered} ||f(x+\Delta x)|{|}_2^2\approx ||f(x)|{|}_2^2+J(x)\Delta x+\frac{1}{2}\Delta x^{T}H\Delta x \\ \Downarrow 其中H是||f(x)|{|}_2^2关于x的二阶导数(海森矩阵) \\ \Downarrow 求右侧关于\Delta x的导数令其等于0，则增量的解为 \\ H\Delta x=-J^T \end{gathered}$$

• 优点：直观，避免了求导的困难。
• 缺点：海森矩阵的计算运算量大。

3. (实用)G-N法

Gauss Newton 简单，它是将$f(x)$ 展开：
$$f(x+\Delta x)\approx f(x)+J(x)\Delta x$$
这里的$J$是$f(x)$关于$x$的导数，将上式带入(1)式中，按照平方展开可得：
$$\begin{gathered} \frac{1}{2}||f(x)+J(x)\Delta x|{|}^2=\frac{1}{2}(||f(x)|{|}_2^2+2f(x{)}^{T}J(x)\Delta x+\Delta x^{T}J(x{)}^{T}J(x)\Delta x) \\ \Downarrow 求关于\Delta x的导数，并令其=0，可得增量方程 \\ J(x{)}^{T}J(x)\Delta x=-J(x{)}^{T}f(x) \\ \Downarrow 令H=J{J}^T，其实就是牛顿法中海森矩阵的近似 \\ \Downarrow 令g=-J(x{)}^{T}f(x) \\ H\Delta x=g \end{gathered}$$
通过以上推导，我们可以将G-N(Gauss-Newton)迭代法步骤列出：

• 缺点： 用$J^{T}J$近似$H$，但是$H$可逆且正定，而$J^{T}J$实际上半正定，还有可能奇异或者病态，稳定性差。
• 优点： 很多算法和一些线搜索算法是它的变种，都借助了G-N的思想。

4. (实用)L-M方法

Levenberg-Marquadt(列文博格-马夸尔特)也叫阻尼牛顿法。是一种信赖域方法(为$\Delta x$添加一个信赖域，不因太大而不准确)，在区域内近似是有效的。
确定信赖域范围：
$$\rho =\frac{f(x+\Delta x)-f(x)}{J(x)\Delta x}$$

• 分子是实际函数的下降值
• 分母是近似模型的下降值
• 两个的比值$\rho$接近1，是好的。

由是，我们可以写出L-M的迭代过程:

• 在上式中，将$D$ 取$I$，相当于把$\Delta x$ 约束在一个半径为$\mu$球中。也可以自定义为椭球，对梯度小的维度约束更大

上述步骤2中的两个式子，含有不等式，用拉格朗日乘子$\lambda$将其转化为无约束问题：

$$\begin{gathered} \underset{\Delta x_k}{\min }\frac{1}{2}||f(x_k)+J(x_k)\Delta x_k|{|}^2+\frac{\lambda }{2}||D\Delta x|{|}^2 \\ \Downarrow 展开，核心还是计算增量的线性方程，取简化D=I \\ (H+\lambda I)\Delta x=g \end{gathered}$$
不难看出，$\lambda$ 小的时候H占据主导地位算法接近G-N，较大的时候算法接近最速下降法。

5. 总结

非线性优化的框架：

• 线搜索： 固定搜索方向，然后寻找步长。含G-N, 最速下降法
• 信赖域：固定搜索区域，寻找区域内的最优点，L-M法。

所有理论来自《数值优化》，只介绍了两种简单的。

• 初始值的选取：不可随意，在视觉SLAM中用 ICP, PnP
• $\Delta x$ 是多维的，几百上千几十万等，求解时要用矩阵论知识，QR，Cholesky分解等，它的系数形式确保了实时性