Kimera: an Open-Source Library for Real-Time Metric-Semantic Localization and Mapping ,2019

Laboratory for In-formation & Decision Systems (LIDS), Massachusetts Institute of Technology, Cambridge

Kimera was partially funded by the DCISTCollaborative Research Alliance,MIT Lincoln Laboratory

代码地址:https://github.com/MIT-SPARK/Kimera

ＲＧＢ, 双目

论文总结和摘要:

Kimera是一个开源的C++实时语义SLAM系统，提供了:

1. 视觉惯性状态估计（VIO模块）

2. 全局一致的相机轨迹估计（姿态图优化器，PGO）

3. 用于快速避障的低延时的局部网格地图（轻量级的３D网格重建模块）

4. 全局语义标注的3D网格地图（稠密的３D语义重建模块）

作者在设计之初使用模块化的思想来设计的SLAM系统，因此Kimera可以单独分解为VIO系统或者完整的SLAM系统.

首先解释下Metric-semantic understanding的概念: 在估计场景的三维几何的同时预测目标和场景结构的语义标签.Kimera 集成了VIO，网格重建、语义理解等多个功能，适用于Metric-semantic understanding的任务

此外,Kimera以RGB 作为输入,运行在CPU上,因此其部署难度相对较小.

论文方法介绍:

Kimera提供了:

1. 精确的状态估计(at IMU rate)
2. 全局轨迹估计
3. 多个环境mesh图(轻量局部mesh以及全局语义mesh)

Kimera使用多线程实现(4线程):

• 线程1: 运行Kimera-VIO前端, 输入Stereo图像和IMU数据, 输出跟踪特征和预集成的IMU测量值,以及IMU的状态估计.
• 线程2: 运行Kimera-VIO后端和Kimera-mesher, 分别输出优化后的状态估计, 以及低延迟的单帧和多帧的mesh图.
• 线程3: 运行Kimera-RPGO, 负责进行闭环检测, 去除噪声离群值, 预测全局一致的轨迹.如图2(a)
• 线程4: 运行Kimera-Semantics, 输入为稠密的stereo以及语义标签, 输出metric-semantic mesh

线程1,2能够得到图2(b)中的单帧网格（若有语义标签则得到图2©的结果），以及图2(d)中的多帧网格

线程3,4以较慢的速度运行, 主要为了支持一些低速的功能, 如导航和规划等.

模块介绍:

Kimera-VIO: Visual-Inertial Odometry Module (视觉惯性里程计模块)

VIO使用论文**《On-manifold preintegration theory for fast and accurate visual-inertial navigation》**中的方法，并扩展到了单目和双目图像数据上。VIO分为前端和后端：

• VIO前端:

  负责处理原始的传感器数据（IMU+vision）

  • IMU前端：执行流形预积分（on-manifold preintegration，详见前述黑体论文），从原始IMU数据中获得两个连续关键帧之间相对状态的紧凑预积分测量。
  • Vision前端：检测 Shi-Tomasi 角点， 使用Lukas-Kanade tracker进行帧间搜索，对双目数据进行匹配，然后几何验证。这里检测角点 、双目匹配以及几何验证都是在关键帧上运行，在中间帧只进行特征追踪。

• VIO后端

  负责融合经过处理的各种测量量，生成传感器的状态预测（位姿，速度，偏移等）

  在每一关键帧，IMU数据和视觉数据会被整合到一个固定滞后的平滑器中, 该平滑器是一个"factor graph",使用 GTSAM 中的iSAM2即可得到。这里引用了很多论文的方法，最后得到了观测特征的3D坐标，去除了视差不足或离群的点，并使用GTSAM将超出平滑范围的状态边缘化。

Kimera-RPGO: Robust Pose Graph Optimization Module

负责检测闭环、计算关键帧位姿。

1. 闭环检测：基于DBoW2 ，并使用词袋模型快速检测闭环。对每一个可能的闭环进行单目和双目验证， 剔除一些离群闭环，将剩余的闭环交由PGO solver 解决。

2. 鲁棒的PGO（pose graph optimizer）：

   该模块在GTSAM（《Factor graphs and GTSAM: A hands-on introduction》）中实现，并使用 增量一致测量集最大化方法（Incremental Consistent Measurement Set Maximization，PCM，《Pairwise consistent measurement set maximization for robust multi-robot map merging》）来剔除异常值。

   实际中分别存储里程数边缘（由Kimera-VIO产生）和闭环（由闭环检测产生）；每次执行PGO时，首先使用修改后的PCM选择最大的一组一致闭环， 然后在姿势图上执行GTSAM，包括里程计和一致的循环闭合。

Kimera-Mesher: 3D Mesh Reconstruction

该mesher模块可以快速生成两个3D mesh: 逐帧的mesh, 以及多帧的mesh

• 逐帧mesh:根据VIO前端生成的当前帧的2D特征, 执行二维的Delaunay 三角测量, 然后将其反投影得到三维mesh. 该mesh被用于低延迟的避障任务, 作者为此设计了根据2D label对mesh进行纹理处理的功能.(“Delaunay三角测量” 见论文《Incremental visual-inertial 3D mesh generation with structural regularities》)

• 多帧mesh:

  融合了逐帧mesh的最终mesh，并对平面进行了规整。mesh网格是以一个顶点位置列表和一个描述三角形面的三个顶点ID列表来编码的。论文中根据列表来将逐帧mesh融合到多帧mesh中。

Kimera-Semantics: Metric-Semantic Segmentation

采用了《“Voxblox: Incremental 3d euclidean signed distance fields for on-board mav planning》中介绍的捆绑式光线投射（bundled raycasting）技术，实现了：建立精确的全局三维网格（覆盖整个轨迹）以及语义标注mesh。

1. 全局mesh：

   首先使用dense stereo 方法（semi-global matching，《Stereo processing by semiglobal matching andmutual information》）从双目数据中得到三维点云。

   然后在每一关键帧上使用bundled raycasting方法得到TSFD，从中使用marching cube算法提取mesh

2. 语义标注：

   使用现成的像素级二维语义分割工具（深度神经网络，或经典的条件随机场方法），可以得到二维语义标签，然后将标签附加到dense stereo图生成的每个三维点上。

   在光线投射（bundled raycasting）时还投射了语义标签，对于光线投射中的每束光线，根据束中观察到的标签的频率建立一个标签概率向量。然后，仅在TSDF截断距离（即接近表面）内传播该信息，以节省计算时间，最后使用贝叶斯更新更新每个体素的标签概率。

   在这个语义光线投射之后，每个体素都有一个标签概率向量，从中选择概率最大的标签。

   最后利用marching cubes算法提取语义mesh，该mesh比Kimera-mesher生成的多帧mesh更加精确，但速度更慢。（> 1.0s）

Debugging Tools：

Kimera提供了一套开源的评估工具，用于VIO、SLAM和度量语义重建的调试、可视化和基准测试。此外，还提供了Jupyter notebook用于可视化中间VIO统计数据（例如，特征轨迹的质量、IMU预集成错误），以及使用Open3D自动评估3D重建的质量。

实验结果和分析:

A. Pose Estimation Performance

结论：SOTA 的状态估计性能，以及对于闭环参数的tuning非常不敏感，更加鲁棒

为了 验证RPGO模块对参数的不敏感性，作者另外设计了一个对比实验，分别使用PCM来拒绝异常值和不使用PCM两组，计算这两组在不同的闭环阈值$\alpha$下的误差：

可见RPGO模块对于不同的闭环阈值几乎没有收到影响。

B. Geometric Reconstruction

两个着色图直观的表示了kimera-semantics模块重建的mesh的精确性和完备性

作者还评估了Kimera-mesher重建的多帧mesh和Kimera-semantics重建的global mesh之间的精确性差别：

注意，虽然Kimera-mesher精度低，但是计算所需的时间要少两个数量级

C. Semantic Reconstruction

Kimera-semantics模块用到了bundled raycasting，pose estimate（from Kimera-VIO）, depth maps

为次作者设计了三个对比实验：

1. 使用pose和depth maps 的真值，评估bundled raycasting的性能损失
2. 使用depth maps的真值，评估bundled raycasting 和pose （from Kimera-VIO）的性能损失
3. 使用bundled raycasting，pose estimate（from Kimera-VIO）, dense stereo，即完整的Kimera系统。
   

结论：

• bundled raycasting 导致了语义和几何重建的很小的性能下降

• Kimera VIO导致了性能损失，但是很小，因为VIO有一个小的漂移

• 最大的性能下降是由于dense stereo的使用，稠密立体[62]很难解决在模拟场景中很常见的无纹理区域的深度，例如墙壁。

混淆矩阵中的大值出现在墙/架和地板/墙之间，这正是密集立体效果最差的地方；没有纹理的墙壁很难重建，而且靠近架子和地板，导致几何和语义错误增加。

D. Timing

视觉前端模块显示为双侧分布，因为对于每一帧，只执行特征跟踪（平均4.5ms），而在关键帧速率下，执行特征检测、立体匹配和几何验证（平均45ms）。Kimera Mesher能够在5 ms内生成每帧3D网格，而构建多帧网格平均需要15 ms。Kimera VIO后端在不到40 ms的时间内解决了因子图优化问题。Kimera-RPGO和Kimera-semantics运行在较慢的线程上，因为它们的输出不需要用于时间关键的操作（例如控制、避障）。Kimera RPGO在EuRoC上的实验平均耗时55ms，但其运行时间通常取决于姿势图的大小。最后，Kimera semantics（为了清晰起见，图中未显示）使用平均1.28秒更新每个关键帧处的全局度量语义网格，融合720×480密集深度图像，作为我们的模拟器生成的图像。此外，使用快速光线投射方法，Kimera-semantics可以在100 ms内运行，无需语义注释。