FAST-LIVO2 源码精读（一）：系统总览——从 LIO 到 LIVO 的多传感器融合架构

FAST-LIVO2 源码精读（一）：系统总览——从 LIO 到 LIVO 的多传感器融合架构

本文是「FAST-LIVO2 激光-惯性-视觉里程计源码精读」专栏的开篇。全专栏共 18 篇，锚定香港大学 MaRS 实验室 2025 年 1 月开源的 FAST-LIVO2 官方代码，逐模块剖析这套登上 T-RO’24 的多传感器融合系统。开篇先建立全局认知：FAST-LIVO2 所要解决的问题、整体框架的构成，以及源码主线的数据流动。

一、引子：纯激光里程计的退化失效场景

考虑一台巡检机器人驶入一条长达数十米的笔直隧道。隧道壁光滑、截面恒定，激光雷达每一帧扫描得到的，是几乎完全相同的圆筒形结构。

对纯激光惯性里程计（LiDAR-Inertial Odometry, LIO）而言，这是典型的退化场景：沿隧道轴线方向，前后两帧点云无法区分——点到面约束在该方向上完全消失，几何上称为"退化"（degeneration）。载体持续前进，里程计却无法感知这一运动，位置估计沿隧道方向快速漂移。

将场景替换为一面正对相机的大面积纯白墙，视觉里程计（Visual Odometry, VO）同样失去约束：画面中不存在任何纹理梯度，光度误差处处为零，相机无法判断自身是否发生运动。而激光雷达在此类结构清晰的场景中工作稳定。

激光与视觉的失效场景几乎互补——这构成了激光-惯性-视觉里程计（LiDAR-Inertial-Visual Odometry, LIVO）的根本动因。隧道场景由视觉纹理提供约束，白墙场景由激光几何提供约束，两者通过一套惯性测量单元（IMU）耦合，构成在严重退化环境下依然稳健的状态估计器。

FAST-LIVO2 代表了该技术路线当前的领先水平之一。它在 LiDAR 几何退化、视觉光照退化的极端场景中仍能稳定实时建图，其背后是一套具有鲜明特点的"反直觉"设计，这些设计将在后续章节反复出现。

图 1　激光与视觉的失效场景几乎互补，构成 LIVO 的根本动因

二、SLAM 谱系：LO → LIO → LIVO 的三级演进

要厘清 FAST-LIVO2 的技术定位，需先梳理其演进谱系。

2.1 LO：仅依赖激光的纯几何里程计

最早的激光里程计（LiDAR Odometry）的核心任务是将当前帧点云配准到地图，求解位姿增量，代表作为 LOAM 系列。其局限有二：一是快速运动时，点云因扫描期间载体运动而产生运动畸变；二是缺乏对尺度漂移与退化方向的约束。

2.2 LIO：惯性测量单元的引入

引入 IMU 后，情况发生本质变化。IMU 以数百赫兹的频率输出角速度与加速度，可在相邻两帧激光之间提供高频运动先验：既可用于去除点云运动畸变，又可在激光退化时抑制短时漂移。

香港大学这一技术线的 FAST-LIO / FAST-LIO2 是其中的代表。FAST-LIO2 的两项关键技术为：

• ikd-Tree：一种支持增量插入、删除与动态平衡的 KD 树，使地图最近邻搜索达到实时；
• 误差状态迭代卡尔曼滤波（ESIKF）：将滤波器迭代化，在精度上逼近图优化，同时保持滤波器的计算效率。

此处需澄清一个常见误区：FAST-LIO2 采用 ikd-Tree，但至 FAST-LIVO2，地图结构已替换为体素地图（VoxelMap），全程不再使用 ikd-Tree（源码中 grep ikd 无任何匹配）。这是从 FAST-LIO2 到 FAST-LIVO2 的一处重要演进，第 9 篇将专门剖析 VoxelMap 的哈希八叉树设计。

2.3 LIVO：相机的接入

LIO 解决了几何退化下的短时稳健性，但对长走廊、空旷地面等持续退化场景，仅依赖 IMU 难以抑制数十秒尺度的漂移。相机提供的是稠密纹理约束——只要环境存在纹理或边缘，视觉即可约束激光无法约束的自由度。

FAST-LIVO2 将三者紧密耦合：激光提供精确的深度与几何，相机提供丰富的纹理与颜色，IMU 提供高频运动先验以在时间上衔接两者。最终输出不仅是轨迹，还包含一张带真实颜色的稠密点云地图。

三、FAST-LIVO2 的三个反直觉设计

通读源码可以发现，FAST-LIVO2 兼具高速与高稳健性，源于三个与常规直觉相悖的设计选择。以下先给出结论，后续各篇均围绕这三点展开。

反直觉之一：视觉部分不提取特征点，采用直接法

主流视觉 SLAM（如 ORB-SLAM）的范式是：提取 FAST 角点、计算 ORB 描述子、进行特征匹配。FAST-LIVO2 则采用不同路径——稀疏直接法：不提取任何特征点，直接利用激光投影到图像上的点，在其周围取一小块图像 patch，通过最小化光度误差（像素亮度差）完成对齐。

其优势在于省去了特征提取与描述子计算的开销，且不受弱纹理区域无法提取特征的限制；代价是对光照变化与初值较为敏感。第 13 篇将深入分析这套直接法的 patch 仿射对齐与金字塔策略。

反直觉之二：摒弃联合优化，采用单一 ESIKF 顺序更新

一种常见的误解是，"紧耦合"意味着将激光残差与视觉残差纳入同一个大型优化问题同时求解。FAST-LIVO2 的做法更为精巧：维护唯一一份 ESIKF 状态，由激光观测与视觉观测先后顺序对其更新。

单帧数据进入后，先以激光执行一轮 ESIKF 迭代更新以校正状态，随后以图像执行第二轮 ESIKF 迭代，对同一状态进一步精修。两轮共享同一份协方差，前者的结果即后者的先验。这种"顺序紧耦合"既保留了紧耦合的信息传递，又避免了构建超大联合系统的复杂度。第 14 篇将专门讨论该机制。

反直觉之三：将相机曝光时间纳入状态在线估计

这是 FAST-LIVO2 最具辨识度的设计。直接法的核心假设是光度一致性——同一空间点在不同帧中的亮度应当相同。而实际相机的自动曝光会破坏这一假设，亮度并不守恒。

FAST-LIVO2 的应对方式是将相机的逆曝光时间（inverse exposure time）作为一个待估状态量，纳入卡尔曼滤波器在线估计。状态向量因此增加一维，专门吸收曝光变化引入的整体亮度偏移。后文解剖状态向量时即可看到该维度。

四、拉取源码：跟读本专栏的代码基线

本专栏自本篇起会大量引用源码，并统一以 文件:行号 的形式标注位置（如 src/LIVMapper.cpp:534）。建议在阅读前先把官方仓库拉到本地，对照着读，行号、函数名都能一一对上。

代码来自香港大学 MaRS 实验室的官方仓库，与开篇链接一致：

# 建议放入 catkin 工作空间的 src 下，便于第二篇直接编译
mkdir -p ~/catkin_ws/src && cd ~/catkin_ws/src
git clone https://github.com/hku-mars/FAST-LIVO2.git

拉取完成后，目录结构与本专栏各篇的对应关系如下：

FAST-LIVO2/
├── src/ 核心源码（C++ 实现）
│ ├── main.cpp 程序入口（本篇第六节）
│ ├── LIVMapper.cpp 主循环与 LIO/VIO 调度（第 8、12、14 篇）
│ ├── voxel_map.cpp VoxelMap 体素地图 + LIO 更新（第 9、12 篇）
│ ├── vio.cpp 视觉直接法子系统（第 13、14 篇）
│ ├── IMU_Processing.cpp IMU 传播与去畸变（第 5、11 篇）
│ ├── preprocess.cpp 多雷达点云预处理（第 10 篇）
│ └── frame.cpp / visual_point.cpp 视觉帧与地图点（第 13 篇）
├── include/ 头文件
│ ├── common_lib.h 19 维状态向量定义（本篇第五节）
│ ├── LIVMapper.h 主类与各子模块指针（本篇第六节）
│ └── voxel_map.h / vio.h … 各模块声明
├── config/ 运行参数 yaml（第 2、3、15 篇）
├── launch/ ROS 启动文件（第 2、3 篇）
└── CMakeLists.txt 编译配置（第 2 篇）

这里仅获取源码以便对照阅读。完整的依赖安装（Sophus 须 checkout 至 a621ff、rpg_vikit 须用 xuankuzcr 分支等）与 catkin_make 编译流程，是下一篇的主题，本篇不展开。若只想先把代码读通，拉取本仓库即可。

五、解剖 19 维状态向量

FAST-LIVO2 的状态定义在 include/common_lib.h。先看维度宏：

// include/common_lib.h:30
#define DIM_STATE (19) // Dimension of states (Let Dim(SO(3)) = 3)

共 19 维。其物理构成可由 StatesGroup 的成员及流形加法 operator+ 明确：

// include/common_lib.h:167
StatesGroup operator+(const Matrix<double, DIM_STATE, 1> &state_add)
{
StatesGroup a;
a.rot_end = this->rot_end * Exp(state_add(0,0), state_add(1,0), state_add(2,0)); // [0:3) 姿态
a.pos_end = this->pos_end + state_add.block<3,1>(3, 0); // [3:6) 位置
a.inv_expo_time = this->inv_expo_time + state_add(6, 0); // [6] 逆曝光时间
a.vel_end = this->vel_end + state_add.block<3,1>(7, 0); // [7:10) 速度
a.bias_g = this->bias_g + state_add.block<3,1>(10, 0); // [10:13) 陀螺零偏
a.bias_a = this->bias_a + state_add.block<3,1>(13, 0); // [13:16) 加速度零偏
a.gravity = this->gravity + state_add.block<3,1>(16, 0); // [16:19) 重力
a.cov = this->cov;
return a;
}

逐项拆解这 19 维：

3+3+1+3+3+3+3 = 19，恰好吻合。

其中有两个值得关注的细节。

其一，姿态的更新是乘法而非加法。 rot_end 一行采用 this->rot_end * Exp(…)，而非直接相加。旋转矩阵属于 SO(3) 流形，“加一个增量"在数学上不成立；正确做法是将增量映射为李代数，再经指数映射 Exp 还原为旋转矩阵，并右乘到当前姿态，即所谓"右扰动模型”。位置、速度、零偏等量位于欧氏空间，直接相加即可。第 4 篇将系统讲解这套流形加减的数学。

其二，gravity 同样是状态量。 FAST-LIVO2 不将重力视为已知常量，而是在线估计其方向。这使系统在初始静止对齐不完美时仍能自我修正，对手持设备尤为有利。

与之对称，状态的减法 operator- 将两个状态之差映射回 19 维误差向量，姿态部分由 Log 将旋转之差映射回李代数：

// include/common_lib.h:194
Matrix<double, DIM_STATE, 1> operator–(const StatesGroup &b)
{
Matrix<double, DIM_STATE, 1> a;
M3D rotd(b.rot_end.transpose() * this->rot_end);
a.block<3,1>(0, 0) = Log(rotd); // 姿态之差 → 李代数
a.block<3,1>(3, 0) = this->pos_end – b.pos_end;
// … 其余各项直接相减
}

operator+ 与 operator- 这对运算，正是后续 ESIKF 反复调用的"流形加减"基础设施。整套滤波器的数学结构，集中体现在这十余行运算符重载中。

19 维状态向量的内存布局（StatesGroup）
索引 0────3 3────6 6 7───10 10──13 13──16 16──19
┌─────────┬─────────┬─────┬───────┬───────┬───────┬─────────┐
│ rot │ pos │inv │ vel │bias_g │bias_a │ gravity │
│ 姿态 │ 位置 │expo │ 速度 │陀螺 │加计 │ 重力 │
│ 3 维 │ 3 维 │1 维 │ 3 维 │零偏3 │零偏3 │ 3 维 │
└─────────┴─────────┴─────┴───────┴───────┴───────┴─────────┘
▲SO(3)流形 ▲直接法在线 ▲方向在线
右乘更新 光度标定(独有) 估计,不设常量
3 + 3 + 1 + 3 + 3 + 3 + 3 = 19

图 3　19 维状态分块：第 6 维的逆曝光时间与末 3 维的在线重力，是 FAST-LIVO2 的两处亮点

六、从 main 到主循环：源码主线导览

明确状态定义后，再看代码的执行流程。入口 src/main.cpp 结构简洁：

// src/main.cpp
int main(int argc, char **argv)
{
ros::init(argc, argv, "laserMapping");
ros::NodeHandle nh;
image_transport::ImageTransport it(nh);
LIVMapper mapper(nh);
mapper.initializeSubscribersAndPublishers(nh, it);
mapper.run();
return 0;
}

三步：构造 LIVMapper（读取参数、初始化各子模块）、注册 ROS 订阅与发布、进入 run() 主循环。系统的核心是 LIVMapper 类，其头文件 include/LIVMapper.h 以指针形式聚合了所有子模块：

// include/LIVMapper.h:157
PreprocessPtr p_pre; // 点云预处理（第 10 篇）
ImuProcessPtr p_imu; // IMU 处理与去畸变（第 11 篇）
VoxelMapManagerPtr voxelmap_manager;// 体素地图 + LIO 更新（第 9、12 篇）
VIOManagerPtr vio_manager; // 视觉子系统（第 13、14 篇）

这四个指针基本对应了本专栏第三篇章的内容编排。

6.1 主循环的五个动作

run() 的核心如下，其简洁程度与系统性能形成鲜明对比：

// src/LIVMapper.cpp:534
void LIVMapper::run()
{
ros::Rate rate(5000);
while (ros::ok())
{
ros::spinOnce();
if (!sync_packages(LidarMeasures)) // ① 多传感器时间同步打包
{
rate.sleep();
continue;
}
handleFirstFrame(); // ② 首帧初始化与重力对齐
processImu(); // ③ IMU 前向传播 + 点云去畸变
stateEstimationAndMapping(); // ④⑤ ESIKF 状态估计与建图
}
savePCD(); // 退出时落盘点云
}

五个动作对应整个系统的数据流：

6.2 LIO 与 VIO 的调度开关

第④步 stateEstimationAndMapping 是整套融合策略的中枢，其本体仅为一个 switch：

// src/LIVMapper.cpp:267
void LIVMapper::stateEstimationAndMapping()
{
switch (LidarMeasures.lio_vio_flg)
{
case VIO:
handleVIO(); // 视觉更新
break;
case LIO:
case LO:
handleLIO(); // 激光更新
break;
}
}

关键在于 lio_vio_flg 标志，其取值定义在 common_lib.h 的枚举中：

// include/common_lib.h:54
enum EKF_STATE { WAIT = 0, VIO = 1, LIO = 2, LO = 3 };

sync_packages 在打包时依据当前应处理激光还是图像，将该标志置为 LIO 或 VIO，于是主循环每次迭代或执行激光更新，或执行视觉更新。同一份状态 _state 由这两条路径交替精修，正是第三节所述"顺序紧耦合"在代码层面的实现。此处不存在复杂的联合大矩阵，融合机制即由这一 switch 结构承载。

另需区分另一枚举 SLAM_MODE，它决定系统整体运行的模式：

// include/common_lib.h:48
enum SLAM_MODE { ONLY_LO = 0, ONLY_LIO = 1, LIVO = 2 };

通过配置可使 FAST-LIVO2 退化为纯激光里程计（ONLY_LO）、激光惯性里程计（ONLY_LIO），或启用全部传感器的激光-惯性-视觉模式（LIVO）。这种可裁剪性使其既能在算力受限平台上运行精简配置，也能在完整传感器套件上发挥完整性能。

图 2　run() 主循环数据流：融合调度由 lio_vio_flg 标志承载

七、handleLIO 与 handleVIO：两条更新支路的轮廓

完整细节将在第 12、14 篇展开，但开篇有必要先明确两条支路各自的职责，建立整体认知。

激光支路 handleLIO 的开头执行降采样并送入 ESIKF：

// src/LIVMapper.cpp:336
void LIVMapper::handleLIO()
{
if (feats_undistort->empty() || (feats_undistort == nullptr)) {
std::cout << "[ LIO ]: No point!!!" << std::endl;
return;
}
double t0 = omp_get_wtime();
downSizeFilterSurf.setInputCloud(feats_undistort);
downSizeFilterSurf.filter(*feats_down_body); // 体素降采样
feats_down_size = feats_down_body->points.size();
// … 构建点到面残差、装配 H 矩阵、ESIKF 迭代更新、增量更新体素地图
}

该支路接收去畸变点云，降采样后在体素地图中匹配对应平面，构建点到面残差，经 ESIKF 迭代校正状态，最后将新点增量地融入地图。

视觉支路 handleVIO 的核心则是将图像交由 vio_manager 处理一帧：

// src/LIVMapper.cpp:305
vio_manager->processFrame(
LidarMeasures.measures.back().img, // 当前帧图像
_pv_list, // 带方差的点（来自激光）
voxelmap_manager->voxel_map_, // 共享同一张体素地图
LidarMeasures.last_lio_update_time – _first_lidar_time);

注意 processFrame 的入参——它同时接收图像与来自激光的带方差点云 _pv_list，并共享 LIO 所用的体素地图 voxel_map_。这一调用将激光与视觉两个子系统的数据通道连接在一起：视觉地图点并非独立提取，而是由激光点反哺生成。该数据流将在第 14 篇详细展开。

两条支路执行完毕后，状态 _state 始终为同一份，协方差也在两者之间传递。系统在隧道场景中更多依赖视觉支路、在白墙场景中更多依赖激光支路，依据的并非显式的场景判断，而是卡尔曼滤波器自动按各自观测的不确定度分配权重——退化方向上观测信息少，权重相应降低。这一特性源于滤波器框架自身的数学性质。

八、整套系统的全景图

将前述各部分归纳为整体图景：

• 输入：Livox/机械式激光雷达点云、相机图像、IMU 数据三路异步流入；
• 同步（第 8 篇）：sync_packages 按时间戳打包，并由 lio_vio_flg 调度本轮执行 LIO 或 VIO；
• 预处理（第 10 篇）：preprocess 按雷达类型组织点云、过滤盲区、降采样；
• IMU 传播与去畸变（第 5、11 篇）：IMU_Processing 前向传播状态、反向补偿运动畸变；
• 激光更新（第 6、7、12 篇）：在 VoxelMap 中构建点到面残差，ESIKF 迭代更新 19 维状态；
• 视觉更新（第 13、14 篇）：稀疏直接法对齐图像 patch，光度残差结合在线曝光估计，ESIKF 再次精修同一状态；
• 地图（第 9 篇）：VoxelMap 哈希八叉树增量维护几何地图，视觉地图点由激光点反哺生成；
• 输出：实时位姿、彩色稠密点云地图，可落盘为 PCD 或对接 COLMAP（第 17 篇）。

数学地基（第 4–7 篇）支撑上层模块，实战调参与落地（第 15–18 篇）使其真正运行于实际设备。这构成了后续 17 篇逐一展开的全部内容。

图 3　FAST-LIVO2 完整框架与全专栏路线图，各模块标注了对应精读篇号（数学地基第 4–7 篇、实战调参第 15–18 篇支撑全局）

九、小结与下篇预告

开篇厘清了四个要点：

需再次强调的校准结论：FAST-LIVO2 的地图结构是 VoxelMap，而非 ikd-Tree，这是相对前作 FAST-LIO2 的重要演进，详见第 9 篇。

理论框架已经建立，但代码需实际运行方能验证。下一篇进入实战第一关——环境搭建与编译避坑：Sophus 为何必须 checkout 至 a621ff 这一特定提交、rpg_vikit 与 fast-livo1 版本的差异、OpenCV/PCL/Eigen 的版本约束，以及 catkin_make 报错的速查表。将这套依赖一次性跑通，是理解后续全部源码的前提。

下一篇：《FAST-LIVO2 源码精读（二）：环境搭建与编译避坑》