稠密建图及voxblox_voxblox 稠密建图

论文理解

基础简介

地图的表达方式中, 一种是occupancy网格地图, 其中比较常用的是八叉树地图octomap. 它使用分层的八叉树结构存储占据(occupancy)的概率大小. 然而很多情况下, 仅仅得到occupancy的概率大小是不够的. 很多路径规划方法要求直到到障碍物的距离. 这样的情况下可以使用ESDF(Euclidean Signed Distance Fields)方法. ESDF可以直接给出到障碍物的距离.

在稠密建图中, 一种常用的方式是利用tsdf进行表面重建. TSDF由于其数据结构的构建方式十分适用于GPU计算. 然而TSDF的主要缺陷是TSDF需要首先设置一个固定大小的voxel网格. 这种方式要求已知地图的大小, 并且需要比较大的内存开销.

voxblox给出了一种利用TSDF增量构建ESDF的方式, 利用grouped raycast方法, voxblox可以加速tsdf的merge过程. 以前的构建ESDF的主要步骤是
1. 构建一个完成的TSDF.
2. 将TSDF转化为occupancy网格.
3. 使用batch方法计算ESDF.
然而Voxblox给出了一种直接从TSDF到ESDF的方法.

ESDF以及TSDF都在在表面的附近(short tuncation radius aroud surface)计算. TSDF很适合于可视化, 很快的滤掉sensor的噪声. ESDF使用projective 距离(从传感器发出的射线方向, 射线指向表面). 如图所示为接种常用的建图方式中的数据结构.

主要流程

Voxblox的主要流程(如下图所示)
1. 将传感器数据(RGBD, 点云)转换为tsdf.
2. 根据TSDF使用 propagate的方式 更新ESDF.

构造TSDF

构造TSDF主要组成包括计算权值和merge. 将一个新的scan积分至TSDF时使用raycast, 计算从sensor原点到每个点的raycast, 在该射线方向上计算距离和权值. 权值的选择对于重建的精度的影响很大.
假设$x$为当前voxel的中心, $p$为点的三维位置. $s$为sensor的原点.那么更新的权值$W$和距离$D$为