参考与前言

Summary: 浩哥推荐的一篇 无人机下的建图 and planning实验
Type: ICRA
Year: 2021

论文链接：https://arxiv.org/abs/2105.04419

youtube presentation video：https://youtu.be/Bojh6ylYUOo

代码链接：https://github.com/zhudelong/VDB-EDT

1. Motivation

Eucliden distance transform EDT 对于机器人运动规划是很重要的，但是生成EDT 是比较费时的一件事，同时需要时刻更新并维护这样一个地图，本篇文章主要 通过优化数据结构和distance transform的过程来提升EDT算法的速度

在本文中，我们采用了树结构进行 hashing-based EDTs，主要是在做规划时发现 最优轨迹其实值需要考虑一定范围的障碍物， full distance information其实对于规划来说是冗余的，所以实际free部分都是一个值。These regions can then be efficiently encoded by a few number of tree nodes. Such a property is called spatial coherency, which can help further reduce memory consumption.

Benefiting from the fast index and caching systems, VDB achieves a much faster random access speed than Octree and also exhibits a competitive performance with the voxel hashing.

Contribution

• the first time introduce the VDB data structure for distance field representation, which significantly reduces the memory consumption of EDT.
• we propose a novel algorithm to facilitate distance transform procedure and significantly improve the running speed of conventional EDT algorithms.

2. Method

首先是问题定义，一个典型的distance transform问题 可以表达为如下公式：

$$\begin{array}{l}d\left(x_i\right)={\min }_{x_j}f\left(x_i,x_j\right),\\ \text{ s.t. }{x}_i\in {\mathcal{M}}_f,x_j\in {\mathcal{M}}_o\end{array}$$

其中，Mf是指free space,Mo是被占据空间，x为在grid map M中的坐标，目标函数f表示xi到xj之间的距离，目标是搜索对于每个xi都找其最近的xj作为距离

随后问题有了d(x) 后 我们就走到了 要找到一条安全的路径，则问题可表述为如下：

$$\begin{array}{ll}{\min }_{x_{0:N}}& {\sum }_{i=0}^N\alpha \Vert x_{i+1}-x_i\Vert +(1-\alpha )\max \left(0,d_{\max }-d\left(x_i\right)\right)\\ \text{ s.t. }& x_i,x_{i+1}\in {\mathcal{M}}_f\\ & x_0=x_s,x_N=x_f\\ & g\left(x_i,x_{i-1},x_{i+1}\right)<\theta \end{array}$$

其中，dmax是最大的transform distance，xs起点，xf终点，alpha为balance coefficient，g<theta主要是限制两个连续点之间产生较大的角度，平滑轨迹用的。目标函数中 前者为路径长度的cost，后者为避障的cost

2.1 数据结构

主要是介绍了VDB结构，由Museth[25] 提出的。It sufficiently exploits the sparsity of volumetric data, and employs a variant of B+ tree [32] to represent the data hierarchically.

下图是1D结构下的VDB，其和B+的几个特性是一致的，root node为索引，由hashmap建立，下面为internal node 和 leaf node保存了数据。也有本质上的不同：

it encodes values in internal nodes, called tile value. The tile value and child pointer exclusively use the same memory unit, and a flag is additionally leveraged to identify the different cases. A tile value only takes up tens of bits memory but can represent a large area in the distance field, which is the key feature leveraged to improve memory efficiency.

B+是一种平衡tree，在数据库中常用，主要原因是对于树结构的查询，程序加载子节点都需要进行一次磁盘IO，磁盘IO 比 读内存IO要慢 所以多叉的B+ tree可以减少I/O的次数
参考：b站视频 “索引”的原理 4min 建议感兴趣的可以再查询进阶数据结构书籍了解 实际上代码是直接openvdb库直接构建的

• VDB: the branching factors are very large and variable, making the tree shallow and wide
• Octree-based: deep and narrow, thus not fast enough for distance transform.

2.2 VDB-EDT

感觉这个看文中会比较好 主要是针对伪代码的解释

The distance field represented by VDB is essentially a sparse volumetric grid, and each field point is represented by a grid cell s indexed by a 3-D coordinate.

更新部分code：

void VDBMap::update_occmap(FloatGrid::Ptr grid_map, const tf::Vector3 &origin, XYZCloud::Ptr xyz)
{auto grid_acc = grid_map->getAccessor();auto tfm = grid_map->transform();openvdb::Vec3d origin3d(origin.x(), origin.y(), origin.z());openvdb::Vec3d origin_ijk = grid_map->worldToIndex(origin3d);for (auto point = xyz->begin(); point != xyz->end(); ++point) {openvdb::Vec3d p_xyz(point->x, point->y, point->z);openvdb::Vec3d p_ijk = grid_map->worldToIndex(p_xyz);openvdb::Vec3d dir(p_ijk - origin_ijk);double range = dir.length();dir.normalize();// Note: real sensor range should stractly larger than sensor_rangebool truncated = false;openvdb::math::Ray<double> ray(origin_ijk, dir);// openvdb::math::DDA<openvdb::math::Ray<double>, 0> dda(ray, 0., std::min(SENSOR_RANGE, range));//        if (START_RANGE >= std::min(SENSOR_RANGE, range)){
//            continue;
//        }openvdb::math::DDA<openvdb::math::Ray<double>, 0> dda(ray, 0, std::min(SENSOR_RANGE, range));// decrease occupancydo {openvdb::Coord ijk(dda.voxel());float ll_old;bool isKnown = grid_acc.probeValue(ijk, ll_old);float ll_new = std::max(L_MIN, ll_old+L_FREE);if(!isKnown){grid_distance_->dist_acc_->setValueOn(ijk);} // unknown -> free -> EDT initializeelse if(ll_old >= 0 && ll_new < 0){grid_distance_->removeObstacle(ijk);} // occupied -> free -> EDT RemoveObstaclegrid_acc.setValueOn(ijk, ll_new);dda.step();} while (dda.time() < dda.maxTime());// increase occupancyif ((!truncated) && (range <= SENSOR_RANGE)){for (int i=0; i < HIT_THICKNESS; ++i) {openvdb::Coord ijk(dda.voxel());float ll_old;bool isKnown = grid_acc.probeValue(ijk, ll_old);float ll_new = std::min(L_MAX, ll_old+L_OCCU);if(!isKnown){grid_distance_->dist_acc_->setValueOn(ijk);} // unknown -> occupied -> EDT SetObstacleelse if(ll_old < 0 && ll_new >= 0){grid_distance_->setObstacle(ijk);} // free -> occupied -> EDT SetObstaclegrid_acc.setValueOn(ijk, ll_new);dda.step();}} // process obstacle} // end inserting/* commit changes to the open queue*/
}

3. 实验及结果

各个阈值对时间的影响，其中对比了几个baseline方法如下：

• A commonly-used general EDT [18] (denoted without -Ex suffix)
• the proposed algorithm (denoted with -Ex suffix).
• Two implementations based on the array and VDB data structures to compare their memory efficiency (denoted with Arr- and VDB- prefix, respectively)

可以看到 在时间上-Ex 的耗时都比无Ex的快，虽然VDB的速度上比arr的还是慢了一点 10%-25%，但是从memeory cost上确实节约了30-60%的 Herein, the increment of time cost is inevitable, as VDB is based on tree structures and has a slower random access speed than the array-based implementation

同样表格是在数据集上的表现，在global 和 incremental transform会慢一点，但是在memory上省了不少

还有一个就是无人机在仿真环境中建图并有planning效果：

4. Conclusion

提出了一种VDB-EDT算法去解决 distance transform problem. The algorithm is implemented based on a memory-efficient data structure and a novel distance transform procedure, which significantly improves the memory and runtime efficiency of EDTs.

这项工作突破了通常的EDT的限制，也可以为后面基于VDB-based mapping, distance transform and safe motion planning的研究进行使用

---

赠人点赞 手有余香 😆；正向回馈 才能更好开放记录 hhh