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，由多元函数的各个偏导数组成的向量
  以二元函数为例，其梯度为：
  
  ∇f(x1,x2)=(∂f∂x1,∂f∂x2)
• Hessian matrix 由多元函数的二阶偏导数组成的方阵，描述函数的局部曲率，以二元函数为例，
  
  H(f(x1,x2))=⎡⎣⎢⎢⎢⎢⎢∂2f∂x21∂2f∂x1∂x2∂2f∂x1∂x2∂2f∂x22⎤⎦⎥⎥⎥⎥⎥
• 雅可比矩阵 Jacobian matrix，是多元函数一阶偏导数以一定方式排列成的矩阵，体现了一个可微方程与给出点的最优线性逼近。以二元函数为例，
  
  J(f(x1,x2))=⎡⎣⎢⎢⎢⎢⎢⎢∂y1∂x1...∂ym∂x1........∂y1∂xn...∂ym∂xn⎤⎦⎥⎥⎥⎥⎥⎥

雅可比矩阵作用，如果P是Rn中的一点，F在P点可微分，那么在这一点的导数由JF(P)给出，在此情况下，由F(P)描述的线性算子即接近点P的F的最优线性逼近：

F(x)≈F(P)+JF(P)(X−P)

newton method

若非线性目标函数f(x)具有二阶连续偏导，在xk为其极小点的某一近似，在这一点取f(x)的二阶泰勒展开，即：

ϕ(x)≈f(xk)+f(xk)TΔx+12ΔxTH(xk)Δx

f(x)的梯度为

∇f(x)≈∇f(xk)+H(xk)Δx

在极小值点满足

∇f(x)≈∇f(xk)+H(xk)Δx=0

so we have

Δx=x−xk=−H(xk)∇f(xk)

如果f(x)是二次函数，则其黑森矩阵H为常数，在这种情况下，从任意一点出发，只要一步可求出f(x)的极小点(假设黑森矩阵正定，所有特征值大于0)
如果f(x)不是二次函数，梯度表达仅是一个近似表达式，此时，按赋值０求得的极小点，只是f(x)的近似极小点。在这种情况下，常按照下面选取搜索方向：

pk=−H(xk)∇f(xk)

Δxk+1=xk−λkpk

λk:=minf(xk+λkpk)

牛顿法收敛的速度很快，当f(x)的二阶导数及其黑森矩阵的逆矩阵便于计算时，这一方法非常有效。【但通常黑森矩阵很不好求】

Gauss-Newton

gauss-newton是如何由上述派生的

有时候为了拟合数据，比如根据重投影误差求相机位姿(R,t为方程系数)，常常将求解模型转化为非线性最小二乘问题。高斯牛顿法正是用于解决非线性最小二乘问题，达到数据拟合、参数估计和函数估计的目的。
假设我们研究如下形式的非线性最小二乘问题：

minx∈R∗f(x)=rT(x)r(x)

r(x)为ｘ的非线性函数，比如对于重投影误差。
位置残差为

rij=zij−z^(Ti,yj)

如果有大量观测点(多维)，我们可以通过选择合理的T使得残差的平方和最小求得两个相机之间的位姿。机器视觉这块暂时不扩展，接着说怎么求非线性最小二乘问题。

若用牛顿法求式3，则牛顿迭代公式为：

Δxk+1=xk−H−1∇f

here

∇f=2∑i=1mri∂ri∂xj

Hjk=2∑i=1m(∂ri∂xj∂ri∂xk+ri∂2ri∂xj∂xk)

Hjk=2∑i=1mJijJjk

用矩阵表示为

r=(r1,...,rm)

Δxk+1=xk−(JTJ)−1JTr

here

Jij=∂ri(xk)∂xjk

看到这里大家都明白高斯牛顿和牛顿法的差异了吧，就在这迭代项上。经典高斯牛顿算法迭代步长λ为1.

那回过头来，高斯牛顿法里为啥要舍弃黑森矩阵的二阶偏导数呢？主要问题是因为牛顿法中Hessian矩阵中的二阶信息项通常难以计算或者花费的工作量很大，而利用整个H的割线近似也不可取，因为在计算梯度 时已经得到J(x)，这样H中的一阶信息项JTJ几乎是现成的。鉴于此，为了简化计算，获得有效算法，我们可用一阶导数信息逼近二阶信息项。 注意这么干的前提是，残差r接近于零或者接近线性函数从而接近与零时，二阶信息项才可以忽略。通常称为“小残量问题”，否则高斯牛顿法不收敛.

refer

1. http://blog.csdn.net/dsbatigol/article/details/12448627
2. http://www.voidcn.com/blog/jinshengtao/article/p-6004352.html

g2o

g2o使用举例
主要步骤
1. SparseOptimizer 是我们最终要维护的东东。它是一个Optimizable Graph，从而也是一个Hyper Graph。一个 SparseOptimizer 含有很多个顶点 （都继承自 Base Vertex）和很多个边（继承自 BaseUnaryEdge, BaseBinaryEdge或BaseMultiEdge）。这些 Base Vertex 和 Base Edge 都是抽象的基类，而实际用的顶点和边，都是它们的派生类。我们用 SparseOptimizer.addVertex 和 SparseOptimizer.addEdge 向一个图中添加顶点和边，最后调用 SparseOptimizer.optimize 完成优化。
2. 在优化之前，需要指定我们用的求解器和迭代算法。从图中下半部分可以看到，一个 SparseOptimizer 拥有一个 Optimization Algorithm，继承自Gauss-Newton, Levernberg-Marquardt, Powell’s dogleg 三者之一（我们常用的是GN或LM）。同时，这个 Optimization Algorithm 拥有一个Solver，它含有两个部分。一个是 SparseBlockMatrix ，用于计算稀疏的雅可比和海塞； 一个是用于计算迭代过程中最关键的一步
HΔx=−b
HΔx=−b
这就需要一个线性方程的求解器。而这个求解器，可以从 PCG, CSparse, Choldmod 三者选一。

在g2o中选择优化方法

一共需要三个步骤：
- 选择一个线性方程求解器，从 PCG, CSparse, Choldmod中选，实际则来自 g2o/solvers 文件夹中定义的东东。
- 选择一个 BlockSolver 。
- 选择一个迭代策略，从GN, LM, Doglog中选。
blog
1. http://blog.csdn.net/heyijia0327/article/details/47813405
2. http://www.cnblogs.com/gaoxiang12/p/5304272.html
3. http://www.cnblogs.com/gaoxiang12/p/3776107.html

alse their code and data is available in the github
1. https://github.com/gaoxiang12/g2o_ba_example
2. https://github.com/HeYijia/GraphSLAM_tutorials_code/tree/master/g2o_test