数学里的图论优化原理简介
图优化是视觉slam中的主流优化方法,所谓的图优化是把常规的优化问题以图的形式来表述。
图(graph)由顶点(Vertex)和边(Edge)组成,在常见的slam问题中,机器人的位姿是一个顶点(Vertex),不同时刻位姿之间的关系构成边(Edge),通过不断累积而成的顶点和边构成图(graph)结构,图优化的目标就是通过调整顶点的位姿最大可能的满足边(Edge)之间的约束。其中通过传感器累计信息构建图的过程在slam中称为前端,调整位姿满足约束的优化过程成为后端。
1、举例说明
举个例子以帮助理解:
如图所示,机器人起始位置在x0处,并观察到前方2m有一个路标l0,然后机器人向前移动,通过编码器检测到它向 前移动了1m,然后观测到路标l0在前方0.8m处,如何通过图优化计算出机器人位姿和路标位置的最优状态?
根据机器人的运动过程构建图如上图所示,边的约束关系如下:
即:
残差平方和:
求偏导数:
最后整理计算:
得到机器人位姿和路标位置:
在构建图的过程中,还需要考虑到不同传感器的精度差别,也就是我们对每个传感器的信任程度是不同,对于不同的信任程度我们赋予不同的权重,这就是SLAM中信息矩阵的概念。
2.理论推导
假设一个带有n条边的图,其目标函数可以写成:
说明:
1、其中e作为优化变量xk与zk的符合程度的度量,越大表示两者越不符。用e的平方形式来表示目标函数:
为了表示我们对误差各分量重视程度的不一样,还使用一个信息矩阵 Ω 来表示各分量的不一致性。
2、信息矩阵 Ω 是协方差矩阵的逆,是一个对称矩阵。它的每个元素Ωi,j作为eiej的系数,可以看成我们对ei,ej这个误差项相关性的一个预计。最简单的是把Ω设成对角矩阵,对角阵元素的大小表明我们对此项误差的重视程度。
3、这里的xk可以指一个顶点、两个顶点或多个顶点,取决于边的实际类型。所以,更严谨的方式是把它写成ek(zk,xk1,xk2,…),但是那样写法实在是太繁琐,我们就简单地写成现在的样子。由于zk是已知的,为了数学上的简洁,我们再把它写成ek(xk)的形式。
于是总体优化问题变为n条边加和的形式:
现在,我们有了一个很多个节点和边的图,构成了一个庞大的优化问题。我们并不想展开它的数学形式,只关心它的优化解。那么,为了求解优化,需要知道两样东西:一个初始点和一个迭代方向。为了数学上的方便,先考虑第k条边ek(xk)吧。
我们假设它的初始点为xk,并且给它一个Δx的增量,那么边的估计值就变为Fk(xk+Δx),而误差值则从 ek(x) 变为 ek(xk+Δx)。首先对误差项进行一阶展开:
这是的Jk是ek关于xk的导数,矩阵形式下为雅可比阵。我们在估计点附近作了一次线性假设,认为函数值是能够用一阶导数来逼近的,当然这在Δx很大时候就不成立了。
于是,对于第k条边的目标函数项,有:
进一步展开:
最后一个式子是个定义整理式,我们把和Δx无关的整理成常数项 Ck ,把一次项系数写成 2bk ,二次项则为 Hk(注意到二次项系数其实是Hessian矩阵)。
请注意 Ck 实际就是该边变化前的取值。所以在xk发生增量后,目标函数Fk项改变的值即为
我们的目标是找到Δx,使得这个增量变为极小值。所以直接令它对于Δx的导数为零,有:
所以归根结底,我们求解一个线性方程组:
如果把所有边放到一起考虑进去,那就可以去掉下标,直接说我们要求解
原来算了半天它只是个线性的!线性的谁不会解啊!
读者当然会有这种感觉,因为线性规划是规划中最为简单的,连小学生都会解这么简单的问题,为何21世纪前SLAM不这样做呢?——这是因为在每一步迭代中,我们都要求解一个雅可比和一个海塞。而一个图中经常有成千上万条边,几十万个待估计参数,这在以前被认为是无法实时求解的。
那为何后来又可以实时求解了呢?
SLAM研究者逐渐认识到,SLAM构建的图,并非是全连通图,它往往是很稀疏的。例如一个地图里大部分路标点,只会在很少的时刻被机器人看见,从而建立起一些边。大多数时候它们是看不见的。体现在数学公式中,虽然总体目标函数F(x)有很多项,但某个顶点xk就只会出现在和它有关的边里面!
这会导致什么?这导致许多和xk无关的边,比如说ej,对应的雅可比Jj就直接是个零矩阵!而总体的雅可比J中,和xk有关的那一列大部分为零,只有少数的地方,也就是和xk顶点相连的边,出现了非零值。
相应的二阶导矩阵H中,大部分也是零元素。这种稀疏性能很好地帮助我们快速求解上面的线性方程。稀疏代数库包括SBA、PCG、CSparse、Cholmod等等。g2o正是使用它们来求解图优化问题的。
3. 图优化的流程
最后总结一下做图优化的流程。
1、选择你想要的图里的节点与边的类型,确定它们的参数化形式;
2、往图里加入实际的节点和边;
3、选择初值,开始迭代;
4、每一步迭代中,计算对应于当前估计值的雅可比矩阵和海塞矩阵;
5、求解稀疏线性方程HkΔx=bk,得到梯度方向;
6、继续用GN或LM进行迭代。如果迭代结束,返回优化值。
参考资料:
https://www.cnblogs.com/gaoxiang12/p/5244828.html
http://blog.****.net/heyijia0327/article/details/47686523
上一篇: 图优化学习笔记
下一篇: 彻底领悟图优化与G2O之图优化详解
推荐阅读
-
数学里的图论优化原理简介
-
F#探险之旅(二):函数式编程(上)-函数式编程范式简介 F#主要支持三种编程范式:函数式编程(Functional Programming,FP)、命令式编程(Imperative Programming)和面向对象(Object-Oriented,OO)的编程。回顾它们的历史,FP是最早的一种范式,第一种FP语言是IPL,产生于1955年,大约在Fortran一年之前。第二种FP语言是Lisp,产生于1958,早于Cobol一年。Fortan和Cobol都是命令式编程语言,它们在科学和商业领域的迅速成功使得命令式编程在30多年的时间里独领风骚。而产生于1970年代的面向对象编程则不断成熟,至今已是最流行的编程范式。有道是“*代有语言出,各领风骚数十年”。 尽管强大的FP语言(SML,Ocaml,Haskell及Clean等)和类FP语言(APL和Lisp是现实世界中最成功的两个)在1950年代就不断发展,FP仍停留在学院派的“象牙塔”里;而命令式编程和面向对象编程则分别凭着在商业领域和企业级应用的需要占据领先。今天,FP的潜力终被认识——它是用来解决更复杂的问题的(当然更简单的问题也不在话下)。 纯粹的FP将程序看作是接受参数并返回值的函数的集合,它不允许有副作用(side effect,即改变了状态),使用递归而不是循环进行迭代。FP中的函数很像数学中的函数,它们都不改变程序的状态。举个简单的例子,一旦将一个值赋给一个标识符,它就不会改变了,函数不改变参数的值,返回值是全新的值。 FP的数学基础使得它很是优雅,FP的程序看起来往往简洁、漂亮。但它无状态和递归的天性使得它在处理很多通用的编程任务时没有其它的编程范式来得方便。但对F#来说这不是问题,它的优势之一就是融合了多种编程范式,允许开发人员按照需要采用最好的范式。 关于FP的更多内容建议阅读一下这篇文章:Why Functional Programming Matters(中文版)。F#中的函数式编程 从现在开始,我将对F#中FP相关的主要语言结构逐一进行介绍。标识符(Identifier) 在F#中,我们通过标识符给值(value)取名字,这样就可以在后面的程序中引用它。通过关键字let定义标识符,如: let x = 42 这看起来像命令式编程语言中的赋值语句,两者有着关键的不同。在纯粹的FP中,一旦值赋给了标识符就不能改变了,这也是把它称为标识符而非变量(variable)的原因。另外,在某些条件下,我们可以重定义标识符;在F#的命令式编程范式下,在某些条件下标识符的值是可以修改的。 标识符也可用于引用函数,在F#中函数本质上也是值。也就是说,F#中没有真正的函数名和参数名的概念,它们都是标识符。定义函数的方式与定义值是类似的,只是会有额外的标识符表示参数: let add x y = x + y 这里共有三个标识符,add表示函数名,x和y表示它的参数。关键字和保留字关键字是指语言中一些标记,它们被编译器保留作特殊之用。在F#中,不能用作标识符或类型的名称(后面会讨论“定义类型”)。它们是: abstract and as asr assert begin class default delegate do donedowncast downto elif else end exception extern false finally forfun function if in inherit inline interface internal land lazy letlor lsr lxor match member mod module mutable namespace new nullof open or override private public rec return sig static structthen to true try type upcast use val void when while with yield 保留字是指当前还不是关键字,但被F#保留做将来之用。可以用它们来定义标识符或类型名称,但编译器会报告一个警告。如果你在意程序与未来版本编译器的兼容性,最好不要使用。它们是: atomic break checked component const constraint constructor continue eager event external fixed functor global include method mixinobject parallel process protected pure sealed trait virtual volatile 文字值(Literals) 文字值表示常数值,在构建计算代码块时很有用,F#提供了丰富的文字值集。与C#类似,这些文字值包括了常见的字符串、字符、布尔值、整型数、浮点数等,在此不再赘述,详细信息请查看F#手册。 与C#一样,F#中的字符串常量表示也有两种方式。一是常规字符串(regular string),其中可包含转义字符;二是逐字字符串(verbatim string),其中的(")被看作是常规的字符,而两个双引号作为双引号的转义表示。下面这个简单的例子演示了常见的文字常量表示: let message = "Hello World"r"n!" // 常规字符串let dir = @"C:"FS"FP" // 逐字字符串let bytes = "bytes"B // byte 数组let xA = 0xFFy // sbyte, 16进制表示let xB = 0o777un // unsigned native-sized integer,8进制表示let print x = printfn "%A" xlet main = print message; print dir; print bytes; print xA; print xB; main Printf函数通过F#的反射机制和.NET的ToString方法来解析“%A”模式,适用于任何类型的值,也可以通过F#中的print_any和print_to_string函数来完成类似的功能。值和函数(Values and Functions) 在F#中函数也是值,F#处理它们的语法也是类似的。 let n = 10let add a b = a + blet addFour = add 4let result = addFour n printfn "result = %i" result 可以看到定义值n和函数add的语法很类似,只不过add还有两个参数。对于add来说a + b的值自动作为其返回值,也就是说在F#中我们不需要显式地为函数定义返回值。对于函数addFour来说,它定义在add的基础上,它只向add传递了一个参数,这样对于不同的参数addFour将返回不同的值。考虑数学中的函数概念,F(x, y) = x + y,G(y) = F(4, y),实际上G(y) = 4 + y,G也是一个函数,它接收一个参数,这个地方是不是很类似?这种只向函数传递部分参数的特性称为函数的柯里化(curried function)。 当然对某些函数来说,传递部分参数是无意义的,此时需要强制提供所有参数,可是将参数括起来,将它们转换为元组(tuple)。下面的例子将不能编译通过: let sub(a, b) = a - blet subFour = sub 4 必须为sub提供两个参数,如sub(4, 5),这样就很像C#中的方法调用了。 对于这两种方式来说,前者具有更高的灵活性,一般可优先考虑。 如果函数的计算过程中需要定义一些中间值,我们应当将这些行进行缩进: let halfWay a b = let dif = b - a let mid = dif / 2 mid + a 需要注意的是,缩进时要用空格而不是Tab,如果你不想每次都按几次空格键,可以在VS中设置,将Tab字符自动转换为空格;虽然缩进的字符数没有限制,但一般建议用4个空格。而且此时一定要用在文件开头添加#light指令。作用域(Scope)作用域是编程语言中的一个重要的概念,它表示在何处可以访问(使用)一个标识符或类型。所有标识符,不管是函数还是值,其作用域都从其声明处开始,结束自其所处的代码块。对于一个处于最顶层的标识符而言,一旦为其赋值,它的值就不能修改或重定义了。标识符在定义之后才能使用,这意味着在定义过程中不能使用自身的值。 let defineMessage = let message = "Help me" print_endline message // error 对于在函数内部定义的标识符,一般而言,它们的作用域会到函数的结束处。 但可使用let关键字重定义它们,有时这会很有用,对于某些函数来说,计算过程涉及多个中间值,因为值是不可修改的,所以我们就需要定义多个标识符,这就要求我们去维护这些标识符的名称,其实是没必要的,这时可以使用重定义标识符。但这并不同于可以修改标识符的值。你甚至可以修改标识符的类型,但F#仍能确保类型安全。所谓类型安全,其基本意义是F#会避免对值的错误操作,比如我们不能像对待字符串那样对待整数。这个跟C#也是类似的。 let changeType = let x = 1 let x = "change me" let x = x + 1 print_string x 在本例的函数中,第一行和第二行都没问题,第三行就有问题了,在重定义x的时候,赋给它的值是x + 1,而x是字符串,与1相加在F#中是非法的。 另外,如果在嵌套函数中重定义标识符就更有趣了。 let printMessages = let message = "fun value" printfn "%s" message; let innerFun = let message = "inner fun value" printfn "%s" message innerFun printfn "%s" message printMessages 打印结果: fun value inner fun valuefun value 最后一次不是inner fun value,因为在innerFun仅仅将值重新绑定而不是赋值,其有效范围仅仅在innerFun内部。递归(Recursion)递归是编程中的一个极为重要的概念,它表示函数通过自身进行定义,亦即在定义处调用自身。在FP中常用于表达命令式编程的循环。很多人认为使用递归表示的算法要比循环更易理解。 使用rec关键字进行递归函数的定义。看下面的计算阶乘的函数: let rec factorial x = match x with | x when x < 0 -> failwith "value must be greater than or equal to 0" | 0 -> 1 | x -> x * factorial(x - 1) 这里使用了模式匹配(F#的一个很棒的特性),其C#版本为: public static long Factorial(int n) { if (n < 0) { throw new ArgumentOutOfRangeException("value must be greater than or equal to 0"); } if (n == 0) { return 1; } return n * Factorial (n - 1); } 递归在解决阶乘、Fibonacci数列这样的问题时尤为适合。但使用的时候要当心,可能会写出不能终止的递归。匿名函数(Anonymous Function) 定义函数的时候F#提供了第二种方式:使用关键字fun。有时我们没必要给函数起名,这种函数就是所谓的匿名函数,有时称为lambda函数,这也是C#3.0的一个新特性。比如有的函数仅仅作为一个参数传给另一个函数,通常就不需要起名。在后面的“列表”一节中你会看到这样的例子。除了fun,我们还可以使用function关键字定义匿名函数,它们的区别在于后者可以使用模式匹配(本文后面将做介绍)特性。看下面的例子: let x = (fun x y -> x + y) 1 2let x1 = (function x -> function y -> x + y) 1 2let x2 = (function (x, y) -> x + y) (1, 2) 我们可优先考虑fun,因为它更为紧凑,在F#类库中你能看到很多这样的例子。 注意:本文中的代码均在F# 1.9.4.17版本下编写,在F# CTP 1.9.6.0版本下可能不能通过编译。 F#系列随笔索引页面