稠密建图及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 为

Di+1(x,p,s)=Wi(x)Di(x)+w(x,p)d(x,p,s)Wi(x)+w(x,p)

Wi+1(x,p,s)=min(Wi(x)+w(x,p),Wmax)

这里

wconst(x,p)=1

d(x,p,s)=∥p−x∥sign((p−x)⋅(p−s))

这里可以看出距离 d 为tuncated距离在raycast方向的投影来确定符号. Voxblox计算上述wconst 修改为 wquad ,

wquad=⎧⎩⎨⎪⎪1z2−ϵ<d1z21δ−ϵ(d+δ)−δ<d<−ϵ0d<−ϵ

这里tuncation distance δ=4v 并且 v=ϵ . $v$表示voxel的大小.
这样减小了voxel的影响,

merge的主要过程是
1. 对于来自Scan的每个点, 将他投影到voxel grid.
2. 将属于同一个voxel的点group(计算平均的color和距离).
3. 一次性计算 raycast.

从TSDF构造ESDF

Voxblox的这种方法可以允许ESDF的地图动态改变, 使用存储在TSDF中的距离, 而不是从最近的occupied voxel中计算距离. 在原始代码中, 每个voxel有一个占据或者free的状态, 不能改变. voxblox将这个概念改变为around surface fixed band. ESDF的voxel从co-located voxel提取值. fixed band的值由ESDF的voxel定义. 一般的算法基于wavefronted的idea, 也就是从一个start voxel传播到他的近邻26-connectivity. 然后更新他们的距离. 把更新后的voxel放入wavefront队列.

wavefront有2中情况, raise和lower
1. raise. 一个voxel加入raise队列, 当一个来及ESDF新的距离>原始值.
2. lower. 一个voxel加入lower队列, 当一个来及ESDF新的距离<原始值.

代码理解

在代码中的主要使用方法是这样的

初始化

1. 使用对用的config类构成一些设置.
2. 分别构造map 和integrator比如 tsdf map和tsdf integrator.
3. 使用配置数据配置对应integrator.

主循环

1. update
2. integrate cloud

保存地图

1. generate mesh
2. voxblox::outputMeshLayerAsPly
3. 使用meshlab显示ply