【记录读论文时遇到的一些算法2】—— Occupancy Grid Map
占据栅格地图(Occupancy Grid Map)
- 1.机器人地图的分类
- 2.占据栅格地图
- 3.TSDF(Truncated Signed Distance Function)
- 参考文章
1.机器人地图的分类
地图有很多种表示方式,例如,用经纬度标识地方的世界地图,城市的地铁图,校园指引图。
第一种我们称为尺度地图(Metric Map),每一个地点都可以用坐标来表示,比如北京在东经116°23′17’‘,北纬39°54′27’';第二种我们称为拓扑地图(Topological Map),每一个地点用一个点来表示,用边来连接相邻的点,即图论中的图(Graph),比如从地铁路线图中我们知道地铁红磡站与旺角东站和尖东站相连;第三种我们称为语义地图(Semantic Map),其中每一个地点和道路都会用标签的集合来表示,例如,有人问我中山大学教学楼E栋在哪里,我会说在图书馆正门右手边靠近图书馆的一侧。
在机器人领域,尺度地图常用于定位于地图构建(Mapping)、定位(Localization)和同时定位与地图构建(Simultaneous Localization And Mapping,SLAM),拓扑地图常用于路径规划(Path Planning),而语义地图常用于人机交互(Human Robot Interaction)。
这节课我们将介绍如何用机器人传感器数据绘制尺度地图。这有什么难点呢?首先也是最重要的一点,传感器数据有噪音。用激光传感器检测前方障碍物距离机器人多远,不可能检测到一个准确的数值。如果准确值是 2\sqrt 22 米,有时会测出1.42米,有时甚至1.35米。另外,传感器数据是本地坐标系的,而机器人要构建的是一个全局的地图。最后,机器人会运动,运动也是有噪音的。总结起来就两个字,噪音。通俗点来讲,“不准”。
2.占据栅格地图
我们首先来介绍机器人Mapping用到的的传感器,它叫做激光传感器(Laser Sensor),如下图所示:
激光传感器会向固定的方向发射激光束,发射出的激光遇到障碍物会被反射,这样就能得到激光从发射到收到的时间差,乘以速度除以二就得到了传感器到该方向上最近障碍物的距离。
这样看来,似乎利用激光传感器,机器人能够很好地完成Mapping这一任务。但是我们前面提到了,传感器数据是有噪音的。例如,假如我们在此时检测到距离障碍物4米,下一时刻检测到距离障碍物4.1米,我们是不是应该把4米和4.1米的地方都标记为障碍物?又或者怎么办呢?
为了解决这一问题,我们引入占据栅格地图(Occupancy Grid Map)的概念。
我们首先来解释这里的占据率 (Occupancy) 指的是什么。在通常的尺度地图中,对于一个点, 它要么有 (Occupied状态,下面用1来表示) 障碍物,要么没有 (Free状态,下面用0来表示) 障碍物 (旁白:那么问题来了,薛定谔状态呢? )。在占据栅格地图中,对于一个点,我们用 p(s=1)p(s=1)p(s=1) 来表示它是Free状态的概率,用 p(s=0)p(s=0)p(s=0) 来表示它是Occupied状态的概率,当然两者的和为 1 。两个值太多了,我们引入两者的比值来作为点的状态: Odd(s)=p(s=1)p(s=0)\operatorname{Odd}(s)=\frac{p(s=1)}{p(s=0)}Odd(s)=p(s=0)p(s=1) 。
对于一个点,新来了一个测量值 (Measurement, z∼{0,1}z \sim\{0,1\}z∼{0,1} ) 之后我们需要更新它的状态。 假设测量值来之前,该点的状态为 Odd(s)\operatorname{Odd}(s)Odd(s) ,我们要更新它为: Odd(s∣z)=p(s=1∣z)p(s=0∣z)\operatorname{Odd}(s \mid z)=\frac{p(s=1 \mid z)}{p(s=0 \mid z)}Odd(s∣z)=p(s=0∣z)p(s=1∣z) 。 这种表达方式类似于条件概率,表示在 zzz 发生的条件下 sss 的状态。
根据贝叶斯公式,我们有:
p(s=1∣z)=p(z∣s=1)p(s=1)p(z)p(s=0∣z)=p(z∣s=0)p(s=0)p(z)\begin{aligned} &p(s=1 \mid z)=\frac{p(z \mid s=1) p(s=1)}{p(z)} \\ &p(s=0 \mid z)=\frac{p(z \mid s=0) p(s=0)}{p(z)} \end{aligned} p(s=1∣z)=p(z)p(z∣s=1)p(s=1)p(s=0∣z)=p(z)p(z∣s=0)p(s=0)
带入之后,我们得
Odd(s∣z)=p(s=1∣z)p(s=0∣z)=p(z∣s=1)p(s=1)/p(z)p(z∣s=0)p(s=0)/p(z)=p(z∣s=1)p(z∣s=0)Odd(s)\begin{aligned} O d d(s \mid z) &=\frac{p(s=1 \mid z)}{p(s=0 \mid z)} \\ &=\frac{p(z \mid s=1) p(s=1) / p(z)}{p(z \mid s=0) p(s=0) / p(z)} \\ &=\frac{p(z \mid s=1)}{p(z \mid s=0)} O d d(s) \end{aligned} Odd(s∣z)=p(s=0∣z)p(s=1∣z)=p(z∣s=0)p(s=0)/p(z)p(z∣s=1)p(s=1)/p(z)=p(z∣s=0)p(z∣s=1)Odd(s)
我们对两边取对数得:
logOdd(s∣z)=logp(z∣s=1)p(z∣s=0)+logOdd(s)\log O d d(s \mid z)=\log \frac{p(z \mid s=1)}{p(z \mid s=0)}+\log O d d(s) logOdd(s∣z)=logp(z∣s=0)p(z∣s=1)+logOdd(s)
这样,含有测量值的项就只剩下了 logp(z∣s=1)p(z∣s=0)\log \frac{p(z \mid s=1)}{p(z \mid s=0)}logp(z∣s=0)p(z∣s=1) 。我们称这个比值为测量值的模型(Measurement Model),标记为 lomeas 。测量值的模型只有两种:
lofree=logp(z=0∣s=1)p(z=0∣s=0)lo_{free} =\log \frac{p(z=0 \mid s=1)}{p(z=0 \mid s=0)}lofree=logp(z=0∣s=0)p(z=0∣s=1) 和 looccu=logp(z=1∣s=1)p(z=1∣s=0)lo_{occu} =\log \frac{p(z=1 \mid s=1)}{p(z=1 \mid s=0)}looccu=logp(z=1∣s=0)p(z=1∣s=1) ,而且都是定值。
这样,如果我们用 logOdd(s)\log O d d(s)logOdd(s) 来表示位置 sss 的状态 SSS 的话,我们的更新规则就进一步简化成 了: S+=S−+lomeasS^{+}=S^{-}+lomeasS+=S−+lomeas。其中 S+S^{+}S+和 S−S^{-}S−分别表示测量值之后和之前 sss 的状态。
另外,在没有任何测量值的初始状态下,一个点的初始状态
Sinit=logOdd(s)=logp(s=1)p(s=0)=log0.50.5=0。 S_{i n i t}=\log O d d(s)=\log \frac{p(s=1)}{p(s=0)}=\log \frac{0.5}{0.5}=0 \text { 。 } Sinit=logOdd(s)=logp(s=0)p(s=1)=log0.50.5=0 。
经过这样的建模,更新一个点的状态就只需要做简单的加减法了。这,就是数学的魅力。
例如,假设我们设定 looccu=0.9lo_{occu}=0.9looccu=0.9 ,lofree=−0.7lo_{free}=-0.7lofree=−0.7 。那么,一个点状态的数值越大,就表 示越肯定它是Occupied状态,相反数值越小,就表示越肯定它是Free状态。
上图就展示了用两个激光传感器的数据更新地图的过程。在结果中,一个点颜色越深表示越肯定它是Free的,颜色越浅表示越肯定它是Occupied的。
3.TSDF(Truncated Signed Distance Function)
TSDF(Truncated Signed Distance Function)是实时3D重建经典算法,简单可并行,极大推动了实时三维重建的发展。TSDF是SDF的改进,讲取值限制在[-1,1]之间,同时仅在物体表面的限定的距离范围内进行计算,降低了计算量。
TSDF思路很朴素,用一个大的空间包围盒(volume)去包括进去待3D构建的场景,volume成多个voxel(小立体方块):
如何计算tsdf?假设已经从深度相机获得多帧RGBD图像以及相机的位姿T。
左图中灰色小方格表示体素voxel,蓝色三角性表示视场,绿色线为截面对应的线。
第一步:计算体素 xxx 在世界坐标系下的坐标。记体素 xxx 在TSDF地图上得坐标为 (vx,vy,vz)\left(v_{x}, v_{y}, v_{z}\right)(vx,vy,vz) -则该体素在世界坐标系下的位置为:
Px,world =(x0+vx⋅Voxel. x,y0+vy⋅Voxel. y,z0+vz⋅Voxel. z)P_{x, \text { world }}=\left(x_{0}+v_{x} \cdot \text { Voxel. } x, y_{0}+v_{y} \cdot \text { Voxel. } y, z_{0}+v_{z} \cdot \text { Voxel. } z\right) Px, world =(x0+vx⋅ Voxel. x,y0+vy⋅ Voxel. y,z0+vz⋅ Voxel. z)
第二步:计算体素在相机坐标系下的坐标。相机的旋转为 RRR 平移为 TTT ,则 Px,camera=RPx,world+TP_{x, c a m e r a}=R P_{x, w o r l d}+TPx,camera=RPx,world+T ;
第三步:根据相机内参 KKK 计算体素在相机坐标系下 zzz 向深度 camz(x)\operatorname{cam}_{z}(x)camz(x) :
第四步:重投影计算沿光心经过体素 xxx 到达物体表面 ppp 点对应的 zzz 向深度:因为RGBD的信息 已经知道,所以根据 Px,cameraP_{x, c a m e r a}Px,camera 可以直接查到 ppp 的深度值 depth(pic(x))\operatorname{depth}(\operatorname{pic}(x))depth(pic(x)) ;
第五步:得到sdf: sdfi(x)=depthi(pic(x))−camz(x)s d f_{i}(x)=\operatorname{depth}_{i}(p i c(x))-\operatorname{cam}_{z}(x)sdfi(x)=depthi(pic(x))−camz(x)
第六步:截断: tsdfi(x)=max(−1,min(1,sdfi(x)t))t s d f_{i}(x)=\max \left(-1, \min \left(1, \frac{s d f_{i}(x)}{t}\right)\right)tsdfi(x)=max(−1,min(1,tsdfi(x))) 即将sdf截断在 [−t,t][-\mathrm{t}, \mathrm{t}][−t,t] 内,sdf超 过这个范围的voxel点不用计算,直接得1或者-1。
第七步:将当前帧得到的值合并到已有的volume。通过将多帧合并成一个TSDF,既可以提升精度 又可以补全单帧缺失的信息。
TSDFi(x)=Wi−1(x)TSDFi−1(x)+wi(x)tsdfi(x)Wi−1(x)+wi(x)Wi(x)=Wi−1(x)+wi(x)\begin{aligned} &\operatorname{TSDFi}(x)=\frac{W_{i-1}(x) T S D F_{i-1}(x)+w_{i}(x) t s d f_{i}(x)}{W_{i-1}(x)+w_{i}(x)} \\ &W_{i}(x)=W_{i-1}(x)+w_{i}(x) \end{aligned} TSDFi(x)=Wi−1(x)+wi(x)Wi−1(x)TSDFi−1(x)+wi(x)tsdfi(x)Wi(x)=Wi−1(x)+wi(x)
对于需要更新的voxel wi(x)=1w_{i}(x)=1wi(x)=1 ,没在当前视场内的点为 0 。
最后: 重复遍历所有帧,得到合并后的TSDF。
参考文章
[1] https://zhuanlan.zhihu.com/p/21738718 占据栅格地图(Occupancy Grid Map)
[2] https://zhuanlan.zhihu.com/p/487277749 TSDF地图算法分析 实时三维重建
【记录读论文时遇到的一些算法2】—— Occupancy Grid Map相关推荐
- 【记录读论文时遇到的一些算法1】——MCMC sampling Gibbs sampling
MCMC sampling & Gibbs sampling 1. 什么是采样 2. 为什么要采样 3. 采样的作用 4. 常用的采样方法 5. 基本采样方法 5.1 马尔科夫链蒙特卡洛采样( ...
- 记录理解程度、一篇至少读3遍,吴恩达建议这样读论文!
↑↑↑关注后"星标"Datawhale 每日干货 & 每月组队学习,不错过 Datawhale推荐 作者:Richmond Alake,来源:机器之心 在科研领域中,读论文 ...
- 记录理解程度、一篇至少读3遍,吴恩达建议这样读论文
选自Medium 作者:Richmond Alake 机器之心编译 参与:杜伟.小舟.魔王 在科研领域中,读论文大概是大家的日常基本操作了.但如何读论文,才能实现效率最大化呢?斯坦福大学教授吴恩达在斯 ...
- 如何读论文?复现代码?
原因:一开始自己以为单纯的看看论文,把论文里面的代码调通,就是复现代码,今天就专门搜了一下,什么算复现代码. 解决:网友的意思大概是:那样顶多孙是调通了编译环境,与复现代码还有很长的路,需要自己懂每一 ...
- 读论文七步走!CV老司机万字长文:一篇论文需要读4遍
视学算法报道 编辑:LRS [新智元导读]读论文对于AI新手和工程师来说可能是一件比较难的事.最近一位从业超5年的CV老司机发布了一篇万字长文,讲述了读论文七步法,从找论文到总结,每篇论文由浅 ...
- 读论文七步走,CV资深博客专家长文:一篇论文需要读4遍
读论文对于AI新手和工程师来说可能是一件比较难的事.最近一位从业超5年的CV老司机发布了一篇万字长文,讲述了读论文七步法,从找论文到总结,每篇论文由浅入深都需要读4遍. 人工智能从业者需要读论文吗? ...
- 【How to read a paper】如何读论文
如何阅读科研论文 2013/09/11 | 分类: 职场分享 | 0 条评论 | 标签: 论文 分享到:11 本文由 伯乐在线 - 乾龙 翻译自 S. Keshav.转载请参见文章末尾处的要求. (感 ...
- 心法利器[55] | 算法工程师读论文思路
心法利器 本栏目主要和大家一起讨论近期自己学习的心得和体会,与大家一起成长.具体介绍:仓颉专项:飞机大炮我都会,利器心法我还有. 近期,我再次总结了我的历史文章,累积起来有50w字,百余篇文章了,有兴 ...
- 算法工程师读论文思路
读论文方面的经验,我应该不是第一次写了,之前有关研究生的经验里多少提过一些(我从研究生生活中得到的经验,心法利器[36] | 开学季:我给研究生的建议),但是我们毕业后,走向算法工程师的工位后,持续学 ...
最新文章
- 【Linux】ps命令
- ExceptionLess新玩法 — 记日志
- 高等组合学笔记(四): 生成函数应用, Catalan问题
- SpringBoot中关于Mybatis使用的三个问题
- JQuery学习记录——jQuery对象和DOM对象的转换
- 教新人如何压制AVI和RMVB(教程及下载)
- 利用MentoHUST在路由器上使用锐捷认证来共享校园网
- 布谷鸟算法的程序(个人注释)
- Halcon教程二:内置实例程序库
- forge下载java_我的世界forge1.9.4
- Android单元测试 - 如何开始?
- m725扫描仪无法找到计算机,惠普m725扫描设置指定的网络文件夹路径不存在?
- 在html编辑器中插入css,怎么给kindeditor编辑器添加引用(blockquote)标签,并选择css样式...
- C语言|求最小公倍数
- 外贸老鸟帮新人点评、修改的5个开发信案例
- python+matplotlib绘图线条类型、颜色、散点类型
- Desolate Era Book 1, Chapter 1
- erp系统有哪些功能模块?
- 《惢客创业日记》2021.08.06-09(周五)惢客与征信的区别(下)
- Swoole基础知识,安装,websocket应用及各种问题详解
热门文章
- js做个随机点名的小游戏
- 微信小程序微信原生小程序如何通过后端返回的二进制流导出excel文件并保存和转发
- 社群怎么建立,怎么做好社群营销?
- 华硕 ZenFone 国产新机盘点
- 绘画新手怎么把人物骨骼画好
- 计算机的网络功能划分为,计算机网络按功能来划分可分为( )子网和( )子网...
- 计算机组成原理 | 穿越功耗墙,我们该从哪些方面提升“性能”?
- 想学IT的必看!黑马培训javaee
- NIN:Network in Network
- 救世之树服务端开服架设服务器搭建教程