图优化在slam中的应用——柏拉图启示录

南山学长在纸上写下了一个公式:

e=z-h(x)

小K:这是什么?
南山学长:这是一个蕴含了宇宙的终极奥秘的公式。
小K:怎么可能?太离谱了,这只是一个普通的公式!
南山学长:那好你用尺子测量一下纸上的这条直线有多长。
小K(一脸疑惑的):3.55cm
南山学长:你何以如此确信这条直线就是3.55cm,我是说它可能介于3.55和3.56之间,如果你再把图像放大,也许就能精确到小数点后三位。
小K(想了想):恩,是这样的,放的越大得到的数据就越精确。
南山学长:那么问题来了,这条直线必然存在一个精确的长度,而且可以无限精确下去。这个值才是……
小K(惊呆了):被观测物体的真值!!!
南山学长:没错,它就是被观测物体的真值。它是如此的完美和精确,存在于柏拉图理想世界中,在这个世界中所有的物体都是绝对精确的。
小K:但是我们在一个现实世界中却得到了这个真值的观测值,这是为什么?
南山学长:因为观测!!!观测桥接了这两个世界,我们甚至可以建立一个观测模型。
小K:观测模型?哪儿有?
南山学长:我忘了告诉你,这把尺子是不精确的,它总是少量了0.01cm
小K(恍然大悟):哦,我明白了。观测模型其实就是一个函数,比如说h(x),x是被观测物体的真值,而我们通过观测模型观测到的却是h(x)。在直线这个例子中观测模型就是h(x)=x-0.01,我们已经有了观测值z=3.55,如果z=h(x),那么真值就是3.56了!嗯,好像也不对啊
南山学长:你说的没错,3.56绝对不是真值!你我皆是凡人,怎能如此轻易的就企及了神的高度?所以我们离真值还是有距离的,假设为e,那么就有了e=z-h(x)这个公式。
小K:但这并没有什么用啊?我们又不能用这个公式求出真值x。
南山学长:我们的却不能,但是我们可以求出一个最优的估计x*,一个使e最小的x值。
小K:函数e(x)的最小值,对,我们的却能求出一个最优的估计,只需要使用高等数学的知识就可以。
南山学长:观测值与观测对象之间存在某种约束,每一次观测都形成一个约束,通过e=z-h(x)我们就能得到最接近真值的一个估值。好,看来你已经明白了,但是你的思维应该上升到哲学的高度,给你做个小练习吧。

小K:这不就是个最小二乘吗?你也太小看我了?
南山学长:我的问题是,如果用边表示观测和观测值,用点表示观测对象,那么在这个问题中应该怎样绘图?
小K:这哪儿跟那?哪里有观测?
南山学长:你得看到事物的本质!给你个提示吧,我们其实观测的是一条直线,用数学的语言来说就是两个参数k和b
小K(幡然醒悟):我们的观测对象是k和b,观测模型是h=kx+b,观测值是10个坐标点,那么这个图可以表示成:

南山学长:嗯,看来你已经明白了,我们这个世界是粗糙的,所以观测无处不在,观测对象也是广泛而抽象的。
小K(感觉像是打开了新世界的大门):我们的每次观测也可以是对多个对象进行的,我们也可能对观测对象集合做了多次观测,那么就存在下面这种奇怪的边:

  哦,不。这已经完全超出了我所学的图论的内容,我的世界观快崩溃了。
南山学长:是时候更新你的世界观了,这种奇怪的边叫超边(Hyper edge),含有这种边的图叫超图(Hyper Graph)。我们的对这类问题的表示方法叫图优化,简单的说就是在已知观测模型的情况下,根据一系列观测数据来估计观测对象集合的全局最优估计。
小K:这真是极好啊!这样一来,好多问题都可以转化为图优化问题了!比如slam问题:

  方框是机器人位姿,圆圈是路标位姿,都是位姿,我们都不知道。但是我们知道运动模型和观测模型,并且传感器能直接给出观测值u和z,通过图优化我们就能估计机器人和路标最优的位姿,我猜这大概就是传说中的同时定位与制图。
南山学长:嗯,很好,你领悟的很透彻。其实vo也是个图优化问题,只不过在计算机视觉中叫bundle adjustment:

  对于三维空间中的一堆点,我们观测到了两个视角下的平面二维坐标,并且已知观测模型:

  z1,z2表示两个视角下的观测值,X表示三维空间点坐标,R和t描述相机运动,最终优化函数如下:

小K:我明白了,z1是对X的观测,z2是对X,R,t的观测,这可以表述成超图:

  哈哈,实在是太神奇了,感觉像突然打通了任督二脉。又仿佛穿越了迷雾,直达事物的本质。

南山学长:上面只是问题的表示与建模,优化估值是如何算出来的你还不知道,所以你还有很长的路要走,还有很多的东西要学。

时间: 2024-11-15 20:05:28

图优化在slam中的应用——柏拉图启示录的相关文章

视觉SLAM漫淡(二):图优化理论与g2o的使用

视觉SLAM漫谈(二):图优化理论与g2o的使用 1    前言以及回顾 各位朋友,自从上一篇<视觉SLAM漫谈>写成以来已经有一段时间了.我收到几位热心读者的邮件.有的希望我介绍一下当前视觉SLAM程序的实用程度,更多的人希望了解一下前文提到的g2o优化库.因此我另写一篇小文章来专门介绍这个新玩意. 在开始本篇文章正文以前,我们先来回顾一下图优化SLAM问题的提法.至于SLAM更基础的内容,例如SLAM是什么东西等等,请参见上一篇文章.我们直接进入较深层次的讨论.首先,关于我们要做的事情,你

深入理解图优化与g2o:图优化篇

前言 本节我们将深入介绍视觉slam中的主流优化方法——图优化(graph-based optimization).下一节中,介绍一下非常流行的图优化库:g2o. 关于g2o,我13年写过一个文档,然而随着自己理解的加深,越发感觉不满意.本着对读者更负责任的精神,本文给大家重新讲一遍图优化和g2o.除了这篇文档,读者还可以找到一篇关于图优化的博客: http://blog.csdn.net/heyijia0327 那篇文章有作者介绍的一个简单案例,而本文则更注重对图优化和g2o的理解与评注. 本

视觉SLAM中的数学基础 第三篇 李群与李代数

视觉SLAM中的数学基础 第三篇 李群与李代数 前言 在SLAM中,除了表达3D旋转与位移之外,我们还要对它们进行估计,因为SLAM整个过程就是在不断地估计机器人的位姿与地图.为了做这件事,需要对变换矩阵进行插值.求导.迭代等操作.例如,在经典ICP问题中,给定了两组3D点,我们要计算它们之间的变换矩阵.假设第一组的3D点为$\mathbf{P}=\{ \mathbf{p}_i | i = [1,2, \ldots, N] \}$,第二组3D点为$\mathbf{Q}=\{ \mathbf{q}

SLAM中的非线性优化

总结一下SLAM中关于非线性优化的知识. 先列出参考: http://jacoxu.com/jacobian%E7%9F%A9%E9%98%B5%E5%92%8Chessian%E7%9F%A9%E9%98%B5/ http://blog.csdn.net/dsbatigol/article/details/12448627 http://www.cnblogs.com/rongyilin/archive/2012/12/21/2827898.html <视觉SLAM十四讲>. 1. 雅克比矩

SLAM中的卡方分布

视觉slam中相邻帧特征点匹配时,动辄上千个特征点,匹配错误的是难免的,而误匹配势必会对位姿精度以及建图精度造成影响,那么如何分辨哪些是误匹配的点对儿呢?如果已知两帧的的单应矩阵,假设单应矩阵是没有误差的,那么两帧中匹配正确的特征点通过单应矩阵是重投影是不应该有误差的或者误差十分小,而误匹配的特征点的重投影误差一定十分显著.那么我们是不是可以设置一个误差门限,从而甄别出这些误匹配点?可是这个误差门限该设置为多少? 假设图像金字塔第n层中一个特征点\(\mathbf{p_c}=\begin{bma

视觉SLAM中李群李代数与四元数总结

在SLAM中经常会用到李群李代数与四元数来表示旋转变换,这些数学公式往往需要推导来推导去,分分钟搞到头都大了.但在SLAM中往往用到其中那么几个固定的性质,所以是没有必要对这些数学基础作过多深入的研究,只需要记住其中一些常用的公式及性质即可.因此,本人在这里对这些数学基础作一个简单的总结,以便日后在工程中使用. 旋转的表示方式 SLAM中,往往会使用三种方式来表达空间中的旋转变换,分别为:旋转向量.旋转矩阵.四元数. 旋转向量 旋转向量可以非常直观地表示空间中的一个旋转变换:假设空间中有一单位向

根据自己的需要,把别人开发好的东西搬过来,优化and重构,在优化的过程中,甚至也会弄出一套全新的东西(转)

赵海平在今年三月份来到阿里,听毕玄(他现任主管)说去年五六月份就跟赵海平聊上了.有人问:为啥 BAT 三大巨头,你看中了阿里巴巴?在今天现场达一千多人的分享中赵海平给出了回复:“因为百度和腾讯没找我呗~”,他笑道,“百度以搜索为核心,优化了很多年了,估计也没啥可以优化的了:而腾讯除了 QQ 和微信,也没什么大型应用(别跟人家说哦)”.这不是原话哈,赵海平还是相当谦虚并且能言的,思维很开阔,两个小时的分享内容丰富,时不时还插两个故事,起初进场的手机和电脑都很自觉的收起来了~旁边的同事侃道:“高 P

MyBatis学习总结(三)——优化MyBatis配置文件中的配置(转载)

孤傲苍狼 只为成功找方法,不为失败找借口! MyBatis学习总结(三)--优化MyBatis配置文件中的配置 一.连接数据库的配置单独放在一个properties文件中 之前,我们是直接将数据库的连接配置信息写在了MyBatis的conf.xml文件中,如下: 1 <?xml version="1.0" encoding="UTF-8"?> 2 <!DOCTYPE configuration PUBLIC "-//mybatis.org

视觉SLAM中的数学基础 第二篇 四元数

视觉SLAM中的数学基础 第二篇 四元数 什么是四元数 相比欧拉角,四元数(Quaternion)则是一种紧凑.易于迭代.又不会出现奇异值的表示方法.它在程序中广为使用,例如ROS和几个著名的SLAM公开数据集.g2o等程序都使用四元数记录机器人的姿态.因此,理解四元数的含义与用法,对学习SLAM来说是必须的.本节我们就来讲讲四元数. 首先,请读者不要对四元数有什么神秘的感觉.四元数仅是3D姿态的一种表达方式,我们用一个单位四元数表达原本用旋转矩阵表示的三维旋转.这样做一个直接的好处是省空间.一