这部分的内容，第一次看觉得很难看懂，不知所云；最近第二次看，可以看明白了。本着“先赶理论，后赶代码，借着代码复习C++”的原则，现在先不去学习代码的内容。 这一章的内容，应该先从宏观上看。第一次看的时候，就是陷入了细节，忘记了每时每刻主要是在做什么，因此看完之后不能融会贯通。说明学习的时候，把握整体的脉络比追究细节更重要（毕竟不是考试，平时不用闭卷）。后者可以临时抱佛脚，现用现查，而前者不行。我在这篇文章的最后，放上了链接，是这章内容的具体用途。如果前面的内容已经熟知了，但仍然不太理解这个非线性优化，希望打开我最后的总结链接看看，融会贯通。
主要有两个方程： 状态方程： 
其中，w，v都是噪声。非线性优化的主要目的就是在有噪声的情况下，找到它的本质规律。 
假设两个噪声满足上述的形式。
P(x|z)∝P(z|x)P(x)
即后验正比与似然乘以先验。当然先验P(x)也有可能是完全不知道的。 我的理解是，想知道在已知观测数据z（比如在图像中的位置）的情况下，估计系统的状态（比如它的位姿），这就是后验概率，想根据z直接求位姿是比较困难的，但我们可以用似然的思路来解决，用一个非常不严谨但是很形象的说法，就是比如不停的试各种状态，看看是在什么样的状态下，最有可能产生现在观测到的数据。找到这个状态了，那就是我要求的位姿了。为了找到一个x和y（位姿和地标），使得利用它们产生的观测数据z2最接近目前直接观测到的z，那这样的x和y不就是我们需要的吗？ 
如上，实际的观测数据的似然概率，满足这样一个形式， 所以要最大化这个似然概率！（这块和之后的卡尔曼滤波器内容很完美的结合了起来，我在里面有对这部分内容展开更详细的叙述：SLAM14讲学习笔记（六）后端（最难一章：卡尔曼滤波器推导、理解以及扩展），如果没有学到卡尔曼滤波器，就先不用看了） 显然优化的思路应该是最小化什么东西，所以采取用负对数的形式，最小化它的负对数，其实就是最大化它的似然概率。 对于矩阵的正态分布，长这个样子： 
求负对数，是下面这个样子： 
右边第一项，和x无关，人也不可能影响它，就是一个客观的自然规律，因此跳过，求第二项的最小值，也就算出最大的后验概率了。 我们把slam的两个方程套进去，先套观测方程： x就是观测值了，也就是zkj；μ作为一个均值，应该是没有误差的，因此就是h(y,x),不带最后那个噪声v。∑作为方差，也就是噪声的方差Q，因此其实就是要求下面这个x： 
如果把误差用e表示，那么一个观测方程，一个状态方程结合起来的优化函数就可以写成这样： 
这个R可不是位姿啊，事实上是状态方程里的那个噪声所服从高斯分布的方差。高博书里用了这个很误导人的符号，这点应该改下。 顺便说一下，如果要融合IMU，也是这种形式，再加一项误差。这个公式很让人害怕，但是这么拆开一看，其实也就是那回事。 接下来的目标，就是怎么求这个优化函数的最小值。
接下来的这块就是这章最难的地方了，第一次看的时候，不知所云，这次看我觉得有所理解了，所以现在从宏观的地方总结一下： 非线性最小二乘这块，就是为了算极值，目的是优化上面的那个联合优化函数，算它的最小值，这样就能求出我想要的位姿了。 两种思路：第一种，固定搜索方向，沿着该方向寻找步长，也就是line search方式。第二种方式是固定搜索区域，再从区域里找最优点，也就是Trust Region方式。
Line search的思路是这样的：
Line search主要有两种解法：梯度下降法和高斯牛顿法。
梯度下降法：
又可以分为两种，一阶梯度法和二阶梯度法。这两种都是把函数在x附近泰勒展开： 
H是二阶导数，海塞矩阵。J是雅克比矩阵，表示fx关于x的导数。这种方式的优点是比较直观，解线性方程组就行了。 如果是一阶梯度法，那就不要第二项，让它最小就完事了。这又叫最速下降法，缺陷是太贪心了，容易走锯齿路线。 如果是二阶梯度法，那么第二项还是要的，求右侧关于△x的导数，并令其为0。这种方法又叫牛顿法，缺陷是它得算一个H矩阵，不好算。 两者结果的区别，都是-JT，后者是H△x=-JT，前者直接△x就是-JT（T是转置符号） 这里用的2范数，理论上是可以换成别的范数的。 总的来说这两种方式不太实用，下面介绍比较实用的方式：
高斯牛顿法：
这的思想是，把fx一阶展开（不是fx的平方了） 
这个也是要求极值，这里的f都是之前上个部分求出的联合优化函数。求极值的目的是让它最小。 因此构建线性最小二乘问题： 
把上面的这个给展开，写成自身转置和自身的乘积，然后把转置加进括号里，再交叉相乘去掉所有的括号，变成多项式的形式。 下一步求△x的导数，令其为0。 最后求得的结果是JT *J *△x=-JT*fx （这里的fx是误差），近似写成H△x=g。 说白了就是用J和自身转置的乘积来代替牛顿法里的H矩阵的计算过程。这是它的优点。 这种方法的限制在于：JT和J构成的近似H矩阵应该是可逆的，正定的（特征值为正，各阶顺序主子式为正）。但实际算出来的是半正定的。如果出现它是奇异矩阵或者是病态矩阵，那算法就不会收敛。 对于这个的改进，有的方法是在找到△x以后，再找一个α，让||f(x+α△x)||2最小，不是简单的让α等于1。
Trust Region的思路是这样的：
列文伯格-马尔夸特方法：
这里的ρ如下定义： 
分子表明实际函数下降的值，分母表明用雅克比矩阵的近似估计值。可以根据ρ来调整近似范围，太小说明分母较大，说明估计的太大，应该缩小范围；太大说明分母较小，说明的估计的小，可以放大近似范围。 上面算法里的阈值3/4和1/4，还有半径的放缩系数2和0.5都是经验值。 列文伯格-马尔夸特方法又可以叫做阻尼牛顿法。对于上面的D，我认为这个是控制区域的形状。如果区域是球，那就取单位阵E，它也可以把球弄成一个椭球。 注意上面步骤里面，是有个st不等式约束的，可以用拉格朗日乘子把它转化成一个无约束优化问题。（对于这点暂时我不是很明白原理，为了不偏离主题，我目前当它这样能解决问题就完事了） 
算出来以后，是这样： 
λ较小，H占主要地位，接近上面的高斯牛顿法。 λ较大，更接近一阶梯度下降法。DT*D可以简化为单位矩阵E。 列文伯格-马尔夸特方法优点就是可以避免非奇异与病态问题，缺点是收敛速度会很慢。
最后总结一下这一章的思路：想通过最大似然估计来求位姿，那最大似然就等同于最小化负对数；把观测方程和状态方程代入进去，这个负对数函数就是要优化的函数，要最小的函数，就和神经网络里的损失函数似的。要最小化它，就是求它的极小点，求的方式就是line search或者trust region方式，line search有梯度下降法（包括一阶的最速下降法和二阶的牛顿法，前者直观但是太贪心，后者直观但是得算H矩阵）和高斯牛顿法（用J和自身转置的乘积来代替H矩阵）。trust region主要是列温伯格马尔夸特方法，避免非奇异性问题，但是收敛较慢。
今天再补充一下，这章的内容由于没有套用后面的知识，所以有些抽象，不能理解意义。看完后面的，这里很多东西也就能明白了。在非线性优化的过程中，目标函数作为想要优化的结果，这个函数的自变量x，其实就是位姿Rt或者位置p。 因此，上面式子中的x，就可以当做位姿，我们可以这么理解：在这个位姿下，对它进行△x的修正，使得目标函数也就是误差最小了，然后把位姿更新成x+△x。正常来说。y=f(x)，我们想知道y的极小值，求的应该是直接去求关于x的导数，在导数为0处是f(x)极小值，目标函数也就是误差值最小，对吧？，那这求△x是干什么呢？ 这恰恰就是这章独特、精髓之处。实际的数据是有误差的，是想要根据很多有误差和噪声的数据，最后求得一个最合适的位姿，让平均的误差最小。用f(x)直接对x求导，是非常不方便的！因此采用f(x+△x)的形式，不断沿着梯度下降的方向走，迭代更新x，从而计算使得目标函数最小的x值。 这章的缺陷在于，读者不知道这是在干什么，实际上这是后面大部分内容的基础，建议配合光度误差和重投影误差的内容去阅读：SLAM14讲学习笔记（五）视觉里程计（光流法和直接法）和对于雅克比矩阵的理解 在这个的最后，我总结了怎么通过误差函数算雅克比矩阵，这样算出雅克比矩阵才能套在上面的非线性优化的地方求出△x，进一步实现x+△x的更新。看完这个，才能真正明白这节的非线性优化内容是干什么的，否则肯定是看了就忘，不理解含义。另外，在后端，这块也会被反复提及和使用，我认为这是实现SLAM理论的精髓部分，后端内容在这里，有点基础的可以进去一看：SLAM14讲学习笔记（七）后端（BA与图优化，Pose Graph优化的理论与公式详解、因子图优化） 其余内容，详见我的SLAM专栏学习笔记，欢迎留言交流。
