相关性扫描匹配CSM与分支限界加速
前言
在激光SLAM中,相关性扫描匹配可是重点中的重点,而分支限界加速更是大大提高了它的实时性。本文就来详细解读一下其中的奥秘。
激光SLAM前端
既然是SLAM,那么通常都会分为前端和后端。前端充当里程计的角色,比如我们熟知的VO(视觉里程计)和VIO(视觉惯性里程计)。激光SLAM也需要前端,对于每一帧点云,通常是使用所谓的扫描匹配(scan-match)方法进行位姿估计。
简单地说,扫描匹配就是想办法把两帧点云对齐,对齐过程中的旋转和平移就是两帧之间的相对位姿。这是最最简单的方式,实际中,为了提高鲁棒性,通常把当前帧点云和地图进行扫描匹配,更不容易出错。
扫描匹配的实现方式有两种,第一种是相关性扫描匹配,第二种是基于优化的扫描匹配。第一种思路简单,但有一些复杂的细节,而第二种则是构建非线性最小二乘优化问题并求解。实际中,通常是两种方法同时用,先用相关性扫描匹配求初值,再用基于优化的扫描匹配进一步优化。
相关性扫描匹配
相关性扫描匹配的思路其实非常简单。想象给定一个栅格地图,再给定当前帧的点云,怎样才能知道激光雷达所在的位置呢?最简单的办法,把激光雷达放在地图的每个格子上,看在这个位置时,点云是否与地图重合,重合程度最高的位置就是激光雷达的真实位姿。
显然,这个方法就是暴力搜索。虽然肯定搜得出来,但速度比较慢。想要加速,就得祭出我们的算法神器了——分支限界。
分支限界加速
如果上过计算机算法课,就一定接触过分支限界法。这是一个通用的算法,一般用于搜索离散空间的最优解。我们这里的解空间显然就是一个离散的空间,而且可以很方便地按照地图分辨率构造树形结构。
举例来说,假设有一张4×4的地图,我们降低其分辨率,每2×2的格子合并成一个,得到一张2×2的地图,再降低其分辨率,得到一张1×1的地图。这样,地图被分成了三层,如下图所示。
有了多分辨率地图后,我们先在第二层的解空间中搜索,找到最优的位姿对应的格子。然后再在该格子中搜索下一层中对应的小格子,找到最优的位姿。
这么简单?当然不是。请注意,上面的做法是完全错误的。它有两个问题。首先,在第二层的格子中搜索最优的位姿,实际上是把位姿放在格子的中心,计算其点云的评分(即hit点的评分之和,下文直接称其为位姿的评分)。但是格子中心的位姿并不能代表格子其它位置的位姿,很有可能格子中心的评分低于其它位置的评分,那么第二层最优的格子中并不一定包含第三层最优的格子。怎么办呢,我们可以想办法让格子中心的评分高于其所有子格子的评分,此时格子的评分是其子格子的评分上界,这样就可以保证子格子的最高评分体现在父格子中。
但是,此时又引入了第二个问题,虽然格子的评分是其子格子的评分上界,但并非上确界。也就是说,可能存在格子的评分很高,但其所有子格子的评分都很低的情况。这样的话,我们仍然无法选取最大评分格子的同时抛弃其它格子。
剩下的选择就只有一个,绝不抛弃任何一个可能存在最优解的格子,只抛弃那些绝对不存在最优解的格子。这正是分支限界的思想。
具体实现的时候,先从根节点开始遍历,把它拆分成4个子格子。这4个子格子分别计算评分,并由高到低排序。从中选出分数最高的格子,进一步拆分成4个子格子。假设这4个子格子已经到了叶子节点,也就是达到了真实地图的分辨率。此时计算出的4个子格子的评分代表了真实的位姿评分(只有叶子节点的评分是真实评分,其它格子的评分都是上界),找出其最大值,记作best_score。以上过程,我们体会了分支是如何实现的。接下来,该轮到限界出场了。第二层的格子还有3个未曾探索,我们当然是选择最大的那个开始分支。但别急,先看一下它的评分是否大于best_score,如果是,继续分支,如果否,就可以直接剪枝了,抛弃这个格子及其所有子格子。因为格子的评分代表了其子格子评分的上界,如果上界都小于best_score,就不可能再有子格子的评分大于它了。按照如此方法,大刀阔斧地剪枝即可。注意,当遇到评分大于best_score的叶子节点时,记得更新best_score,这样可以更快地缩小搜索空间。
讲到这里,你是不是有点摸不着头脑?别怕,因为这里有个最最关键的问题我还没有解释,也就是如何能够快速计算出格子评分的上界。
计算评分上界
先说一个平凡方法。既然要算子格子评分的上界,那就把每一个子格子拿出来算一遍,求个最大值。对了,子格子还会有子格子,所以这是个递归运算,直到把叶子节点都算出来才行。那这跟直接暴力搜索就没什么两样了,不行不行。
我们还是得追求在O(1)时间复杂度内求出格子的评分。想了想,之所以叶子节点评分可以在O(1)时间内求出,是因为它是直接在地图中查表得到的(严谨地说,是通过查其所有hit点的评分再求和得到的,但由于hit点数量固定,可以认为时间复杂度是O(1))。但我们构造的多分辨率地图能不能也维护类似的概率表格,使得格子的评分可以在对应分辨率地图中查询得到呢?
多么机智的想法啊!试想,把地图中每个格子的概率用其附近区域内的最大概率代替。这样的话,直接查对应分辨率地图就可以得到格子的评分,因为这个评分已经事先在附近区域中取了最大值。
上面提到的“某个范围”,需要与地图分辨率保持一致。也就是说,越低分辨率的地图(也就是越高层的地图),应该在越大的区域求最大值,其大小与当前分辨率下格子的大小一致。
不同分辨率的地图需要事先计算好,大概长这个样子:
可以看到,越靠后的地图分辨率越低,格子中的概率越接近1,也就代表了越高的上限。这与我们的直观理解是一致的。
总结
说实话,本文内容非常抽象,又限于笔者画图能力太差,实在画不出合适的插图,只能靠读者自己想象了。
不得不说,除非事先有所了解,否则读了之后可能还是一头雾水。所以,如果你对激光SLAM还没有整体的了解,还不知道什么是点云、什么是地图、如何计算概率。那么请关注一下文末的参考资料,相信会对你有所帮助。
相关性扫描匹配及分枝限界加速在谷歌Cartographer的论文中有详细介绍,建议大家去看一看。
参考资料
Real-Time Loop Closure in 2D LIDAR SLAM Wolfgang Hess, Damon Kohler, Holger Rapp, Daniel Andor
相关性扫描匹配CSM与分支限界加速相关推荐
- 相关性扫描匹配CSM与分支限界
一家之言,仅作分享,如有不合理或需要改进的地方,欢迎各位讨论. 前言 作者在先前激光SLAM项目中借鉴的是karto中的匹配方法相关性扫描匹配,在google的cartographer中的前端匹配方法 ...
- (02)Cartographer源码无死角解析-(48) 2D点云扫描匹配→扫描匹配基本原理讲解,代码总体框架梳理AddAccumulatedRangeData()
讲解关于slam一系列文章汇总链接:史上最全slam从零开始,针对于本栏目讲解(02)Cartographer源码无死角解析-链接如下: (02)Cartographer源码无死角解析- (00)目录 ...
- (02)Cartographer源码无死角解析-(50) 2D点云扫描匹配→相关性暴力匹配2:RealTimeCorrelativeScanMatcher2D
讲解关于slam一系列文章汇总链接:史上最全slam从零开始,针对于本栏目讲解(02)Cartographer源码无死角解析-链接如下: (02)Cartographer源码无死角解析- (00)目录 ...
- 激光扫描匹配之基于点的扫描匹配
本文主要将激光扫描匹配分为三类: (1)基于点的扫描匹配: (2)基于特征的扫描匹配: (3)基于数学特性的扫描匹配. 下面重要介绍基于点的扫描匹配 一,基于点的扫描匹配直接对扫描获取的原始数据点进行 ...
- (02)Cartographer源码无死角解析-(49) 2D点云扫描匹配→相关性暴力匹配1:SearchParameters
讲解关于slam一系列文章汇总链接:史上最全slam从零开始,针对于本栏目讲解(02)Cartographer源码无死角解析-链接如下: (02)Cartographer源码无死角解析- (00)目录 ...
- c# 联合halcon 基于相关性 模板匹配_宣城seo公司_有效利用及时相关性解决关键词排名上不到首页的难题...
发布时间:2020-11-04 09:11:33 当你写一条热门新闻时,百度总是能很快地收录和显示你.这就是即时新闻的魅力所在,考虑到全站的权重不算太多. 它完全基于即时新闻和页面相关性,因此,如果你 ...
- 【图像处理】——上采样、下采样、在模板匹配中的金字塔加速策略
目录 1.下采样 Python自带函数 自定义函数 Python实现下采样 2.上采样 Python自带函数 自定义函数 Python实现 3.金字塔加速策略 金字塔加速模板匹配具体步骤: 具体可参考 ...
- matlab按列扫描图像,Matlab实现图像扫描匹配
这是暑期的一门项目实践课,初次接触Matlab,把学习做的实验做个梳理和总结,方便日后回顾. 图像识别的应用广泛,如在军事领域中,轰炸机通过航拍的图像匹配目标,确定投弹的坐标. 实验目的 在figur ...
- c# 联合halcon 基于相关性 模板匹配_机器视觉之halcon入门(5)-字符识别exe生成...
2.3.2 第二个halcon程序转EXE程序:字符识别 老规矩,每一段halcon代码得用C#二次开发下.根据上一节所教的,我们配置下C#的环境,顺便添加好控件,如下图(2-3-2-1). 图 2- ...
最新文章
- Apache Commons Math3学习笔记(2) - 多项式曲线拟合(转)
- python的基础知识可以应用到哪方面-Python基础知识
- 宏锦软件2015年的计划
- Retrofit2源码分析(一)
- [导入]ASP.NET2.0中Tabs的简单实现
- linux服务器做301跳转,linux主机如何实现301重定向
- Android Studio 使用夜神模拟器查看apk打印日志
- 收款码三合一生成系统源码
- Linux禁用搜狗输入法的简繁切换快捷键
- 机械师笔记本电脑使用小常识
- 【Minecraft】建立Bukkit/Spigot插件实时调试环境,并避免断点调试时客户端断开连接
- Android 打开相册选择图片(史上最简单)
- php 汉字、字母验证码 例子
- MySQL深翻页、MySQL跳页
- Tensorflow-- 第一天使用过程中的报错records
- GPS经纬度转化为百度地图/Google坐标及互转方案
- CTO、技术VP、技术总监、首席架构师
- 分子动力学模拟Amber/Gromacs结合自由能计算 药效团模型构建RMSD、RMSF
- 2019年大厂面试题合集:Java架构师技术栈为什么竞争越来越激烈?程序员必看!
- WCF 第五章 行为 实现自定义行为
热门文章
- ARP协议详解(3)--arp欺骗
- Gartner云端盘点,浅谈2017IaaS魔力象限
- SpringCloud(八) 微服务安全实战 Prometheus配置grafana可见性监控,ELK日志,pinpoint追踪(Tracing,Metrics,Logging)
- uniApp APP端调起微信支付失败errCode:-100的踩坑
- 微信、网站、APP相关分享
- Docker原理及常见命令
- ajax 报错 Content-Type is not allowed by Access-Control-Allow-Headers in preflight respon
- 程序员的进化 - 在拉勾1024程序员节上的演讲
- 论文笔记 EMNLP 2020|Graph Transformer Networks with Syntactic and Semantic Structures for Event Argument
- 如何在几百万qps的网关服务中实现灵活调度策略