SLAM代码（优化及常用库）

在SLAM的后端优化中，比较常用的一种方法是g2o，该图优化方法在优化时现成可以选用的有２种优化方法。一是Ｇauss-Newton，另外是LM方法。这里首先介绍Gauss-Newton方法，其次介绍g2o使用时的一般流程和例程。

Gauss-Newton

Gauss-Newton算法是解决非线性最优问题的常见算法之一.

基本概念定义

非线性方程定义及最优化方法简述

指因变量与自变量之间的关系不是线性的关系，比如平方关系、对数关系、指数关系、三角函数关系等等。对于此类方程，求解n元实函数f在整个n维向量空间Rn上的最优值点往往很难得到精确解，经常需要求近似解问题。
求解该最优化问题的方法大多是逐次一维搜索的迭代算法，基本思想是在一个近似点处选定一个有利于搜索方向，沿这个方向进行一维搜索，得到新的近似点。如此反复迭代，知道满足预定的精度要求为止。根据搜索方向的取法不同，这类迭代算法可分为两类：
- 解析法：需要用目标函数的到函数，
- 梯度法：又称最速下降法，是早期的解析法，收敛速度较慢
- 牛顿法：收敛速度快，但不稳定，计算也较困难。高斯牛顿法基于其改进，但目标作用不同
- 共轭梯度法：收敛较快，效果好
- 变尺度法：效率较高，常用DFP法(Davidon Fletcher Powell)
- 直接法：不涉及导数，只用到函数值。有交替方向法(又称坐标轮换法)、模式搜索法、旋转方向法、鲍威尔共轭方向法和单纯形加速法等。

非线性最小二乘问题

非线性最小二乘问题来自于非线性回归，即通过观察自变量和因变量数据，求非线性目标函数的系数参数，使得函数模型与观测量尽量相似。高斯牛顿法解决非线性最小二乘问题的最基本方法，并且它只能处理二次函数。

Unlike Newton’smethod, the Gauss–Newton algorithm can only be used to minimize a sum ofsquared function values

基本数学表达

• 梯度gradient，由多元函数的各个偏导数组成的向量
  以二元函数为例，其梯度为：
  $$\nabla f(x_1,x_2)=\left(\frac{\partial f}{\partial x_1},\frac{\partial f}{\partial x_2 }\right)$$
• Hessian matrix 由多元函数的二阶偏导数组成的方阵，描述函数的局部曲率，以二元函数为例，
  $$H(f(x_1,x_2))=\begin{bmatrix}\frac{\partial^2 f}{\partial x^2_1}&\frac{\partial^2 f}{\partial x_1\partial x_2}\\\frac{\partial^2 f}{\partial x_1\partial x_2}& \frac{\partial^2 f}{\partial x^2_2 }\end{bmatrix}$$
• 雅可比矩阵 Jacobian matrix，是多元函数一阶偏导数以一定方式排列成的矩阵，体现了一个可微方程与给出点的最优线性逼近。以二元函数为例，
  
  J(f(x1,x2))=⎡⎣⎢⎢⎢⎢⎢⎢∂y1