Voxblox算法

1. 算法简介

Voxblox 算法出现于文献《Voxblox: Incremental 3D Euclidean Signed Distance Fields for On-Board MAV Planning》，PDF 链接：https://arxiv.org/pdf/1611.03631，代码实现：https://github.com/ethz-asl/voxblox。
无人机的轨迹优化需要障碍物距离信息，这些信息可以从欧几里得有符号距离场(Euclidean Signed Distance Fields, ESDF)获取。ESDF 中的每个体素都包含其到最近障碍物的欧几里得距离。论文提出了一种通过 TSDF 在线增量式构建 ESDF的方法。关于 TSDF 的概念可以参考博客：截断符号距离场TSDF。
计算 TSDF 有两种方式：射线投射(Raycasting) 和投影映射(Projection Mapping)，而 Voxblox 选择的是 Raycasting，并且对 Raycasting 的过程做出了一些改进，如下所示：

Our approach, grouped raycasting, significantly speeds up raycasting without losing much accuracy. For each point in the sensor scan, we project its position to the voxel grid, and group it with all other points mapping to the same voxel. Then we take the weighted mean of all points and colors within each voxel, and do raycasting only once on this mean position.

论文采用的分组射线投射法，能在不大幅降低精度的情况下显著加快光线投射的速度。对于传感器扫描中的每一个点，将其位置投影到体素网格上，并将其与所有映射到同一体素的点归为一组。然后，对每个体素内的所有点和颜色取加权平均值，并仅在此平均位置上进行一次光线投射操作。
现在对论文中选取的射线投射法进行简要说明：
假设 $d$ 表示到障碍物表面的距离，$\mathbf{x}$ 是当前体素的中心位置，$\mathbf{p}$ 是传感器观测到的三维点云的位置，$\mathbf{s}$ 是传感器原点位置。当前体素的距离值和权重分别为 $D$ 和 $W$，则有如下方程：
$$\begin{array}{rl}d(\mathbf{x},\mathbf{p},\mathbf{s})& =\Vert \mathbf{p}-\mathbf{x}\Vert \mathrm{sign}((\mathbf{p}-\mathbf{x})\cdot (\mathbf{p}-\mathbf{s}))\\ w_{\text{const}}(\mathbf{x},\mathbf{p})& =1\\ D_{i+1}(\mathbf{x},\mathbf{p})& =\frac{W_i(\mathbf{x})D_i(\mathbf{x})+w(\mathbf{x},\mathbf{p})d(\mathbf{x},\mathbf{p})}{W_i(\mathbf{x})+w(\mathbf{x},\mathbf{p})}\\ W_{i+1}(\mathbf{x},\mathbf{p})& =\min (W_i(\mathbf{x})+w(\mathbf{x},\mathbf{p}),W_{\max })\end{array}\text{\hspace{1em}(1)(2)(3)(4)}$$ 公式中的 $\mathrm{sign}((\mathbf{p}-\mathbf{x})\cdot (\mathbf{p}-\mathbf{s}))$ 是为了判断体素处于障碍物的前方还是后方，如下图所示：

事实上，论文中并没有完全将 $w$ 恒设置为 1。对处于障碍物后方的体素的权重进行了调整，调整公式如下：
$$\begin{array}{c}{w}_{\text{quad}}(\mathbf{x},\mathbf{p})=\{\begin{array}{ll}\frac{1}{z^2}& -ϵ<d\\ \frac{1}{z^2}\frac{1}{\delta -ϵ}(d+\delta )& -\delta <d<-ϵ\\ 0& d<-\delta ,\end{array}\end{array}\text{\hspace{1em}(5)}$$其中，$v$ 表示体素的尺寸，$z$ 是传感器测量到的深度值，$\delta =4v$ 是截断距离，$ϵ=v$。
为什么会障碍物后方的体素的权重做出这样的调整？直观上来看，这会减少没有被直接观察到的体素（位于障碍物表面后方的体素）的影响，论文表示这会取得更好的效果。

2. 由 TSDF 构建 ESDF 的方法

2.1. 论文解读

One of the key improvements we have made is to use the distance stored in the TSDF map, rather than computing the distance to the nearest occupied voxel.

论文的一个关键改进是：利用 TSDF 中存储的距离信息来重建 ESDF，而不是直接计算到最近占用体素的距离。我们知道在 TSDF 网格中，如果一个体素在截断范围 $[-d_{trunc},d_{trunc}]$ 以内（即该体素靠近障碍物表面），体素中存储的 TSDF 数值（该数值反应了该体素到障碍物表面的距离）是比较准确的。

each voxel had an occupied or free status that the algorithm could not change. Instead, we replace this concept with a fixed band around the surface: ESDF voxels that take their values from their co-located TSDF voxels, and may not be modified. The size of the fixed band is defined by TSDF voxels whose distances fulfill |vT.d| < γ, where γ is the radius of the band.

与以往的算法会为每个体素都分配一个被占用或空闲的状态不同的是，Voxblox 基于障碍物表面定义一个固定区域，固定区域内的 ESDF 体素的距离值取自其相应位置上的 TSDF 体素，并且不能被修改。固定区域的大小由距离满足 $|v_T.d|<\gamma$ 的 TSDF 体素来定义，其中 $\gamma$ 是该区域的半径。

The general algorithm is based on the idea of wavefronts – waves that propagate from a start voxel to its neighbors (using 26-connectivity), updating their distances, and putting updated voxels into the wavefront queue to further propagate to their neighbors.

Voxblox 选取了一种波前传播的方式进行体素距离信息的更新，即以一个体素为起点，找到该体素的 26 个邻居，更新它们存储的距离信息，然后以这 26 个邻居为起点，重复之前的过程。

We use two wavefronts: raise and lower. A voxel gets added to the raise queue when its new distance value from the TSDF is higher than the previous value stored in the ESDF voxel. This means the voxel, and all its children, need to be invalidated. The wavefront propagates until no voxels are left with parents that have been invalidated.
The lower wavefront starts when a new fixed voxel enters the map, or a previously observed voxel lowers its value. The distances of neighboring voxels get updated based on neighbor voxels and their distances to the current voxel. The wavefront ends when there are no voxels left whose distance could decrease from its neighbors.

在具体实现中，Voxblox 通过维护两个队列：raise 和 lower 来进行体素距离信息更新。

• raise 队列作用：存储需要废除距离信息的体素（当 TSDF 网格中一个体素的新距离值大于对应 ESDF 网格中对应位置的体素存储的距离值时，这说明之前计算的距离值是不准确的）。PROCESSRAISEQUEUE 函数作用：逐个遍历 raise 队列中的体素，将遍历到的体素和以该体素为父体素的所有体素的距离值都作废（设置为 $d_{max}$），后续重新计算。
• lower 队列的作用：存储需要更新距离信息的体素。PROCESSLOWERQUEUE 函数中会对 lower 中的体素重新计算距离信息，即以当前遍历到的体素为基准，更新其邻居体素的距离信息，当不再有体素的距离能够从其相邻体素那里减小时，就结束该过程。

we do not update unknown voxels. For each voxel, we store the direction toward the parent, rather than the full index of the parent.

算法不会更新未知的体素。对于每个体素，只存储其指向父体素的方向信息，而非父体素的完整索引。
TSDF 构建 ESDF 的思路大致如下图所示：

2.2. 伪代码实现

伪代码中的 $v_T$ 表示 TSDF 网格中的体素，而 $v_E$ 表示 ESDF 网格中相同位置上的体素。

问题：上述伪代码中，$v_T$ 为固定体素，但 $v_E.d<v_T.d$ 且 $v_E$ 是未被观测到的体素时，为什么设置 $v_E.d$ 为 $sign(v_T.d)\cdot d_{max}$ 而不是直接设为 $v_E.d=v_T.d$ ？
回答：在 TSDF 中体素存储的是局部距离信息，例如传感器观测到的障碍物表面附近的距离值（范围通常在 $[-d_{trunc},d_{trunc}]$ 内）。而 ESDF 中体素存储的是每个体素到最近障碍物的实际欧几里得距离（带符号，正负分别表示障碍物的前方或者后方）。$sign(v_T.d)$ 保证了 $v_E.d$ 的符号与 $v_T.d$ 一致。$v_T.d$ 是局部的 TSDF 值，仅反映传感器观测到的局部障碍物信息，无法代表全局距离。例如，如果 $v_T$ 是三维空间中的一个点（$v_T.d$ = +0.1m），但实际该点到障碍物的距离是 +5m，则直接使用 $v_T.d$ 会导致 ESDF 错误（低估距离）。将 $v_E.d$ 设置为 $sign(v_T.d)\cdot d_{max}$ 表示该体素点的数值由周围体素点来更新。