FASTLIVO2 算法解析与实战（一）：SLAM 系统架构详解

1.4 与 R3LIVE 的对比

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

• 采用两阶段对齐：1. 帧间光流（frame-to-frame）获取初始位姿

2. 帧到地图（frame-to-map）优化位姿

• 此流程对初始位姿敏感，在快速运动或弱纹理场景易失败 | - VIO 中操作图像块级别
• 创新点：直接帧到地图稀疏图像对齐（一步到位）
• 基于 ESIKF 滤波器紧耦合融合 IMU、LiDAR 和相机数据
• 通过 LiDAR 点云构建的体素地图提供几何先验
• 省去光流初始化，显著提升在退化场景（如纯色墙面、低光照）的稳定性 | | 计算性能 | - VIO 模块采用稠密直接法
• 需对大量像素（甚至全图像素）计算光度误差，计算开销大 | - 稀疏直接法仅选取关键视觉地图点（附着图像块）构建残差
• 视觉地图点来源自 Lidar 点云，无需额外特征提取
• 图像对齐仅在图像块（8×8 像素）进行
• 配合图像金字塔分层优化（由粗至精），进一步减小计算量 | | 地图管理 | - 受限于其点地图的分辨率 | - 激光雷达和视觉模块共享单一统一地图
• 可以直接利用激光雷达点提供的平面先验来加速图像对齐
• 利用了原始图像块分辨率级别的信息 |

2. 术语解释

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

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

四个核心模块包括：

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 end // 将误差状态注入名义状态：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 像素） ├── 参考位姿 ├── 法向量（单独线程优化） └── 光度残差约束 

优势：

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

实战案例补充：

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

代码映射：

• 文件：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 更新：更新状态和协方差

顺序更新优势补充：

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

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

代码映射：

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

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

4.1 完整数据流

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

4.2 顺序更新流程（核心）

FASTLIVO2 采用顺序更新策略，而非并行更新：

时间轴 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. 总结

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

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

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