文献阅读笔记：Topomap: Topological Mapping and Navigation Based on Visual SLAM Maps

文献阅读笔记：Topomap: Topological Mapping and Navigation Based on Visual SLAM Maps

参考链接：
实验演示视频：https://www.youtube.com/watch?v=UokjxSLTcd0
作者讲解视频：https://www.youtube.com/watch?v=BJwxyuTFg-g

1 概述

这篇文献是为规划任务服务的导航算法，但是很多导航算法只能局部运行，并且对规划的目标没有概念性的理解。这限制了能够处理的任务的复杂性，并且难以处理实时机器人在应用过程中出现的不确定性。

1.1 主要思想

将基于稀疏特征的地图从视觉slam系统中转换为3D拓扑图。一共分为两步：（1）直接从有噪声的稀疏点云中提取占用信息；（2）创建一系列凸自由空间簇作为拓扑图的顶点。优点：提升了全局规划的效率，降低了计算效率和空间存储需求。
拓扑地图直接从视觉路标三角化的3D位置中获得，隐式地实现了定位和导航地图的紧耦合。

1.2 处理细节

（1） 环境划分为一组自由空间簇，对应于拓扑图的顶点；
（2） 加强了形状的凹凸性，机器人可以穿过区域不会遇到静态障碍；
（3） 邻接区域可以从一个顶点安全过渡到另一个点。

1.3 系统

输入稀疏的视觉slam地图，带有三角化的3D路标，路标的位置用多视图几何和VIO追踪估计，从路标中提取自由空间信息，用凸体元集群做顶点，邻接区域作为边（portals），顶点表示自由可遍历的区域。

1.4 拓扑图的理解

如下图，空间中的空闲区域是彩色的，但是被分成很多个模块，每个模块聚类得到一个中心点（vertex，拓扑图的顶点），然后通行区连接，相邻通行区的相邻边作为portals（下图中为了方便画导航图，取相邻边的中点）。

1.5 贡献点

（1） 利用一个稀疏的视觉slam地图创建一个可靠的自由空间表示和区域拓扑地图；
（2） 提出了以视觉地图作为输入，创建可用于全局规划的全局地图；
（3） 提出了一种基于体积占用网格的体素簇增长和合并算法，可以从有噪声和部分不完整的视觉slam数据中生成凸自由空间簇；
（4） 用具有不同拓扑特征的真实生活数据集对框架进行了广泛的评估。

1.6 构建拓扑地图的几种方法

（1） 在2D网格地图上将空闲区域划分为不相连的区域，然后构建拓扑地图；
（2） 基于相似度测量将地图分为有意义的关键帧或者路标聚类，但是共视聚类没有自由空间或者凸性聚类的概念
（3） 沿着metric slam地图在不同的地方附加局部占用网格。

2 本文方法

2.1 拓扑地图表达

将整个自由空间分为一系列凸区域，每个顶点关联一个封闭空间，和相邻顶点通过portals连接。凸区域用体素簇表示，体素簇可以直接从占用地图中获得，然后占用地图不用存储，而是把拓扑顶点序列化，也就是说最终存储的只有顶点序列、拓扑连接和portals。

2.2 从稀疏特征中提取占用信息

通过使用基于体素的截短符号距离字段TSDF【22】从稀疏路标中提取近似的自由空间信息。（TSDF多和深度传感器一起使用）：
先将深度测量融合到一个体积的TSDF网络中——类似于传统的传感器模式，通过光线追踪从传感器原点到测量的3D点。

2.3 生成紧凑的聚类——集群生长算法

基于二值化的TSDF环境重建生成紧凑的凸聚类。
在轨道上的任一个位置初始化第一个聚类中心体素，然后开始初始聚类的迭代增长，主要包含了三个步骤：
（1） 找当前体素簇中所有直接相邻的违背占用的体素簇；
（2） 用PCA找当前集群形状的主轴，这个主轴包含了大多数当前包含的体素（假设集群是椭球型）。只把短半轴+δ距离内的体素加入到集群中（紧凑候选）；
（3） 检查从所有候选体素到集群体素的射线是否无障碍确保当前簇的土包不包含任何障碍体素。
集群增长是一个一个进行的。

2.4 凸集群合并

合并区域的凸包包含较少的障碍体素，则合并。这一步实际上参考于【25】提出的动态区域合并算法。首先用绝对不含有障碍的保守集群提供预分割，然后再迭代寻找相邻的所有合并候选，采取随机的方式合并候选集群。注意每个集群中利用Quickhull算法【26】计算组合凸包。

2.5 拓扑导航

集群作为拓扑图的顶点，当两个集群相邻，则创建拓扑边。导航的时候，路径只能在集群内，找离起点和终点最近的portals，然后用A*算法搜索路径。

3 实验

三个实验：评估基于稀疏视觉3D路标点的TSDF积分的性能；集群增长算法性能验证；拓扑规划和RRT*对比。
数据来源：设备【27】算法VIO【28】全局BA用maplab闭环（开源匹配框架【29】）

4 结论

可以处理有噪声的稀疏视觉测量，减少了硬件需求。算法重点在于基于体素的增长与合并算法，从而将自由空间分成紧凑的聚类。所选择的拓扑图结构是生成的地图非常紧凑。
后续想把它集成到一个飞行平台上，浙江利用这个框架的全部3D能力；在拓扑图中包含语义信息，进一步推进机器人自主边界。