一种ESDF地图实现方法：FIESTA

背景：

在机器人定位、行动规划中建图是一个很重要的工作，只有通过感知器感知到自己在哪、周围有什么；才能为下一步行动作出决策的依据。然而要知道自己在哪，就必须要有一个整体规划和参照也就是所谓的地图。地图相当于是一次规划的绝对坐标构建出了一个空间，你只有在这个空间中才有“在哪”的说法，也就是说所谓的在哪就是想对你所在的空间某一个参考坐标而言的。这个空间的参考坐标可以是一个绝对的世界坐标，也可以是以你为参考的更大范围作为坐标。在较低的速度下两者甚至是可以等效的。所以现在问题就是如何去构建这张图，现在又有哪些种类的图。

绝对位置地图：

栅格地图
类似于2D栅格地图，3D地图相对比较占内存。

八叉树地图
如果某个位置上没有障碍物，那么可以用该位置上的大正方体表示，如果某个位置上是小障碍物，那就把大正方体切分到刚好可以包含这个障碍物的小正方形。
这样就可以减少计算量，节约内存。而这刚好是八叉树这种数据结构，寻找障碍物先从最大的正方体开始找，然后再向由这个正方体平均分的八个正方体里去找。

体素地图
类似于八叉树地图，是由最小的正方体（体素）为单位构成的，每个体素存储一个SDF、颜色和权重。

TSDF地图
TSDF地图是截断符号距离函数场，这里的体素储存的是投影距离（projective distance），就是沿着传感器射线到已测量的表面的距离。

相对位置图

ESDF地图
ESDF地图是欧几里得符号距离场，在这个场里面的每一个体素都包含了距离它最近的障碍物的欧几里得距离。

小结：

图在人类的眼睛中是一个可视的有长宽高的真实可以看的到的世界；然而在机器眼睛中，图其实就是一堆的数据，包含了每个体素的属性、位置、相对位置，以及一堆的方法：包括评判规则、取、存、删、增操作的方法。
所以可以很明确的知道图的代码应该也是包括这些模块的：
1.体素的数据结构：属性、相对位置、测量值、位置
2.图中元素的操作方法：
2.1静态操作方法：存、检索
2.2动态规则：基于规则的存储、取数、计算、增删改、构建图
2.3功能方法：降低存储成本、增加取数速度、增加规则判断速度

正文

欧几里得有符号距离场（ESDF）对于空中机器人的在线运动规划非常有用，因为它可以轻松查询与障碍物的距离和梯度信息。快速递增构建的ESDF地图是进行实时运动规划的瓶颈。在本文中，我们研究了这个问题，并提出了一个名为FIESTA的映射系统，用于递增地构建全局ESDF地图。通过引入两个独立的更新队列分别插入和删除障碍物，并使用索引数据结构和双向链表进行地图维护，我们的算法使用BFS框架更新尽可能少的节点。我们的ESDF地图具有高计算性能，并产生近乎最优的结果。我们通过理论和实验显示我们的方法在性能和准确性方面优于其他最新的方法。我们将FIESTA集成到一个完整的四轴飞行器系统中，并通过模拟和机载实验进行验证。我们将我们的方法作为开源软件发布给社区。

图1. 使用FIESTA进行ESDF地图计算的运动规划实验。四旋翼飞行器计划通过迷宫飞往指定目的地。运动规划使用了[8]。 (a) 显示了实验中的四旋翼飞行器，(b) 是FIESTA的相应可视化，其中立方体表示障碍物，平面是ESDF地图的一个切片，黄色和红色线条表示规划轨迹。有关实验的详细信息请参见此视频2。
对于一个完全自主的微型空中车辆（MAV），感知-规划-控制管道将环境测量作为输入并生成控制命令。映射模块，为机载运动规划提供基础，是该系统中最重要的组件之一。
有许多成熟的数据结构和算法用于映射。映射系统的基本要求是平衡融合深度测量的准确性和存储环境精细表示的开销。代表性的映射框架包括Octomap [1]和TSDF（截断符号距离场）[2]等。然而，对于自主四轴飞行器导航的目的，真正有用的是自由空间的信息，而不是障碍物。一个理想的规划地图必须具有快速查询自由/占用状态，或者获取到障碍物的距离信息的能力，例如欧几里得有符号距离场（ESDF）地图。
ESDF地图具有评估距离和梯度信息与障碍物的优势，因此，对于基于梯度的规划方法，如CHOMP [3]，是必要的。基于梯度的方法倾向于将生成的轨迹推离障碍物，以提高路径清晰度。因此，它在四轴飞行器规划中特别有用，如[4]，[5]和[6]中所示。对于实时机载运动规划，维护和更新ESDF地图的效率和准确性始终是瓶颈。在[15]中，提出了一种使用距离抛物线曲线的有效方法来全局计算ESDF。然而，对于四轴飞行器平台，感测范围和机载计算资源都非常有限，实时递增更新ESDF地图对于实时机载规划是有要求的。另一种实时更新ESDF地图的方法是只保持和维护一个随四轴飞行器滑动的小型本地地图，如[6]中所示。尽管这种妥协在四轴飞行器的本地（重新）规划中是有效的，但它丢弃了所有过去的地图信息，不能用于需要全局或可重复规划的应用。Voxblox [7]也被提出来递增地构建ESDF地图。它直接从截断符号距离场（TSDF）地图计算ESDF地图。它利用TSDF地图中截断半径内已经包含的距离信息，然后使用BFS（广度优先搜索）将其扩展到地图中的所有体素。它可以在动态增长的地图上实时工作，然而，在最终的ESDF地图中实际上存在两类错误。首先，它进行BFS并根据准欧几里得距离更新ESDF。因此，计算出的距离可能与实际的欧几里得距离相比有较大的误差。其次，它依赖于基于TSDF的映射，但是TSDF投影距离可能会高估到最近表面的实际欧几里得距离。
在本文中，我们提出了FIESTA（快速递增欧几里得距离场），这是一个轻量级和灵活的映射框架，用于递增地构建ESDF地图。我们的方法将姿态估计和深度测量作为输入，并以最小的计算开销全局更新ESDF地图。这是通过我们精心设计的数据结构和ESDF更新算法完成的。我们的数据结构为我们的高效ESDF更新算法奠定了基础，也提供了一个权衡时间复杂性和空间复杂性的选项。我们提出的算法使用BFS框架扩展尽可能少的节点，并得到更准确的ESDF值。更重要的是，与Voxblox [7]依赖于TSDF不同，我们的算法不依赖于任何特定类型的映射框架。这意味着我们的方法可以应用于任何通用的映射框架，包括占用网格地图和基于TSDF的地图。本文的贡献包括：
• 精心设计的数据结构，用于快速递增更新ESDF地图。
• 一种新的ESDF更新算法，它扩展尽可能少的节点并获得近乎最优的结果。
• 时间和空间复杂性、准确性和最优性的理论和实践分析。
• 将提出的ESDF地图集成到一个完整的四轴飞行器系统中，并演示机载MAV运动规划。
• 将提出的映射框架作为开源软件发布。
在四轴飞行器规划中使用了很多不同的映射框架。其中一些是从用于机器人感知的通用地图派生出来的。在[9]中，使用了一个网格地图进行快速碰撞检查和占用评估。使用占用地图，进行了一个基于场的路径搜索，然后生成了一个硬约束的分段轨迹。在[10]中，作者利用Octomap将大的自由空间分组，以搜索碰撞自由路径和生成安全轨迹。此外，点云直接用于规划[11]，其中环境的安全空间通过随机查询点云的Kd-tree的最近邻居来提取。最近还提出了一些为机器人导航量身定制的其他映射框架。Topomap [12]基于视觉SLAM系统的稀疏特征点构建凸簇。作者使用这些凸簇构建了一个稀疏的拓扑图，其中可以非常有效地找到一条路径。Sparsemap [13]与Topomap有类似的想法。作者提出了一个完整的管道，基于ESDF提取一个3D广义Voronoi图（GVD），并获得一个表示环境拓扑结构的细骨骼图。MAV规划在这个拓扑图中进行。Nanomap [14]提出了一种新的地图结构，可以高效地构建和查询范围搜索。在Nanomap [14]中，作者提出只保留最近的深度原始测量来进行碰撞检查，而不是显式地维护一个融合的地图。这种地图结构支持快速的四轴飞行器飞行。

图2. 我们提出的ESDF映射框架的系统概述。

一个 VIS 的成员和方法 以及伪代码中的缩写
ESDF地图在基于梯度的机器人运动规划中被广泛使用，其中对障碍物的距离和梯度信息是必要的。CHOMP [3]使用方法[15]来计算ESDF地图。它从一个确定性的占用网格地图开始，这意味着值要么是0要么是1，并为地图计算EDT（欧几里得距离变换）。使用[15]计算EDT是有效的。然而，运动规划受到反复计算ESDF值的困扰。它无法同时满足地图的准确性和更新频率，这反过来限制了机载规划模块的性能。
与我们的工作最相关的是Voxblox [7]，它也关注于递增地构建ESDF地图。Voxblox首先将传感器数据集成到一个TSDF地图中，然后使用BFS（广度优先搜索）框架从TSDF计算ESDF。Voxblox充分利用了TSDF地图中的距离信息，具有实时的CPU性能，并允许动态增长的地图大小。然而，这种方法有一些缺点。首先，ESDF值不够准确。在最终的ESDF地图中实际上存在两类错误。
(i)它使用BFS更新邻居点的ESDF信息，并以折线的长度更新它们，这也被称为准欧几里得扩展。这样，最终的ESDF不是真正的欧几里得距离，与实际的欧几里得距离相比可能有较大的误差。
(ii)它依赖于基于TSDF的映射，其中TSDF投影距离可能会高估到最近表面的实际欧几里得距离。
相反，我们的工作从占用网格地图直接递增计算ESDF，这是最简单的映射数据结构之一，这从根本上消除了来自(ii)的错误。至于(i)，我们也使用BFS但以另一种方式更新其邻居。它也不准确，因为我们在第V-E节中证明了所有基于BFS的ESDF更新算法都不能准确。然而，它比准欧几里得方式在理论和实践中都好一个数量级，这将分别在第V-E节和第VI-A节中显示。其次，Voxblox高度依赖于TSDF映射，这导致其复杂性高和可扩展性低。其复杂性也使得它很难移植到其他应用中。并且其更新始于TSDF体素的变化，使得整个算法难以分析和优化。相反，我们提出的系统是一个轻量级的解决方案，它引入了两个独立的更新队列分别插入和删除障碍物，给出了BFS的起始点的自然解释，使得它更容易分析和优化。
我们的系统FIESTA的概述如图2所示。首先，我们从立体声、RGB-D传感器或单目深度估计中获取深度测量，从外部设备如GPS和Vicon或内部姿态估计如VIO（视觉-惯性测距）中获取姿态测量。然后我们使用光线投射将它们集成到占用网格地图中，这是我们在FIESTA中选择用来存储占用信息的数据结构。在这个过程中，所有改变其占用状态的体素都被分别添加到名为insertQueue和deleteQueue的两个队列中。之后，我们通过一个名为ESDF更新初始化的过程将这两个队列合并成一个名为updateQueue的队列，并使用基于BFS的ESDF更新算法更新所有可能改变的ESDF的体素。
本节描述了FIESTA中使用的所有数据结构。占用握手地图用于集成占用信息。体素信息结构存储了与体素相关的所有信息。索引数据结构是从体素坐标到相应体素信息结构的映射。双向链表特别用于在一个占用的体素变为空闲时高效地更新ESDF。

A. 占用网格地图和体素信息结构
占用网格地图在我们的方法中用于存储体素的占用概率。当给出新的深度测量时，它会继续集成光线投射获得的新的占用信息数据。概率性的占用信息存储在体素的体素信息结构（VIS）中。此外，VIS中还有一些其他重要的成员。表I列出了所有有用的成员及其在本文伪代码中的含义和缩写。其中最后两行是体素的两种方法。dll将在第IV-C节中描述，nbrs表示所有观察到的邻居，具有指定的连通性，如6连通性或26连通性。
B. 索引数据结构
取决于规划区域的边界框是否预先知道以及为此算法保留的内存，数组或哈希表用于索引工作，即将体素坐标映射到相应的VIS指针。
具体来说，如果规划区域的边界框是预先知道的，并且内存足够大，所有体素的VIS指针可以简单地放入一个数组中。这样，当通过其坐标查询一个体素时，我们只需要计算它在该数组中的索引，然后返回相应的VIS指针。相反，如果内存不足或边界框未知，则需要一个哈希表将体素坐标转换为其相应的VIS指针。使用这种方法，内存消耗最小，因为这个数据结构中的所有体素都已经被观察到。然而，由于哈希表中的查找操作数量巨大，这将使性能比数组实现差很多。
因此，我们通过将(block size)3个体素分到一个块中，就像Voxel Hashing [16]所做的那样，给出了这两者之间的一个可操作的权衡。这里的哈希表只用于管理块。在我们从体素坐标计算出块坐标后，使用该哈希表找到相应的块。所有体素的VIS指针都存储在与该块对应的数组中。因为用于块的哈希表的大小比用于体素的哈希表小得多，所以通过更多地使用内存可以提高性能。我们甚至可以使用位操作来加速转换工作，通过使用块大小为2的整数幂，如4、8、16等。
无论使用哪种数据结构，查找操作的时间复杂性总是平均为Θ(1)。虽然不同数据结构的加速因子只是常数级别的，但对于需要在有限资源上实时性能的系统来说，这是至关重要的。

C. 双向链表
双向链表（DLLs）特别用于在一个占用的体素变为空闲时高效地更新ESDF（更多细节见第V-C节）。一个DLL是一个链接数据结构，由一组顺序链接的节点组成，每个节点有两个链接字段，分别引用前一个和后一个节点。我们使用体素vox的dll，即表I中的dll方法，来表示所有最近的障碍物是vox的体素。它们被链接成一个DLL，这个DLL的头指针可以通过vox的VIS的头成员获得。dll方法实际上是从头指针开始遍历整个DLL。图3显示了索引数据结构、体素信息结构和双向链表之间的关系。
在开始时，所有体素的最近障碍物都被初始化为理想点，即无穷远点。我们用IP来表示它。当一个体素被新观察到时（更多细节见第V-D节），它将被添加到IP的dll中。因此，所有新观察到但尚未更新的体素都链接在IP的dll中。
D. 有限观察的ESDF更新算法 在前两节中，我们忽略了有限观察的影响。新观察到的体素可能只更新其邻居，而不被先前存在的体素更新，这将使整个系统不一致。例如，在一维情况下，开始时只观察到障碍物（0）。在下一个占用积分中，观察到（1）-（3），并且（3）是一个障碍物。执行算法1后，（1）的ESDF将更新为2，应该从障碍物（0）更新为1。这是因为在算法1中，新观察到的ESDF可能不会从先前存在的体素更新。因此，我们需要修改算法，在算法1的第3-4行之间添加算法3。

E. 理论分析

1）最优性：我们的算法等效于通过最短欧几里得距离更新体素的ESDF值，该距离在该体素和其邻居的最近障碍物之间。ESDF值总是一个“欧几里得”距离。显然，它比准欧几里得更准确，因为后者使用折线的长度作为ESDF值。然而，无论我们选择哪种连通性，基于BFS的ESDF更新算法都不能完全准确。我们将在下面证明这个结论，并给出连通性和最优性之间的关系。 考虑一个2D情况，其连通性为4。 需要更新的点是原点，其中 邻居是nbrs = {(0, 1),(0, -1),(1, 0),(-1, 0)}。 假设他们的最近障碍物是obsnbrs = {(0, 5),(0, -5),(5, 0),(-5, 0)}。根据 ESDF更新算法，原点的距离 将更新为5，最近的障碍物是obsnbrs中的一个。如图4（a）所示，红圈的中心 位于原点，其半径为5，蓝圈的中心是 nbrs中的点，它们的半径为4。假设 obsnbrs是nbrs的最近障碍物，蓝圈内没有 障碍物。然而，仍然有 空间（称为剩余空间）在蓝圈外面而在红圈内。点在 那个空间和nbrs之间的距离比obsnbrs和nbrs之间的距离远，而点在 那个空间和原点之间的距离比obsnbrs和原点之间的距离近。图4（b）也显示了一个连通性为8的情况。事实上，无论我们选择哪种连通性，这 算法都不能准确，因为使用有限的小圈在大圈内覆盖大圈是不可能的， 总是有剩余的空间来给出反例。 实际上，这是使用BFS的ESDF更新算法的最坏情况。如果obsnbrs中的一个移动到其原始对应蓝圈上的另一个点， 如图4（c）所示，原点的最近障碍物将 被改变，原点的ESDF将变小，这反过来使得红圈和剩余空间变小。 如果它移动到其原始对应蓝圈的外面， 如图4（d）所示，那个蓝圈变得更大 也使剩余空间变小。误差来自 算法计算的距离和实际距离之间的差异。从插图中可以看出， 误差随着半径的增加而减小。考虑到最坏情况的发生次数少，以及 只有整数点在剩余空间内才会影响 我们的算法，我们的算法的均方根（RMS）误差在实践中是可以接受的。实验结果 将在第VI-B节中显示。
2）时间复杂度：对于算法2，每个体素要么在insertQueue中，要么其最近的障碍物在deleteQueue中，只处理一次。如果我们使用FIFO队列，时间复杂度是Θ(k)，其中k是需要处理的所有必要体素的数量。 对于算法1，如果我们使用优先队列进行BFS过程，我们确保所有体素都从updatequeue中弹出并只处理一次以更新其邻居。然后，时间复杂度是Θ(n log n)，其中n是需要更新的所有体素的数量，log n因子来自优先队列的性质。
3）空间复杂度：如我们在第IV-B节中所述，与基于哈希表的纯占用网格图相比，我们的系统只修改了VIS。所以它只比纯占用网格图高一个常数级别。从我们设计的数据结构，我们甚至可以使它等于Θ(m)，如果我们选择使用块大小=1，其中m是所有曾经观察到的体素的数量，尽管它的效率不高。因此，具有高度可定制的索引数据结构是非常重要的，需要考虑实际系统的需求。 VI. 结果 A. 实际数据集上的实验 在本节中，我们测试了我们系统FIESTA的不同参数，然后我们将其与不同体素大小级别的Voxblox进行比较。本部分的所有实验都使用i7-8700k在3.7GHz下进行，只使用了一个线程。

图5. 针对不同连接性的时间和RMS误差。x轴表示连接性类型。左y轴表示平均更新时间性能，右y轴表示以体素为单位的RMS误差。

图6. 针对不同索引数据结构的时间和空间。x轴表示索引数据结构类型，其中HT(n)表示哈希表，块大小为n。左y轴表示平均更新时间性能，右y轴表示以VIS数量为单位的内存消耗。
VI. 结果
A. 对真实世界数据集的实验 在本节中，我们测试了我们的系统FIESTA的不同参数，然后我们在不同的体素大小级别上将其与Voxblox进行比较。本部分的所有实验都使用i7-8700k，频率为3.7GHz，只使用一个线程。

1. 参数调整：我们的系统中有很多参数，比如连接性和使用哪种索引数据结构。我们将提供一组实验来测试我们的系统在准确性、性能和内存消耗方面的最优参数。我们的系统每0.5秒请求一次ESDF更新，性能通过平均更新时间来衡量。最大的内存消耗是VIS，所以我们使用VIS的数量作为内存消耗的度量。在运行完整个数据集后，我们使用占用网格地图获得的占用信息建立一个K-D树，然后查询地图内每个体素的距离作为真实值。然后，使用体素单位的均方根（RMS）误差来表示误差的规模。在本节中，使用RGB-D相机的Cow and Lady数据集3[7]，其中包含点云数据和姿态信息，作为数据集。

首先，我们将索引数据结构固定为块大小=8的哈希表，并测试准确性与连接性的关系。我们尝试了6-(面)，18-(面和边)，26-(面，边和角)，24-(面，边和2步面，即曼哈顿距离小于或等于2)和32-(前两者的组合)连接性。更多的连接性导致更多的邻居需要更新，因此提供了更高的准确性和更差的计算性能。因此，我们需要在性能和准确性之间选择一个折衷。图5显示了结果。从插图中可以看出，24-连接性是RMS误差和性能的最佳选择之一。然后，比较不同的索引数据结构。如第IV-B节所示，我们提供了很多选择，以在性能和内存消耗之间提供一个折衷。我们将连接性固定为24，然后测试数组实现和哈希表实现，块大小=1，2，4，8，16。图6显示了结果。根据结果，如果地图边界未知，哈希表的块大小=8是时间和空间复杂性的最佳选择之一。否则，数组具有最佳性能。

1. 与Voxblox的比较：提供了FIESTA和Voxblox在性能和准确性方面的比较，不同的体素大小。使用RGB-D相机的Cow and Lady数据集和使用立体相机的EuRoC数据集作为这次比较的数据集。我们选择使用块大小=8的哈希表作为索引数据结构和24-连接性进行比较。图8显示了结果，很明显，我们的系统在性能和准确性方面都超过了Voxblox一个数量级。使用我们的系统，体素大小=0.05，运行Cow and Lady数据集，显示了占用网格地图和ESDF地图切片的可视化。图7显示了结果。

B. 在模拟中测试四轴飞行器的运动规划 为了证明我们提出的地图可以用于运动规划，我们展示了模拟的四轴飞行器飞行，其中运动规划方法采自[8]。在模拟中，地图以及MAV的起始点和目标点是随机生成的。只有在MAV当前位置的半径范围内的障碍物可以被感知。我们的系统FIESTA增量地构建全局ESDF地图，以帮助运动规划算法高效运行。我们的样本结果显示在图9中。

图7. 在Cow和Lady数据集以及EuRoC数据集上的比较结果,比较了FIESTA(F)和Voxblox(V)在不同体素尺寸下的性能(Perf)和精度(Acc)。x轴为体素尺寸,左y轴为性能指标即平均更新时间,右y轴为单位为体素的均方根误差。

图8. 在Cow和Lady数据集上,使用体素尺寸为0.05的我们的系统,获得的占用栅格地图和ESDF地图切片的可视化结果。
C. 自主四轴飞行器机载实验 我们在未知的复杂环境中进行了机载实验，使用的运动规划方法与第VI-B节相同。我们使用的平台是一架装备了Velodyne VLP-16 3D激光雷达的四轴飞行器，该雷达用于姿态估计和深度测量。所有的状态估计、运动规划、控制和我们提出的映射系统都在双核3.00GHz Intel i7-5500U处理器上运行。尽管我们的ESDF地图可以在20ms内完成更新，但由于我们的实验中飞行速度的要求较低，我们以20Hz的频率更新ESDF。此外，以相对较低的频率更新ESDF地图可以为其他模块（如规划和定位）节省大量的机载资源。实验的快照以及增量映射和规划结果在图1和10中给出。
VII. 结论 在本文中，我们提出了一个轻量级和灵活的映射框架，FIESTA。FIESTA增量地构建全局ESDF地图，这对运动规划非常重要。通过引入两个独立的更新队列分别插入和删除障碍物，并使用索引数据结构和双向链表维护体素，我们的算法使用BFS框架更新尽可能少的节点。本文给出了时间和空间复杂性、准确性和最优性的理论和实践分析。我们证明了我们的更新规则在Voxblox的准欧几里得方法上表现更好。在实践中，使用块大小=8和24-连接性的哈希表，我们的系统在准确性和时间复杂性方面都超过了Voxblox一个数量级。所提出的ESDF地图也被集成到一个完整的四轴飞行器系统中。通过模拟和机载实验，我们通过高效地增量构建全局ESDF地图验证了我们的系统。

图9. 仿真实验结果

图10. 机载实验结果