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/srccd~/catkin_ws/srcgitclone 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#defineDIM_STATE(19)// Dimension of states (Let Dim(SO(3)) 3)共 19 维。其物理构成可由StatesGroup的成员及流形加法operator明确// include/common_lib.h:167StatesGroupoperator(constMatrixdouble,DIM_STATE,1state_add){StatesGroup a;a.rot_endthis-rot_end*Exp(state_add(0,0),state_add(1,0),state_add(2,0));// [0:3) 姿态a.pos_endthis-pos_endstate_add.block3,1(3,0);// [3:6) 位置a.inv_expo_timethis-inv_expo_timestate_add(6,0);// [6] 逆曝光时间a.vel_endthis-vel_endstate_add.block3,1(7,0);// [7:10) 速度a.bias_gthis-bias_gstate_add.block3,1(10,0);// [10:13) 陀螺零偏a.bias_athis-bias_astate_add.block3,1(13,0);// [13:16) 加速度零偏a.gravitythis-gravitystate_add.block3,1(16,0);// [16:19) 重力a.covthis-cov;returna;}逐项拆解这 19 维区间物理量维度含义[0:3)rot_end3载体姿态旋转属于 SO(3) 流形[3:6)pos_end3载体位置[6]inv_expo_time1相机逆曝光时间用于直接法在线光度标定[7:10)vel_end3载体速度[10:13)bias_g3陀螺仪零偏[13:16)bias_a3加速度计零偏[16:19)gravity3重力向量不假设已知在线估计方向3313333 19恰好吻合。其中有两个值得关注的细节。其一姿态的更新是乘法而非加法。rot_end一行采用this-rot_end * Exp(...)而非直接相加。旋转矩阵属于 SO(3) 流形“加一个增量在数学上不成立正确做法是将增量映射为李代数再经指数映射Exp还原为旋转矩阵并右乘到当前姿态即所谓右扰动模型”。位置、速度、零偏等量位于欧氏空间直接相加即可。第 4 篇将系统讲解这套流形加减的数学。其二gravity同样是状态量。FAST-LIVO2 不将重力视为已知常量而是在线估计其方向。这使系统在初始静止对齐不完美时仍能自我修正对手持设备尤为有利。与之对称状态的减法operator-将两个状态之差映射回 19 维误差向量姿态部分由Log将旋转之差映射回李代数// include/common_lib.h:194Matrixdouble,DIM_STATE,1operator-(constStatesGroupb){Matrixdouble,DIM_STATE,1a;M3Drotd(b.rot_end.transpose()*this-rot_end);a.block3,1(0,0)Log(rotd);// 姿态之差 → 李代数a.block3,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.cppintmain(intargc,char**argv){ros::init(argc,argv,laserMapping);ros::NodeHandle nh;image_transport::ImageTransportit(nh);LIVMappermapper(nh);mapper.initializeSubscribersAndPublishers(nh,it);mapper.run();return0;}三步构造LIVMapper读取参数、初始化各子模块、注册 ROS 订阅与发布、进入run()主循环。系统的核心是LIVMapper类其头文件include/LIVMapper.h以指针形式聚合了所有子模块// include/LIVMapper.h:157PreprocessPtr 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:534voidLIVMapper::run(){ros::Raterate(5000);while(ros::ok()){ros::spinOnce();if(!sync_packages(LidarMeasures))// ① 多传感器时间同步打包{rate.sleep();continue;}handleFirstFrame();// ② 首帧初始化与重力对齐processImu();// ③ IMU 前向传播 点云去畸变stateEstimationAndMapping();// ④⑤ ESIKF 状态估计与建图}savePCD();// 退出时落盘点云}五个动作对应整个系统的数据流sync_packages——从激光、IMU、图像三路缓冲队列中按时间戳组装出时间对齐的测量包。该步骤还包含 LIO/VIO 的调度逻辑第 8 篇详述。handleFirstFrame——系统启动时依据首帧确定起始时间并执行重力对齐将世界坐标系 z 轴对准重力方向。processImu——调用p_imu以 IMU 测量执行状态前向传播同时反向遍历去除激光点云的运动畸变产出feats_undistort。stateEstimationAndMapping——系统核心下文单独分析。savePCD——循环退出后将累积的彩色点云保存为 PCD。6.2 LIO 与 VIO 的调度开关第④步stateEstimationAndMapping是整套融合策略的中枢其本体仅为一个switch// src/LIVMapper.cpp:267voidLIVMapper::stateEstimationAndMapping(){switch(LidarMeasures.lio_vio_flg){caseVIO:handleVIO();// 视觉更新break;caseLIO:caseLO:handleLIO();// 激光更新break;}}关键在于lio_vio_flg标志其取值定义在common_lib.h的枚举中// include/common_lib.h:54enumEKF_STATE{WAIT0,VIO1,LIO2,LO3};sync_packages在打包时依据当前应处理激光还是图像将该标志置为LIO或VIO于是主循环每次迭代或执行激光更新或执行视觉更新。同一份状态_state由这两条路径交替精修正是第三节所述顺序紧耦合在代码层面的实现。此处不存在复杂的联合大矩阵融合机制即由这一switch结构承载。另需区分另一枚举SLAM_MODE它决定系统整体运行的模式// include/common_lib.h:48enumSLAM_MODE{ONLY_LO0,ONLY_LIO1,LIVO2};通过配置可使 FAST-LIVO2 退化为纯激光里程计ONLY_LO、激光惯性里程计ONLY_LIO或启用全部传感器的激光-惯性-视觉模式LIVO。这种可裁剪性使其既能在算力受限平台上运行精简配置也能在完整传感器套件上发挥完整性能。图 2 run() 主循环数据流融合调度由 lio_vio_flg 标志承载七、handleLIO 与 handleVIO两条更新支路的轮廓完整细节将在第 12、14 篇展开但开篇有必要先明确两条支路各自的职责建立整体认知。激光支路handleLIO的开头执行降采样并送入 ESIKF// src/LIVMapper.cpp:336voidLIVMapper::handleLIO(){if(feats_undistort-empty()||(feats_undistortnullptr)){std::cout[ LIO ]: No point!!!std::endl;return;}doublet0omp_get_wtime();downSizeFilterSurf.setInputCloud(feats_undistort);downSizeFilterSurf.filter(*feats_down_body);// 体素降采样feats_down_sizefeats_down_body-points.size();// ... 构建点到面残差、装配 H 矩阵、ESIKF 迭代更新、增量更新体素地图}该支路接收去畸变点云降采样后在体素地图中匹配对应平面构建点到面残差经 ESIKF 迭代校正状态最后将新点增量地融入地图。视觉支路handleVIO的核心则是将图像交由vio_manager处理一帧// src/LIVMapper.cpp:305vio_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 篇支撑全局九、小结与下篇预告开篇厘清了四个要点LIVO 的必要性——激光与视觉的退化场景互补融合方能在隧道、白墙等极端环境中稳健运行FAST-LIVO2 的三个反直觉设计——直接法不提取特征、单 ESIKF 顺序更新、将相机曝光纳入状态在线估计19 维状态向量的精确构成以及姿态采用流形右乘而非加法的原因源码主线——main → run五步循环以及lio_vio_flg标志如何调度激光与视觉两条更新支路。需再次强调的校准结论FAST-LIVO2 的地图结构是 VoxelMap而非 ikd-Tree这是相对前作 FAST-LIO2 的重要演进详见第 9 篇。理论框架已经建立但代码需实际运行方能验证。下一篇进入实战第一关——环境搭建与编译避坑Sophus 为何必须 checkout 至a621ff这一特定提交、rpg_vikit 与 fast-livo1 版本的差异、OpenCV/PCL/Eigen 的版本约束以及catkin_make报错的速查表。将这套依赖一次性跑通是理解后续全部源码的前提。下一篇《FAST-LIVO2 源码精读二环境搭建与编译避坑》
FAST-LIVO2 源码精读（一）：系统总览——从 LIO 到 LIVO 的多传感器融合架构
发布时间：2026/6/14 10:22:57
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/srccd~/catkin_ws/srcgitclone 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#defineDIM_STATE(19)// Dimension of states (Let Dim(SO(3)) 3)共 19 维。其物理构成可由StatesGroup的成员及流形加法operator明确// include/common_lib.h:167StatesGroupoperator(constMatrixdouble,DIM_STATE,1state_add){StatesGroup a;a.rot_endthis-rot_end*Exp(state_add(0,0),state_add(1,0),state_add(2,0));// [0:3) 姿态a.pos_endthis-pos_endstate_add.block3,1(3,0);// [3:6) 位置a.inv_expo_timethis-inv_expo_timestate_add(6,0);// [6] 逆曝光时间a.vel_endthis-vel_endstate_add.block3,1(7,0);// [7:10) 速度a.bias_gthis-bias_gstate_add.block3,1(10,0);// [10:13) 陀螺零偏a.bias_athis-bias_astate_add.block3,1(13,0);// [13:16) 加速度零偏a.gravitythis-gravitystate_add.block3,1(16,0);// [16:19) 重力a.covthis-cov;returna;}逐项拆解这 19 维区间物理量维度含义[0:3)rot_end3载体姿态旋转属于 SO(3) 流形[3:6)pos_end3载体位置[6]inv_expo_time1相机逆曝光时间用于直接法在线光度标定[7:10)vel_end3载体速度[10:13)bias_g3陀螺仪零偏[13:16)bias_a3加速度计零偏[16:19)gravity3重力向量不假设已知在线估计方向3313333 19恰好吻合。其中有两个值得关注的细节。其一姿态的更新是乘法而非加法。rot_end一行采用this-rot_end * Exp(...)而非直接相加。旋转矩阵属于 SO(3) 流形“加一个增量在数学上不成立正确做法是将增量映射为李代数再经指数映射Exp还原为旋转矩阵并右乘到当前姿态即所谓右扰动模型”。位置、速度、零偏等量位于欧氏空间直接相加即可。第 4 篇将系统讲解这套流形加减的数学。其二gravity同样是状态量。FAST-LIVO2 不将重力视为已知常量而是在线估计其方向。这使系统在初始静止对齐不完美时仍能自我修正对手持设备尤为有利。与之对称状态的减法operator-将两个状态之差映射回 19 维误差向量姿态部分由Log将旋转之差映射回李代数// include/common_lib.h:194Matrixdouble,DIM_STATE,1operator-(constStatesGroupb){Matrixdouble,DIM_STATE,1a;M3Drotd(b.rot_end.transpose()*this-rot_end);a.block3,1(0,0)Log(rotd);// 姿态之差 → 李代数a.block3,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.cppintmain(intargc,char**argv){ros::init(argc,argv,laserMapping);ros::NodeHandle nh;image_transport::ImageTransportit(nh);LIVMappermapper(nh);mapper.initializeSubscribersAndPublishers(nh,it);mapper.run();return0;}三步构造LIVMapper读取参数、初始化各子模块、注册 ROS 订阅与发布、进入run()主循环。系统的核心是LIVMapper类其头文件include/LIVMapper.h以指针形式聚合了所有子模块// include/LIVMapper.h:157PreprocessPtr 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:534voidLIVMapper::run(){ros::Raterate(5000);while(ros::ok()){ros::spinOnce();if(!sync_packages(LidarMeasures))// ① 多传感器时间同步打包{rate.sleep();continue;}handleFirstFrame();// ② 首帧初始化与重力对齐processImu();// ③ IMU 前向传播 点云去畸变stateEstimationAndMapping();// ④⑤ ESIKF 状态估计与建图}savePCD();// 退出时落盘点云}五个动作对应整个系统的数据流sync_packages——从激光、IMU、图像三路缓冲队列中按时间戳组装出时间对齐的测量包。该步骤还包含 LIO/VIO 的调度逻辑第 8 篇详述。handleFirstFrame——系统启动时依据首帧确定起始时间并执行重力对齐将世界坐标系 z 轴对准重力方向。processImu——调用p_imu以 IMU 测量执行状态前向传播同时反向遍历去除激光点云的运动畸变产出feats_undistort。stateEstimationAndMapping——系统核心下文单独分析。savePCD——循环退出后将累积的彩色点云保存为 PCD。6.2 LIO 与 VIO 的调度开关第④步stateEstimationAndMapping是整套融合策略的中枢其本体仅为一个switch// src/LIVMapper.cpp:267voidLIVMapper::stateEstimationAndMapping(){switch(LidarMeasures.lio_vio_flg){caseVIO:handleVIO();// 视觉更新break;caseLIO:caseLO:handleLIO();// 激光更新break;}}关键在于lio_vio_flg标志其取值定义在common_lib.h的枚举中// include/common_lib.h:54enumEKF_STATE{WAIT0,VIO1,LIO2,LO3};sync_packages在打包时依据当前应处理激光还是图像将该标志置为LIO或VIO于是主循环每次迭代或执行激光更新或执行视觉更新。同一份状态_state由这两条路径交替精修正是第三节所述顺序紧耦合在代码层面的实现。此处不存在复杂的联合大矩阵融合机制即由这一switch结构承载。另需区分另一枚举SLAM_MODE它决定系统整体运行的模式// include/common_lib.h:48enumSLAM_MODE{ONLY_LO0,ONLY_LIO1,LIVO2};通过配置可使 FAST-LIVO2 退化为纯激光里程计ONLY_LO、激光惯性里程计ONLY_LIO或启用全部传感器的激光-惯性-视觉模式LIVO。这种可裁剪性使其既能在算力受限平台上运行精简配置也能在完整传感器套件上发挥完整性能。图 2 run() 主循环数据流融合调度由 lio_vio_flg 标志承载七、handleLIO 与 handleVIO两条更新支路的轮廓完整细节将在第 12、14 篇展开但开篇有必要先明确两条支路各自的职责建立整体认知。激光支路handleLIO的开头执行降采样并送入 ESIKF// src/LIVMapper.cpp:336voidLIVMapper::handleLIO(){if(feats_undistort-empty()||(feats_undistortnullptr)){std::cout[ LIO ]: No point!!!std::endl;return;}doublet0omp_get_wtime();downSizeFilterSurf.setInputCloud(feats_undistort);downSizeFilterSurf.filter(*feats_down_body);// 体素降采样feats_down_sizefeats_down_body-points.size();// ... 构建点到面残差、装配 H 矩阵、ESIKF 迭代更新、增量更新体素地图}该支路接收去畸变点云降采样后在体素地图中匹配对应平面构建点到面残差经 ESIKF 迭代校正状态最后将新点增量地融入地图。视觉支路handleVIO的核心则是将图像交由vio_manager处理一帧// src/LIVMapper.cpp:305vio_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 篇支撑全局九、小结与下篇预告开篇厘清了四个要点LIVO 的必要性——激光与视觉的退化场景互补融合方能在隧道、白墙等极端环境中稳健运行FAST-LIVO2 的三个反直觉设计——直接法不提取特征、单 ESIKF 顺序更新、将相机曝光纳入状态在线估计19 维状态向量的精确构成以及姿态采用流形右乘而非加法的原因源码主线——main → run五步循环以及lio_vio_flg标志如何调度激光与视觉两条更新支路。需再次强调的校准结论FAST-LIVO2 的地图结构是 VoxelMap而非 ikd-Tree这是相对前作 FAST-LIO2 的重要演进详见第 9 篇。理论框架已经建立但代码需实际运行方能验证。下一篇进入实战第一关——环境搭建与编译避坑Sophus 为何必须 checkout 至a621ff这一特定提交、rpg_vikit 与 fast-livo1 版本的差异、OpenCV/PCL/Eigen 的版本约束以及catkin_make报错的速查表。将这套依赖一次性跑通是理解后续全部源码的前提。下一篇《FAST-LIVO2 源码精读二环境搭建与编译避坑》
相关文章
信用评分中Logistic Regression与XGBoost的实战选型指南
1. 这不是理论课，是银行风控团队每天在Excel和Python里反复调试的真实战场“传统逻辑回归 vs 现代机器学习在信用评分中的实战对比”——这个标题背后，不是教科书里的模型优劣辩论，而是某城商行风控建模组凌晨两点还在争论的一份上线评审意见…
探索Windows Cleaner：重新定义你的C盘空间管理体验
探索Windows Cleaner：重新定义你的C盘空间管理体验 【免费下载链接】WindowsCleaner Windows Cleaner——专治C盘爆红及各种不服！ 项目地址: https://gitcode.com/gh_mirrors/wi/WindowsCleaner 当你的电脑开始变得迟钝，C盘空间像沙漏…
如何快速掌握MTKClient：联发科设备调试与刷机的终极指南
如何快速掌握MTKClient：联发科设备调试与刷机的终极指南 【免费下载链接】mtkclient MTK reverse engineering and flash tool 项目地址: https://gitcode.com/gh_mirrors/mt/mtkclient 你是否曾因手机变砖而束手无策？或者想要深度掌控联发科设备…
FigmaCN：让全球顶尖设计工具说中文，设计师效率提升30%的秘密武器
FigmaCN：让全球顶尖设计工具说中文，设计师效率提升30%的秘密武器 【免费下载链接】figmaCN 中文 Figma 插件，设计师人工翻译校验 项目地址: https://gitcode.com/gh_mirrors/fi/figmaCN 还在为Figma的英文界面而烦恼吗？每次…
保姆级教程：用ENVI+Erdas搞定Landsat地表温度反演（含单窗算法建模避坑指南）
从零到精通的Landsat地表温度反演实战指南当第一次接触遥感温度反演时，许多初学者都会陷入公式复杂、软件操作繁琐的困境。本文将以ENVI和Erdas为例，手把手带你完成从数据预处理到温度反演的全流程，特别针对单窗算法建模中的常见陷阱提供解决…
3分钟掌握Jable视频下载工具：轻松实现离线观看
3分钟掌握Jable视频下载工具：轻松实现离线观看 【免费下载链接】jable-download 方便下载jable的小工具 项目地址: https://gitcode.com/gh_mirrors/ja/jable-download 还在为网络不稳定时无法流畅观看Jable视频而烦恼吗？想要将喜欢的视频保存到本…
新手避坑指南：用pnpm+vite创建React项目，这些配置细节你可能不知道
新手避坑指南：用pnpmvite创建React项目，这些配置细节你可能不知道刚接触前端开发时，包管理器和构建工具的选择往往让人眼花缭乱。npm作为老牌工具虽然稳定，但性能问题日益凸显；Yarn在速度上有所改进，却依然…
UniversalUnityDemosaics：3分钟学会Unity游戏视觉体验完整恢复终极指南
UniversalUnityDemosaics：3分钟学会Unity游戏视觉体验完整恢复终极指南 【免费下载链接】UniversalUnityDemosaics A collection of universal demosaic BepInEx plugins for games made in Unity3D engine 项目地址: https://gitcode.com/gh_mirrors/un/Universa…
魔兽争霸3兼容性增强工具：WarcraftHelper全面优化指南
魔兽争霸3兼容性增强工具：WarcraftHelper全面优化指南 【免费下载链接】WarcraftHelper Warcraft III Helper , support 1.20e, 1.24e, 1.26a, 1.27a, 1.27b 项目地址: https://gitcode.com/gh_mirrors/wa/WarcraftHelper 还在为经典游戏《魔兽争霸III》在现…
音乐文件解锁实战指南：3个场景解决你的播放困境
音乐文件解锁实战指南：3个场景解决你的播放困境 【免费下载链接】unlock-music 在浏览器中解锁加密的音乐文件。原仓库： 1. https://github.com/unlock-music/unlock-music ；2. https://git.unlock-music.dev/um/web 项目地址: https://git…
从Landsat到高分系列：手把手教你选择适合自己项目的遥感卫星数据
遥感卫星数据选型实战指南：从参数解析到场景化应用当面对GEE、PIE-Engine等云平台上数十种遥感数据源时，许多研究者常陷入选择困难——Landsat的历史连续性、Sentinel-2的红边波段优势、高分系列的亚米级分辨率各有千秋。本文将打破常规参数罗列式对比&a…
MC68302 AutoBaud技术：硬件级串口波特率自动检测原理与实现
1. 项目概述：MC68302 AutoBaud技术深度解析在嵌入式系统开发，尤其是那些需要与外部设备进行串口通信的场景里，最让人头疼的环节之一就是波特率匹配。想象一下，你设计了一个数据采集终端，需要连接来自不同厂家、不同年代…
音乐文件解锁实战指南：3个场景解决你的播放困境
音乐文件解锁实战指南：3个场景解决你的播放困境 【免费下载链接】unlock-music 在浏览器中解锁加密的音乐文件。原仓库： 1. https://github.com/unlock-music/unlock-music ；2. https://git.unlock-music.dev/um/web 项目地址: https://git…
从Landsat到高分系列：手把手教你选择适合自己项目的遥感卫星数据
遥感卫星数据选型实战指南：从参数解析到场景化应用当面对GEE、PIE-Engine等云平台上数十种遥感数据源时，许多研究者常陷入选择困难——Landsat的历史连续性、Sentinel-2的红边波段优势、高分系列的亚米级分辨率各有千秋。本文将打破常规参数罗列式对比&a…
MC68302 AutoBaud技术：硬件级串口波特率自动检测原理与实现
1. 项目概述：MC68302 AutoBaud技术深度解析在嵌入式系统开发，尤其是那些需要与外部设备进行串口通信的场景里，最让人头疼的环节之一就是波特率匹配。想象一下，你设计了一个数据采集终端，需要连接来自不同厂家、不同年代…
Zotero Duplicates Merger：5步彻底清理文献库重复条目
Zotero Duplicates Merger：5步彻底清理文献库重复条目 【免费下载链接】ZoteroDuplicatesMerger A zotero plugin to automatically merge duplicate items 项目地址: https://gitcode.com/gh_mirrors/zo/ZoteroDuplicatesMerger 还在为文献库中堆积如山的重…
利用随机有限集理论对蜂群的ILQR和MPC控制研究附Matlab代码
✅作者简介：热爱科研的Matlab仿真开发者，擅长数据处理、建模仿真、程序设计、完整代码获取、论文复现及科研仿真。🍎 往期回顾关注个人主页：Matlab科研工作室🍊个人信条：格物致知,完整Matlab代码及仿真咨询…
为什么你的Gemini邮件CTE低于行业均值2.8倍？：从Prompt架构到发送时序的深度归因
更多请点击： https://intelliparadigm.com 第一章：为什么你的Gemini邮件CTE低于行业均值2.8倍？：从Prompt架构到发送时序的深度归因 Gemini邮件的客户转化效率（CTE）显著偏低，根本原因常被误判为…