1. 核心摘要

这篇笔记主要记录了 FAST-LIVO2 (arXiv:2408.14035) 的核心思想。该论文提出了一种快速且直接的 LIVO 框架，通过 ESIKF（误差状态迭代卡尔曼滤波）高效融合 IMU、LiDAR 和视觉数据。其最大的创新在于视觉和 LiDAR 均采用直接法（不提取特征点），并在统一的体素地图（Voxel Map）中进行联合优化，利用 LiDAR 的几何结构（平面先验）来辅助视觉图像块的对齐，实现了高精度与鲁棒性的统一。

2. 关键概念/术语

  • Direct Method (直接法)：不提取角点或边缘特征，而是直接利用像素灰度误差（视觉）或点面距离（LiDAR）进行优化的方法。
  • Unified Voxel Map (统一体素地图)：一种将 LiDAR 的几何结构与视觉的图像块（Image Patches）存储在同一数据结构中的地图形式。
  • Plane Prior (平面先验)：利用 LiDAR 点云在体素中拟合出的平面信息，约束视觉图像块的投影位置，提高视觉里程计的精度。
  • ESIKF (Error-State Iterated Kalman Filter)：误差状态迭代卡尔曼滤波，是目前多传感器融合 SLAM 中主流的状态估计后端。

FAST-LIVO2 系统框架图

3. 详细内容整理

3.1 核心架构

  • 输入数据：LiDAR 点云、IMU 数据、相机图像。
  • 前端处理：
    • LiDAR：直接注册原始点，不提取边缘或平面特征。
    • 视觉：最小化直接光度误差，不提取 ORB/FAST 角点。
  • 后端融合：
    • 使用 ESIKF 框架进行紧耦合。
    • 维护一个全局的统一体素地图。

3.2 关键创新点

  • 全直接法融合：
    • 传统 LIVO 通常视觉部分使用特征点法（Feature-based），LiDAR 使用特征或直接法。FAST-LIVO2 两者均采用直接法，减少了特征提取的耗时。
  • 几何辅助视觉：
    • 视觉模块在对齐图像块时，利用体素地图中 LiDAR 点构建的平面先验。
    • 甚至可以反向优化平面参数，利用视觉纹理细化几何结构。
  • 动态参考块更新：
    • 当新图像对齐后，动态更新参考图像块（Reference Patch），适应环境变化。

4. 关键知识点详解

4.1 统一体素地图与测量模型

FAST-LIVO2 的核心在于如何在一个滤波器中处理两种不同模态的残差。

LiDAR 残差 (点面距离)

对于每一个 LiDAR 原始点 $p_i$，在全局地图中寻找其所在的体素，并拟合局部平面。残差 $r_L$ 定义为点到平面的距离： \(r_L = n^T (R_k p_i + t_k - q)\) 其中 $n$ 是平面法向量，$q$ 是平面上的一点，$R_k, t_k$ 是当前的位姿。

视觉残差 (光度误差)

对于视觉部分，系统选取具有梯度的像素块（Patch）。残差 $r_V$ 定义为参考帧与当前帧的光度差： \(r_V = I_{curr}(\pi(R_k p_j + t_k)) - I_{ref}(p_j)\) FAST-LIVO2 的巧妙之处在于，$p_j$ 的深度不再仅仅依赖三角化，而是可以查询统一体素地图中的 LiDAR 平面信息来获取更准确的深度先验。

4.2 为什么使用直接法？

  1. 稀疏纹理环境鲁棒性：特征点法在白墙或低纹理区域容易失效，而直接法只要有梯度变化即可工作。
  2. 计算效率：省去了繁重的特征描述子计算和匹配过程。
  3. LiDAR 辅助：直接法对深度的依赖较高，而 LIVO 系统中 LiDAR 恰好能提供精确的深度，两者互补性极强。