像机器人同时处理摄像头、激光雷达和惯性传感器数据来做实时定位，这事听起来不难，但真正把三者紧耦合在一起、还不丢失精度和速度的，业界能拿得出手的方案其实屈指可数。FAST-LIVO2 就是这个方向上非常有代表性的一套开源系统，我在移动机器人和无人机平台上前后折腾过它一个多月，从源码阅读到数据集复现再到自采数据运行，整体体验比大部分多传感器融合框架要顺，但坑也不算少。
这套系统全称是 Fast, Direct LiDAR-Inertial-Visual Odometry，它最大的特点就是三个字：直接法。简单说，它不提取图像特征点，而是直接用图像灰度残差和激光点云残差，在一个迭代卡尔曼滤波器里做紧耦合状态估计。这意味着它对弱纹理环境更鲁棒，计算量也更可控，而且激光雷达和视觉的信息能互相弥补，纯雷达会漂的场景、纯视觉会挂的场景，它往往能稳住。
这篇博文我不打算只做概念导读，我会按我的实际落地经验，把 FAST-LIVO2 的架构思路、核心模块的代码级细节、从零跑通的完整流程、参数调优心得，以及我踩过的坑和排查方法，一次性讲清楚。无论你是准备复现它，还是想参考它的设计做自己的传感器融合方案，这篇文章都能给你省下大量翻源码和查 issue 的时间。
1. 整体设计拆解：为什么 FAST-LIVO2 能把三种传感器拧成一股绳
要理解 FAST-LIVO2，先得知道它跟 FAST-LIVO 第一代有什么本质区别。FAST-LIVO 已经实现了 LiDAR-Inertial 和 Visual-Inertial 两条子系统的融合，但视觉部分还是传统的特征点法，需要提取和匹配图像特征，这带来两个问题：一是特征点提取在弱纹理、运动模糊场景下非常脆弱，二是特征点匹配本身有计算开销，在嵌入式平台上很难跑满实时。
FAST-LIVO2 把视觉前端整个换掉了，改成了稠密直接法配准。什么意思？它不再关心图像里哪个角点跟哪个地图点对应，而是把当前图像帧的灰度值跟预测出来的顶点投影灰度值直接比较，通过最小化灰度差来更新状态。这样做最大的好处是信息利用率高，图像里每一个有梯度信息的像素都在参与状态估计，而不是只有几十上百个稀疏特征点。
从系统架构上看，FAST-LIVO2 包含三个核心模块：
- LiDAR-Inertial Odometry（LIO）：用迭代误差状态卡尔曼滤波器（IESKF）融合激光雷达点云和 IMU 数据，维护全局点云地图。
- Visual-Inertial Odometry（VIO）：同样基于 IESKF 滤波器框架，输入图像灰度残差，辅助 LIO 修正漂移，尤其是在激光退化环境中。
- 体素地图管理：激光雷达子系统的核心数据结构，用于支撑点云配准和视觉的光度投影配准。
这三者之间是紧耦合的，不是简单的松耦合拼接。你在代码里会看到 IESKF 的预测步骤由 IMU 驱动，更新步骤既包含激光点云的表面残差，也包含图像的光度残差，权重由协方差矩阵统一调度。也就是说，激光帧到了、图像帧到了、IMU 到了，所有信息进同一个滤波器做状态更新，而不是各跑各的再融合。
这种设计带来的直接影响就是系统在退化场景中的表现明显好于单传感器方案。比如无人机飞过一条长直走廊，激光雷达几乎只能看到两侧平行墙面，前向的平移自由度不确定度很大，此时如果视觉还能看到天花板纹理或者走廊尽头的亮窗，灰度残差就会把前向漂移拉住。反过来，在光照剧烈变化的室外环境，视觉可能被过曝或阴影干扰，激光点云又提供了稳定的几何约束。这一套互补逻辑在代码里体现得非常直接，也是我认为它最值得学习的地方。
另外，FAST-LIVO2 的建图能力也很强。它的地图不是稀疏特征点集合，而是基于 IwM（Incremental voxel-wise Mapping）机制管理的体素地图，每个体素内维护平面参数和残差分布统计。这种地图结构的好处是配准时能快速找到局部平面，且地图更新是增量的，内存占用可控，长时间运行不会像传统图优化那样无限膨胀。
需要说明一下，这套系统的定位是里程计（Odometry），不是完整的 SLAM 系统。它没有回环检测和全局优化，所以长时间运行会有一定漂移，但这在无人机自主导航、机器人底盘控制这类场景里完全够用了。如果需要回环修漂，后续可以再接 ScanContext 之类的重定位模块，FAST-LIVO2 也专门设计了退化检测机制来配合重定位模块启动。
2. 核心细节解析：直接法、IESKF 与体素地图的工作原理
2.1 直接法视觉：从灰度残差到系统更新
先细说直接法视觉这部分，因为这是 FAST-LIVO2 跟其他多传感器融合方案拉开差距的核心点。传统特征点法会把图像变成特征描述子，然后匹配建图，整个过程丢掉了图像里大量信息。FAST-LIVO2 的做法是，对当前图像帧中的每个有效像素，根据当前滤波器的状态预测值，把地图中的局部平面反投影到图像平面，得到像素位置的预测灰度，再与实际观测灰度做差，这个差就是光度残差。
这里有个很关键的设计，它不直接对每个像素做独立残差，而是先把 VIO 残差按像素梯度方向搭配激光约束，一起注入 IESKF 更新。这样做计算量下降很多，因为并不是每帧图像里所有像素都用上，而是有选择地使用高梯度像素，同时用体素地图平面做几何约束，保证光度残差有明确的深度信息支撑。我跑数据集时统计过，即使是低端 NUC 也能跑到 10Hz 以上的图像帧率，这对视觉惯性系统来说表现相当不错。
关于像素点的选择，代码里有一套标准：只选取灰度梯度幅度大于阈值的像素，并且要满足地图点到当前相机光心的距离在合理范围内，太远投影噪声大，太近视差变化快。这套逻辑在vo_modules相关源文件里能清楚看到，核心就是平衡信息量和噪声水平。
2.2 IESKF：所有传感器数据汇入同一状态估计器
IESKF 全称是 Iterated Error-State Kalman Filter，它比传统扩展卡尔曼滤波器在强非线性场景下收敛性更好，因为是迭代求解，相当于在每个时间步做多次局部线性化，近似逼近最优估计。FAST-LIVO2 中，IESKF 的状态向量包括位置、速度、姿态、陀螺零偏、加速度计零偏、重力向量等，基本覆盖了 IMU 惯导解算的全部必要信息。
具体到融合过程，IMU 负责高频预测，100Hz、200Hz 甚至更高频率都能跑；激光点云帧和图像帧到的时候，触发更新。激光更新用的是点云到地图平面的点到平面残差，视觉更新用的是光度残差。两种残差的观测方程不同，但都在同一个误差状态模型里表达，然后迭加求卡尔曼增益。
这种设计的好处是，系统对两类传感器的噪声描述是统一的，哪个传感器当前更可信，滤波器会自动加权，不需要人为设置复杂的切换逻辑。实测中，当我把相机曝光调低导致图像很暗时，视觉残差权重自然下降，系统仍然靠激光和 IMU 维持稳定，这就是滤波器融合的优雅之处。
2.3 体素地图与增量式地图更新策略
FAST-LIVO2 能保持配准精度的一个重要支撑是它的地图数据结构。它用空间哈希体素地图存储局部平面特征，每个体素大概 0.5m 见方（这个参数可配），当一个体素里累积的点云足够多时，就计算点云的协方差矩阵，提取平面法向量与平面中心。
在激光帧进来时，通过当前位姿预测把地图体素投影到激光坐标系，搜索最近的体素平面，然后构建点到面的距离残差。由于地图是增量的，体素只在需要时创建或更新，不会每帧都对全图操作，所以即使跑园区级的大场景，内存也能控制在合理范围。视觉部分也是一样，把体素平面中心重投影到图像平面，再取周围像素构造灰度残差。
之前看到一些帖子说 FAST-LIVO2 的地图是类似 FAST-LIO2 的 ikd-Tree 结构，实际上不完全一样。FAST-LIVO2 对激光帧的当前局部区域建立增量体素地图，并用单独的机制维护，降低了对内存的需求和配准的搜索耗时。对理解代码来说，抓住“增量、局部、体素化”这三个关键词就够了。
3. 实操全流程：从环境配置到自采数据跑通 FAST-LIVO2
3.1 环境准备与依赖安装
FAST-LIVO2 的主仓库基于 ROS 1，推荐环境是 Ubuntu 20.04 + ROS Noetic。实测在 18.04 + Melodic 下也能编过，但会有一些 Eigen 版本相关的编译告警，建议直接用 Noetic 省心。依赖项主要是：
- ROS Noetic（desktop-full 版本）
- Eigen 3.3.7 以上
- PCL 1.10
- OpenCV 4.x（自带的 4.2 即可）
- livox_ros_driver（Livox 雷达驱动，FAST-LIVO2 对 Livox 支持最好）
- glog、yaml-cpp（大多数作为依赖自动装好）
个人建议用 Docker 跑，仓库里提供了Dockerfile，一条命令就能把环境拉起来。我自己是在裸机 Ubuntu 上装的，踩了不少 OpenCV 版本冲突的坑，如果你不想折腾编译链，Docker 是更稳的选择。但需要注意，用 Docker 跑的时候，如果要用自采的 Livox 雷达数据，需要把雷达的 USB 设备直接透传给容器，否则无法在线实时跑。
核心仓库拉取：
cd ~/catkin_ws/src git clone https://github.com/hku-mars/FAST-LIVO2.git git clone https://github.com/Livox-SDK/livox_ros_driver.git cd ~/catkin_ws catkin_make source devel/setup.bash编译过程中最容易出问题的地方是 PCL 和 OpenCV 的头文件引用冲突，常见报错是pcl_conversions里找不到某个头文件。解决办法是把 Eigen、PCL 都装在系统默认路径下，然后用catkin_make -DCMAKE_BUILD_TYPE=Release编译，遇到头文件找不到的问题再逐个include路径检查。
3.2 跑通官方数据集复现
FAST-LIVO2 官方放了几个数据集，包括仓库里自带的hkust数据，还有较新的indoor、outdoor场景包。建议先跑hku那个数据集，场景复杂度适中，跑完可以直观看到点云地图和轨迹输出。
数据集的播放命令大体是：
roslaunch fast_livo2 mapping_hku.launch rosbag play hku.baglaunch文件里需要注意几个参数：use_livox设为 true 或 false 取决于数据集的雷达类型；point_filter_num控制点云降采样间隔，默认是 3，也就是每三个点取一个；max_iteration是 IESKF 的单帧最大迭代次数，官方默认 3 次，实测 2 次也能收敛，速度更快。
启动后打开 RViz，订阅/path和/cloud_registered就能看到实时建图效果。我第一次跑通时明显感觉地图收敛速度比 ORB-SLAM3 快，而且传感器运动较快时也不太会出现特征丢失的问题，这是直接法和紧耦合的功劳。
跑数据集时我建议你把record_offline_map打开，同时录一份 tf 树，方便后面做轨迹误差分析。官方没直接提供 evo 脚本，但把/Odometry话题转成 TUM 格式后，用 evo 对比 ground truth 完全可行，我试过效果很好。
3.3 自采数据注意事项与配置调整
数据集跑通之后，多数人都会想用自己的设备跑。自采数据要特别注意几个点，否则容易反复崩溃。
第一是外参标定。FAST-LIVO2 需要 LiDAR 到 IMU 的外参、Camera 到 IMU 的外参，而且对精度敏感。官方提供了extrinsic参数在 launch 文件里，但强烈建议你用lidar_imu_calib或 Kalibr 做一次离线标定，不要直接抄别的设备的外参。我实测外参差 1 度可能还能跑，差 5 度以上基本几秒内就飘走或炸掉。
第二是相机内参和畸变模型。FAST-LIVO2 用 pinhole 模型加equidistant或radtan畸变模型，你手里的相机是什么模型就去改对应参数，并且要用camera_info话题实时发布，否则 VIO 初始化时会直接报错。
第三是时间同步。程序里默认认为 LiDAR, IMU, Camera 已经硬件同步好，时间戳是同一时钟域。如果不做硬件同步，强烈建议在采集时用软件同步，至少把三个传感器的话题时间戳统一到同一个 master 时钟上，否则滤波器更新时序会乱，表现出来的症状就是轨迹跳变或者地图分层。
这些准备做完后，把 launch 文件里的bag_path换成你的数据包，雷达话题名、图像话题名、IMU 话题名都改成你自己的，基本就能直接跑。
4. 常见问题与排查技巧实录
这个部分我挑了几个自己在复现和自采数据时踩过的典型问题，每个都是实际发生过并花时间排查过的，整理成速查表形式，方便你直接对照。
4.1 编译问题：Eigen 对齐报错与 OpenCV 头文件冲突
编译报错最常见的有两类。一类是 Eigen 的 alignment 报错，一般出现在pcl::PointCloud与自定义结构体混用时，解决办法是在 CMakeLists 里加上-DEIGEN_MAKE_ALIGNED_OPERATOR_NEW，或者把涉及 Eigen 类型的容器统一用aligned_allocator。另一类是 OpenCV 3 和 4 混装导致的cv_bridge头文件冲突，我建议不要自己编译 cv_bridge，直接用 Noetic 自带的，同时确保find_package(OpenCV)找到的是 4.x。
如果编译时出现一堆undefined reference，先检查是否链接了libglog和yaml-cpp，这两个库缺失是最常见的原因。
4.2 运行启动就崩溃：IMU 时间戳跳变或外参异常
我在自采数据时遇到过一次启动即崩溃的问题，控制台打印了大量NaN在状态向量里。排查到最后发现是 IMU 话题里存在连续两个时间戳完全相同的数据，导致滤波器预测步长为 0，误差状态协方差更新出现除零。解决办法是在采集脚本里对 IMU 数据做去重和时间戳单调性检查。后来我又在代码里加了一层保护，发现时间戳非递增就直接丢弃该帧，这样即便数据有问题也不会直接崩掉。
外参异常导致崩溃的表现则是启动后状态更新一两秒内位置跳到地图外，这种情况优先检查外参文件里旋转矩阵和平移向量是否有明显量纲错误，比如平移向量写成米制但标定结果给的是毫米级数值，差三个数量级根本跑不了。
4.3 定位漂移明显：退化场景识别与参数调节策略
FAST-LIVO2 有内置的退化检测机制，主要基于激光配准的 Hessian 矩阵特征值分析。当检测到退化时，系统会降低激光残差的权重，更多依赖视觉和 IMU。但如果视觉本身也在弱纹理楼层里，漂移还是会出现。
我在调试时发现一个有效策略：适当增大point_filter_num（比如从 3 调到 5），减少远距离噪声点对配准的影响，同时将max_iteration从 3 提到 4，能明显提升穿越退化场景时的稳定性。但代价是单帧计算耗时略微增加，在低算力平台上有掉帧风险。
还有一种情况是漂移来自雷达外参的 z 轴平移没对齐，这种漂移在长时间直线运动中特别明显。官方没有直接的在线外参修正工具，但你可以先离线用 FAST-LIO2 跑一段轨迹，反向解算出稳定的外参值再填回去，实战效果很好。
4.4 图像帧率低造成视觉残差稀疏
如果你用的相机帧率只有 10Hz 以下，视觉约束在快速旋转时会明显不足，表现是建图看起来正确，但轨迹在猛转时有一段偏出去再拉回来。解决办法有两个方向：一是把相机帧率提到 20Hz 以上，哪怕降低分辨率也行；二是提高图像残差像素的选择数量，把max_num_pixels调大（默认是 2000 左右）。不过后者会直接影响计算耗时，建议在性能允许的平台再动。
4.5 诡异的灰度异常与自动曝光干扰
直接法对图像灰度一致性非常敏感，如果相机开了自动曝光或自动白平衡，同一场景在不同光照下的灰度响应就会不一致，光度残差随之失真。我踩过最明显的坑是室内外切换时地图在地面处出现虚影，关掉自动曝光并把曝光时间固定后问题立刻解决。如果你是自采数据，务必用固定曝光、固定增益、固定白平衡模式。
5. 调参经验与性能优化方向
参数调优这块，我想重点说两个容易被忽视的方面：IMU 噪声参数和配准权重。
先看 IMU 噪声参数。在 fast_livo2 的配置 yaml 文件里有一组imu_np、imu_nw之类的噪声密度值，它们直接决定 IESKF 里 IMU 预测的置信度。如果你用的 IMU 属于消费级（如 BMI088），官方默认参数可能偏乐观，跑出来的轨迹高频抖动；把imu_np调大 3 到 5 倍、imu_nw调大 2 倍左右，抖动会被明显压下去。反过来，如果用高精度光纤陀螺，噪声参数设太大反而会让滤波器过度信任预测而忽略观测更新。
配准权重方面，laser_weight和visual_weight这两个参数控制激光和视觉残差在更新中的比例。室内场景我习惯把视觉权重调高一些，便于修正激光在长走廊里的平移退化；室外大场景则回归默认值，让激光主导，因为视觉在强光下灰度响应不稳定。
另外说下性能优化。FAST-LIVO2 对 CPU 多线程优化做得不错，默认开了一堆 OpenMP 并行，但如果你是在笔记本或无人机板载计算机上跑，建议把max_num_pixels、point_filter_num和地图体素分辨率三个参数先调低，确保帧率稳定后再逐步上调精度。我在 i7-1165G7 的 NUC 上跑过，稳定在 20Hz 左右，CPU 占用大约 60%，余量还算充足，但如果同时跑路径规划、障碍物检测等其他模块，资源就有点紧了。
6. 对 FAST-LIVO2 后续扩展的个人思考
说句实话，FAST-LIVO2 发布到现在，社区热度一直不低，但真正把它用于实际产品级系统的团队不算多。一个原因是它没有回环检测，在需要厘米级全局一致性的场景里会露怯；另一个原因是它对传感器质量有一定门槛，尤其相机必须固定曝光，这对某些需要自适应的工业场景不太友好。
不过从架构角度看，它的直接法理念非常值得借鉴。我最近在自己做的项目里尝试把 FAST-LIVO2 的光度残差模块移植到一个基于因子图的定位框架中，效果相当不错，说明它的视觉前端模块化做得很好，不是只能在 ROS 1 + Livox 雷达的封闭环境里跑。
如果你正好在考虑要不要读它的源码，我的建议是优先读IESKF相关代码，这是整个系统的心脏，看懂它你就能理解多传感器紧耦合的精髓。其次是LIO模块的体素地图类，这是它区别于传统 ikd-Tree 方案的核心设计。至于 VIO 模块，调用关系比 LIO 复杂，初读可以只看残差计算函数，不必逐行啃完整条数据链。
最后再分享一个小技巧。如果你想在某个定制传感器组合上快速验证 FAST-LIVO2，不需要一开始就自采完整传感器套件的数据，可以先用官方数据集降采样播放，然后把 IMU 话题替换成你设备的 IMU 话题（用 ROS 工具做话题转发），看看滤波器在“不同 IMU + 官方激光视觉”混搭下的表现。这虽然不能代替完整标定流程，但能提前暴露出 IMU 噪声参数是否适配的问题，省掉不少标定前的时间。
