Cartographer无限期学习体会

slam工程师

已于 2022-07-27 17:59:47 修改

阅读量829

点赞数

文章标签：自动驾驶人工智能

于 2022-07-22 23:01:59 首次发布

本文链接：https://blog.csdn.net/qq_26675881/article/details/125941434

版权

一. Cartographer框架:

基本思路就是一点: 以前的做法scan都是和全局地图匹配, 但是全局地图时间长了有误差, 谷歌这些兄弟就想着和子图匹配, 减小误差. 并通过分枝定界减少了回环检测时对相对位姿估计的计算量. 实际效果也是大大的好. 2D和3D思路都是一样的, 只是数据格式不一样. 暂时只仔细分析2D情况.

(Local Slam)构建submap:
1. 每2*num_range_data个连续的scans会构建一个子图, (num_range_data个在初始化时插入, num_range_data个在匹配时插入). 代码的插入方法: 通过ceres优化scan的世界坐标, 然后就可以换算到submap中(max与点云坐标差/resolution). 注意scan原点的局部坐标(x,y)是可以根据里程计算的, 这样可以给ceres优化一个初始值. 在优化过程中再额外估计旋转角.
2. 活跃的子图有两个, 第一个用于匹配新来的scan, 第二个用于下一个子图的初始化. 新来的scan都会插入到两个子图. 注意每次新建的子图的中心都是当时scan原点的local坐标, 然后以一定cells扩展.
3. 子图的构建利用的是短时间内的帧之间的匹配, 所以误差比较小, 子图nice.
(Global Slam)修正子图的位置:
1. 上述构建的submap之间由于时间间隔会比较长, 是有误差的, 影响对全局地图的恢复, 我们需要后端优化.
2. 当一个submap的构建完成时(scan数满了)，该submap就会加入到之后的闭环检测中. 闭环检测会考虑所有的已完成创建的submap. 当一个新的laser scan加入到地图中时，如果该laser scan的估计位姿与地图中某个submap的某个laser scan的位姿比较接近的话，说明laser回到了之前经过的位置附近, 产生回环, 准备进行子图位姿的修正. 首先使用分枝定界法(BBS)在离散格点上快速估计scan和子图的相对位姿(submap中心与scan原点的平移, 以及scan在短时间内旋转了多少度, 都可以有一个预估值, 然后在 $(dx, dy, d\theta)$ 附近打格点快速搜索. 由于大多结点(格点)的score都会低于阈值, 很多解都被pass掉, 这极大减少了计算量.
3. 上述BBS匹配的相对位姿, 作为约束去优化回环检测中的子图以及scans位姿. 优化采用稀疏位姿调整法(SPA). 不同的子图在优化后, 以最匹配的全局位姿"粘在"在一起.
其他框架介绍reference: 泡泡机器人原创专栏-Cartographer】Cartographer理论及实现浅析

making......

二. 特殊变量, 文件等:

(一)lua:

详细学习可见: Lua 教程 | 菜鸟教程.

Lua 是一种轻量小巧的脚本语言，用标准C语言编写并以源代码形式开放. 非常容易使用由宿主语言(通常是C或C++)提供的扩展接口和机制. 它自动进行内存管理, 只提供了一种通用类型的表（table），用它可以实现数组，哈希表，集合，对象.

动态性: 譬如之前这个变量存int, 下一时刻我可以放一个string进去存, 非常灵活. 在很多大型程序中, 都会考虑把易变的部分用lua编写, 稳定性更高.

在cartographer里面编成了一个类: LuaParameterDictionary

(二)proto:

类似与jason, xml, 是一种用于数据交换的文件格式，并且在效率和兼容性上都非常的好.

用格式message [name] {} 定义, 形式上像结构(struct). 允许嵌套proto, 只要在头部import相应proto文件即可.

name.set_xxx(yyy): 把xxx设置为yyy
name.has_xxx(): 判定有无xxx
name.xxx(): 返回属性xxx
name.mutable_xxx()": 返回属性xxx的引用, 没有则用new创建一个.

making......

三. 代码阅读理解:

(一)注意点:

ros的地图坐标系局部(全局)坐标系子图的像素坐标系

^ y ^ x 0------> x

| | |

0 -----> x y <------ 0 y

局部坐标系: x轴向上, y轴向左. submap坐标系和局部坐标系之间有一个平移.
submap的像素坐标系, x轴向右, y轴向下. 根据子图左上角坐标和cell数, 可以定范围,以及局部坐标系中某个点在此图的像素坐标.
局部坐标系和全局坐标系都是世界地图坐标系, 只是有无进行SPA后端优化之分. local frame并不是指代robot坐标系.

(二)变量或代码文件:

RangeData结构: 结构类型, 属性包含雷达坐标原点, hits, misses. 三者皆为local坐标.
MapLimits类: 设置地图参数. 成员变量: (矩形)地图左上角的局部坐标, 分辨率, x,y方向的cell数. 局部坐标和cells即可确定地图范围.
Grid2D类(抽象基类): 定义栅格地图. 成员变量: MapLimits, 自由值(value)表, 自由度(correspondence cost)最大小值, 已经更新过的格点索引, 已知格点的坐标., 自由值(int)到自由度(float)的转化表. 新的点插入地图时, 如果超过了MapLimits, 会以原中心沿xy轴扩大一倍, 不够再扩大一倍, 直至包含该点.
ProbabilityGrid类: Grid2D的派生类. 更新概率值时, 调用ApplyLookupTable(cell_index, table), 这里的table代表correspondence旧值到新值的转换, 因为更新公式是固定的, 可以先用一个表计算好, 更新时直接查找即可.
Submap(抽象基类): 成员变量: submap中心的local坐标, 雷达数据插入个数(一个雷达数据有多个扫描点, 这个要看雷达的构造), 构建是否完成(bool) . 后二者默认为0, false.
Submap2D类: 二维情况的子图类, 派生于Submap. 新增成员: 栅格地图(指针)以及转换表. 每一次新建子图时, 子图的中心是当前scan的原点. 活跃的子图有两个(ActiveSubmaps2D类), 一个用于匹配, 另一个用于下一个子图的初始化, 二者之间有有50%的数据点重合.
CeresScanMatch2D: 用来优化一个位姿, 以进行laser数据到栅格的插入.
correlative_scan_matcher_2d.h: 这个文件为分枝定界做准备.
1. 函数GenerateRotatedScans: scan按不同旋转角旋转之后的点云scans $(\theta_0 + j * \delta_\theta)$
2. 函数DiscretizeScans: 把上述旋转的scans添加一个平移量.
fast_correlative_scan_matcher_2d.h: 整套分枝定界流程.
1. PrecomputationGrid2D类: 根据给定的原始grid, 指定宽度, 得到分辨率更大的栅格地图.
2. FastCorrelativeScanMatcher2D::MatchFullSubmap: 将当前scan和某一子图匹配, 返回成功与否. 若成功, 输出搜索得到的最优解, 最大得分. 将其作为约束然后进行spa.
RealTimeCorrelativeScanMatcher2D: 暴力遍历每个搜索格点, 没有进行分枝.