FAST-LIVO2 算法解析与实战（一）：多传感器融合 SLAM 核心原理

1.4 与 R3LIVE 的对比

FAST-LIVO2 与当前主流方案 R3LIVE 在多个维度存在显著差异：

2. 术语解释

3. 系统架构：四大核心模块

FAST-LIVO2 系统由四个核心模块组成，形成完整的感知 - 估计 - 建图闭环。下图展示了系统的整体架构和数据流：

四个核心模块包括：

1. 顺序更新的 ESIKF：负责状态预测和顺序更新
2. 局部地图构建：基于体素八叉树的地图管理
3. 激光雷达观测模型：提供几何配准的观测约束
4. 视觉观测模型：提供光度误差的观测约束

其中模块 1 主要负责 ESIKF 状态预测/更新方程，模块 3 和 4 则提供 ESIKF 的状态观测方程。

3.1 模块一：ESIKF（误差状态迭代卡尔曼滤波）顺序更新

状态向量（19 维）：

// 状态向量 x = [
//   旋转 δθ (3) - 旋转误差（李代数表示）
//   位置 δp (3) - 位置误差
//   曝光时间 δt_exp (1) - 曝光时间倒数误差
//   速度 δv (3) - 速度误差
//   陀螺仪偏差 δb_g (3) - 陀螺仪零偏误差
//   加速度计偏差 δb_a (3) - 加速度计零偏误差
//   重力 δg (3) - 重力向量误差
// ]

核心功能：

• 状态预测：基于 IMU 运动学模型进行状态传播
• 顺序更新：先激光雷达更新，后视觉更新
• 协方差传播：维护状态不确定性，实现最优估计

ESIKF 数学补充：

误差状态卡尔曼滤波（ESKF）的核心思想是维护一个名义状态（nominal state）和一个误差状态（error state）。名义状态跟踪大信号，误差状态跟踪小扰动。这种分离使得：

1. 线性化更准确：误差状态总是小量，线性化假设成立
2. 数值稳定性：误差状态协方差矩阵条件数更好
3. 无奇异性：旋转用李代数表示，无万向锁问题

ESIKF 在 ESKF 基础上引入迭代更新（Iterated Update），通过多次线性化提高非线性测量下的估计精度。其迭代过程为：

for i = 1:N_iter {
    // 计算测量残差 z - h(x_nominal ⊕ δx)
    // 计算雅可比矩阵 H = ∂h/∂δx
    // 计算卡尔曼增益 K
    // 更新误差状态 δx = K * residual
}
// 将误差状态注入名义状态：x_nominal = x_nominal ⊕ δx
// 重置误差状态 δx = 0

代码映射：

• 文件：include/common_lib.h 中的 StatesGroup 结构体
• 函数：src/LIVMapper.cpp 中的 stateEstimationAndMapping()

3.2 模块二：局部地图构建

基于体素八叉树的高效局部地图管理，用于存储激光雷达点云和视觉地图点。

地图结构：

体素八叉树 (VoxelOctoTree)
├── 叶节点（VoxelPlane）
│   ├── 中心点 center_
│   ├── 法向量 normal_
│   ├── 协方差 covariance_
│   ├── 平面参数 d_
│   ├── 点数量 points_size_
│   └── 是否平面 is_plane_
└── 根节点（OctoTree）
    ├── 8 个子节点 leaves_[8]
    ├── 体素中心 voxel_center_
    └── 层级信息 layer_

关键特性：

• 哈希索引：基于体素坐标的哈希表，O(1) 查询
• 动态生长：根据点云密度自动调整八叉树深度
• 滑动窗口：维护局部活跃区域，移除过时地图点
• 多模融合：同一体素存储激光雷达平面和视觉地图点

激光雷达和视觉模块共享同一个体素地图：

体素地图内容
├── 激光雷达平面（VoxelPlane）
│   ├── 几何结构（中心、法向量、协方差）
│   └── 点面残差约束
└── 视觉地图点（VisualPoint）
    ├── 参考图像块（8×8 像素）
    ├── 参考位姿
    ├── 法向量（单独线程优化）
    └── 光度残差约束

优势：

• 数据结构统一，减少内存开销
• 激光雷达提供几何结构，视觉提供纹理信息
• 两者相互增强，提高鲁棒性

实战案例补充：

在实际部署中，体素地图的动态生长特性对于室内外混合场景尤为重要。例如，在从开阔广场进入狭窄走廊时，点云密度急剧变化。FAST-LIVO2 的八叉树能自动调整深度：在开阔区域使用较大体素（低深度）以节省内存，在狭窄区域自动分割为小体素（高深度）以保持精度。这种自适应机制无需手动参数调整，显著提升了系统的环境适应性。

代码映射：

• 文件：include/voxel_map.h 中的 VoxelOctoTree 和 VoxelMapManager 类
• 函数：src/voxel_map.cpp 中的 BuildVoxelMap(), UpdateVoxelMap()

3.3 模块三：激光雷达观测模型

功能描述：基于点面（Point-to-Plane）距离约束的激光雷达观测方程，用于几何配准。

观测模型：

残差 r_LIO = n^T · (R_wb · p_b + t_wb - p_plane)
其中：
- n: 平面法向量（从体素地图获取）
- p_b: 激光雷达点（body 系）
- R_wb, t_wb: 位姿估计（ESIKF 状态）
- p_plane: 平面上某点（体素中心）

流程：

1. 点云预处理：降采样、去畸变（IMU 辅助）
2. 点云重组：根据相机采样时刻切分激光雷达扫描
3. 平面提取：查询体素地图，获取局部平面参数
4. 残差构建：计算点面距离
5. ESIKF 更新：将残差注入 ESIKF，更新状态和协方差

代码映射：

• 文件：include/voxel_map.h 中的 PointToPlane 结构体
• 函数：src/voxel_map.cpp 中的 BuildResidualListOMP(), build_single_residual()

3.4 模块四：视觉观测模型

功能描述：基于稀疏直接法的视觉里程计，通过图像块光度误差实现视觉约束。

观测模型：

残差 r_VIO = I_cur(x_warped) - I_ref(x_ref)
其中：
- I_cur: 当前图像
- I_ref: 参考图像（存储在视觉地图点中）
- x_warped: 基于位姿估计投影的像素坐标
- x_ref: 参考像素坐标

关键创新：

• 视觉地图点：将图像块'附着'到激光雷达点云上
• 直接法：无需特征提取，直接比较像素灰度
• 法向量优化：在独立线程中细化参考图像块的法向量
• 光度误差：使用仿射变换模型处理光照变化

流程：

1. 视觉地图点检索：从体素地图查询当前 FOV 内的视觉地图点
2. 图像块获取：基于参考像素坐标提取图像块
3. 仿射变换：考虑位姿变化和光照变化
4. 残差构建：计算光度误差
5. ESIKF 更新：更新状态和协方差

顺序更新优势补充：

FAST-LIVO2 采用'先激光雷达，后视觉'的顺序更新策略，而非并行更新。这种设计具有两个关键优势：

1. 维度匹配：激光雷达观测维度高（数千点），视觉观测维度低（数百图像块）。先处理高维观测能提供良好的初始位姿，再处理低维视觉观测进行精细优化，避免同时处理时的维度不匹配问题。
2. 计算效率：激光雷达更新后，体素地图已包含最新的几何信息。视觉更新时可以直接利用这些平面先验，加速图像对齐的收敛。实验表明，这种顺序更新相比并行更新，在保持精度的同时减少了约 30% 的计算时间。

代码映射：

• 文件：include/vio.h 中的 VIOManager 和 VisualPoint 类
• 函数：src/vio.cpp 中的 computeJacobianAndUpdateEKF(), processFrame()

4. 数据流与顺序更新流程

4.1 完整数据流

传感器数据流
├── 激光雷达点云
│   ├── 点云重组（按相机采样时刻）
│   ├── 预处理（降采样、去运动畸变）
│   ├── 体素地图查询
│   ├── 构建点面残差
│   └── ESIKF 状态更新（LIO）
├── IMU 数据
│   ├── 静态初始化（重力、偏差）
│   ├── 状态传播（连续）
│   └── 去畸变辅助
└── 相机图像
    ├── 视觉地图点检索（FOV 内）
    ├── 参考图像块获取
    ├── 仿射变换与光度误差
    └── ESIKF 状态更新（VIO）

4.2 顺序更新流程（核心）

FAST-LIVO2 采用顺序更新策略，而非并行更新：

time t0 → t1 → t2 → t3 → ...
每帧的处理流程：
Step 1: IMU 状态传播
┌─────────────────────────────────────┐
│ 从 t0 到 t1:                          │
│ - IMU 积分，预测位姿                  │
│ - 协方差传播                          │
│ - 输出先验状态                        │
└─────────────────────────────────────┘

Step 2: 激光雷达更新（LIO）
┌─────────────────────────────────────┐
│ - 点云分割重组（按相机采样时刻）      │
│ - 点云预处理（降采样、去畸变）        │
│ - 体素地图查询，获取平面参数          │
│ - 构建点面残差                        │
│ - ESIKF 状态更新                      │
│ - 更新体素地图（几何结构）            │
└─────────────────────────────────────┘

Step 3: 视觉更新（VIO）
┌─────────────────────────────────────┐
│ - 视觉地图点检索（FOV 内）            │
│ - 参考图像块获取                      │
│ - 仿射变换（位姿 + 光照）             │
│ - 构建光度误差残差                    │
│ - ESIKF 状态更新                      │
│ - 更新视觉地图点（当前图像块）        │
│ - 启动法向量优化线程                  │
└─────────────────────────────────────┘

Step 4: 局部地图维护
┌─────────────────────────────────────┐
│ - 地图滑动窗口（移除远距离地图）      │
│ - 体素地图维护（添加新点、分割体素）  │
│ - 视觉地图点管理（添加、更新、删除）  │
└─────────────────────────────────────┘

5. 代码结构映射

FAST-LIVO2/
├── include/
│   ├── LIVMapper.h           # 主映射器（系统控制器）
│   ├── IMU_Processing.h      # IMU 处理（状态传播、去畸变）
│   ├── vio.h                 # 视觉惯性里程计（VIO 模块）
│   ├── voxel_map.h           # 体素地图（地图管理）
│   ├── preprocess.h          # 预处理（点云降采样）
│   ├── frame.h               # 图像帧（图像金字塔）
│   └── common_lib.h          # 公共定义（状态向量、数据结构）
├── src/
│   ├── LIVMapper.cpp         # 主逻辑（顺序更新流程）
│   ├── IMU_Processing.cpp    # IMU 实现细节
│   ├── vio.cpp               # VIO 实现细节
│   ├── voxel_map.cpp         # 体素地图实现细节
│   └── main.cpp              # ROS 入口
└── config/                   # 配置文件（传感器参数、算法参数）

6. 总结

FAST-LIVO2 系统通过四个核心模块的紧密协作，实现了激光雷达 - 视觉 - 惯性传感器的紧耦合。其创新点主要体现在：

1. 顺序更新的 ESIKF 框架：解决了维度不匹配问题，提高了鲁棒性
2. 平面先验的利用与优化：显著提升了图像对齐精度
3. 参考图像块更新策略：通过高质量参考图像块提高视觉约束质量
4. 在线曝光时间估计：增强了光照变化下的适应性
5. 按需体素光线投射：弥补了激光雷达近距离盲区的缺陷

系统采用'先激光雷达，后视觉'的顺序更新策略，既保证了维度匹配，又提高了计算效率。体素地图的双哈希表设计实现了激光雷达和视觉数据的统一管理，为多传感器融合提供了坚实的基础。