这篇文章开始前，大家一起思考两个问题：

1、为什么需要用到最小二乘法？

2、为什么采用数值解法？

想明白这两个问题，才能够带着目的去了解接下来要学习的内容。

首先，得先了解定位问题，那就是求解位姿，那么位姿怎么求呢？有两个思路，就是利用上一时刻的位姿，更具各种测量出来的读数，计算下一时刻可能的位姿。就是说，位姿不能够直接测量到，转而测量那些能够测量到的，并且将会影响到位姿的变量。这就是卡尔曼滤波器的做法。

另外的一个思路就是从概率论的角度，我能够利用得到的观测方程数据，去估计我目前所在的大概位子，反过来说，就是在怎么样的位子，能够看到这些数据。这时候我们就建立了估计状态变量的条件分布。把问题变成去求一个最优的状态估计，使得后验的概率最大化。（贝叶斯法则）

因此问题的难点就变为怎么去用数学表述后验概率，用怎么样的函数来描述slam问题。（这个问题先留着。。。后边有彩蛋）

slam十四讲中有推导，大家可以去看看，大致思路就是

第二个问题：为什么采用数值解法呢？这个比较好解答，当问题描述函数不是简单的线性函数（线性函数容易求导，计算方便）之后，我们要求函数极值是否就比较麻烦，这时候我们就借助计算机善于计算的优势，将问题答案通过大量的计算，去逼近这个真实的答案。那么我们要做的事情就是用什么策略让计算机去逼近计算。

一、最小二次问题的数值解法

总结迭代法：

1、 将对目标函数求导，找出函数极值点的问题转变为通过迭代，下降策略，找到函数的最低点。

2、迭代的关键三要素：初值、下降方向、步长

通过对目标函数进行泰勒展开，根据展开的公司保留的项数，又分为一阶梯度和二阶梯度。分别对应最快速下降法和牛顿法；

但是这两种方法都有缺点，实际工程更为常用的求解方法是优化后的，GN、LM方法。

下边在说明下这四种方法在求解上是具体怎么做的：

二、用高斯牛顿法做曲线拟合

1、之所以我们定义了指数函数作为我们的目标优化函数。当然我们也能够将函数定义为y=kx+b、ax2 +bx+c=y。完全没有问题，一样的数据集，待会也能够优化出最大程度贴近这些数据集的方程。

2、exp

 为什么需要用exp定义呢？因为ax2 +bx+c=y，值域是能够区遍负无穷到正无穷的，而这个值域刚好是指数函数的定义域，指数函数在定义域内是单调递增或者递减的。因此适用来进行迭代。（函数图像如果跌但起伏，很难算出收敛的值）。

 因此我们的目标函数就是

 大家不用太过于纠结这里的函数，看这里。

 就是离散点的y值就是通过这个函数算出来的，我们是要利用这些离散的数据，算回去ar br c能够取多少，尽可能还原这个函数。

3、再看看高斯牛顿的推导过程

我们需要求出来的量是H=J(x)Jt(x) 以及g=-J(x)f(x) ,对应到代码里边

 这里用指数函数的便捷性就出来了，就是求偏导十分方便。

最后这个步骤

对应代码

 这里为什么是加法呢，难道步长还带方向？？

设置停止条件。这就是整个过程。

三、使用 Ceres 进行曲线拟合

1、定义代价函数（需要拟合的函数）也叫残差块

2、 定义参数块，需要优化出来的函数参数，需要他们他们写出完成函数

3、构建最小二乘问题，利用自动求导计算雅可比矩阵 

4、配置求解器，把所有的参数块和残差块加入 Ceres 定义的 Problem 对象中，调用 Solve 函数求解即 可。求解之前，我们可以传入一些配置信息，例如迭代次数、终止条件等，也可以使用默认 的配置。

 基本思想和迭代法求解还是比较契合的。下边来看看图优化思想。 

四、使用 g2o 进行曲线拟合

图优化是一种将非线性优化与图论结合起 来的理论。

图优化，是把优化问题表现成图(Graph)的一种方式。这里的图是图论意义上的图。一个图 由若干个顶点(Vertex)，以及连接着这些顶点的边(Edge)组成。进而，用顶点表示优化变量，用 边表示误差项。于是，对任意一个上述形式的非线性最小二乘问题，我们可以构建与之对应的一个 图。我们可以简单地称它为图。

1、定义顶点和边的类型。

2、构建图

添加顶点，顶点还是那些最后计算出来函数的参数

添加边

 大量的离散数据作为了图优化过程的误差项，利用大量的误差项来减少误差。

3、选择优化算法。

4、调用 g2o 进行优化，返回结果。

五、总结

非线形优化，要记住下边三个要点。

1、确定优化变量

2、能够提供优化看到的误差项（大量观测数据）

3、怎么构造一个优化函数？就是开篇说到的用怎么样的函数来描述slam问题？？

带着第三个问题，开启第三篇文章。