深入理解图优化与g2o:图优化篇
来源:互联网 发布:mac声音键不能调节 编辑:程序博客网 时间:2024/06/11 05:06
深入理解图优化与g2o:图优化篇
前言
本节我们将深入介绍视觉slam中的主流优化方法——图优化(graph-based optimization)。下一节中,介绍一下非常流行的图优化库:g2o。
关于g2o,我13年写过一个文档,然而随着自己理解的加深,越发感觉不满意。本着对读者更负责任的精神,本文给大家重新讲一遍图优化和g2o。除了这篇文档,读者还可以找到一篇关于图优化的博客: http://blog.csdn.net/heyijia0327 那篇文章有作者介绍的一个简单案例,而本文则更注重对图优化和g2o的理解与评注。
本节主要介绍图优化的数学理论,下节再讲g2o的组成方式及使用方法。
预备知识:优化
图优化本质上是一个优化问题,所以我们先来看优化问题是什么。
优化问题有三个最重要的因素:目标函数、优化变量、优化约束。一个简单的优化问题可以描述如下:
当
梯度为零的地方可能是函数的极大值、极小值或者鞍点。由于现在
但是我们为什么不这样用呢?因为很多时候
迭代的策略主要体现在如何选择下降方向,以及如何选择步长两个方面。主要有 Gauss-Newton (GN)法和 Levenberg-Marquardt (LM)法两种,它们的细节可以在维基上找到,我们不细说。请理解它们主要在迭代策略上有所不同,但是寻找梯度并迭代则是一样的。
图优化
所谓的图优化,就是把一个常规的优化问题,以图(Graph)的形式来表述。
图是什么呢?
图是由顶点(Vertex)和边(Edge)组成的结构,而图论则是研究图的理论。我们记一个图为
顶点没什么可说的,想象成普通的点即可。
边是什么呢?一条边连接着若干个顶点,表示顶点之间的一种关系。边可以是有向的或是无向的,对应的图称为有向图或无向图。边也可以连接一个顶点(Unary Edge,一元边)、两个顶点(Binary Edge,二元边)或多个顶点(Hyper Edge,多元边)。最常见的边连接两个顶点。当一个图中存在连接两个以上顶点的边时,称这个图为超图(Hyper Graph)。而SLAM问题就可以表示成一个超图(在不引起歧义的情况下,后文直接以图指代超图)。
怎么把SLAM问题表示成图呢?
SLAM的核心是根据已有的观测数据,计算机器人的运动轨迹和地图。假设在时刻
由于误差的存在,
那么,如果我们以
你说的优化变量
这个取决于我们的参数化(parameterazation)。
- 机器人两个Pose之间的变换;
- 机器人在某个Pose处用激光测量到了某个空间点,得到了它离自己的距离与角度;
- 机器人在某个Pose处用相机观测到了某个空间点,得到了它的像素坐标;
同样,它们的具体形式很多样化,这允许我们在讨论slam问题时,不局限于某种特定的传感器或姿态表达方式。
我明白优化是什么意思了,但是它们怎么表达成图呢?
在图中,以顶点表示优化变量,以边表示观测方程。由于边可以连接一个或多个顶点,所以我们把它的形式写成更广义的
- 机器人两个Pose之间的变换;——一条Binary Edge(二元边),顶点为两个pose,边的方程为
T1=ΔT⋅T2 。 - 机器人在某个Pose处用激光测量到了某个空间点,得到了它离自己的距离与角度;——Binary Edge,顶点为一个2D Pose:
[x,y,θ]T 和一个Point:[λx,λy]T ,观测数据是距离r 和角度b ,那么观测方程为:[rb]=⎡⎣⎢(λx−x)2+(λy−y)2−−−−−−−−−−−−−−−−−√tan−1(λy−yλx−x)−θ⎤⎦⎥(5) - 机器人在某个Pose处用相机观测到了某个空间点,得到了它的像素坐标;——Binary Edge,顶点为一个3D Pose:
T 和一个空间点x=[x,y,z]T ,观测数据为像素坐标z=[u,v]T 。那么观测方程为:z=C(Rx+t)(6)
举这些例子,是为了让读者更好地理解顶点和边是什么东西。由于机器人可能使用各种传感器,故我们不限制顶点和边的参数化之后的样子。比如我(丧心病狂地在小萝卜身上)既加了激光,也用相机,还用了IMU,轮式编码器,超声波等各种传感器来做slam。为了求解整个问题,我的图中就会有各种各样的顶点和边。但是不管如何,都是可以用图来优化的
(暗黑小萝卜 小眼神degined by Orchid Zhang)以后找不到工作我就去当插画算了……
图优化怎么做
现在让我们来仔细看一看图优化是怎么做的。假设一个带有
关于这个目标函数,我们有几句话要讲。这些话都是很重要的,请读者仔细去理解。
e 函数在原理上表示一个误差,是一个矢量,作为优化变量xk 和zk 符合程度的一个度量。它越大表示xk 越不符合zk 。但是,由于目标函数必须是标量,所以必须用它的平方形式来表达目标函数。最简单的形式是直接做成平方:e(x,z)Te(x,z) 。进一步,为了表示我们对误差各分量重视程度的不一样,还使用一个信息矩阵Ω 来表示各分量的不一致性。- 信息矩阵
Ω 是协方差矩阵的逆,是一个对称矩阵。它的每个元素Ωi,j 作为eiej 的系数,可以看成我们对ei,ej 这个误差项相关性的一个预计。最简单的是把Ω 设成对角矩阵,对角阵元素的大小表明我们对此项误差的重视程度。 - 这里的
xk 可以指一个顶点、两个顶点或多个顶点,取决于边的实际类型。所以,更严谨的方式是把它写成ek(zk,xk1,xk2,…) 但是那样写法实在是太繁琐,我们就简单地写成现在的样子。由于zk 是已知的,为了数学上的简洁,我们再把它写成ek(xk) 的形式。
于是总体优化问题变为
重复一遍,边的具体形式有很多种,可以是一元边、二元边或多元边,它们的数学表达形式取决于传感器或你想要描述的东西。例如视觉SLAM中,在一个相机Pose
现在,我们有了一个很多个节点和边的图,构成了一个庞大的优化问题。我们并不想展开它的数学形式,只关心它的优化解。那么,为了求解优化,需要知道两样东西:一个初始点和一个迭代方向。为了数学上的方便,先考虑第
我们假设它的初始点为
这是的
于是,对于第
进一步展开:
在熟练的同学看来,这个推导就像
请注意
我们的目标是找到
所以归根结底,我们求解一个线性方程组:
如果把所有边放到一起考虑进去,那就可以去掉下标,直接说我们要求解
原来算了半天它只是个线性的!线性的谁不会解啊!
读者当然会有这种感觉,因为线性规划是规划中最为简单的,连小学生都会解这么简单的问题,为何21世纪前SLAM不这样做呢?——这是因为在每一步迭代中,我们都要求解一个雅可比和一个海塞。而一个图中经常有成千上万条边,几十万个待估计参数,这在以前被认为是无法实时求解的。
那为何后来又可以实时求解了呢?
SLAM研究者逐渐认识到,SLAM构建的图,并非是全连通图,它往往是很稀疏的。例如一个地图里大部分路标点,只会在很少的时刻被机器人看见,从而建立起一些边。大多数时候它们是看不见的(就像后宫怨女一样)。体现在数学公式中,虽然总体目标函数
这会导致什么?这导致许多和
相应的二阶导矩阵
要补充一点的是,在数值计算中,我们可以给出雅可比和海塞的解析形式进行计算,也可以让计算机去数值计算这两个阵,而我们只需要给出误差的定义方式即可。
流形
等一下老师!上面推导还有一个问题!
很好,小萝卜同学,请说说是什么问题。
我们在讨论给目标函数
小萝卜同学看到了一个严重的问题,这确实是在先前的讨论中忽略掉了。由于我们不限制顶点的类型,
最简单的就是常见的四维变换矩阵
但是,如果图优化不能处理
回想我们先前讲过的李代数知识。虽然李群
这样的好处是我们完全不用重新推导公式。这件事比我们想的更加简单。在程序里,我们只需重新定义一个优化变量
核函数
又是核函数!——学过机器学习课程的同学肯定要这样讲。
但是很遗憾,图优化中也有一种核函数。 引入核函数的原因,是因为SLAM中可能给出错误的边。SLAM中的数据关联让科学家头疼了很长时间。出于变化、噪声等原因,机器人并不能确定它看到的某个路标,就一定是数据库中的某个路标。万一认错了呢?我把一条原本不应该加到图中的边给加进去了,会怎么样?
嗯,那优化算法可就慒逼了……它会看到一条误差很大的边,然后试图调整这条边所连接的节点的估计值,使它们顺应这条边的无理要求。由于这个边的误差真的很大,往往会抹平了其他正确边的影响,使优化算法专注于调整一个错误的值。
于是就有了核函数的存在。核函数保证每条边的误差不会大的没边,掩盖掉其他的边。具体的方式是,把原先误差的二范数度量,替换成一个增长没有那么快的函数,同时保证自己的光滑性质(不然没法求导啊!)。因为它们使得整个优化结果更为鲁棒,所以又叫它们为robust kernel(鲁棒核函数)。
很多鲁棒核函数都是分段函数,在输入较大时给出线性的增长速率,例如cauchy核,huber核等等。当然具体的我们也不展开细说了。
核函数在许多优化环境中都有应用,博主个人印象较深的时当年有一大堆人在机器学习算法里加各种各样的核,我们现在用的svm也会带个核函数。
小结
最后总结一下做图优化的流程。
- 选择你想要的图里的节点与边的类型,确定它们的参数化形式;
- 往图里加入实际的节点和边;
- 选择初值,开始迭代;
- 每一步迭代中,计算对应于当前估计值的雅可比矩阵和海塞矩阵;
- 求解稀疏线性方程
HkΔx=−bk ,得到梯度方向; - 继续用GN或LM进行迭代。如果迭代结束,返回优化值。
实际上,g2o能帮你做好第3-6步,你要做的只是前两步而已。下节我们就来尝试这件事。
- 深入理解图优化与g2o:g2o篇
- 深入理解图优化与g2o:g2o篇
- 深入理解图优化与g2o:图优化篇
- 深入理解图优化与g2o:图优化篇
- 深入理解图优化与g2o:图优化篇
- 深入理解图优化与g2o : 图优化理论与g2o的使用
- 图优化--g2o入门
- g2o图优化
- 图优化理论与g2o的使用
- 图优化理论与g2o的使用(1)
- 图优化理论与g2o的使用(2)
- 图优化理论与g2o的使用(3)
- windows编译g2o图优化框架(vs2010)
- g2o:一种图优化的C++框架
- RANSAC、ICP、图优化g2o学习笔记
- g2o:一种图优化的C++框架
- g2o:一种图优化的C++框架
- 视觉SLAM漫谈(二):图优化理论与g2o的使用
- Linux配置之解决CentOS中:xx is not in the sudoers file的问题
- winform 调用声音文件
- DataGridView控件添加数据
- 【Inno Setup】设置控制面板中的程序卸载的图标
- ora-003114
- 深入理解图优化与g2o:图优化篇
- 文章标题
- linux的find命令详解
- 二分法 保留10位小数
- 数据库性能优化之SQL语句优化
- java之阻塞IO(BIO)
- android6.0程序在运行时申请权限
- 如何在linux下安装mysql数据库并配置
- 修改表结构alter table