深入理解图优化与g2o
slam中的主流优化方法——图优化（graph-based optimization）
优化
图优化本质上是一个优化问题，所以我们先来看优化问题是什么。
优化问题有三个最重要的因素：目标函数、优化变量、优化约束。一个简单的优化问题可以描述如下：
其中𝑥为优化变量，而𝐹(𝑥)为优化函数。此问题称为无约束优化问题，因为我们没有给出任何约束形式。由于slam中优化问题多为无约束优化，所以我们着重介绍无约束的形式。
当𝐹(𝑥)有一些特殊性质时，对应的优化问题也可以用一些特殊的解法。例如，𝐹(𝑥)为一个线性函数时，则为线性优化问题（不过线性优化问题通常在有约束情形下讨论）。反之则为非线性优化。对于无约束的非线性优化，如果我们知道它梯度的解析形式，就能直接求那些梯度为零的点，来解决这个优化：
◎ \frac{\mathrm{d} F}{\mathrm{d} x} = 0
梯度为零的地方可能是函数的极大值、极小值或者鞍点。由于现在𝐹(𝑥)的形式不确定，我们只好遍历所有的极值点，找到最小的作为最优解。
但是我们为什么不这样用呢？因为很多时候𝐹(𝑥)的形式太复杂，导致我们没法写出导数的解析形式，或者难以求解导数为零的方程。因此，多数时候我们使用迭代方式求解。从一个初值𝑥0出发，不断地导致当前值附近的，能使目标函数下降的方式（反向梯度），然后沿着梯度方向走出一步，从而使得函数值下降一点。这样反复迭代，理论上对于任何函数，都能找到一个极小值点。
迭代的策略主要体现在如何选择下降方向，以及如何选择步长两个方面。主要有 Gauss-Newton （GN）法和 Levenberg-Marquardt （LM）法两种，它们的细节可以在维基上找到，我们不细说。请理解它们主要在迭代策略上有所不同，但是寻找梯度并迭代则是一样的。
图优化
所谓的图优化，就是把一个常规的优化问题，以图（Graph）的形式来表述。 图是由顶点（Vertex）和边（Edge）组成的结构，而图论则是研究图的理论。我们记一个图为𝐺={𝑉,𝐸}，其中𝑉为顶点集，𝐸为边集。
顶点没什么可说的，想象成普通的点即可。
边是什么呢？一条边连接着若干个顶点，表示顶点之间的一种关系。边可以是有向的或是无向的，对应的图称为有向图或无向图。边也可以连接一个顶点（Unary Edge，一元边）、两个顶点（Binary Edge，二元边）或多个顶点（Hyper Edge，多元边）。最常见的边连接两个顶点。当一个图中存在连接两个以上顶点的边时，称这个图为超图（Hyper Graph）。而SLAM问题就可以表示成一个超图（在不引起歧义的情况下，后文直接以图指代超图）。
怎么把SLAM问题表示成图呢？
SLAM的核心是根据已有的观测数据，计算机器人的运动轨迹和地图。假设在时刻𝑘，机器人在位置𝑥𝑘处，用传感器进行了一次观测，得到了数据𝑧𝑘。传感器的观测方程为：
𝑧𝑘=ℎ(𝑥𝑘)
由于误差的存在，𝑧𝑘不可能精确地等于ℎ(𝑥𝑘)，于是就有了误差：
𝑒𝑘=𝑧𝑘−ℎ(𝑥𝑘)
那么，如果我们以𝑥𝑘为优化变量，以min𝑥𝐹𝑘(𝑥𝑘)=‖𝑒𝑘‖为目标函数，就可以求得𝑥𝑘的估计值，进而得到我们想要的东西了。这实际上就是用优化来求解SLAM的思路。
优化变量𝑥𝑘，观测方程𝑧𝑘=ℎ(𝑥𝑘)
取决于我们的参数化（parameterazation）。𝑥可以是一个机器人的Pose（6自由度下为 4×4的变换矩阵𝐓 或者 3自由度下的位置与转角[𝑥,𝑦,𝜃]，也可以是一个空间点（三维空间的[𝑥,𝑦,𝑧]或二维空间的[𝑥,𝑦]）。相应的，观测方程也有很多形式，如：
- 机器人两个Pose之间的变换；
- 机器人在某个Pose处用激光测量到了某个空间点，得到了它离自己的距离与角度；
- 机器人在某个Pose处用相机观测到了某个空间点，得到了它的像素坐标； 同样，它们的具体形式很多样化，这允许我们在讨论slam问题时，不局限于某种特定的传感器或姿态表达方式。
在图中，以顶点表示优化变量，以边表示观测方程。由于边可以连接一个或多个顶点，所以我们把它的形式写成更广义的 𝑧𝑘=ℎ(𝑥𝑘1,𝑥𝑘2,…),以表示不限制顶点数量的意思。对于刚才提到的三种观测方程，顶点和边是什么形式呢？
机器人两个Pose之间的变换；——一条Binary Edge（二元边），顶点为两个pose，边的方程为𝑇1=Δ𝑇⋅𝑇2。 机器人在某个Pose处用激光测量到了某个空间点，得到了它离自己的距离与角度；——Binary Edge，顶点为一个2D Pose：[𝑥,𝑦,𝜃]𝑇和一个Point：[𝜆𝑥,𝜆𝑦]𝑇，观测数据是距离𝑟和角度𝑏，那么观测方程为：
机器人在某个Pose处用相机观测到了某个空间点，得到了它的像素坐标；——Binary Edge，顶点为一个3D Pose：𝑇和一个空间点𝐱=[𝑥,𝑦,𝑧]𝑇，观测数据为像素坐标𝑧=[𝑢,𝑣]𝑇。那么观测方程为： 𝑧=𝐶(𝑅𝐱+𝑡)(6)
𝐶为相机内参，𝑅,𝑡为旋转和平移。
举这些例子，是为了让读者更好地理解顶点和边是什么东西。由于机器人可能使用各种传感器，故我们不限制顶点和边的参数化之后的样子。比如我（丧心病狂地在小萝卜身上）既加了激光，也用相机，还用了IMU，轮式编码器，超声波等各种传感器来做slam。为了求解整个问题，我的图中就会有各种各样的顶点和边。但是不管如何，都是可以用图来优化的。