MIT Kimera论文阅读笔记

论文出处

1.title = {Kimera: an Open-Source Library for Real-Time Metric-Semantic Localization and Mapping}
2.author = {Rosinol, Antoni and Abate, Marcus and Chang, Yun and Carlone, Luca}
3.year = {2020}
4.booktitle = {IEEE Intl. Conf. on Robotics and Automation (ICRA)}
5.url = {https://github.com/MIT-SPARK/Kimera}
6.pdf = {https://arxiv.org/pdf/1910.02490.pdf}

一、Kimera简介

在过去的研究中，几何信息对于机器人安全导航和操纵对象至关重要，而基于深度学习的语义信息提供可以被机器人理解的信息从而执行人类指令。在一开始这两个领域是相互独立的，但最近这两者的交叉领域很受关注，语义SLAM兴起，Kimera也由此诞生。Kimera为实时度量语义的视觉惯性SLAM提供了一个开源的C++库，该库可使用相机图像和惯导数据来构建语义注释的3D环境网格。其中度量语义理解是一种同时估计场景的3D几何图形并将语义标签附加到对象的能力。

二、Kimera创新点与优势

1.模块化结构，这些模块可以单独运行或组合运行，因此Kimera可以轻松退回到VIO或完整的SLAM系统。
2.支持ROS，快速轻量化，在CPU上可运行。
3.通过深度学习方法获得语义标注图像，由此得到度量语义的3D网格。
4.采用最先进的出格点剔除方法，做最小的参数调整可以实现在实际基准数据和逼真模拟数据上鲁棒运行。
5.灵活性好，计算效率高，鲁棒性强，工作环境室内室外均可实现。

三、Kimera的四个组成模块

该库由四个模块组成：
(1)快速准确的视觉-惯导里程计VIO流水线（Kimera-VIO）
(2)基于鲁棒姿势图优化的完整SLAM实现（Kimera-RPGO）
(3)单帧和多帧3D网格生成器（Kimera-Mesher）
(4)语义注释的3D网格生成器（Kimera-Semantics）

A.视觉惯导里程计模块：Kimera-VIO
Kimera-VIO实现了基于关键帧的最大后验视觉惯性估计器。在这个模块里，估计器可以根据指定的时间范围执行完全平滑或固定滞后平滑；这里通常使用后者来限制估计时间。Kimera-VIO包括一个负责处理原始传感器数据的（视觉和惯性）前端，以及一个用于融合处理后的测量值以获得传感器状态（即姿态，速度和传感器偏差）的后端。
1）VIO前端（负责处理原始数据）：
a.IMU前端：执行在线预积分，从原始IMU数据中获得两个连续关键帧之间相对状态的紧凑预积分测量值。
b.视觉前端：检测Shi-Tomasi角，使用Lukas-Kanade跟踪器来跨帧跟踪他们，找到左右立体匹配，然后执行几何验证。包括使用了5点RANSAC执行单目验证，并使用3点RANSAC进行双目验证。该代码还提供了使用IMU旋转并分别使用2点和1点RANSAC执行单目和双目验证的选项。特征检测，立体匹配和几何验证都在每个关键帧处执行，而中间帧只跟踪特征。
2）VIO后端（融合数据得到传感器姿态）：
对于每个关键帧，将预积分的IMU和视觉测量值添加到构成VIO后端的固定滞后平滑器中。这里使用了预积分的IMU模型和无结构视觉模型。使用GTSAM中的iSAM2求解因子图。在每次iSAM2迭代中，无结构视觉模型都使用DLT观察到的特征的3D位置，并从VIO状态中消除相应的3D点。在消除之前，先去除退化点（即相机后方的点或没有足够的视差进行三角测量的点）和离群点（即具有较大重投影误差的点），以提供额外的鲁棒性层。最后，使用GTSAM将超出平滑范围的状态做边缘化处理。

B.基于鲁棒姿势图优化的完整SLAM实现模块：Kimera-RPGO
Kimera-RPGO负责（i）检测当前关键帧与过去关键帧之间的闭环，（ii）使用可靠的PGO计算全局一致的关键帧姿态。
1）闭环检测：
闭环检测依赖于DBoW2库，并使用词袋表示法来快速检测假定的闭环。对于每个推定的闭环，使用单目和立体几何验证拒绝异常闭环，将留下的闭环传递给鲁棒PGO求解器。由于感知混叠（例如，建筑物不同楼层的两个相同房间），最终的闭环仍可能包含异常值。
2）鲁棒PGO：
该模块采用开源软件GTSAM实现，采用了一种出格点剔除方法，即增量一致测量集最大化PCM，其中针对单机和在线设置进行了调整。系统分别存储里程计边（由Kimera-VIO输出）和闭环（由闭环检测产生）。每次执行PGO时，首先使用修改的PCM版选择一致闭环最大的一组，然后在包括里程计和一致闭环的姿态图执行GTSAM。

C.单帧和多帧3D网格生成器模块：Kimera-Mesher
Kimera-Mesher可以快速生成两种类型的3D网格：（i）单帧3D网格（ii）跨VIO固定滞后平滑器中所有关键帧的多帧3D网格。
1）单帧网格：
首先通过当前关键帧成功跟踪的2D特征（由VIO前端生成）做2D Delaunay三角剖分。然后，用来自VIO后端的3D点估计对2D Delaunay三角剖分进行反投影以生成3D网格。虽然单帧网格只提供低延迟障碍物检测，但通过2D标签对网格进行纹理化提供了来对生成网格进行语义标记的选项。
2）多帧网格：
多帧网格将在VIO往后时间段收集的每帧网格融合到单个网格中，并使规则化平面（planar surfaces）。单帧3D网格和多帧3D网格都被编码为顶点位置列表以及顶点ID三元组（triplets）列表来描述三角面。假设在时间t -1处已经有一个多帧网格，对于生成的每个新单帧3D网格（在时间t），遍历其顶点和三元组，并在每个顶点添加单帧网格中存在在多帧网格中丢失的顶点和三元组。然后，遍历多帧网格的顶点并根据最新的VIO后端估计更新其3D位置。最后，删除VIO时间范围之外旧特征相对应的顶点和三元组。结果是最新的跨当前VIO时间范围内所有关键帧的3D网格。如果在网格中检测到平面，则在VIO后端添加规则性因子，可以形成VIO和网格规则化的紧耦合关系。

D.语义注释的3D网格生成器模块：Kimera-Semantics
采用光线投影技术bundled raycasting。
1）构建一个精确的全局的3D网络（覆盖整个轨迹）：
本文的实现基于Voxblox，并使用基于体素TSDF的模型来滤除噪声并提取全局3D网格。在每个关键帧使用密集立体图像（SGM视差估计）从当前立体图像对获取3D点云。然后，使用Voxblox的快速集束光线投射选项。在每个关键帧重复此过程，并生成TSDF，然后使用marching cubes方法提取网格。
3）利用语义标注网格：
Kimera-Semantics使用2D语义标注的图像（在每个关键帧处生成）语义标注全局网格；可以用像素级2D语义分割的现成工具获得2D语义标签，例如，深度神经网络或经典MRF的方法。为此，在集束光线投射期间，传播语义标签。2D语义分割将标签附加到致密立体视觉产生的每个3D点，对于每个射线束（bundle of rays）从束中观察到的标签频率构建标签概率向量。然后，仅在TSDF截断距离（即靠近表面）内，沿射线传播此信息，以节省计算量。换句话说，不花精力来更新“空”标签概率。沿着射线遍历所有体素时，用贝叶斯更新（Bayesian update）方法来刷新每个体素的标签概率。经过集束光线投射之后，每个体素都有一个标签概率向量，从中可以提取最可能的标签。最后使用marching cubes方法提取度量语义网格。生成的网格比多帧网格精确得多，但计算速度较慢（> 1s）。

以下为图示：（a）以IMU帧率进行视觉惯导状态估计以及全局一致且对出格点鲁棒的轨迹估计，（b）可用于快速避障的低时延局部场景3D网格，（c）全局语义注释的3D网格，（d）地面真实模型。

四、Kimera的四个线程

Kimera将立体图像帧和高速惯导测量值作为输入并返回：
1)以IMU帧率进行的高精状态估计
2)全局一致的轨迹估计
3)多个环境网格，包括快速局部网格和语义注释的全局网格。
下面是系统框图：
Kimera高度并行化，用四个线程以不同帧率接收输入和输出（例如IMU、图像帧和关键帧）。
a.第一个线程：Kimera-VIO前端，该前端可获取立体图像和IMU数据，并输出特征轨迹和预先积分的IMU测量值。前端还输出IMU帧率的状态估计值。
b.第二个线程：包括（i）Kimera-VIO后端输出优化的状态估计，（ii）Kimera-Mesher计算低延迟单帧（<20ms）和多帧的3D网格。这两个线程允许创建单帧网格（也可以带语义标签）以及多帧网格。
c.第三个线程：Kimera-RPGO，一种鲁棒的姿态图优化（PGO）实现，可检测闭环、剔除出格点并估计全局一致轨迹。
d.第四个线程：Kimera-Semantics，它基于Kimera-VIO的姿势估计，输入的致密立体图像和2D语义标签，获得细化的度量语义网格。
其中最后两个线程的运行速度较慢，旨在支持低频运转的功能，例如路径规划。

五、Kimera的性能评估

A.从下表中得出，在EuRoC数据集上，Kimera拥有性能最好的VIO通道。
在实验中主要比较的是均方根误差，根据它们是否使用固定滞后平滑、完全平滑和闭环来对技术进行分组。可以看出Kimera各个领域都取得了优异的表现，实现了一流的状态估计性能，同时鲁棒的PGO减轻用户耗时的参数调整。

B.展示了Kimera在EuRoC上的3D网格重建，使用场景子集提供了一个基本的点云。
(a)显示出了估计的云(对应于V1 01上的Kimera-Semantics的全局网格)，通过到地面真实云中最近点的距离进行颜色编码(准确性)；
(b)显示了地面真实云，用到估计云中最近点的距离(完整性)进行了颜色编码。
通过颜色分布可以看出3D重建的准确性和完整性都非常高。