港大FAST-LIVO2强势登场：快速、直接的LiDAR-惯性-视觉里程计

作者 | Fu Zhang  编辑 | 3D视觉之心

点击下方卡片，关注“自动驾驶之心”公众号

戳我-> 领取自动驾驶近15个方向学习路线

>>点击进入→自动驾驶之心『SLAM』技术交流群

本文只做学术分享，如有侵权，联系删文

多传感器使用的挑战

视觉SLAM利用成本较低的CMOS传感器和镜头，能够建立精确的数据关联，从而实现一定水平的定位精度。丰富的颜色信息进一步增强了语义感知，通过利用这种增强的场景理解，深度学习方法能够用于提取稳健的特征以及过滤动态物体。视觉SLAM的局限性：

• 缺乏直接的深度测量，需要通过如三角测量或深度过滤等操作来同时优化地图点，这引入了显著的计算开销，往往限制了地图的精度和密度

• 测量噪声在不同尺度下的变化

• 对光照变化的敏感性

• 无纹理环境对数据关联的影响

LiDAR SLAM使用LiDAR传感器直接获取精确的深度测量，与视觉SLAM相比，LiDAR SLAM在定位和建图任务中表现出更高的精度和效率。尽管如此，LiDAR SLAM也存在显著的缺点：

• 重建的点云地图虽然详细，但缺乏颜色信息，从而减少了它们所包含的信息量

• 在几何约束不足的环境中（如狭窄的隧道或单一且延伸的墙面），LiDAR SLAM的性能往往会大幅下降

高效且准确的LiDAR-惯性-视觉里程计（LIVO）和建图仍然是一个具有挑战性的问题：

1. 整个LIVO系统需要处理LiDAR测量数据（每秒数百到数千个点）以及高频率、高分辨率的图像。为了充分利用这些大量数据，特别是在有限的机载资源下，需要极高的计算效率

2. 许多现有系统通常采用LiDAR-惯性里程计（LIO）子系统和视觉-惯性里程计（VIO）子系统，各自需要从视觉和LiDAR数据中提取特征以减少计算负荷。然而，在缺乏结构或纹理的环境中，这一特征提取过程往往导致特征点的不足。此外，为了优化特征提取，还需要进行广泛的工程调整，以适应LiDAR扫描模式和点密度的变化；

3. 为了减少计算需求并实现摄像头和LiDAR测量的更紧密集成，需要一个统一的地图来同时管理稀疏点和观察到的高分辨率图像测量。然而，设计和维护这样一个地图是一个特别具有挑战性的任务，考虑到LiDAR和摄像头的异构测量数据

4. 为了确保重建的彩色点云的准确性，姿态估计需要达到像素级的精度。实现这一标准带来了相当大的挑战：合适的硬件同步、LiDAR和摄像头之间外部参数的严格预校准、曝光时间的精确恢复，以及能够在实时中达到像素级精度的融合策略。

本文介绍的FAST-LIVO2是一种高效的LIVO系统，紧密集成了LiDAR、图像和IMU测量，通过顺序更新的误差状态迭代卡尔曼滤波器（ESIKF）：

• 利用IMU传播的先验，系统状态首先通过LiDAR测量更新，然后通过图像测量更新，两个更新过程都利用基于单一统一体素地图的直接方法。

• 在LiDAR更新过程中，系统将原始点注册到地图上以构建和更新几何结构；在视觉更新过程中，系统直接将LiDAR地图点重新利用为视觉地图点，而无需从图像中提取、三角化或优化任何视觉特征。

• 地图中的选定视觉地图点附有先前观察到的参考图像补丁，然后投影到当前图像中，通过最小化直接光度误差来对齐姿态（即稀疏图像对齐）。

• 为了提高图像对齐的准确性，FAST-LIVO2动态更新参考补丁，并使用LiDAR点提供的平面先验。

• 为了提高计算效率，FAST-LIVO2使用LiDAR点来识别当前图像中可见的视觉地图点，并在没有LiDAR点的情况下按需进行体素光线投射。

• FAST-LIVO2还实时估算曝光时间，以应对光照变化。

整体系统

图2展示了系统流程，系统包含四个部分：ESIKF、局部建图、LiDAR测量模型和视觉测量模型。

异步采样的LiDAR点首先通过扫描重组，在摄像头采样时间点上重新组合成扫描数据。然后，通过顺序更新的ESIKF紧密结合LiDAR、图像和惯性测量（IMU）。在此过程中，系统状态首先通过LiDAR测量进行更新，然后通过图像测量进行更新，这两个过程都基于单一的统一体素地图并采用直接方法。为了构建ESIKF更新中的LiDAR测量模型，计算了帧到地图的点到平面残差。为了建立视觉测量模型，从地图中提取当前视野中的视觉地图点，利用可见体素查询和按需光线投射；在提取之后，识别并剔除视觉地图点中的离群点（例如被遮挡的点或深度不连续的点）；然后计算帧到地图的图像光度误差以进行视觉更新。

用于视觉和LiDAR更新的局部地图是一个体素地图结构：LiDAR点用于构建和更新地图的几何结构，而视觉图像将图像补丁附加到选定的地图点（即视觉地图点）上，并动态更新参考补丁。更新后的参考补丁会在独立线程中进一步细化其法线向量。

具有顺序状态更新的误差状态迭代卡尔曼滤波器

符号和状态转移模型

系统假设三个传感器（LiDAR、IMU和摄像头）之间的时间偏移是已知的，并且可以通过校准或提前同步来获得。将IMU坐标系（记作I）作为机体坐标系，并将第一个机体坐标系作为全局坐标系（记作G）。此外，假设这三个传感器是刚性连接的，且其外参已经预先校准。然后，第i个IMU测量时刻的离散状态转移模型为：

其中， 是IMU的采样周期，状态 、输入 、过程噪声  和函数  定义如下：

其中，、 和  分别表示IMU在全局坐标系中的姿态、位置和速度， 是全局坐标系中的重力向量， 是相对于第一个帧的逆曝光时间， 是建模为随机游走的高斯噪声， 和  是IMU的原始测量值， 和  分别是  和  中的测量噪声， 和  是IMU的偏置，建模为由高斯噪声驱动的随机游走。

扫描重组

通过扫描重组，将高频、顺序采样的LiDAR原始点云重新组合成与摄像头采样时刻同步的扫描数据，如图3所示。这确保了摄像头和LiDAR数据以相同的频率（例如10 Hz）进行同步，从而允许在同一时间点更新状态。

传播

在ESIKF框架中，状态和协方差从时间 （接收到上一个LiDAR扫描和图像帧的时刻）传播到时间 （接收到当前LiDAR扫描和图像帧的时刻）。这种前向传播通过设置过程噪声  为零，来预测每个IMU输入  在  和  之间的状态。将传播状态记作  和协方差记作 ，它们将作为随后的更新的先验分布。此外，为了补偿运动畸变进行反向传播，确保点在LiDAR扫描结束时刻  被“测量”。

顺序更新

IMU传播得到的状态  和协方差  提供了时间点  系统状态  的先验分布：

将上述先验分布记作 ，并将LiDAR和摄像头的测量模型表示为：

其中， 和  分别表示LiDAR和摄像头的测量噪声。

标准的ESIKF将使用所有当前测量数据（包括LiDAR测量  和图像测量 ）来更新状态 。然而，LiDAR和图像测量是两种不同的感知方式，它们的数据维度不匹配。此外，图像测量的融合可以在图像金字塔的各个层次上进行。为了处理维度不匹配并为每个模块提供更多灵活性，提出了一种顺序更新策略。该策略理论上等同于使用所有测量值的标准更新，前提是给定状态向量  的LiDAR测量  和图像测量  是统计独立的（即测量值被统计独立的噪声污染）。

为了引入顺序更新，将当前状态  的总条件分布重写为：

公式（5）表明，总条件分布  可以通过两次顺序贝叶斯更新得到。第一步仅融合LiDAR测量  与IMU传播的先验分布  以获得分布 ：

第二步然后融合摄像头测量  与  以获得最终的状态后验分布：

有趣的是，两个融合步骤（6）和（7）遵循相同的形式：

为了进行公式（8）中的融合，无论是LiDAR还是图像测量，详细介绍先验分布  和测量模型  如下。对于先验分布 ，将其表示为  且 。在LiDAR更新的情况下（即第一步）， 和  是从传播步骤中获得的状态和协方差。在视觉更新的情况下（即第二步）， 和  是从LiDAR更新中收敛得到的状态和协方差。

为了得到测量模型分布 ，将状态在第κ次迭代中的估计表示为 κ，其中 。通过在 κ 处对测量模型（4）（无论是LiDAR还是摄像头测量）进行一阶泰勒展开，得到：

其中 κκ， κ 是残差，κ 是集成的测量噪声，κ 和 κ 分别是 κκ 对 κ 和  的雅可比矩阵，分别在零处求值。

然后，将先验分布  和测量分布  代入后验分布（8），并执行最大似然估计（MLE），可以通过ESIKF框架中的标准更新步骤获得δ κ（从而获得 κ）的最大后验估计（MAP）：

收敛的状态和协方差矩阵然后作为后验分布  的均值和协方差。

ESIKF的顺序更新的具体实现详见算法1。在第一步（第6-10行），通过迭代从LiDAR测量中更新误差状态，直到收敛。收敛的状态和协方差估计，再次记作  和 ，用于更新地图的几何结构，然后在第二步视觉更新中（第13-23行）在图像金字塔的每个层次上进一步细化，直到收敛。优化后的状态和协方差，分别记作  和 ，用于传播后续IMU测量并更新地图的视觉结构。

局部建图

地图结构

地图采用了自适应体素结构，该结构由哈希表和每个哈希条目的八叉树组成（见图2）。哈希表管理根体素，每个根体素的固定尺寸为0.5 × 0.5 × 0.5米。每个根体素封装了一个八叉树结构，用于进一步组织不同大小的叶体素。一个叶体素表示一个局部平面，存储一个平面特征（即平面中心、法线向量和不确定性），以及位于该平面上的一组LiDAR原始点。这些点中的一些附有三层图像补丁（补丁尺寸为8 × 8），称之为视觉地图点。收敛的视觉地图点仅附有参考补丁，而未收敛的点附有参考补丁和其他可见补丁。叶体素的大小可变，使其能够适应不同结构的环境。

为了防止地图尺寸无限增长，仅保留一个局部地图，范围为LiDAR当前所在位置周围长度为L的区域，如图4中的二维示例所示。最初，地图是以LiDAR起始位置p0为中心的立方体。LiDAR的探测区域表示为以其当前位置为中心的球体，其半径由LiDAR的探测范围决定。当LiDAR移动到新位置p1，使得探测区域触及地图边界时，我们将地图从边界向外移动距离d。随着地图的移动，超出局部地图范围的区域所占用的内存将被重置，用于存储移动进来的新区域。这种环形缓冲区方法确保我们的局部地图维持在固定大小的内存中。地图移动检查在每次ESIKF更新步骤后进行。

几何结构的构建与更新

在ESIKF中的LiDAR更新之后，将扫描中的所有点注册到全局坐标系中。对于每个注册的LiDAR点，确定其所在的哈希表中的根体素。如果该体素不存在，用新点初始化该体素并将其索引到哈希表中。如果已在地图中找到相应的体素，将该点添加到现有的体素中。在分配完扫描中的所有点后，按如下方式进行几何构建和更新。

对于新创建的体素，确定其包含的所有点是否位于同一平面上，这通过奇异值分解来判断。如果是，计算平面中心点q = p̄、平面法线n以及平面的协方差矩阵Σn,q，后者用于表征由姿态估计不确定性和点测量噪声引起的平面不确定性。如果包含的点不在同一平面上，则体素将继续细分为八个更小的八叉树节点，直到确定这些点位于同一平面上，或达到最大层数（例如3）。在后一种情况下，叶体素中的点将被丢弃。因此，地图只包含被确定为平面的体素（无论是根体素还是子体素）。

对于已有的新点被添加的体素，评估这些新点是否仍然与现有的点位于同一平面上。如果不是，将如上所述进行体素细分。如果是，则更新平面参数（q,n）和协方差Σn,q，如上所述。一旦平面参数收敛，该平面将被视为成熟平面，平面上的新点将被丢弃。此外，成熟平面的估计平面参数（q,n）和协方差Σn,q将被固定。

位于平面上的LiDAR点（无论在根体素还是子体素中）将用于生成后续部分中的视觉地图点。对于成熟平面，最近的50个LiDAR点将作为生成视觉地图点的候选点，而对于未成熟平面，所有LiDAR点都是候选点。视觉地图点生成过程将从这些候选点中选择一些作为视觉地图点，并将图像补丁附加到这些点上用于图像对齐。

视觉地图点的生成与更新

为了生成和更新视觉地图点，选择了地图中当前帧视野中可见的候选LiDAR点，以及在当前图像中表现出显著灰度梯度的点。将在视觉更新后将这些候选点投影到当前图像中，并为每个体素的局部平面保留深度最小的候选点。然后，将当前图像划分为每个包含30×30像素的网格单元。如果某个网格单元中没有投影到该处的视觉地图点，将使用灰度梯度最高的候选点生成新的视觉地图点，并将当前图像补丁、估计的当前状态（即帧姿态和曝光时间）以及由LiDAR点计算的平面法线附加到该点上。附加到视觉地图点的补丁具有三个相同尺寸的层（例如11×11像素），每层是上一层的二分采样，形成一个补丁金字塔。如果某个网格单元中已有视觉地图点投影到该处，会在以下两种情况下向现有的视觉地图点添加新补丁（金字塔的所有三层）：（1）距离上次补丁添加已超过20帧，或（2）当前帧中的像素位置与上次补丁添加时的像素位置偏离超过40像素。因此，地图点可能具有以均匀分布的视角观测到的有效补丁。与补丁金字塔一起，我们还将估计的当前状态（即姿态和曝光时间）附加到地图点上。

参考补丁更新

由于视觉地图点可能具有多个补丁，需要选择一个参考补丁用于视觉更新中的图像对齐。根据光度相似性和视角对每个补丁f进行评分：

其中，NCC(f ,g) 表示归一化互相关系数，用于衡量补丁f和g在补丁金字塔最高层（即分辨率最高的层）的相似性，f̄ 和ḡ 为相应补丁的均值；c 表示法线向量n与视角方向p/∥p∥的余弦相似度，当补丁正对地图点所在的平面时，c的值为1。总分S由补丁f与其他补丁gi的平均相似性（NCC）和法线向量的余弦相似度c加权求和得到，tr(Σn)表示法线向量协方差矩阵的迹。

在所有附加到视觉地图点的补丁中，得分最高的那个将被更新为参考补丁。上述评分机制倾向于选择

1. 与大多数其他补丁外观相似（NCC高）的参考补丁，这一技术用于MVS 避免选择动态物体上的补丁

2. 视角正交于平面的参考补丁，从而保持纹理细节的高分辨率。

法线优化

每个视觉地图点被假定位于一个小的局部平面上。现有工作假设补丁中的所有像素具有相同的深度，这一假设通常并不成立。使用由LiDAR点计算得出的平面法线来初始化平面，随后进一步细化法线以提高图像对齐的准确性。

在视觉地图点生成期间，LiDAR点附近的局部平面法线n被初始化为体素地图中平面的法线向量。随后在独立线程中进行进一步优化。在这些独立线程中，局部平面法线和附加补丁的姿态状态根据光度误差优化目标：

其中n和ξ分别表示法线和图像位姿。所有补丁的法线和姿态都被优化。被优化后的法线成为平面法线的最终估计。由于图像对齐中的姿态状态在全局坐标系中是已知的，法线的优化也会同步进行。这一优化过程通常在20ms内完成。

LiDAR测量模型

LiDAR 测量模型构建

通过计算每个扫描点到其最近平面的距离来构建LiDAR测量模型。对于一个给定的LiDAR点（在全局坐标系下表示为），如果它位于一个平面上，则该点到平面的距离可以近似表示为点到平面法线的垂直距离。因此，平面残差可定义为：

在ESIKF框架中，LiDAR测量模型通过最小化所有平面残差的平方和来估计位姿增量，这个过程会通过迭代逐步优化系统状态。

配准过程

为了在扫描与地图之间找到最佳配准，需要迭代计算扫描中的所有LiDAR点与地图中各平面之间的残差，并利用这些残差更新位姿估计。在每次迭代中，地图中的每个平面都会被重新计算，法线n和点q也会更新。这一过程可以通过以下步骤实现：

1. 初始配准：首先，使用上一次迭代的位姿估计结果将LiDAR点转换到全局坐标系下，并计算其到当前位姿的平面残差。

2. 残差计算：计算所有LiDAR点到其各自平面的残差，并构建测量残差向量。对于每个残差向量，我们都需要求解最优的位姿增量，以最小化这些残差。

3. 迭代优化：采用迭代最小二乘法来优化位姿增量，直到残差的平方和收敛至某个阈值以下。每次迭代中，使用更新后的位姿重新计算残差，并继续优化。

在LiDAR测量更新的最后一步，将优化后的位姿用于更新系统的状态估计。

测量不确定性

为了更精确地反映测量中的噪声和不确定性，还将平面残差的协方差纳入到ESIKF更新中。假设测量噪声是高斯分布的，其协方差矩阵为。协方差的大小取决于LiDAR点测量的准确性和地图中平面的几何形状。测量不确定性的引入使得ESIKF能够更准确地估计系统状态，并提高整体系统的鲁棒性。

视觉测量模型

可见体素查询

在每次ESIKF更新之前，首先从体素地图中提取当前帧视野中的可见视觉地图点。这个过程通过对所有包含视觉地图点的体素进行光线投射来完成。具体来说，对于每个体素，确定其是否被当前摄像头观察到，并对观察到的体素进行可见性查询。如果体素内的视觉地图点被认为是可见的（即未被遮挡），这些点将被提取用于随后的图像对齐过程。

光度误差计算

在提取出可见的视觉地图点后，将这些点的图像补丁与当前帧中的相应区域进行匹配。将地图中的视觉地图点投影到当前帧中，并计算其与当前图像中的像素之间的光度误差。

在ESIKF框架中，通过最小化光度误差来优化位姿增量。这一优化过程通过迭代求解每个视觉地图点的光度残差来更新系统状态。

图像金字塔与多分辨率匹配

为了提高图像对齐的稳健性和计算效率，使用图像金字塔进行多分辨率匹配。具体来说，将每个图像补丁构建为多层金字塔，每一层的分辨率是上一层的一半。在优化过程中，首先在金字塔的较低分辨率层次上进行粗略对齐，然后逐渐过渡到较高分辨率的层次进行精细对齐。这种逐层递进的对齐方法能够有效应对大范围的初始误差，并减少局部极小值问题。

曝光时间补偿

光照变化会显著影响图像对齐的准确性。为了减轻这一影响，引入了曝光时间补偿机制。在每次视觉更新中，我们根据实时估算的曝光时间调整图像的光度值。通过这一补偿机制，能够在不同光照条件下获得更加一致的光度测量值，从而提高图像对齐的准确性。

图像对齐中的平面先验

为了进一步提高图像对齐的精度，在图像对齐过程中引入了来自LiDAR测量的平面先验。具体来说，假设视觉地图点位于某个局部平面上，并使用LiDAR测量中的平面法线来初始化该平面。在图像对齐过程中，平面先验能够为姿态估计提供额外的几何约束，减少由于图像纹理不足或光照变化带来的误差。

通过结合这些方法，视觉测量模型能够在ESIKF框架中提供更加稳健和精确的姿态估计。特别是，通过直接光度误差方法、多分辨率匹配、曝光时间补偿和平面先验的结合，我们的系统能够在不同的环境条件下实现高精度的视觉-惯性融合。

实验效果

总结一下

FAST-LIVO2系统在多个方面展示了其在实时SLAM应用中的卓越性能：

• 通过顺序更新的ESIKF框架，系统有效地解决了LiDAR和视觉测量之间的维度不匹配问题，并通过直接方法显著提高了计算效率。

• 通过在统一的体素地图上结合视觉和LiDAR信息，实现了更高精度的图像对齐和地图构建。

• 此外，系统在各种苛刻环境下的实验结果表明，其在精度、稳健性和计算负荷方面均优于现有的其他SLAM系统。特别是在无人机机载导航、空中测绘和3D模型渲染等实际应用中，FAST-LIVO2展示了其在实时操作和高精度建图中的巨大潜力。

未来的工作将着重于进一步优化系统的计算效率以及扩展其在更广泛应用场景中的适用性。

参考

[1] FAST-LIVO2: Fast, Direct LiDAR-Inertial-Visual Odometry

投稿作者为『自动驾驶之心知识星球』特邀嘉宾，欢迎加入交流！重磅，自动驾驶之心科研论文辅导来啦，申博、CCF系列、SCI、EI、毕业论文、比赛辅导等多个方向，欢迎联系我们！

① 全网独家视频课程

BEV感知、BEV模型部署、BEV目标跟踪、毫米波雷达视觉融合、多传感器标定、多传感器融合、多模态3D目标检测、车道线检测、轨迹预测、在线高精地图、世界模型、点云3D目标检测、目标跟踪、Occupancy、cuda与TensorRT模型部署、大模型与自动驾驶、Nerf、语义分割、自动驾驶仿真、传感器部署、决策规划、轨迹预测等多个方向学习视频（扫码即可学习）

网页端官网：www.zdjszx.com

② 国内首个自动驾驶学习社区

国内最大最专业，近3000人的交流社区，已得到大多数自动驾驶公司的认可！涉及30+自动驾驶技术栈学习路线，从0到一带你入门自动驾驶感知（2D/3D检测、语义分割、车道线、BEV感知、Occupancy、多传感器融合、多传感器标定、目标跟踪）、自动驾驶定位建图（SLAM、高精地图、局部在线地图）、自动驾驶规划控制/轨迹预测等领域技术方案、大模型、端到端等，更有行业动态和岗位发布！欢迎扫描下方二维码，加入自动驾驶之心知识星球，这是一个真正有干货的地方，与领域大佬交流入门、学习、工作、跳槽上的各类难题，日常分享论文+代码+视频

③【自动驾驶之心】技术交流群

自动驾驶之心是首个自动驾驶开发者社区，聚焦感知、定位、融合、规控、标定、端到端、仿真、产品经理、自动驾驶开发、自动标注与数据闭环多个方向，目前近60+技术交流群，欢迎加入！扫码添加汽车人助理微信邀请入群，备注：学校/公司+方向+昵称（快速入群方式）

④【自动驾驶之心】全平台矩阵