MIT最近推出的：Kimera-实时度量语义SLAM开源系统
arXiv上在2019年10月6日MIT上传论文“Kimera: an Open-Source Library for Real-Time Metric-Semantic Localization and Mapping“（12月19日上传v2版），介绍这个开源的SLAMC++代码库，链接如下：
Kimera用于实时的度量-语义SLAM，该库使用相机图像和惯导数据来构建语义注释的3D环境网格。和现有的视觉SLAM和视觉-惯导SLAM库（例如ORB-SLAM，VINS-Mono，OKVIS，ROVIO）比较，Kimera是模块化结构，支持ROS，在CPU上运行。Kimera包含四个模块：
- 快速准确的视觉-惯导里程计VIO流水线（Kimera-VIO）
- 基于鲁棒姿势图优化的完整SLAM实现（Kimera-RPGO）
- 单帧和多帧3D网格生成器（Kimera-Mesher）
- 语义注释的3D网格生成器（Kimera-Semantics）
如上图，（a）以IMU帧率进行视觉惯导状态估计以及全局一致且对出格点鲁棒的轨迹估计，（b）可用于快速避障的低时延局部场景3D网格，（c）全局语义注释的3D网格，（d）地面真实模型。
这些模块可以单独运行或组合运行，因此Kimera可以轻松退回到VIO或完整的SLAM系统。 Kimera通过深度学习方法获得语义标注图像，并由此得到度量语义的3D网格。
如图是系统框图：
尽管现有工作集中在RGB-D感知数据上，但Kimera使用视觉RGB和惯导感知数据，工作环境可以在室内和室外。 尽管相关方法需要使用GPU进行3D映射，但Kimera提供基于CPU的快速、轻巧和可扩展的解决方案。 Kimera采用最先进的出格点剔除方法，做最小的参数调整可以实现在实际基准数据和逼真模拟数据上鲁棒运行。
Kimera将立体图像帧和高速惯导测量值作为输入并返回（i）以IMU帧率进行的高精状态估计，（ii）全局一致的轨迹估计，（iii）多个环境网格，包括快速局部网格和语义注释的全局网格。 Kimera高度并行化，用四个线程以不同帧率接收输入和输出（例如IMU、图像帧和关键帧）。
第一个线程包括Kimera-VIO前端，该前端可获取立体图像和IMU数据，并输出特征轨迹和预先积分的IMU测量值。前端还发布IMU帧率的状态估计值。
第二个线程包括（i）Kimera-VIO后端输出优化的状态估计，以及（ii）Kimera-Mesher计算低延迟单帧（<20ms）和多帧的3D网格。这两个线程允许创建单帧网格（也可以带语义标签）以及多帧网格。
最后两个线程的运行速度较慢，旨在支持低频运转的功能，例如规划。第三个线程包括Kimera-RPG，一种可靠的姿态图优化（PGO）实现，可检测闭环、剔除出格点并估计全局一致轨迹。最后一个线程包括Kimera-Semantics，它基于Kimera-VIO的姿势估计，输入的致密立体图像和2D语义标签，获得细化的度量语义网格。
Kimera-VIO实现了基于关键帧的视觉惯导后验最大估计器。 在实现中，估计器可以根据指定的时间范围执行完全平滑或固定滞后平滑。 通常使用后者，因为它可以确保限制的估计时间。 它还扩展系统以便与单目和立体双目框架一起使用。 Kimera-VIO包括一个负责处理原始传感器数据（视觉和惯导）的前端，以及一个用于融合处理过的测量值获得传感器状态（即姿态、 速度和传感器偏差）的后端。
1）VIO前端：IMU前端执行在线预先积分，从原始IMU数据中获得两个连续关键帧之间相对状态的简洁预积分测量值。视觉前端检测Shi-Tomasi角点，使用Lukas-Kanade跟踪器跨帧跟踪它们，找到左右立体匹配，并执行几何验证。用5点RANSAC进行单目验证和3点RANSAC进行立体视觉验证。该代码还提供了使用IMU旋转并分别使用2点和1点RANSAC执行单目和立体视觉验证选项。特征检测、立体匹配和几何验证在每个关键帧处执行，而只在中间帧处跟踪特征。
2）VIO后端：在每个关键帧处，将预积分的IMU和视觉测量值添加到构成VIO后端的固定滞后平滑器（一个因子图）中。用预积分IMU模型和无结构的视觉模型。用开源代码GTSAM （Georgia Tech Smoothing and Mapping，https://gtsam.org/）中的iSAM2求解因子图。在每次iSAM2迭代中，无结构视觉模型都用DLT方法估计观察特征的3D位置，并从VIO状态中消除相应的3D点。在消除它们之前，先去除退化点（degenerate points ），即相机后方或没有足够的视差进行三角测量的点），和出格点，即具有较大重投影误差的点，从而提供一个额外的鲁棒层。最后，用GTSAM将超出整个平滑范围的状态做边缘化（marginalized out）处理。
Kimera-RPGO负责（i）检测当前关键帧与过去关键帧之间闭环，以及（ii）使用可靠的PGO计算全局一致的关键帧姿态。
- 1）闭环检测：闭环检测依赖于DBoW2库，并使用词袋（bag-of-word）表示法来快速检测假定的闭环。对于每个推定的闭环，使用单目和立体几何验证拒绝异常闭环，将留下的闭环传递给鲁棒PGO求解器。由于感知混叠（例如，建筑物不同楼层的两个相同房间），最终的闭环仍可能包含异常值。
- 2）鲁棒PGO：该模块采用开源软件GTSAM实现，采用了一种出格点剔除方法，即增量一致测量集最大化（Incremental Consistent Measurement Set Maximization，PCM），其中针对单机和在线设置进行了调整。系统分别存储里程计边（由Kimera-VIO输出）和闭环（由闭环检测产生）。每次执行PGO时，首先使用修改的PCM版选择一致闭环最大的一组，然后在包括里程计和一致闭环的姿态图执行GTSAM。
Kimera-Mesher可以快速生成两种类型的3D网格：（i）单帧3D网格，以及（ii）跨VIO固定滞后平滑器中所有关键帧的多帧3D网格。
- 1）单帧网格：首先通过当前关键帧成功跟踪的2D特征（由VIO前端生成）做2D Delaunay三角剖分。然后，用来自VIO后端的3D点估计对2D Delaunay三角剖分进行反投影以生成3D网格。虽然单帧网格只提供低延迟障碍物检测，但通过2D标签对网格进行纹理化提供了来对生成网格进行语义标记的选项。
- 2）多帧网格：多帧网格将在VIO往后时间段（receding horizon）收集的每帧网格融合到单个网格中，并使规则化平面（planar surfaces）。单帧3D网格和多帧3D网格都被编码为顶点位置列表以及顶点ID三元组（triplets）列表来描述三角面。假设在时间t − 1处已经有一个多帧网格，对于生成的每个新单帧3D网格（在时间t），遍历其顶点和三元组，并在每个顶点添加单帧网格中存在在多帧网格中丢失的顶点和三元组。然后，遍历多帧网格的顶点并根据最新的VIO后端估计更新其3D位置。最后，删除VIO时间范围之外旧特征相对应的顶点和三元组。结果是最新的跨当前VIO时间范围内所有关键帧的3D网格。如果在网格中检测到平面，则在VIO后端添加规则性因子，这带来VIO和网格规则化的紧耦合关系。
量度语义分割中，将集束光线投射（bundled raycasting）技术应用于（i）建立准确的全局3D网格（覆盖整个轨迹），以及（ii）在语义上对网格进行注释。
- 1）全局网格：实现基于Voxblox（https://github.com/ethz-asl/voxblox），并使用基于体素（TSDF， truncated signed distance function）的模型来滤除噪声并提取全局3D网格。在每个关键帧使用密集立体图像（SGM视差估计）从当前立体图像对获取3D点云。然后，使用Voxblox的快速集束光线投射选项。在每个关键帧重复此过程，并生成TSDF，然后使用marching cubes方法提取网格。
- 2）语义标注：Kimera-Semantics使用2D语义标注的图像（在每个关键帧处生成）语义标注全局网格；可以用像素级2D语义分割的现成工具获得2D语义标签，例如，深度神经网络或经典MRF的方法。为此，在集束光线投射期间，传播语义标签。2D语义分割将标签附加到致密立体视觉产生的每个3D点，对于每个射线束（bundle of rays）从束中观察到的标签频率构建标签概率向量。然后，仅在TSDF截断距离（即靠近表面）内，沿射线传播此信息，以节省计算量。换句话说，不花精力来更新“空”标签概率。沿着射线遍历所有体素时，用贝叶斯更新（Bayesian update）方法来刷新每个体素的标签概率。经过集束光线投射之后，每个体素都有一个标签概率向量，从中可以提取最可能的标签。最后使用marching cubes方法提取度量语义网格。生成的网格比多帧网格精确得多，但计算速度较慢（> 1.0s）。
值得一提的是，Kimera还提供了一套开源评估工具套件，用于VIO、SLAM和度量语义重建的调试、可视化和基准测试。 Kimera包含一个持续集成服务器（Continuous Integration server），该服务器可以断言代码的质量（编译和单元测试），还可以使用开源评估Python软件包evo（https://github.com/MichaelGrupp/evo）在EuRoC数据集上自动评估Kimera-VIO和Kimera-RPGO。 此外，提供Jupyter Notebooks来可视化中间VIO统计信息（例如，特征轨迹的质量和IMU预积分的错误），使用开源Open3D（http://www.open3d.org/）自动评估3D重建的质量。
