cartographer从入门到放弃5---核心算法分析---Global SLAM

算法的推理是件头疼的事，牵扯的知识点太多了，慢慢梳理，相信现在！

文章更新比较慢，可以先看看参考文献中大神的解释

重点参考：SPA优化算法详解：以Cartographer后端为例

SLAM从概率来讲分为在线SLAM和全SLAM：在线SLAM只包含了t时刻的变量的估计问题，在线SLAM的很多算法是增量的，在这些算法中，过去的测量值和控制量一但被处理立即丢弃，如EKF-SLAM；全SLAM计算的是全路径和地图的后验，是一种离线的SLAM算法，Graph-SLAM; cartographer是用了Graph-SLAM.

1.图优化

1.1 优化

在实际工程问题中，存在各种的问题，为了得到最优的解，经常采用优化求解的方法，优化的表达如下：

借用高翔的说明：

　优化问题有三个最重要的因素：目标函数、优化变量、优化约束。一个简单的优化问题可以描述如下

其中为优化变量，而()为优化函数。此问题称为无约束优化问题，因为我们没有给出任何约束形式。由于slam中优化问题多为无约束优化，所以我们着重介绍无约束的形式。

当()有一些特殊性质时，对应的优化问题也可以用一些特殊的解法。例如，()为一个线性函数时，则为线性优化问题（不过线性优化问题通常在有约束情形下讨论）。反之则为非线性优化。对于无约束的非线性优化，如果我们知道它梯度的解析形式，就能直接求那些梯度为零的点，来解决这个优化：

梯度为零的地方可能是函数的极大值、极小值或者鞍点。由于现在()的形式不确定，我们只好遍历所有的极值点，找到最小的作为最优解。是我们为什么不这样用呢？因为很多时候()的形式太复杂，导致我们没法写出导数的解析形式，或者难以求解导数为零的方程。因此，多数时候我们使用迭代方式求解。从一个初值0出发，不断地导致当前值附近的，能使目标函数下降的方式（反向梯度），然后沿着梯度方向走出一步，从而使得函数值下降一点。这样反复迭代，理论上对于任何函数，都能找到一个极小值点。

　　迭代的策略主要体现在如何选择下降方向，以及如何选择步长两个方面。主要有 Gauss-Newton （GN）法和 Levenberg-Marquardt （LM）法两种，它们的细节可以在维基上找到，我们不细说。请理解它们主要在迭代策略上有所不同，但是寻找梯度并迭代则是一样的。

1.2 图优化

举个简答的例子：

假如机器人初始点在A处，然后通过激光雷达机器人向前移动0.3m到B，再利用激光雷达测距向前运行0.7m到C处，再用激光雷达测距1米到D点。可以看出，实际上D点并没有和A点重合，存在误差，如何调节误差，使得误差分配到各个位姿中，更好的形成回环，图优化就是用来解决这个问题

图的概念：这里的图可以理解为为图论中的图，图是由顶点和边组成，一个点可以连接几个边，边可以有方向和加权值。有点抽象，举个例子，如上图所示，位置A、B、C、D四个点可以理解为顶点，它们之间的距离可以理解为加权的值。

cartographer详细的SPA可以参考参考文献中的，写的很详细，很精彩，为了不误导大家，大家可以去看看，推荐的是我见过的最好的文章。

---------------------------------------------------------------------

参考文献：

1.SPA优化算法详解：以Cartographer后端为例

2.Cartographer ROS

3.深入理解图优化与g2o：图优化篇