解读《视觉SLAM十四讲》，带你一步一步入门视觉SLAM—— 第 6 讲 非线性优化

Hello，各位好朋友，还记得第五讲的时候，我跟你们说过纯数学理论的内容暂时告一段落！好吧，我骗你们了，哈哈！
　　
　　这一讲不但是纯数学理论，而且还是很有难度的纯数学理论，如果你在看我这篇博客之前，已经看过这一讲的内容了，我相信第一次见到这些内容你可能根本不知道他在说啥！书上眼花缭乱的公式，似乎一直在向你传达两个字——放弃！哈哈，开个玩笑！

一、原书内容

由于噪声的影响，我们的运动方程和观测方程都不是严格成立的，所以从带有噪声干扰的数据中心进行准确的状态估计就显得非常重要。
　　
　　作者文中提到状态估计问题，在很长一段时间里都是采用的扩展卡尔曼滤波器，由于卡尔曼滤波器是建立在马尔科夫性条件下的，认为当前的运动状态只和上一个运动状态有关，实际上这种假设并不是完全符合实际情况。
　　
　　我们可以很容易就判断出，当前的状态是前面所有状态累计而得到的结果，所以基于卡尔曼滤波的方法就显得有些局限了。以至于现在视觉SLAM的主流方法基本都是采用非线性优化的方法，它可以将所有状态一起就行优化得到一个最佳的结果，显然这是最合理的。
　　
　　书中作者首先从概率分布的角度，通过贝叶斯公式推导出了如何求取最大后验概率。如果你先前对概率论不是很熟悉，这部分可能理解起来非常吃力，你如果实在看不懂，那就先放一边吧，实际上并不会对后面内容的理解带来影响，而且这一讲主要是介绍非线性优化的方法和操作，主要目的不是介绍最大后验的求取。另外一方面主要也是因为，我对概率分布这一部分的内容，不是特别熟悉，我担心我讲完之后你可能更迷糊，所以干脆就不解读这部分内容。后续在后端优化部分的内容中，作者还会带你进一步去学习这里的内容，而且会更加的直观。
　　
　　总之我们只要知道作者通过前面很复杂概率分布的推导，得到了一个非常复杂的函数，现在我们想要求这个函数的极小值。作者这里介绍求极小值的方法，采用的是非线性优化。
　　
　　求极小值的内容，我准备从一个例子入手，用最通俗的话告诉你，什么是非线性优化，而且我一定能把你讲明白，这一点我有信心。因为我先前给我女朋友都讲明白了这个，她只是一个文科生，都没有接触过高等数学。
　　
　　那就让我们开始吧！

下面我要挑战用最通俗的话，给你讲明白非线性优化，如果没有给你讲明白，请留言区踩我！由于这篇博客是解读《十四讲》，所以我只是从主观上带你去简单地认识一下，严格的数学推导，你还是需要看书！由于我还没有一个顺手的画图软件，我就用手画图，不过我会尽力让图看起来不那么乱。

二、 解读：非线性优化

1.二维情况

那我就直接开始了，我先以简单的二维坐标系下的例子开始！
　　
　　我们现在有一个形式很复杂的函数，它的函数图像如下图。　
　　现在我们想求函数的极小值值！
　　
　　这也太简单了吧！我们立马想到给$f(x)$函数求导，然后令导数为零，然后求得$x$的值，把$x$带入$f(x)$，问题得到求解！完美！
　　
　　可是你别忘了，我说过$f(x)$的函数形式很复杂，$f^{\prime }(x)=0$是不可能直接求出$x$。
　　
　　所以求导的方法是行不通，下面我们来想一个非常笨的办法！　
在上图中，我以这样一种思路来计算极小值：（下面的步骤你可能觉得很荒唐，但是也一定要仔细体会一下）

• 第一步，先取一个初始值$x_0$，计算出$f(x_0)$的值；
• 第二步，为了保证$f(x)$的值随着我$x$的选取而下降，我似乎只能在$x_0$的右侧取值，于是取$x_1=x_0+\Delta x$，然后计算出$f(x_1)$的值；
• 第三步，我继续按照前面的规则取$x_2=x_1+\Delta x$，然后计算$f(x_2)$；
• 第四步，继续重复上面的步骤，当我们计算到$f(x_8)$的时候，我们发现$f(x_8)$相对于$f(x_7)$，只减小一点点；
• 第五步，于是我们不再往下计算了，就认为$f(x_8)$就是我们要求的极小值。

此时你的内心肯定充满了怀疑，这个方法简直就是胡扯，于是你举出了一个如下图的函数：
　　然后你举一反三按照我刚刚的步骤，然后选择了初识点$x_0$，一步一步往左边对$x$取值然后计算$f(x)$，最后在$x=0$附近，$f(x)$的函数值变化很小，于是我们得到$f(0)$是函数的极小值！一个多么让我打脸的例子啊！
　　
　　你为了让我彻底颜面无存，于是乎举出了很多例子：

　　上图中的第二个函数，在进行前述方法下降的时候，直接跳过了最小值。
　　
　　似乎我们上面的求最小值的方法并不能用，或者委婉一点儿说，我们上面采用的方法局限性非常大。

不管怎么说，先让我们总结一下这个步骤：

• 选择一个初始值$x_0$；
• 对于第$k$次迭代，寻找一个增量$\Delta x$，使得$f(x+\Delta x)$值在下降；
• 如果$f(x+\Delta x)$值变化的非常小，就停止。当前的函数值就是极小值。

上面我们提出的迭代方法虽然有些简陋，而且很多类型的函数都不能适用，但是我们至少可以有这样一个结论，那就是迭代下降这个思路本身并没有问题，应该是我们在计算的过程中采用的策略有问题。
　　
　　于是乎聪明的数学家，不断地想出了各种下降策略，解决各种你能想到的或者碰到的问题，有的数学家提出最快速的下降方法，速度很快但是接近极小值的时候，会出现左右震荡，有的数学家提出的方法可以非常接近极小值，但是速度很慢，可能需要迭代很多步，有的……
　　
　　对于我上面采用的那种下降策略求极小值，你无需太过于纠结，我只是为了向你介绍下降策略是如何工作的。事实上这里可以采用的策略非常之多，但是他它们的核心都是一样的，就是通过下降的方法，不断地逼近极小值。
　　
　　总之，为了解决下降过程中碰到的问题，数学家想出了各种各种的方法，以至于已经形成了一门庞大的数学体系。
　　
　　这整个通过下降的策略求解极小值的方法，它的大名就叫做 非线性优化。为啥叫非线性优化呢？因为一般这种下降方法都是用在非线性函数的。

2.三维情况

上面我讲了在二维坐标系中的例子，现在我们可以再探讨一下三维坐标系下的情况，由于我实在不能用笔画出三维的效果，所以我就网上偷了一张图，在此感谢这位供图的兄台。

　　我们现在想要求上图这个三维函数的极小值，我们是否还可以按照刚刚二维情况那样采用下降的方法呢！？答案是肯定的，但是情况稍微复杂一点儿！下面我们来分析一下：
　　
　　请把这个函数形状想成一座大山，你现在站在山顶，你要最快到达山底，你怎么走啊？
　　
　　很显然，聪明的你，已经想到了方法。

• 首先，你站在山顶观察一周找到一个最陡峭的路，然后往下走一大步；
• 然后，你又在新的位置观察四周找到一个最陡峭的路，又往下走一大步；
• 你不断地重复上面的方法，当你发现身边再也没有往下的路了，就到达了山底了。

与二维情况一样，这是三维情况下的非线性优化的思路，在不断地研究中形成了各种下降策略，有的下降的快，有的下降的准。

我们从数学的角度来简单的看一下这个过程；
　　
　　我们为什么要找最陡峭的路下山呢？因为这个路线下山最快。对应到三维函数中，我们如何找到一个函数下降最快的方向呢？如果你还记得我们在高等数学中学过的 梯度，那么你就知道上图这个三维函数，在某点下降最快的方向，就是它的负梯度方向。所以我们求解非线性优化问题的方法就总的称为 梯度下降法。（如果你对梯度没什么印象了，你可以找本高等数学的数学的书，稍微补习一下，这并不会花费太久的时间）
　　
　　有了梯度方向，我们就知道了下降最快的方向，光有方向还不行，我们沿着这个方向走多长一步呢？走到什么程度就是最小值了呢？实际围绕着这两个问题，数学家们研究了很久，提出了各种各样的方法。这一讲主要介绍了高斯牛顿方法和Levenberg-Marquadt方法。
　　
　　至此我相信，你虽然还不会操作梯度下降的方法，但是你肯定已经理解了梯度下降法是如何工作的，这一点十分重要，我们很多教材或者老师，都告诉了我们方法，但是却很少告诉我们原理。当你知道了原理，实际上去理解方法，就是顺理成章的事儿。
　　
　　我们在实际的操作中，求解非线性优化问题，常常将函数写成如下形式，也就是我们常说的最小二乘的形式，然后求它的最小值，所以对于这类问题，我们也常常叫做非线性最小二乘：
$$\underset{x}{\min }\Vert f(x){\Vert }_2^2$$下面我再把梯度下降法的步骤给你总结一下，我不断地重复着这个，希望你不要嫌麻烦，实际上这个很重要。我使用作者总结的步骤：

• 1.给定某个初始值$x_0$；
• 2.对于第$k$次迭代，寻找一个增量$\Delta {\mathbf{x}}_k$，使得$\Vert f(x_k+\Delta {\mathbf{x}}_k){\Vert }_2^2$达到极小值；
• 3.若$\Delta {\mathbf{x}}_k$足够小，则停止；
• 4.否则，另${\mathbf{x}}_{k+1}={\mathbf{x}}_k+\Delta {\mathbf{x}}_k$，返回 2。

我相信当你知道了梯度下降法的原理之后，再看《十四讲》中作者主要提到了三种梯度下降方法，应该问题不会太大了。我在此再另外给大家找一些梯度下降算法，以供大家阅读学习。

1.《优化算法之——最速下降法》
2.《[优化] Gauss-Newton非线性最小二乘算法》
3.《Levenberg-Marquardt算法浅谈》
　　
有一些建议，我还是想分享给大家，非线性优化是一个非常重要，而且应用非常广的工具，在现在的机器学习中，它也占据了非常重要的角色，所以我建议你多花些时间，仔细地去理解一下非线性优化的几种常用方法，但是光理解还是远远不够的，一定要自己去实际编代码测试和体验一下。我在上面的内容中只是抛砖引入带你简单地看了一下非线性优化的原理，我并没有在解法上讲太多，实际上你真正去实际操作一个解法的时候，你会碰到很多理解上的疑惑。在SLAM中要优化的变量是一个维度几百或者上千向量，其中求导产生的雅克比矩阵也是非常之大，这势必会让我们理解和操作增加难度，所以我强烈地建议你自己亲自用手在草稿本上去推演一些梯度下降法，选择一个维度低一点儿的矩阵去体验一下，这一点儿很重要！

3.强烈推荐阅读的文章

《Methods for Non-Linear Least Squares Problems》
　　To be honest，我上面关于非线性最小二乘问题的引入，有很多写的不好的地方，而且它也只适合一个没有接触过非线性优化的同学，帮助迅速了解非线性优化，所以你还必须得读一些理论性完整一点儿内容，如果英文还不错，这篇内容强烈建议大家读一下，当做教科书去读，因为它总结了常用算法的优缺点，而且通过实验进行了比较。

三、实践

作者在书中介绍了两种常用的非线性优化库，如果你第一次使用，肯定有很多的疑惑，暂且现将你的疑惑放一放，不要在代码上花费太多时间，先不要将经历放在这些代码库是怎么实现的，你可以将书中的例子，在你的IDE上多手敲几遍，越多遍越好，只有这样，你才能真正体会它们是怎么使用的。

黑暗总会过去，黎明终会到来。