C-Blox中文翻译

C-Blox

摘要

维护一个一致性的环境稠密地图对于机器人进行高层次决策是重要的。然而，在长时间和大空间的建图过程中，累积漂移误差将导致地图不一致性。本文提出了一个新的稠密建图方法，解决建图一致性挑战的同时实现了可扩展性。我们方法的中心是将环境表示为一个TSDF子地图的集合。这些子地图通过基于特征的相机跟踪和BA优化进行定位。我们的主要贡献在于一个能够识别地图中稳定区域的pipeline，以及对子地图的融合。所提出的方法使得我们在降低地图增长的同时保持了一致性。我们在公共数据集上证明了我们方法的有效性。

引言

稀疏地图是目前主要SLAM方法所采取的地图形式，但是不适用于机器人任务执行。稠密建图能够提供任务执行所需要的环境信息。但是受到漂移误差的影响，容易产生不一致的环境表达。

本文提出c-blox，一种新的建图方法能够使得地图具有一致性和可扩展性。本文将环境表示为相互重叠的TSDF子地图集合，对于每个子地图，限制其体素变形的程度，针对地图规模随着环境规模上升而增大的问题，通过子地图融合限制地图的增长。首先基于特征共视关系识别包含潜在冗余视角的子地图，然后判断这些候选子地图是否相比于另外一个具有准确的相对位置关系。这种确定性度量是在同步构建的稀疏地图中提取得到的。

本文的贡献是：

1. 基于概率度量，在密集的TSDF地图中保持一致性的系统方法，同时限制地图的增长;
2. 完全基于cpu的实现，允许在缺乏GPU硬件的轻量级机器人平台上使用;
3. 为开源TSDF库Voxblox添加了线程基础设施和一个近似的快速集成器;

相关工作

划分工作空间意味着构建的地图大小与观测环境的大小线性增长，而不是时间或轨迹长度。Patch Volumes[9]将观测到的场景划分为“map patches”，这些Patch与环境中的平面对齐。在类似的方法Kähler等[18]尝试最小化子卷重叠。虽然空间有效，但追求细分的方法有两个缺点。首先，当重新查看现有的子体积时，必须确保相机姿态与该体积一致，以便整合信息，这在实践中是困难的。其次，[9]和[18]都需要同时将光线投射到多个子卷中进行摄像机跟踪，大大增加了系统的计算量。

相比之下，一些系统[19]、[7]、[20]没有尝试划分空间或控制子卷重叠。[19]中的作者将重构构建为网格片段的组合，并产生了令人信服的结果。然而，这两个系统都旨在在后处理中产生高质量的重建，导致高计算时间，使得这些方法不适合实时机器人应用。[7]和[20]中的作者将观测到的世界表示为一些潜在重叠的TSDF体积，并表明这对于保持地图一致性是有效的。我们扩展了这种方法。现有的工作在这个方向上的主要缺点是，他们产生的地图线性增长的轨迹长度[7]，或时间[20]。我们提出的方法在许多技术方面与这些系统不同(参见第四节)，但是我们系统的主要科学新颖性在于通过引入系统方法来限制地图的增长来解决可伸缩性问题。

总而言之，现有的方法解决了在密集地图中保持一致性的挑战:

A)以需要进一步处理的形式维护地图，例如点/冲波云或关键帧深度图;

B)难以将新信息融合到先前映射的位置，例如基于网格的表示;

C)使用离线或计算成本高的技术对重建进行全局优化;

D)构建不受限制的地图，即使是在有限制的环境中。

与现有的工作相比，拟议的系统以一种适合机器人使用的形式维护地图，允许无形中的地点重访，并限制在线地图的增长。

系统描述

所提出的系统建立在TSDF映射技术的基础上，该技术首先在[4]中提出，后来被[21]，[5]，[14]的一些作品所扩展。我们的系统概述如图2所示，由相机跟踪/定位(第IV-B节)、体积集成(第IV-C节)、地图融合(第IV-D节)和子体积融合(第IV-E节)模块组成。我们首先将映射表示描述为子卷集合。

A. TSDF子卷

当将观测数据融合到一张地图上时，由于集成来自局部化程度较差的传感器的数据，可能会导致不一致。特别是在密集地图的情况下，这种错误往往难以恢复，因为为了确保绘图仍然有效，通常会丢失观测和定位信息之间的相关性。我们追求的地图结构在构造上保持一致。我们将场景表示为局部一致的子卷的集合。但是，我们没有尝试对全局空间进行分区，而是允许世界和地图之间存在一对多的关系。这有利于保持一致性，因为关于环境结构的多个(潜在冲突的)假设能够在地图中保持独立，直到冲突能够消除歧义，这可以任意延迟。

每个TSDF子卷表示环境的一小部分，整个映射由所有这些子卷的集合表示，如下所示

其中N是当前构建的子卷的数量。每个子体积${\pi }^P$都有一个与之相关联的局部坐标系$M^P$，参数化为$T_{G{M}^p}\in SE(3)$。对子卷集合可能的3种操作是:

1)添加一个新的子卷(第IV-C节)

2)修改一个子卷的姿态(第IV-D节)

3)通过将一个子卷的内容与另一个子卷融合来销毁一个子卷(第IV-E节)。

B. 相机定位（ORB-SLAM2）

我们利用稀疏的、基于特征的SLAM来提供当前传感器姿态以及对过去姿态的更新。典型的TSDF系统倾向于依赖深度图像配准来进行相机跟踪，然而在稀疏工作的背景下，基于特征的定位提供了两个优势:

1)现代稀疏系统在CPU上高效运行，允许其应用于轻量级机器人平台(见V-C节);

2)稀疏特征图，其中定位信息和环境之间的相关性得到维护，允许基于概率的决策(见IVD节)。

在这项工作中，我们使用了ORB-SLAM2[2]，并在第IV-D节中提出了修改。在现代视觉slam的事实上的标准，ORB-SLAM2保持由一组关键帧和地标组成的图，其中关键帧姿态K，地标位置C。然后，摄像机跟踪和映射被表述为图像平面上特征重投影误差的非线性最小二乘优化，

C.体积积分

传入的图像数据通过将到达的帧$I^i$和$D^i$集成到当前活动的子卷${\pi }_A\in \Pi$中添加到集合中。我们首先将由摄像机跟踪提供的估计传感器姿态转换为活动子体积帧$M_A$

⊕为SE(3)上的组成。为了集成深度帧数据，我们使用[21]中引入的开源TSDF工具箱voxblox，并扩展它以提供子映射的接口。

定期创建一个新的子卷，标记为活动的子卷，最后一个活动子卷被转移到集合中，从而增加映射。子卷在创建后被严格地附加到第一个关键帧，这样$M^p=C^k$，其中k是第k个关键帧的图像索引。我们假设子卷中的数据是一致的，因此我们的方法是尽早且经常地生成新的子卷，依靠映射融合模块来改善这样做的可伸缩性成本。我们在以下两种情况下生成一个新的活动子卷:贡献于子卷的关键帧数量达到最大值，或者稀疏映射中有重大变化，这可能会导致相机跟踪的跳跃。

最初的voxblox系统采用了一种计算要求很高的帧集成方法，不适合在计算有限的平台上进行实时密集映射。此外，与大多数TSDF库不同，voxblox操作的是通用点云，而不是深度图像。它还显式地映射所有自由空间。这意味着其他库用于提高运行时性能的许多策略不能应用(主要是图像空间预处理和体素投影)。为了克服这些问题，我们实现了一种新的多线程“快速”集成方法。

在两种情况下，当一个区域的结构可能已经被其他射线捕获时，快速积分器通过提前终止射线投射来操作。第一种情况规定了起始点的最大密度。每个点首先被插入一个**“起始位置”集**，其分辨率是标准体素地图的两倍。如果一个点已经占据了这个起始位置，那么这个点将被丢弃。第二个终止条件是检查一条射线是否正在更新已经被同一帧中的其他射线更新过的体素。这种检查是通过将点插入到**“观察体素”集来完成的。如果一条射线试图更新一行中已经被更新的两个体素**，则它被认为是添加了最少的信息，并被终止。当光线在靠近相机位置时聚集在一起，这将在表面后不久终止许多光线，同时仍然确保绝大多数自由空间体素被分配和至少部分更新。这种方法的速度取决于插入到集合中的速度。它们被实现为固定大小、每个桶一个元素的散列集，其中冲突通过丢弃旧值来解决。这允许使用原子比较和交换指令进行同步无锁插入和读取。这些集合在每帧集成结束时被清除。

整合一个帧的时间取决于输入点云的结构，因此一些帧可能比其他帧要费时得多。为了确保始终保持实时性能，实现了最大集成时间，如果超过时间预算，将提前终止集成。

D.地图融合

在本节中，我们将提出一种方法来保持地图的一致性，同时限制其增长。在循环结束后，相机定位提供了一组更新的关键帧姿态，这是由稀疏映射(2)的全局优化产生的。每个子卷中的环境表示相对于其基本帧，意味着只需通过更新子卷坐标帧 { M p } p = 1 N = 1 \{{M^p}\}_{p = 1}^N=1 {Mp}p=1N​=1来校正集合。

到目前为止所描述的系统将产生一组子卷，该子卷的大小随轨迹长度线性增加，这意味着在操作过程中，即使在有限大小的环境中，地图也会无限制地增长。这限制了所提出的系统的实际适用性。然而，当重新访问一个地方时，集合可能包含同一区域的多个，潜在的冗余视图。我们限制地图增长的方法是识别这些冗余视图并融合它们，这样做有很大可能产生一致的结果。我们利用稀疏特征图中包含的信息来控制这个过程。

为了识别包含冗余视图的子卷，我们建议使用修改版的共视图[10]。在构建地图的过程中，我们关联每一个关键帧$K^i$和它所贡献的子卷。然后，我们构建了一个加权图G = (V, E)，其中顶点集V表示子卷 { π p } p = 1 N \{π^p\}_{p=1}^N {πp}p=1N​，边集E，带有相关权值W，表示地标共视信息。形式上，顶点$i$和$j$有一条权值边

式中$L_i$和$L_j$是构成${\pi }_i$和${\pi }_j$子体积的关键帧观测到的地标指数集，其中|·|表示集合的基数。在这个图中，与高权重边连接的子体积对很可能已经查看了相同的区域，因此作为候选对传递到下一阶段。

在这个阶段，我们有一个子卷对列表，其中包含环境中相似部分的视图。然而，这些子体的姿态估计可能包含重大的定位误差。对这些视图的简单融合可能会产生不一致的结果，因此我们宁愿在集合中分别维护这些视图(就像它们目前存在的那样)。因此，在融合子体积对之前，我们确定了它们相对定位精度的测量。形式上，给定一个候选对$({\pi }_i，{\pi }_j)$，具有基本帧$M_i$和$M_j$，并与关键帧$K_i$和$K_j$相关联，我们定义$q(i,j)$它们相对局部化的质量度量为

其中${\Sigma }_{i|j}$是与条件分布$P(K_i|K_j)$相关的协方差矩阵。对于这项工作，范数|·|是2范数，它与由${\Sigma }_{i|j}$定义的具有恒定概率密度的椭球的体积成正比。满足最小值q的子体积对被融合在一起。在本节的剩余部分中，我们将讨论如何确定${\Sigma }_{i|j}$。

1)子体积条件协方差的提取:给定优化参数$\theta =(K,X)$的初始猜测，通过迭代线性化图和求解线性系统来求解束调整，
$$H\theta =b$$
其中典型的稀疏矩阵H和向量b是由测量值、它们的雅可比矩阵、协方差和线性化点确定的(详细说明见[22])。在收敛时，我们只剩下信息矩阵的近似值
$$L=H^{\ast }$$
其中$H^{\ast }$来自于在收敛点O*处对图进行线性化。反演(通常很大)信息矩阵I以获得与后验分布相关的协方差矩阵Σ在计算上是昂贵的，并且不能扩展到实际问题。然而，**为每个子卷对确定${\Sigma }_{i|j}$只需要整个矩阵Σ的很少块;两个对角线块和一个非对角线块。**我们的目标是执行提取这些块所需的最小计算，然后进行如下操作。

我们首先利用所谓的舒尔补技巧从图中边缘化地标来形成简化相机矩阵，这里记为$I_P$。对于SLAM中遇到的典型问题，这种缩减可以非常有效地执行(参见[23]对这种方法的完整介绍)。在此基础上，我们利用了[24]中介绍的基于Cholesky分解信息矩阵的动态规划方法。该算法计算了一个要求的协方差矩阵元素${\sigma }_{i,j}$，作为其他协方差矩阵项的集合的函数，这些项遵循稀疏模式，是Cholesky因子R的非零项的子集。

我们对原算法进行了改进，提高了计算少量协方差矩阵元素的速度。原算法的速度主要取决于计算所需元素${\sigma }_{i,j}$时所需的中间项数。所需要的这些元素的数量是在Cholesky分解之前优化变量排序的乘积。使用旨在减少整体填充的排序，例如近似最小度(AMD)[25]，在提取非常小的元素集时，可能会导致变化很大且潜在的较大计算时间。然后，我们建议在Cholesky分解之前对每个需要协方差项的关键帧候选对重新排序矩阵$I_P$。通过对每对进行重新排序，我们可以自由地强制与${\Sigma }_{i|j}$相关的块在Cholesky因子中出现在最后。这大大减少了计算中间元素的数量，并相应地提高了速度。为了实现这个重排序，我们使用了AMD[25]的约束版本。

我们以两种方式支付这种排序方法。首先，限制$I$可能会增加Cholesky因子的整体填充的某些块的位置。其次，我们对每个候选子卷对执行$I_P$分解，而不是像原始算法[24]中所做的那样，对所有候选子卷对只分解一次。在我们的测试中，尽管有这些缺点，采用所建议的方法还是有很大的好处。

E.子卷融合

我们经常需要融合子卷，一方面是应映射维护模块的请求，另一方面是为了可视化子卷集合的当前状态。给定两个子体积${\pi }_i$和${\pi }_j$在参考系$M_i$和$M_j$下被融合;然后找到它们与全局地图之间的转换。

然后我们转换${\pi }_j$中的体素，在需要的地方分配${\pi }_i$中的新体素，并使用三线性插值来整合它们的贡献。为了使整个流形可视化，我们重复这个过程多次，将所有子体积融合为${\pi }_1$。