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})?(\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})?(\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)?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)?d_{max}$ 表示該體素點的數值由周圍體素點來更新。