基于多模态路标跟踪的紧耦合激光—视觉—惯导里程计（ICRA2021）

Unified Multi-Modal Landmark Tracking for Tightly Coupled Lidar-Visual-Inertial Odometry

来源：Wisth D, Camurri M, Das S, et al. Unified Multi-Modal Landmark Tracking for Tightly Coupled Lidar-Visual-Inertial Odometry[J]. ICRA 2021单位：Oxford University、KTH Royal Institute of Technology, Sweden、Scania AB,Sweden

作者：Maple|来源微信公众号：3D视觉工坊

针对问题：

面向复杂场景的多传感器融合里程计

提出方法：

系统构建了一个联合视觉、激光雷达和惯性信息进行优化的因子图，将视觉提取的点特征、激光雷达提取的线、面特征以及IMU预积分构建的残差放到同一个因子图中进行优化。达到效果：针对激光雷达与视觉退化场景实现自然切换，轻量级基元信息的建模及跟踪使得系统能够在单个CPU上实时运行，在Newer College Dataset、DARPA SubT Challenge（Urban）数据集上进行了系统性能评估，典型场景下与最先进的系统性能相当，极端情况下表现出更强的鲁棒性。

Abstract

我们提出了一个用于移动平台的高效的多传感器融合里程计，该系统构建了一个联合视觉、激光雷达和惯性信息进行优化的因子图。这在全帧率下使用固定滞后平滑的方式实时运行。为了进行此紧耦合系统，提出了一种从激光雷达点云中提取三维线和平面基元的新方法。这种方法克服了典型的帧对帧跟踪方法的次优性，将基元视为路标，并在多次扫描中跟踪它们。利用激光雷达和相机帧的被动同步，使激光雷达特征与标准视觉特征和IMU的真正整合成为可能。三维特征的轻量级建模允许系统能够在单个CPU上实时运行。我们提出的系统在各种平台和场景中进行了测试，包括足型机器人进行地下探索和用动态移动的手持设备进行户外扫描，总时长为96分钟，行走距离为2.4公里。在这些测试序列中，只使用一个外感觉传感器会导致失败，原因是几何形状限制不足（影响激光雷达），或者是由激进的光照变化造成的无纹理区域（影响视觉）。在这些条件下，我们的因子图自然地使用了每个传感器模式的最佳信息，而没有任何硬性的开关切换。

Motivation

与融合IMU、激光雷达和相机关联的两个主要挑战是。1）在移动平台有限的计算资源下实现实时性能；2）以不同的频率和采集方法运行的三种信号的适当同步。以前的工作通过采用松耦合的方法或通过运行两个独立的系统（一个用于激光雷达-惯性，另一个用于视觉惯性里程计）来解决这两个问题。相反，我们的动机是通过以下方式来解决这些问题。1）提取和跟踪稀疏的轻量级基元；2）开发一个连贯的因子图，利用IMU预积分，将动态的点云转换为附近相机帧的时间戳。前者避免了匹配整个点云（如ICP）或跟踪数百个特征点（如LOAM）。后者使所有传感器的实时平滑耦合成为可能。

Contribution

一种新的因素图表述，将视觉、激光雷达和IMU测量紧耦合在一个统一的优化过程中。一种提取激光雷达特征的有效方法，然后将其优化为路标。激光雷达和视觉特征都有一个统一的表示，因为路标都被当作n维的参数流形（即点、线和平面）。这种紧凑的表示方法使我们能够以额定的帧率处理所有的激光扫描信息。在一系列场景中进行了广泛的实验评估，与更多的典型方法相比，这些方法在个别传感器失效时表现出卓越的鲁棒性。此工作建立我们前期提出的VILENS基础上，相较于VILENS，本系统增加了激光雷达特征跟踪和激光雷达辅助的视觉跟踪。相机和激光雷达的融合使其能够在运动剧烈的便携式设备上使用，并且它可以自然地应对由于缺乏激光雷达或视觉特征的退化场景。

问题描述

我们的目标是估计一个装配有单目或双目相机、激光雷达、IMU的移动平台的位置、方向和线速度（在我们的实验中，是一个足型机器人或一个手持设备）。相关的参考系如下图所示，包括机器人固定的基座坐标系B、左侧相机坐标系C、IMU坐标系I和激光雷达坐标系L。系统估计的是基座坐标系相对于固定世界坐标系W的位置。

状态定义

ti时刻的状态向量定义如下：

其中：Ri是方向，pi是位置，vi是线速度，最后两个元素IMU陀螺仪和加速度计的偏置。

其中Xk、Mk、Pk、Lk是在固定滑窗内跟踪的所有状态和路标。

MAP建模

通过最小二乘法进行求解Zk观测下Xk的最大后验概率：

其中Iij是ti和tj之间的IMU测量值，Kk是tk之前的所有关键帧的索引。上式中的残差定义为：IMU、激光雷达平面和线特征、视觉路标和状态先验残差。

因子图模型

系统所构建的因子图如上所示，其中包含IMU因子、视觉-激光雷达路标因子、平面和直线路标因子。

IMU预积分因子

IMU预积分残差的形式为：

通过Lidar恢复深度的单目路标因子

为了充分使视觉和激光雷达多模态传感器进行融合，我们跟踪单目视觉特征，但使用激光雷达的重叠视场来为特征提供深度。

由于激光雷达和相机的分辨率不同，当我们无法将激光雷达的深度与视觉特征进行关联时，或者由于动态障碍物或噪声，使得两帧之间的深度变化>0.5米的不稳定情况时，使用立体匹配结果。

双目路标因子

路标m`在状态xi处的残差是：

平面路标因子

我们使用单位法线∈和其与原点距离的标量d参数化一个平面p：

为将齐次变换T应用于平面p的所有点的算子，定义两个平面(pi, pj )之间误差的算子为：

其中Bp∈是的切线空间，定义如下:

当一个平面在ti时刻被观测到时，相应的残差是和转化为与估计平面之间的差异：

直线路标因子

两条线li,lj之间的误差算子定义为：

测量线和其预测之间的残差定义如下：

Implementation

系统结构如下图所示。使用四个并行线程进行传感器处理和优化，系统以相机关键帧频率输出因子图估计的状态，以IMU频率输出IMU前向传播的状态，分别用于导航/制图和控制。

因子图使用iSAM2进行求解，在这些实验中，我们使用5到10秒的滞后时间。所有的视觉和激光雷达因素都使用DCS鲁棒核函数进行处理，以减少外点影响。

视觉特征跟踪

我们使用FAST角检测器检测特征，并使用KLT特征跟踪器在连续的帧之间跟踪它们，使用RANSAC剔除异常值。由于采用了并行结构和增量优化，每隔一帧都被用作关键帧，系统实现了15赫兹的额定输出。

LiDAR特征提取及跟踪

我们算法的一个主要特点是，我们从激光雷达点云中提取特征基元，与相机帧同时表示，这样可以一次对所有传感器进行优化。处理流程包括以下步骤：点云去畸变、同步、过滤、基元提取和跟踪，以及因子创建。

运动畸变补偿及同步

上图比较了我们传感器的不同输出频率。当IMU和相机数据被瞬间捕获时，激光雷达继续进行数据采集，一旦完成一个完整的旋转，累积的激光回波被转换成点云，然后立即开始新的扫描。由于激光回波是在移动过程中捕获的，需要通过一个运动先验进行点云运动去畸变，并与唯一的任意时间戳相关联--通常是扫描的开始，这种方法意味着照相机和激光雷达的测量有不同的时间戳。我们选择去畸变的激光雷达观测结果定为最接近相机时间戳的值，一般情况下，激光去畸变时以L2时刻为参照，但是这样会导致激光帧与图像获取时刻不同，因此我们选择与C3最邻近的点云时间为基准进行去畸变，考虑到来自IMU模块的前向传播的状态，运动先验使用与点云的每个点相关的时间戳进行线性插值(为简单起见，我们避免高斯过程插值或用时间偏移进行状态增广)。由于点云现在与C3相关联，激光雷达路标被关联到与C3相同的节点，而不是创建一个新节点。这个细节处理不仅保证了在图的优化中加入一致数量的新节点和因子，而且还保证了优化是在IMU、照相机和激光雷达输入之间联合进行的。这也确保了一个固定的输出频率，即相机帧率或激光雷达帧率，但不是两者的混合。

特征滤波

一旦点云去畸变完成，我们就对其进行分割，将点分成几个群组。少于5个点的聚类被标记为离群值并被丢弃，因为它们很可能是噪声。然后，计算滤波后点云中每个点的局部曲率。具有最低和最高曲率的点分别被分配到平面候选点CP和直线候选点CL的集合中。分割和基于曲率的过滤通常会将点云中的点的数量减少90%，显著降低了平面和直线处理的耗时。

平面和直线特征提取及跟踪

如果两条线l i和l j的方向和它们的中心距离都小于阈值，则被认为关联成功。

在我们的场景中，α p = α l = 0.35 rad, β p = β l = 0.5 m。一旦一个特征被追踪到，该特征的离群值就会从相应的候选集合中移除，然后对剩余的路标重复这一过程。追踪完成后，我们在剩余的候选云中检测新路标。点云首先用欧氏聚类法对线进行划分，对平面进行基于法线的区域增长。然后，我们使用与路标跟踪相同的方法在每个聚类中检测新的路标。点云特征只有在被追踪了最低连续扫描次数后才会被纳入优化。

零速度状态因子

为了限制平台静止时的漂移，我们在相机、激光雷达、IMU中的两种模式的报告没有运动时，给因子图添加零速度约束。

EXPERIMENTAL RESULTS

我们的系统在Newer College Dataset、DARPA SubT Challenge（Urban）数据集上进行了测试：

Results

下表展示了算法在10米距离内的平均相对位姿误差（RPE）。

VILENS-LI表示只带IMU和激光雷达的VILENS；VILENS-LVI表示带有IMU、视觉（带激光雷达深度）和激光雷达特征的VILENS；应该注意的是，数据集没有任何闭环，而且与LOAM和LeGO-LOAM方法相比，我们的系统没有进行建图。对于SubT数据集中，VILENS-LVI在平移/旋转方面比LOAM平均高出38%/21%，VILENS-LI则高出46%/21%。下图描述了ST-A数据集上的估计轨迹和地面真实轨迹。VILENS-LVI能够在没有建图系统或闭环的条件下实现非常低的漂移率。

对于动态因素最小的NC数据集NC-1，VILENS-LVI取得了与LeGO-LOAM相当的性能。然而，对于动态较高的数据集（高达3.37rad/s），即NC-2和NC-3，VILENS方法的表现明显优于LeGO-LOAM。这一性能的关键是激光雷达云与相机时间戳的不失真，视觉特征深度来自激光雷达，同时最大限度地减少计算量。总的来说，表现最好的算法是VILENS-LVI，显示了视觉和激光雷达特征的紧耦合使我们能够有效避免仅在激光雷达-惯性方法的缺点。

Multi-Sensor Fusion

虽然很大一部分数据集为激光雷达和视觉特征跟踪提供了有利条件，但在一些情况下，紧耦合的系统展示了更加鲁棒的性能。如下图显示了NC-2的一个例子，当从明亮的阳光下移到阴暗处时，照相机的自动曝光功能需要3秒的时间来调整。在这段时间里，视觉特征的数量从大约30个下降到不到5个（都集中在图像的一个角落里）。这将严重影响状态估计的鲁棒性，通过与激光雷达的紧耦合，我们能够使用少量的视觉特征和激光雷达特征，而不会造成任何性能的下降。这与使用单独的视觉-惯性和激光雷达-惯性子系统形成鲜明对比，他们的做法意味着必须明确处理退化情况。

同样，在激光雷达路标不足以完全约束估计的情况下（或接近退化），视觉特征的紧耦合使优化能够利用激光雷达的约束，同时避免退化的问题。

Analysis

在优化中使用轻量级基元的一个主要好处是提高了效率。上述数据集的平均计算时间为：视觉特征跟踪10ms，点云特征跟踪50ms，消费级笔记本电脑的优化耗时为20ms。这使得该系统在仅使用激光雷达-惯性测量时能以10HZ（激光雷达帧率）输出，而在融合视觉、激光雷达和惯性测量时能以15HZ（相机关键帧率）输出。

CONCLUSION

我们提出了一种紧耦合相机、激光雷达和IMU测量的因子图模型。这种融合能够优雅地处理传感器退化情况——在仅有激光雷达的特征跟踪和视觉跟踪(有激光雷达深度)之间进行融合，这取决于每种模式在特定环境中所能提供的约束。我们已经证明了在典型条件下与最先进的激光雷达-惯性里程计的性能相当，而在极端条件下，如剧烈的运动或突然的光线变化情况下性能更好。我们的方法还提出了一种在同一因子图中联合优化激光雷达和视觉特征的新方法，这使得在极端的环境中，如长走廊和黑暗的环境下仍然可以进行鲁棒的估计。备注：作者也是我们「3D视觉从入门到精通」特邀嘉宾：一个超干货的3D视觉学习社区本文仅做学术分享，如有侵权，请联系删文。

3.彻底搞透视觉三维重建：原理剖析、代码讲解、及优化改进

4.国内首个面向工业级实战的点云处理课程

5.激光-视觉-IMU-GPS融合SLAM算法梳理和代码讲解

6.彻底搞懂视觉-惯性SLAM：基于VINS-Fusion正式开课啦

7.彻底搞懂基于LOAM框架的3D激光SLAM: 源码剖析到算法优化

8.彻底剖析室内、室外激光SLAM关键算法原理、代码和实战(cartographer+LOAM +LIO-SAM)

9.从零搭建一套结构光3D重建系统[理论+源码+实践]

10.单目深度估计方法：算法梳理与代码实现