Fast-LIO2在ROS2上的部署实践与避坑手册

做移动机器人和无人机的同行，应该都对Fast-LIO2不会陌生。它是一款基于激光雷达和IMU紧耦合的里程计与建图方案，用迭代扩展卡尔曼滤波把点云配准和状态估计放在一个流程里，配合增量式ikd-Tree维护地图，在室内外、无人机、手持设备上都能保持不错的实时性。我最早是拿它跑ROS1，后来整个项目迁移到ROS2，中间踩了不少编译和通信的坑。这篇文章我就围绕Fast-LIO2的代码结构、ROS2部署、参数配置和实际避坑经验来写，给正准备从ROS1转过来或刚开始接触这套代码的同学一份能直接照着操作的手册。

1. Fast-LIO2到底解决了什么问题

1.1 为什么需要紧耦合激光惯性里程计

在激光SLAM领域，激光雷达惯性里程计大致可以分成两条技术路线。一条是先做点云配准，再把配准结果交给滤波或者图优化框架，典型的代表是LOAM和LIO-SAM。另一条是把原始点云和IMU测量直接放进同一个状态估计器里求解，Fast-LIO系列就是这条路线。前者逻辑容易懂，但点云配准产生的噪声会被当作一个独立的观测误差送入后端，误差传递不够精细；而且LOAM的地图管理用体素栅格，LIO-SAM又引入因子图优化，代码量和参数个数都上来了。我最早用LIO-SAM跑一个室内小场景，回环检测确实漂亮，但到了长走廊或者弱纹理区域，后端的优化反而容易把人带到沟里，CPU占用也不小。

Fast-LIO2之所以在无人机、手持扫描枪这类资源受限的设备上流行，核心在于它把精度和效率重新做了平衡。它使用迭代扩展卡尔曼滤波（IEKF），在每帧点云配准的过程中同步更新IMU状态和位姿，不需要单独的全局后端。你可以把IEKF理解为“每一帧都在做小范围的非线性优化”，但它的迭代次数通常只有几次，计算量远小于因子图。实测下来，在同样一颗Jetson Orin NX上，Fast-LIO2跑Livox MID-360，CPU占用能控制在单核60%以内，而LIO-SAM整体要高出不少。这意味着在移动机器人上还能同时跑导航、感知等其他模块。

Fast-LIO2没有主动回环检测，它的定位更偏向里程计而非完整的SLAM系统。如果项目需要全局一致的轨迹和地图，可以把Fast-LIO2的输出作为前端，再接一个后端做位姿图优化。很多手持建模设备就是这么干的。明白这一点再去读代码，就不会因为“没有回环模块”而担心是不是代码缺了东西。

1.2 ikd-Tree给地图管理带来的改变

传统激光里程计的地图管理大多依赖体素栅格（voxel map）。体素栅格的思路是把空间切成固定大小的小方块，每个方块里只保留一个点或几个点的统计结果。这种方法实现简单，但分辨率固定，在点云密度变化剧烈的场景容易丢细节，而且随着地图范围变大，体素数量会膨胀，检索效率也会下降。普通KD-Tree查询快，但建树时要求一次性拿到所有点，没法增量更新。做实时里程计时，每一秒都在新增几千个点，如果用普通KD-Tree，每帧重建一次的代价是灾难性的。

Fast-LIO2使用的ikd-Tree解决了这个问题。ikd-Tree是一种增量式KD-Tree，支持在已有树结构上插入新点、删除离当前位姿太远的旧点，树结构只做局部重平衡，不需要整体重建。这样无论地图规模怎么增长，单帧的更新和最近邻搜索都能维持在一个可接受的复杂度内。配合局部地图的管理策略，地图点始终以当前位置为中心保留一个动态范围，超出范围的点会被标记删除。实际跑起来，在室内几个房间连在一起的场景里，Fast-LIO2的内存占用和CPU占用都要明显低于LIO-SAM，而且测试环境越复杂，优势越明显。

2. 代码结构拆解：从入口到核心模块

2.1 仓库目录与核心文件

Fast-LIO2仓库源码结构其实非常集中，核心基本都在src/laserMapping.cpp里。一个约两千行的文件包含了主流程的大部分逻辑。很多刚接触的人第一次打开会有点懵，尤其是看到各种状态量定义、雅可比矩阵计算、esekfom封装之后，头会大。我建议不要一开始就逐行读，而是先按文件理解结构：include/FAST_LIO/common_lib.h放了常用结构体和常量；IMU_processing.hpp里封装了IMU前向传播和点云畸变校正；laserMapping.hpp定义了laserMapping类，成员函数和核心成员变量都在这里；thirdparty/ikfom目录里则是ikd-Tree的完整实现。

config目录下一个yaml文件控制所有运行参数。你不需要改代码去换话题，改yaml就行，这比早期ROS1版本要方便。src/preprocess.cpp负责点云预处理，比如提取有效点、赋予时间戳、按线束分类等。src/IMU_processing.cpp实现IMU数据的前向传播，主要是运动和零偏预测。真正的状态更新逻辑在laserMapping.cpp里，它调用esekfom库完成IEKF的迭代更新。看代码时可以先把主要流程画在纸上，再对照看具体函数。

2.2 从回调函数看数据流

程序入口main函数里主要做三件事：读取参数、创建节点句柄、订阅话题并绑定回调。订阅的核心话题有两个，一个是激光点云，另一个是IMU。对于Livox雷达，默认通常是/livox/lidar和/livox/imu。IMU回调做的事情很简单，就是把IMU数据放队列里，同时更新最新的IMU时间戳。复杂逻辑全部放在点云回调里。

点云回调会先调用preprocess把原始点云转成当前帧的点云数组，给每个点附上相对时间。接着进入主流程：先判断当前帧时间戳和IMU队列里的时间戳是否同步。如果IMU数据落后于点云时间戳，节点会等待；得到可以用的IMU后，先做前向传播，也就是把IMU预积分的结果作为当前状态预测值。这个预测值会告诉系统雷达大概移动到了哪里。然后用预测位姿把点云投影到地图坐标系，再到ikd-Tree里搜索最近邻点，计算点到平面的残差，运行IEKF更新。更新之后，新的位姿会被写回状态量，点云也会被加入地图，同时检查地图范围，把太远的旧点清掉。

这里有一个关键点：Fast-LIO2不是先单独配准再优化，而是配准过程本身就在更新状态。代码里关键的预测和更新调用，大致是：

esekfom::predict(imu_poses, IMUdata); esekfom::update_iterated_dyn_share_modified(point_cloud);

第一行做IMU预测，第二行做迭代更新。如果你只想改运行逻辑，只需要理解这两个调用之间的关系；如果你想深入数学细节，再去esekfom库里看协方差、雅可比的具体计算。

2.3 点云配准与状态更新的关键环节

点云配准的核心目标，是让当前帧点云在地图上找到最合理的位置。Fast-LIO2用的不是ICP或NDT，而是点到平面的残差模型。每一帧选出一部分点，根据当前估计位姿投影到地图，搜索最近邻并拟合成局部平面，然后计算当前点到平面的距离作为残差。把残差对状态量的雅可比代入IEKF，完成一次迭代更新。更新后，再用新的位姿重新投影、重新搜索，反复迭代，直到收敛或达到max_iteration。

实际代码里会有几个过滤条件：距离太近的点会被blind参数滤掉，因为近距离点通常有较大噪声；法向量不稳定或离群太远的点也会被剔除。这些过滤看似简单，却直接影响建图质量。我遇到过一个问题：把blind设成5米，在室内只有一两米宽的走廊里扫描，基本切掉了全部有效点，地图自然建不起来。后来把blind降到0.5米，效果立刻正常。所以调试时如果发现地图突然空了，先检查blind和其他过滤阈值是否过大。

3. ROS2环境准备与依赖安装

3.1 系统版本与ROS2发行版选型

Fast-LIO2从ROS1迁到ROS2，最顺的路就是Ubuntu22.04加ROS2 Humble。Humble支持周期长，官方文档和社区教程都比较多，Livox的驱动也同时维护了Foxy和Humble分支。如果是新装系统，建议一步到位，不要用太老的ROS发行版。ROS2的安装方式官方文档写得很清楚，先添加apt源和密钥，再安装ros-humble-desktop即可。如果你只是跑Fast-LIO2，装ros-base也能编，但调试时没有rviz2很不方便，所以我推荐desktop，省得后面单独安装。

装完ROS2后再装基础依赖，主要是编译工具和PCL、Eigen等。我在实际环境里执行的是：

sudo apt install git build-essential cmake g++ libeigen3-dev sudo apt install ros-humble-pcl-ros ros-humble-pcl-conversions ros-humble-robot-state-publisher

PCL相关包是编译Fast-LIO2时由package.xml声明依赖的，即使当前节点不多，也建议一次性装齐。Eigen库要注意版本，太老的版本可能不支持C++17的部分特性。装完可以用pkg-config --modversion eigen3检查一下版本，通常3.4以上问题不大。

如果你是刚接触ROS2，先不要把Fast-LIO2的报错和ROS1的经验完全对应起来。ROS2从编译到运行都与ROS1完全不同，比如用colcon代替catkin，用ros2 node list代替rostopic list，通信中间件也换成了DDS。先花半天时间把ROS2的基本概念，比如节点、话题、服务、QoS弄明白，后面处理Fast-LIO2的问题会轻松很多。

3.2 Livox雷达驱动的选择与编译

如果你用的是Livox系列雷达，比如MID-360、Avia，必须用livox_ros_driver2。这个名字很多人会看错，以为和ROS1的livox_ros_driver差不多，结果直接把旧包扔进工作空间，编译了半天报一堆错。livox_ros_driver2支持ROS2的话题接口和DDS通信，和Fast-LIO2的ROS2分支是配套关系。clone的时候要选择ros2分支：

git clone -b ros2 https://github.com/Livox-SDK/livox_ros_driver2.git

把仓库放到~/fastlio2_ws/src下面，后续和Fast-LIO2一起用colcon build编译即可。需要注意livox_ros_driver2默认发布的点云和IMU话题名，不同版本可能不一样。有些版本发布的是/livox/lidar和/livox/imu，有些则是/pointcloud。这直接决定了Fast-LIO2的config该怎么配，所以编译完第一次运行前，一定要先启动驱动，执行ros2 topic list确认实际话题名。

3.3 工作空间创建与Fast-LIO2源码编译

创建工作空间很直接：

mkdir -p ~/fastlio2_ws/src cd ~/fastlio2_ws colcon build --symlink-install source install/setup.bash

ROS2用colcon而不是catkin，这是很多从ROS1过来的人会踩的第一个坑。Fast-LIO2源码如果下载的是官方仓库，需要注意分支是否支持ROS2。有些历史版本只有ROS1的CMakeLists，直接colcon build会报package not found或者没有生成ament相关的构建文件。建议优先选择明确标注支持ROS2的fork，或者按照官方README去切换分支。编译过程中最常见的报错是缺少某个ros-humble-xxx依赖，根据编译日志的package名去安装对应包就行。

编译完成后，可以用ros2 pkg list | grep fast_lio确认包是否被识别。如果看不到，检查package.xml里的package名是否和目录名一致，路径是否真的在src下。ROS2 colcon对包名有严格约束，目录名和package name不一致经常导致Unable to find package。

4. 从零到跑通的部署实操

4.1 config参数逐项解析

Fast-LIO2的config/mid360.yaml是官方提供的一套默认参数。每一行都值得理解：

看到这里，可能会觉得配置很简单。但注意两个容易忽略的地方：一是外参矩阵extrinsic_R和extrinsic_T，这代表雷达相对于IMU的安装关系。如果传感器装歪了又不改这里，地图会飘得不成样子。二是time_scale，如果为负或不对，点云运动补偿就会反着来，地图会出现奇怪的重影。实际调参时不要同时改多个参数，每次只改一个，观察地图变化，这样更容易定位问题。

4.2 用ros2 bag回放数据验证流程

在没有雷达或不想把设备搬到室外的情况下，用ros2 bag回放历史数据是最快的验证方式。先确认bag里的话题：

ros2 bag info recorded_bag

如果包含/livox/lidar和/livox/imu，直接回放：

ros2 bag play recorded_bag --rate 1.0

如果电脑性能紧张，可以加--rate 0.5降速回放，给Fast-LIO2留足计算时间。另开一个终端启动Fast-LIO2：

ros2 launch fast_lio mapping.launch.py config_file:=mid360.yaml

这时不要急着看rviz2，先看终端打印。正常情况会有里程计更新和地图规模的信息，如果一直停在等待IMU数据的状态，就要开始排查话题名和QoS。回放数据这个环节是排查系统的利器：因为bag是固定的，每次改代码或参数后的表现是可以复现的。我建议把一段具有代表性场景的bag保存下来作为基准数据，任何改动都用它做回归测试。

4.3 实车部署的外参、时间同步与运动激励

实车部署比回放多出几个硬条件。首先是外参标定。Fast-LIO2要求雷达相对IMU的位姿已知，如果你用的是一体化设备如MID-360，厂商一般会给出初始外参，但安装稍有偏差也会导致建图偏转。精标可以使用kalibr、livox_camera_calib这类工具，或者自己设计一个标定板场景。标定结果填进config后，还需要重启节点才生效。

其次是时间同步。Fast-LIO2的IMU前向传播依赖IMU和点云时间戳在同一时间基准上。如果雷达用硬件时钟，IMU用系统时间，几十毫秒的偏差就会让点云去畸变失效。我在一台工控板上遇到的情况是，点云边缘反复撕裂，后来把IMU和激光都统一到雷达主时钟，问题才解决。如果是纯软件方案，至少要把两个话题的时间戳改成同一台主机的时钟源，再检查是否有固定延迟。运动激励也容易被忽视：启动时如果设备完全静止，IMU零偏的初始估计可能不准确，建议启动后缓慢画一个“8”字，让状态更快收敛。

5. 常见问题与排查技巧实录

5.1 编译报错的三大类原因

编译期报错在任何一个开源项目迁移中都很常见，Fast-LIO2迁到ROS2最容易遇到三类。第一类是缺依赖，比如找不到pcl头文件或者找不到eigen，装好对应ROS2包后重新编译即可。第二类是C++标准版本问题，如果CMakeLists没设置-std=c++14或c++17，使用新版编译器时会报错，直接在CMakeLists里加上对应选项即可。第三类是colcon识别不到包，通常是package.xml缺失或包名不一致，把package.xml补齐、保证文件夹名和 一致就行。

遇到编译错误时，我的经验是先看第一行错误而不是最后一行，先确认是编译错误还是链接错误。编译错误通常指向某个头文件或语法，而链接错误常见的是某个库找不到，比如-lstdc++fs。ROS2中很多头文件路径都经过了ament_export，如果source环境不完整，明明装了包却找不到头文件的情况也时有发生。每次新终端记得source install/setup.bash，或者把source命令写进~/.bashrc，能省掉不少幺蛾子。

5.2 话题收不到或QoS不匹配

Fast-LIO2启动后如果没有进入主流程，十有八九是话题或QoS问题。ROS2和ROS1一个显著区别是QoS策略，如果发布端和订阅端的reliability、durability不一致，数据就传不过去。livox_ros_driver2默认可能是best_effort，Fast-LIO2如果强要求reliable，两边就匹配不上。检查方法很简单：

ros2 topic info /livox/lidar --verbose

输出里能看到Publisher和Subscription的QoS profile，对照一下。不一致时有两种改法：改livox驱动launch文件里的QoS参数，或者改Fast-LIO2订阅器的QoS设置。如果不想深究每个字段，最简单的办法是让Fast-LIO2的订阅器使用SensorDataQoS，这个策略是ROS2中为传感器数据设计的，reliability为best_effort、history为keep_last，和激光、IMU类话题配合最顺。

5.3 地图漂移和抖动怎么定位

漂移问题不要一上来就调滤波参数，先看现象定位。如果地图整体在某个方向缓慢偏移，先怀疑外参；如果地图局部抖动、点云撕裂，先怀疑时间戳和运动补偿；如果地图完全发散、轨迹乱飞，先怀疑IMU数据本身是否正常。一个快速判断方法：把IMU话题echo出来，看角速度和加速度是否随运动合理变化。如果输出全是0或数值异常，检查IMU驱动配置和话题名。

另一个常见原因是初始化阶段没有足够的运动激励。Fast-LIO2在刚启动时会尝试估计IMU零偏，如果设备静止太长时间，零偏估计值可能收敛到一个奇怪的位置，导致后续轨迹漂移。这时可以重新启动节点，并且在启动后让设备做小幅度的平移和旋转，让可观性尽快建立。还有一点，长时间运行后地图点会积累很多，如果不开启或调大filter_size_map，地图点过于密集也可能造成搜索耗时增加和轻微抖动。适当增加降采样能在性能和稳定性之间取得平衡。

5.4 Rviz2可视化与性能优化

Rviz2是ROS2的标准可视化工具，Fast-LIO2跑通后，在rviz2里添加PointCloud2显示，话题选FAST_LIO/map_global，Fixed Frame设为camera_init，就能看到实时地图。如果看不到，先检查话题是否发布，再检查frame_id是否匹配。Rviz2对大量点云的渲染效率不高，尤其当地图点达到几十万甚至上百万时，会很卡。

这个时候与其调Rviz2的参数，不如回到算法侧降低点云量。point_filter_num这个参数表示每多少个点取1个参与计算，增大它可以直接减少参与建图的点数，从而降低CPU和地图规模。我在机载设备上常用的组合是point_filter_num=8、filter_size_surf=0.5，精度损失很小但实时性明显提升。如果是调试阶段，还可以把rviz2里的Point Size调小一点，关闭不必要的网格显示，让刷新率提上来。

6. 进阶调优：不同传感器适配与地图复用

6.1 机械旋转雷达与Livox的切换

Fast-LIO2不仅支持Livox系列，也支持部分机械旋转雷达。切换前要看两个地方：config里的lidar_type，以及点云预处理逻辑。lidar_type为1时走Livox专用分支，点云去畸变和通道处理方式都不同；换成长距离机械雷达如Velodyne后，需要把lidar_type改为对应值，并重新设置scan_line等参数。这个参数如果不对，最直观的表现就是点云被错误投影，地图上出现一圈一圈的错层。

我自己的体会是，机械雷达的角分辨率、雷达频率都和Livox不同，参数不能复用。机械雷达点云数量大，point_filter_num要相应调大；beam号设置错误会导致点云按错误线束去畸变。如果你不确定当前雷达对应的lidar_type，最简单的方法是直接看Fast-LIO2的README，不同雷达的适配方式写得比较清楚。不要企图用一套参数跑所有传感器，不同传感器之间的数据特性差异很大。

6.2 里程计结果的二次开发接口

Fast-LIO2除了发布地图点云之外，还会发布里程计位姿，也就是odometry话题，通常话题名是/odom或/aft_pcl。这个话题包含位置和四元数，下游的路径规划、导航、避障模块可以直接订阅。如果你做的是移动机器人应用，建议从这个话题获取当前位姿，而不是自己再去解析点云。很多激光SLAM方案会同时发布tf变换，Fast-LIO2也一样，可以把雷达坐标系和IMU坐标系之间的tf输出给机器人基础框架。

二次开发时，要注意状态协方差不一定输出完整。Fast-LIO2重点在里程计精度，对协方差矩阵的维护可能不像其他滤波器那样完整。如果你需要把不确定性传给下游规划，可能得根据实际应用做近似或额外处理。这一点在代码里可以看到，发布Odometry消息时，有时协方差会被置成常量或单位阵，不要依赖它做太高精度的融合。

6.3 点云地图保存与后端复用

Fast-LIO2建出来的地图可以直接保存成PCD文件。config里把pcd_save_enable设为true，运行结束后会在指定目录生成点云地图。保存下来的地图可以交给其他模块做定位，比如ndt_scan_matcher或者ICP定位。我自己的流程是先用Fast-LIO2录一段高质量地图，保存PCD，然后用这张地图做后续的实时定位，这样前端建图和后端定位可以解耦，工程上更稳。

保存PCD要注意一个问题：地图坐标系是camera_init点开始累积的，如果设备移动距离很大，PCD文件也会很大。这时候可以用PCL的命令行工具或CloudCompare做降采样和裁剪，只保留有效区域。保存前先通过rviz2确认地图质量，如果发现局部扭曲或重影，不要急着保存，先修复参数或数据。地图是定位的底子，底子脏了后面整个系统都会受影响。

最后说一句，Fast-LIO2这套代码，真正难的不是编译和使用，而是理解它为什么这样做。建议拿到代码后先跟着数据流走一遍，再动手改参数。跑通之后，把默认参数、bag数据、外参标定结果都记录下来，以后换环境能省很多时间。这也是我在多个项目里踩过坑之后最想分享的经验。