摘要

在SLAM的基本概念中，我们提到可以用非线性优化的方式来对SLAM问题进行求解。
这篇博客将从非线性优化的角度，谈一谈对SLAM问题求解的思路。

SLAM问题

回顾SLAM问题的运动方程和观测方程。
运动方程：$x_k=f(x_{k-1}, u_k)$
观测方程：$z_k = h(x_k)$

SLAM问题求解的关键，就是对$x$进行估计，使得运动方程和观测方程尽可能的成立。
根据运动方程和测量方程，可以构造误差项：
$e_{x,k} = x_k - f(x_{k-1},u_k)$
$e_{z,k} = z_k - h(x_k)$

因此可以将误差项构造成最小二乘问题：
$ J(x)=\sum_k e_{x,k}^T R_k^{-1} e_{x,k} + \sum_ke_{z,k}^T Q_k^{-1} e_{z,k} $

最小二乘问题

对于一个函数形式比较简单的最小二乘问题，可以通过令目标函数导数为0，得到极值。它们可能是极大、极小或鞍点处的值，只要挨个比较它们的函数值大小即可。
遗憾的是，在SLAM问题中，函数形式往往比较复杂，因此通常通过迭代的方式求解。也就是从一个初始值$x_0$出发，不断地更新当前的优化变量$x_k=x_{k-1}+\Delta x$，使函数值不断减小，最终得到目标函数的最小值和对应的变量值。因此问题的关键变为了如何求解$\Delta x$。对于$\Delta x$的计算是优化领域中非常关键的问题，最常用的有两种方法：高斯牛顿法和列文伯格-马夸尔特方法。
对于这两种方法的详细介绍，等我看懂了，再另写博客。(> <)

图优化理论

在SLAM问题中构造的最小二乘问题有一个特殊的地方在于虽然整个问题的目标函数的变量维度很高，但每个误差项都是简单的，仅与少量变量有关。因此为了直观的展示变量与变量之间的关系，可以把优化问题用图的方式来展现。其中顶点表示优化变量，边表示误差项。用图来表示一个优化问题有如下几个好处：
1）可以直观展示变量之间的关系结构，非常符合SLAM问题的表述
2）可以利用一些图论的方法来改进优化问题，比如去掉孤立顶点，优先优化边数较多的顶点等
3）在SLAM问题中，用图的这种形式可以非常方便的添加优化变量。

基于以上几个好处，图优化理论几乎已经称为SLAM问题中非线性优化的标准解法。
顺便提一句，g2o用c++实现了利用图优化进行优化求解的方法，很多开源的SLAM系统中都会用到g2o。

参考文献

1.《视觉SLAM十四讲》第六讲