FASTLIVO2算法解析与实战（一）：SLAM领域的新标杆，如何让机器人“看得更清、跑得更稳”
FASTLIVO2系统概述
1. 背景介绍
1.1 传感器特性
FASTLIVO2 系统融合了三种互补的传感器：激光雷达（LiDAR）、相机（Camera）和惯性测量单元（IMU）。它们在感知方式、输出数据和环境适应性上各具特点，通过融合实现优势互补。
| 特性 | 激光雷达（LiDAR） | 相机（Camera） | IMU |
|---|---|---|---|
| 工作方式 | 主动发射激光，通过反射测量距离和方位 | 被动接收环境光，捕捉 2D 图像信息 | 主动测量自身运动 |
| 感知内容 | 环境几何结构（深度、形状、表面） | 环境纹理与颜色（语义、细节、动态物体） | 自身运动状态（姿态、速度、加速度） |
| 数据输出 | 3D 点云（精确深度） | 2D 像素矩阵（RGB 或灰度） | 6 自由度运动参数 |
| 优势 | - 直接深度测量，精度高- 不受光照影响- 在结构化环境中鲁棒 | - 丰富语义信息- 成本低- 高分辨率细节捕捉 | - 高频更新（100–400 Hz）- 短期运动估计精确- 不受环境退化影响 |
| 劣势 | - 缺乏颜色信息- 在无结构环境（如单一墙面）易退化- 近距离盲区失效 | - 依赖光照和纹理- 无直接深度（需三角化）- 动态物体 / 光照变化易失效 | - 长期漂移累积误差- 仅提供相对运动，无法感知环境 |
| 环境敏感性 | 对几何结构退化敏感（如狭窄隧道） | 对光照和纹理退化敏感（如黑暗、无纹理表面） | 几乎不受环境影响 |
| 互补性体现 | ✅ 为相机提供几何先验（如平面、法向量）✅ 补偿相机在黑暗 / 纹理缺失时的失效 | ✅ 为 LiDAR 点云添加颜色 / 纹理✅ 在 LiDAR 退化时通过图像对齐提供约束 | ✅ 为 LiDAR / 相机提供高频运动先验✅ 在传感器退化时维持系统可观性 |
1.2 LIVO面临的挑战
激光雷达-惯性-视觉里程计（LIVO）系统在实际部署中面临四个核心挑战：
- 计算效率瓶颈：激光雷达每秒产生数百到数千个点，相机产生高分辨率图像。在机载资源有限的情况下，充分利用如此大量的数据需要极高的计算效率。
- 特征提取的局限性：许多现有系统采用独立的LIO和VIO子系统，分别从激光雷达和视觉数据中提取特征。在缺乏结构或纹理的环境中，特征点有限，且需要大量工程调整来适应激光雷达扫描模式和点密度的可变性。
- 统一地图的设计难题：为实现相机和激光雷达测量的紧密集成，需要设计一个能同时匹配稀疏点云和高分辨率图像的统一地图。然而，激光雷达和相机的异质测量使得地图的构建和维护极具挑战性。
- 像素级精度要求：为重建带颜色的点云，位姿估计需要达到像素级精度。这要求严格的硬件同步、外参预标定、曝光时间恢复以及实时高精度融合策略。
1.3 FAST-LIVO2的创新贡献
FAST-LIVO2 在 FAST-LIVO 的基础上进行了五项关键改进：
- 顺序更新的ESIKF框架：提出了一种采用顺序更新的高效ESIKF框架，解决了LiDAR和视觉测量之间的维度不匹配问题，提高了使用异步更新的FAST-LIVO的鲁棒性。
- 平面先验的利用与优化：使用（甚至优化）来自LiDAR点的平面先验以提高精度。相比之下，FAST-LIVO假设一个图像块中的所有像素共享相同的深度，这一粗略假设显著降低了图像对齐中仿射变换的精度。
- 参考图像块更新策略：通过选择具有大视差和足够纹理细节的高质量、内点参考图像块，来提高图像对齐的精度。FAST-LIVO基于与当前视角的接近程度选择参考图像块，这常常导致低质量的参考图像块，从而降低了精度。
- 在线曝光时间估计：进行了在线曝光时间估计以处理环境光照变化。FAST-LIVO没有解决这个问题，导致在显著光照变化下图像对齐收敛性差。
- 按需体素光线投射：提出了按需体素光线投射，以增强系统在因LiDAR近距离盲区而缺乏LiDAR点测量情况下的鲁棒性。
1.4 与R3LIVE的对比
FAST-LIVO2 与当前主流方案 R3LIVE 在多个维度存在显著差异：
| 维度 | R3LIVE | FAST-LIVO2 |
|---|---|---|
| 鲁棒性 | - VIO中操作单个像素- 采用两阶段对齐： 1. 帧间光流（frame-to-frame）获取初始位姿 2. 帧到地图（frame-to-map）优化位姿- 此流程对初始位姿敏感，在快速运动或弱纹理场景易失败 | - VIO中操作图像块级别- 创新点：直接帧到地图稀疏图像对齐（一步到位）- 基于ESIKF滤波器紧耦合融合IMU、LiDAR和相机数据- 通过LiDAR点云构建的体素地图提供几何先验- 省去光流初始化，显著提升在退化场景（如纯色墙面、低光照）的稳定性 |
| 计算性能 | - VIO模块采用稠密直接法- 需对大量像素（甚至全图像素）计算光度误差，计算开销大 | - 稀疏直接法仅选取关键视觉地图点（附着图像块）构建残差- 视觉地图点来源自Lidar点云，无需额外特征提取- 图像对齐仅在据图图像块（8×8像素）进行- 配合图像金字塔分层优化（由粗至精），进一步减小计算量 |
| 地图管理 | - 受限于其点地图的分辨率 | - 激光雷达和视觉模块共享单一统一地图- 可以直接利用激光雷达点提供的平面先验来加速图像对齐- 利用了原始图像块分辨率级别的信息 |
2. 术语解释
| 英文术语 | 中文翻译 | 简要解释 |
|---|---|---|
| Error State Iterated Kalman Filter (ESIKF) | 误差状态迭代卡尔曼滤波器 | 一种在误差状态空间进行迭代优化的卡尔曼滤波算法。用于提高估计精度。 |
| Direct Method | 直接法 | 直接利用传感器原始数据（如像素强度、点云）进行优化的方法，无需预先提取特征点。 |
| Photometric Error | 光度误差 | 基于图像像素强度值差异计算得到的一种误差度量方式，常用于直接法中作为优化目标。 |
| Voxel Map | 体素地图 | 将三维空间分割成许多小立方体单元来表示环境信息的地图形式，适合于表示复杂场景。 |
| Image Alignment | 图像对齐 | 通过调整参数使得两张或多张图像之间或图像与模型之间的对应关系达到最佳匹配的过程。 |
| Plane Prior | 平面先验 | 已知或者假设存在的局部平面几何信息，在某些优化问题中被用作约束条件以提高解的质量。 |
| RayCasting | 射线投射 | 模拟传感器发出的射线（例如激光雷达的光线）与环境中物体交互过程的技术，广泛应用于渲染和感知。 |
| Visual Landmark | 视觉地图点 | 复用LiDAR模块已经构建好的高精度3D点云来充当视觉地图点，3D坐标由Lidar直接提供。将图像块“附着”到激光雷达点云上 |
| Image Patch | 图像块 | 视觉地图点投影的像素坐标为中心，划一个一定大小像素范围的块（如8×8像素）,用于视觉匹配和光度误差计算，动态更新以保持纹理细节 |
| Reference Image Patch | 参考图像块 | 视觉地图点的多个图像块中质量最好的图像块（基于光度相似性和视角） |
| Pyramid of Images | 图像金字塔 | 多尺度图像表示方法，每一层都是原图像的一个缩放 |
| Scan Reassembly | 点云/扫描重组 | 将异步采集的激光雷达点云数据，在相机拍摄时刻重新组合成完整的扫描帧，确保多传感器数据时间对齐 |
| Sequential Update | 顺序更新 | 在卡尔曼滤波中，先处理激光雷达数据更新位置状态，再处理图像数据优化状态，避免同时处理导致维度不匹配问题 |
| Point-to-Plane Residual from Scan to Map | 帧到地图的点-面残差 | 计算当前激光雷达扫描点与地图中已有平面之间的位置误差，用于校正定位偏差 |
| On-demand RayCasting | 按需光线投射 | 当视野中地图点不足时，模拟光线投射生成虚拟点云，补充约束以增强定位鲁棒性。 |
| Visible Voxel Query | 可见体素查询 | 快速查找地图中位于当前相机视野范围内的体素单元（小方块区域）,仅处理可见部分以提升效率。 |
3. 系统架构：四大核心模块
FASTLIVO2 系统由四个核心模块组成，形成完整的感知-估计-建图闭环。下图展示了系统的整体架构和数据流：
四个核心模块包括：
- 顺序更新的ESIKF：负责状态预测和顺序更新
- 局部地图构建：基于体素八叉树的地图管理
- 激光雷达观测模型：提供几何配准的观测约束
- 视觉观测模型：提供光度误差的观测约束
其中模块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）。名义状态跟踪大信号，误差状态跟踪小扰动。这种分离使得：
- 线性化更准确：误差状态总是小量，线性化假设成立
- 数值稳定性：误差状态协方差矩阵条件数更好
- 无奇异性：旋转用李代数表示，无万向锁问题
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: 平面上某点（体素中心） 流程：
- 点云预处理：降采样、去畸变（IMU 辅助）
- 点云重组：根据相机采样时刻切分激光雷达扫描
- 平面提取：查询体素地图，获取局部平面参数
- 残差构建：计算点面距离
- 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: 参考像素坐标 关键创新：
- 视觉地图点：将图像块“附着”到激光雷达点云上
- 直接法：无需特征提取，直接比较像素灰度
- 法向量优化：在独立线程中细化参考图像块的法向量
- 光度误差：使用仿射变换模型处理光照变化
流程：
- 视觉地图点检索：从体素地图查询当前 FOV 内的视觉地图点
- 图像块获取：基于参考像素坐标提取图像块
- 仿射变换：考虑位姿变化和光照变化
- 残差构建：计算光度误差
- ESIKF 更新：更新状态和协方差
顺序更新优势补充：
FASTLIVO2采用“先激光雷达，后视觉”的顺序更新策略，而非并行更新。这种设计具有两个关键优势：
- 维度匹配：激光雷达观测维度高（数千点），视觉观测维度低（数百图像块）。先处理高维观测能提供良好的初始位姿，再处理低维视觉观测进行精细优化，避免同时处理时的维度不匹配问题。
- 计算效率：激光雷达更新后，体素地图已包含最新的几何信息。视觉更新时可以直接利用这些平面先验，加速图像对齐的收敛。实验表明，这种顺序更新相比并行更新，在保持精度的同时减少了约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 系统通过四个核心模块的紧密协作，实现了激光雷达-视觉-惯性传感器的紧耦合。其创新点主要体现在：
- 顺序更新的ESIKF框架：解决了维度不匹配问题，提高了鲁棒性
- 平面先验的利用与优化：显著提升了图像对齐精度
- 参考图像块更新策略：通过高质量参考图像块提高视觉约束质量
- 在线曝光时间估计：增强了光照变化下的适应性
- 按需体素光线投射：弥补了激光雷达近距离盲区的缺陷
系统采用“先激光雷达，后视觉”的顺序更新策略，既保证了维度匹配，又提高了计算效率。体素地图的双哈希表设计实现了激光雷达和视觉数据的统一管理，为多传感器融合提供了坚实的基础。
本文由mdnice多平台发布
