深入理解图优化与g2o
slam中的主流优化方法——图优化(graph-based optimization)
优化
图优化本质上是一个优化问题,所以我们先来看优化问题是什么。
优化问题有三个最重要的因素:目标函数、优化变量、优化约束。一个简单的优化问题可以描述如下:
其中𝑥为优化变量,而𝐹(𝑥)为优化函数。此问题称为无约束优化问题,因为我们没有给出任何约束形式。由于slam中优化问题多为无约束优化,所以我们着重介绍无约束的形式。
当𝐹(𝑥)有一些特殊性质时,对应的优化问题也可以用一些特殊的解法。例如,𝐹(𝑥)为一个线性函数时,则为线性优化问题(不过线性优化问题通常在有约束情形下讨论)。反之则为非线性优化。对于无约束的非线性优化,如果我们知道它梯度的解析形式,就能直接求那些梯度为零的点,来解决这个优化:
梯度为零的地方可能是函数的极大值、极小值或者鞍点。由于现在𝐹(𝑥)的形式不确定,我们只好遍历所有的极值点,找到最小的作为最优解。
但是我们为什么不这样用呢?因为很多时候𝐹(𝑥)的形式太复杂,导致我们没法写出导数的解析形式,或者难以求解导数为零的方程。因此,多数时候我们使用迭代方式求解。从一个初值𝑥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。为了求解整个问题,我的图中就会有各种各样的顶点和边。但是不管如何,都是可以用图来优化的。