1. 摘要我们为实时度量-语义视觉-惯性同步定位和建图 (SLAM) 提供了一个开源C库。该库超越了现有的视觉和视觉惯性 SLAM 库例如ORB-SLAM、VINS-Mono、OKVIS、ROVIO实现了 3D 网格重建和语义标记。Kimera在设计时考虑了模块化并有四个关键组件用于快速准确状态估计的视觉惯性里程计 (VIO) 模块用于全局轨迹估计的强大姿态图优化器用于快速网格重建的轻量级3D网格划分器模块以及用于快速网格重建的密集3D度量语义重建模块这些模块可以单独运行或组合运行因此Kimera可以很容易地回退到最先进的VIO或完整的SLAM系统。Kimera在CPU上实时运行并从语义标记的图像中生成3D度量语义网格这可以通过现代深度学习方法获得。我们希望Kimera提供的灵活性计算效率健壮性和准确性将为未来的度量语义SLAM和感知研究奠定坚实的基础并将允许多个领域例如VIOSLAM3D重建分割的研究人员对自己的工作进行基准测试和原型设计而不必从头开始。2. 附加材料2.1. 项目地址Kimera: an Open-Source Library for Real-Time Metric-Semantic Localization and Mapping | SPARKlab2.2. 代码地址https://github.com/MIT-SPARK/Kimera2.3. 视频https://www.youtube.com/watch?v-5XxXRABXJs3. 引言度量语义理解是同时估计场景的3D几何图形并将语义标签附加到对象和结构例如桌子墙壁的能力。几何信息对于机器人安全导航和操纵物体至关重要而语义信息为机器人理解和执行人类指令例如“给我一杯咖啡”“从红门退出”并为人类提供易于理解的环境模型提供了理想的抽象水平。尽管在几何重建例如SLAM [1]运动结构[2]和多视图立体[3]和基于深度学习的语义分割例如[4]-[10]方面取得了前所未有的进展但这两个领域的研究传统上都是孤立进行的。然而最近人们对这些领域的交叉点的研究和应用越来越感兴趣[1], [11]-[15]。这种日益增长的兴趣促使我们创建并发布了Kimera这是一个用于度量语义定位和建图的库它将几何和语义理解的最新技术结合到现代感知库中。与针对视觉惯性里程计 (VIO) 和SLAM的相关努力相反我们将视觉惯性SLAM网格重建和语义理解相结合。我们的努力还在几个方面补充了度量和语义理解之间边界的方法。首先虽然现有的努力集中在RGB-D传感上但Kimera使用视觉 (RGB) 和惯性传感这在更广泛的室内和室外环境中效果很好。其次虽然相关作品[16]–[18]需要GPU进行3D建图但我们提供了一个快速轻量级和可扩展的基于CPU的解决方案。最后我们专注于鲁棒性我们包括最先进的异常值拒绝方法以确保Kimera在各种场景中稳健地执行并且参数调整最少从真实的基准测试数据集[19]到照片级真实模拟[20][21]。4. 相关工作我们请读者参考表 I以便与现有的 VIO 和视觉 SLAM 系统进行视觉比较并参考 [1] 以获取有关 SLAM 的更广泛评论。虽然度量语义理解[11]和[33]的早期工作是为离线处理而设计的但近年来由SLAM[16]等开创性工作引发人们对实时度量语义映射的兴趣激增。这些作品中的大多数 (i) 依赖于RGB-D相机(ii) 使用GPU处理(iii) 替代跟踪和建图表I中的“交替”以及 (iv) 使用基于体素的例如截断的有符号距离函数TSDF表面或对象表示。例子包括SemanticFusion[17]郑等人的方法[15]泰特诺等人[34]以及李等人[35]Fusion [36]Mask-fusion [29]Co-fusion [37]和MID-Fusion [38]。最近的工作研究了基于CPU的方法例如Wald等人[39]PanopticFusion [40]和Voxblox [14]这些也依赖于 RGB-D 传感。一组稀疏的贡献涉及其他传感模式包括单目摄像头例如CNN-SLAM [41]VSO [42]VITAMIN-E [43]XIVO [32]和lidar例如SemanticKitti [44]SegMap [31]。XIVO [32] 和 Voxblox [14] 最接近我们的提案。XIVO [32]是一种基于EKF的视觉惯性方法可生成基于对象的地图。Voxblox [14] 依靠 RGB-D 传感、车轮里程测量和使用 maplab [26] 的预构建地图来获得视觉惯性姿态估计值。与这些工作相反Kimera (i) 提供了基于高精度实时优化的VIO(ii) 使用强大而通用的姿势图优化器以及 (iii) 提供轻量级网格重建。5. 主要贡献我们发布了Kimera这是一个开源C库它使用视觉惯性传感来估计机器人的状态并构建一个轻量级的度量语义网格环境模型。Kimera这个名字源于我们图书馆的混合性质它统一了各个研究领域的最新工作包括VIO姿势图优化 (PGO)网格重建和3D语义分割。Kimera包括四个关键模块Kimera-VIO用于快速准确的IMU速率状态估计的VIO模块。基美拉-VIO的核心是采用基于GTSAM的VIO方法[45]使用IMU预集成和无结构视觉因子[27]并在EuRoC数据集上实现最佳性能[19]Kimera-RPGO一种鲁棒的位姿图优化 (RPGO) 方法利用现代技术进行异常值拒绝[46]。Kimera-RPGO增加了一个健壮性层避免了由于感知混叠而导致的SLAM故障并使用户免于耗时的参数调整Kimera-Mesher一种计算快速每帧和多帧正则化3D网格以支持避障的模块。该网格划分器建立在作者和其他小组先前的算法的基础上 [43], [47]–[49]Kimera-Semantics一个语义模块它使用体积法[28]构建一个更慢但更准确的全局3D网格并使用2D像素语义分割对3D网格进行语义注释。Kimera既可以使用离线数据集也可以使用机器人操作系统 (ROS) 在线工作[50]。它在CPU上实时运行并提供有用的调试和可视化工具。此外它是模块化的允许替换每个模块或单独执行它们。例如它可以回退到VIO解决方案或者如果语义标签不可用它可以简单地估计几何网格。6. Kimera图2显示了Kimera的建筑。Kimera 将立体帧和高速率惯性测量作为输入并返回 (i) 以 IMU 速率计算的高度准确的状态估计值(ii) 全局一致的轨迹估计值以及 (iii) 环境的多个网格包括快速局部网格和全局语义注释网格。Kimera高度并行化并使用四个线程来容纳不同速率的输入和输出例如IMU帧关键帧。在这里我们按线程描述体系结构而每个模块的描述将在以下各节中给出。第一个线程包括Kimera-VIO前端第II-A节它获取立体图像和IMU数据并输出特征轨迹和预集成的IMU测量。前端还发布 IMU 速率状态估计值。第二个线程包括 (i) 返回优化状态估计值的 Kimera-VIO 后端以及 (ii) Kimera网格第 II-C 节用于计算每帧和多帧 3D 网格的低延迟 20 毫秒。这两个线程允许创建图 2 (b) 中的每帧网格也可以带有图 2 (c) 中的语义标签以及图 2 (d) 中的多帧网格。最后两个线程以较慢的速率运行旨在支持低频功能例如路径规划。第三个线程包括 Kimera-RPGO第 II-B 节这是一个健壮的 PGO 实现可检测循环闭包、拒绝异常值并估计全局一致的轨迹图 2 (a)。最后一个线程包括Kimera语义第 II-D 节它使用密集的立体和 2D 语义标签使用 Kimera-VIO 的姿势估计来获得精细的度量语义网格。6.1. Kimera-VIO视觉惯性里程计模块Kimera-VIO 实现了 [27] 中介绍的基于关键帧的最大后验视觉惯性估计器。在我们的实现中估计器可以执行完全平滑或固定滞后平滑具体取决于指定的时间范围我们通常使用后者来限制估计时间。我们还扩展了 [27] 以处理单目和立体声帧。Kimera-VIO包括一个视觉和惯性前端负责处理原始传感器数据以及一个后端它融合处理的测量结果以获得传感器状态的估计值即姿态速度和传感器偏差。6.1.1. VIO 前端我们的IMU前端执行流形预集成[27]从原始IMU数据中获得两个连续关键帧之间相对状态的紧凑预集成测量值。视觉前端检测Shi-Tomasi角[51]使用Lukas-Kanade跟踪器[52]跨帧跟踪它们找到左右立体声匹配并执行几何验证。我们使用 5 点 RANSAC [53] 执行单声道透明验证并使用 3 点 RANSAC [54] 进行立体声验证该代码还提供了使用IMU旋转的选项并分别使用2点[55]和1点RANSAC执行单声道和立体声验证。特征检测、立体匹配和几何验证在每个关键帧执行而我们只跟踪中间帧的特征。6.1.2. VIO 后端在每个关键帧上预集成的IMU和视觉测量被添加到构成我们的VIO后端的固定滞后平滑器因子图中。我们使用预集成的IMU模型和[27]的无结构视觉模型。因子图在 GTSAM [57] 中使用 iSAM2 [56] 求解。在每次 iSAM2 迭代中无结构视觉模型使用 DLT [58] 估计观测到的特征的 3D 位置并分析地从 VIO 状态 [59] 中消除相应的 3D 点。在消除之前将去除退化点即相机后面的点或没有足够的视差进行三角测量的点和异常值即具有较大重投影误差的点从而提供额外的鲁棒性层。最后脱离平滑地平线的国家被GTSAM边缘化。6.2. Kimera-RPGO稳健的姿势图优化模块Kimera-RPGO 负责 (i) 检测当前和过去关键帧之间的循环闭包以及 (ii) 使用可靠的 PGO 计算全局一致的关键帧姿势。6.2.1. 闭环检测闭环检测依赖于 DBoW2 库 [60]并使用词袋表示来快速检测推定的环路。对于每个假定的环路闭包我们使用单目和双目几何验证拒绝异常值环路闭包如第 II-A 节所述并将剩余的环路闭包传递给鲁棒 PGO 求解器。请注意由于感知混叠生成的循环闭包仍可能包含异常值例如建筑物不同楼层的两个相同房间。6.2.2. Robust PGO该模块在GTSAM中实现包括一种现代异常值抑制方法增量一致性测量集最大化 (PCM) [46]我们将其定制为单个机器人和在线设置。我们分别存储里程线边缘由Kimera-VIO产生和环闭包由环闭合检测产生每次执行 PGO 时我们首先使用 PCM 的修改版本选择最大的一致循环闭包集然后在姿势图上执行 GTSAM包括里程法和一致循环闭包。PCM专为多机器人案例而设计仅检查机器人间循环闭合是否一致。我们开发了一个C的PCM实现该实现 (i) 在环闭合上添加了里程测量一致性检查并且 (ii) 增量更新了一致测量集以实现在线操作。里程测量检查验证每个环闭包例如l1在图2 (a)中与里程计一致图中为红色在没有噪声的情况下沿着由里程计形成的循环和环l的姿态1必须组成标识。与 PCM 一样我们将沿周期累积的误差与使用卡方检验的测量噪声不一致的异常值循环标记为异常值循环。如果在当前时间检测到的环 t 通过了里程检查我们将测试它是否与 [46] 中的先前循环闭包成对一致例如检查循环 l1和 l2图 2 (a) 中的图 2 (a) 彼此一致。当 PCM [46] 构建邻接矩阵 A ∈ R 时长×升从头开始跟踪成对一致的循环其中L是检测到的循环闭包的数量我们通过增量构建矩阵A来实现在线操作。每次检测到新循环时我们都会向矩阵 A 中添加一行和列并且仅针对以前的循环测试新循环。最后我们使用 [61] 的快速最大集团实现来计算最大的一致循环闭包集。一组一致的测量值被添加到姿态图中与里程法一起并使用 Gauss-Newton 进行优化。6.3. Kimera-Mesher: 3D Mesh ReconstructionKimera-Mesher 可以快速生成两种类型的 3D 网格(i) 每帧 3D 网格以及 (ii) 跨越 VIO 固定滞后平滑器中的关键帧的多帧 3D 网格。6.3.1. Per-frame mesh与 [47] 中一样我们首先对当前关键帧中成功跟踪的 2D 特征由 VIO 前端生成执行 2D Delaunay 三角测量。然后我们使用 VIO 后端的 3D 点估计值反向投影 2D Delaunay 三角测量以生成 3D 网格图 2 (b)。虽然每帧网格旨在提供低延迟障碍物检测但我们还提供了通过测量2D标签的网格对3D网格进行语义标记图 2 (c)。6.3.2. Multi-frame mesh多帧网格将 VIO 后退地平线上收集的每帧网格融合成单个网格图 2 (d)。每帧和多帧 3D 网格都编码为顶点位置列表以及顶点 ID 的三元组列表用于描述三角形面。假设我们在时间 t−1时已经有了一个多帧网格对于我们生成的每个新的每帧 3D 网格在时间 t 处我们循环遍历其顶点和三元组并添加位于每帧网格中但在多帧网格中缺失的顶点和三元组。然后我们循环遍历多帧网格顶点并根据最新的 VIO 后端估计值更新其 3D 位置。最后我们删除了与在VIO时间范围之外观察到的旧特征相对应的顶点和三元组。结果是一个最新的 3D 网格该网格跨越当前 VIO 时间范围内的关键帧。如果在网格中检测到平面则会将规则因子 [47] 添加到 VIO 后端从而导致 VIO 和网格正则化之间的紧密耦合有关更多详细信息请参阅 [47]。6.4. Kimera-Semantics: Metric-Semantic Segmentation我们将 [28] 中引入的BA技术改编为 (i) 构建精确的全局 3D 网格覆盖整个轨迹以及 (ii) 对网格进行语义注释。6.4.1. Global mesh我们的实现建立在 Voxblox [28]的基础上并使用基于体素 (TSDF) 的模型来过滤掉噪声并提取全局网格。在每个关键帧中我们使用密集立体声半全局匹配[62]从当前立体声对中获取3D点云。然后我们使用 Voxblox [28] 应用捆绑的光线投射使用 [28] 中讨论的“快速”选项。在每个关键帧上重复此过程并生成一个 TSFD使用行进立方体 [63] 从中提取网格。6.4.2. Semantic annotationKimera-Semantics 使用2D语义标记图像在每个关键帧处生成对全局网格进行语义注释2D语义标签可以使用现成的工具获得像素级2D语义分割例如深度神经网络[7]–[9][64]–[69]或经典的基于MRF的方法[70]。为此在BA过程中我们还传播语义标签。使用2D语义分割我们将标签附加到由密集立体产生的每个3D点上。然后对于BA中的每个光线束我们从BA中观察到的标签的频率构建标签概率向量。然后我们仅在TSDF截断距离即靠近表面内沿射线传播此信息以进行备用计算。换句话说我们节省了更新“空”标签的概率的计算工作。在沿光线遍历体素时我们使用贝叶斯更新来更新每个体素的标签概率类似于 [17]。捆绑语义光线投射后每个体素都有一个标签概率向量我们从中提取最可能的标签。度量语义网格最终使用行进立方体 [63] 提取。生成的网格比 II-C 部分的多帧网格精确得多但计算速度较慢≈ 0.1 秒请参见第 III-D 节。6.5. Debugging Tools虽然我们出于篇幅原因限制了讨论但值得一提的是Kimera 还提供了一套开源评估工具用于 VIO、SLAM 和度量语义重建的调试、可视化和基准测试。Kimera包括一个持续集成服务器Jenkins它断言代码的质量编译单元测试但也使用evo [71]自动评估EuRoC数据集上的Kimera-VIO和Kimera-RPGO。此外我们还提供 Jupyter 笔记本来可视化中间 VIO 统计数据例如特征轨迹的质量、IMU 预集成错误以及使用 Open3D [72] 自动评估 3D 重建的质量。7. 实验评估7.1. 位姿估计性能表 II 使用 [77] 中的独立报告值和 [24] 中的独立报告值将 Kimera-VIO 的绝对平移误差 (ATE) 与最先进的开源 VIO 管道进行比较OKVIS [73]、MSCKF [74]、ROVIO [75]、VINS-单声道 [24] 和 SVO-GTSAM [76]。请注意这些算法使用单目相机而我们使用双目相机。在评估误差之前我们使用 SE(3) 变换来对齐估计轨迹和真实轨迹。使用 Sim(3) 对齐如 [77] 所示会给 Kimera 带来更小的误差我们更喜欢 SE(3) 对齐因为它更适合 VIO由于 IMU比例尺是可观测到的。我们根据这些技术是否使用固定滞后平滑、完全平滑和循环闭包来对它们进行分组。Kimera-VIO 和 Kimera-RPGO 在整个频谱中实现了最佳性能。此外Kimera-RPGO确保了强大的性能并且对循环闭包参数调整不太敏感。表III显示了DBoW2中使用的不同环路闭合阈值α有和没有异常值抑制 (PCM) 的PGO精度。较小的α值会导致更多的循环闭包检测但这些检测不太保守更多异常值。表三表明通过使用PCMKimera-RPGO对α的选择相当不敏感。表II中的结果使用α 0.001。7.2. 几何重建我们使用EuRoC V1和V2数据集中提供的地面实况点云来评估Kimera生成的3D网格的质量。我们使用 [78, Sec. 4.3] 中的准确性和完整性指标根据实况评估每个网格(i) 我们通过对均匀密度为 10 的网格进行采样来计算点云3点/米2(ii) 我们使用CloudCompare [80]用ICP[79]记录估计云和地面真实云并且 (iii) 我们评估从真实点云到估计点云中其最近邻居的平均距离精度反之亦然完整性。图 3 (a) 显示了估计的点云对应于V1_01上的Kimera-Semantics的全球网格由到地面实况云中最近点的距离精度进行颜色编码图 3 (b) 显示了真实点云用颜色编码到估计云中最近点的距离完整性。7.3. 语义重建为了评估来自Kimera-Semantics的度量语义重建的准确性我们使用麻省理工学院林肯实验室提供的基于Unity的照片级真实感模拟器该模拟器为场景的几何和语义提供传感器流在ROS中和地面实况并且具有类似于[20], [21]的接口。为了避免将结果偏向于特定的2D语义分割方法我们使用基本事实2D语义分割并向读者推荐[70]以获取潜在的替代方案。Kimera-Semantics根据VIO姿态估计值构建了3D网格并使用密集立体和BA的组合。我们通过运行三个不同的实验来评估这些组件中每个组件的影响。首先我们使用具有真实值 (GT) 姿势和真实深度图在模拟中可用的 Kimera-Semantics 来评估由于BA而导致的初始性能损失。其次我们使用Kimera-VIO的姿势估计。最后我们使用完整的Kimera语义管道包括密集立体声。为了分析语义性能我们计算了平均交并比 (mIoU) [13]以及正确标记的点 (Acc) 的整体部分[81]。我们还报告了ATE以将结果与Kimera-VIO产生的漂移相关联。最后我们评估了度量重建将估计的网格与地面实况进行注册并计算了点的 RMSE如第 III-B 节所示。7.4. 时间安排图5报告了Kimera模块的时序性能。IMU前端需要大约40μs进行预集成因此可以以IMU速率200Hz生成状态估计值。视觉前端模块显示双峰分布因为对于每个帧我们只需执行特征跟踪平均需要4.5ms而在关键帧速率下我们执行特征检测立体匹配和几何验证这些组合平均需要45ms。Kimera网格能够在不到5ms的时间内生成每帧3D网格而构建多帧网格平均需要15ms。后端在不到40ms的时间内求解因子图优化。Kimera-RPGO和Kimera-Semantics在较慢的线程上运行因为它们的输出不需要时间关键型操作例如控制避障。在我们在EuRoC上的实验中Kimera-RPGO平均需要55毫秒但一般来说它的运行时间取决于姿势图的大小。最后Kimera-Semantics为清楚起见图中未报告平均需要0.1秒来更新每个关键帧的全局度量语义网格融合720×480密集深度的图像就像我们的模拟器生成的图像一样。8. 结论Kimera是一个用于度量语义 SLAM 的开源C库。它包括视觉惯性里程测量、稳健的姿态图优化、网格重建和 3D 语义标记的最新实现。它在CPU上实时运行并提供一套持续集成和基准测试工具。我们希望Kimera能够为未来机器人感知研究提供坚实的基础并为社区的研究人员提供易于使用的基础设施。参考文献主要内容来自下文略有增删改【视觉SLAM】Kimera: an Open-Source Library for Real-Time Metric-Semantic Localization and Mapping_振华OPPO的博客-CSDN博客