做SLAM的人如果2024年还没听说过FAST-LIO2那基本等于学视觉SLAM不知道ORB-SLAM。这套出自香港大学火星实验室MaRS Lab的开源LiDAR-Inertial Odometry框架从论文公开到代码开源一直是激光惯性里程计方向绕不过去的参考实现。它把紧耦合迭代卡尔曼滤波、增量式ikd-Tree地图、原始点云直接配准这些事情揉在了一起做出来的效果能打代码结构又足够清晰很多人入门LIO的第一站就是它。这篇文章我想用实战的方式聊一聊怎么把FAST-LIO2从零搭起来。不是复述论文也不是看README而是按照我实际踩坑的顺序讲环境怎么准备、代码怎么编译、官方数据集怎么跑通、跑完怎么判断好坏、以及那些文档里不会明说但你在编译到运行时一定会撞上的问题。适合刚接触SLAM的机器人方向学生也适合已经跑过LOAM、LIO-SAM但想换一套紧耦合框架看看的人。1. 先把框架看透FAST-LIO2到底解决了什么问题1.1 从LIO到FAST-LIO2这套框架的定位先明确一个概念。LIO全称是LiDAR-Inertial Odometry核心是把激光雷达点云和IMU惯性测量单元数据融合在一起估计机器人的位姿同时在线建一张点云地图。和纯LiDAR里程计相比多加一个IMU有两个明显好处高速运动、旋转剧烈的时候不至于跟丢点云运动畸变也能靠IMU做补偿。FAST-LIO2在这条技术路线上做了两个很关键的事情。第一它采用紧耦合方式IMU状态和雷达观测在一个迭代误差状态卡尔曼滤波IESKF里一起估计不需要像LOAM那样先做特征提取、再分odometry和mapping两个模块。第二它维护一张全局增量点云地图用自己实现的ikd-Tree做索引每一帧新点云直接和全局地图做配准而不是跟一个滑动窗口里的局部子图做配准。这一点从FAST-LIO一代到二代是最大变化带来的好处是大场景下不容易丢地图一致性更好。用一句话概括FAST-LIO2是一个依赖比较少、计算效率高、适合中低速机器人平台轮式、四足、无人机都有人用的紧耦合LIO框架。你要说它有没有缺点当然有后面我会聊到无回环、退化场景漂移这些问题但作为学习和落地的起点它足够优秀。1.2 核心模块拆解ikd-Tree、IESKF、运动畸变补偿理解FAST-LIO2我觉得看三个模块就够了。第一个是ikd-Tree这是港大MaRS Lab自己提的一种增量kd-tree。传统PCL里的KdTree是静态的建一次树之后如果往地图里插新的点要么整棵树重建要么用效率很低的方式逐点插入。ikd-Tree支持增量式插入、删除、动态re-balance并且维护了每个节点的信息做范围搜索和最近邻搜索时性能非常稳。在FAST-LIO2里面全局地图就是一棵ikd-Tree新点不断插进去旧点可能被删掉树本身会自己保持平衡。这个东西你在跑的时候看不到但它是FAST-LIO2能在大规模环境里保持实时性的根本。第二个是IESKF迭代误差状态卡尔曼滤波。传统扩展卡尔曼EKF在强非线性场景下容易发散ESKF是把误差状态作为估计对象而迭代的意思是用当前估计值重投影、重算观测残差、再更新状态反复迭代几次类似Gauss-Newton的思想。FAST-LIO2在每次雷达帧到达时做多轮迭代直到收敛或者达到最大迭代次数这套做法让滤波精度逼近优化类方法但计算量又比图优化小很多。第三个是运动畸变补偿。雷达单帧扫描是有时间戳的一个旋转周期内雷达在运动点云相对世界坐标系的坐标是“歪”的。FAST-LIO2利用IMU做反向传播backward propagation把一帧内各个点按时间戳对应的位姿重新投影到起始时刻的坐标系下再做配准。这套流程纯LiDAR里程计也有类似做法但FAST-LIO2因为紧耦合畸变补偿用的运动先验来自IMU和滤波状态所以更稳。1.3 对比LIO-SAM、LOAM该怎么选很多人在选框架时会纠结LIO-SAM和FAST-LIO2。我说点个人看法。LOAM系列是经典但它把位姿估计拆成odometry和mapping两个节点特征提取依赖线面特征在结构化环境里很好用一旦遇到植被、碎石这种无特征场景就容易拉胯。LIO-SAM是在LOAM的基础上加了因子图优化支持回环检测GTSAM、ScanContext等所以它做出来的轨迹长期一致性更好适合有大回环的场景但代价是模型更复杂、依赖更多、计算更重。FAST-LIO2走的是滤波路线没有显式回环单帧计算很轻地图是一棵增量树跑起来CPU占用低非常适合嵌入式、实时性要求高的平台。怎么选如果你要做园区巡检、无人机、需要轻量化部署首选FAST-LIO2如果你的场景有大量回环、希望后端能优化出漂亮的全局轨迹可以看LIO-SAM或者FAST-LIO2加ScanContext回环的方案。两个都跑一遍其实对这些框架的理解会一下子立体起来。维度FAST-LIO2LIO-SAM核心方法迭代误差状态卡尔曼滤波因子图优化回环检测官方无需自己接内置ScanContext计算量低中等偏高特征依赖无原始点云直接配准依赖线面特征典型场景实时性要求高、嵌入式大场景、带回环上手难度较低中等2. 动手前准备环境搭建与依赖安装2.1 系统与ROS版本怎么选最省心FAST-LIO2官方支持ROS1我推荐Ubuntu 20.04加ROS Noetic这个组合。为什么因为默认源里的PCL是1.10、Eigen是3.3.7正好满足FAST-LIO2的编译要求几乎所有依赖都能用apt装完不需要自己编译一堆老库。如果你用Ubuntu 18.04加Melodic也不是不行但PCL版本偏低编译时撞到Eigen对齐和C标准问题的概率大不少。Ubuntu 22.04加ROS 2也能跑但官方没有提供完整支持需要自己改CMake或者依赖一些第三方移植新手不建议一上来就折腾。还有一点尽量不要在虚拟机里跑FAST-LIO2的实时建图。虚拟机里USB设备透传和图形加速都是坑跑数据集虽然勉强能行但rviz会卡到怀疑人生。有条件就装物理机双系统没条件就上Docker镜像建议用ros:noetic-ros-base-focal然后自己按步骤装依赖。2.2 依赖库逐个装Eigen、PCL、livox_ros_driver先装基础的Eigen和PCLUbuntu 20.04下两条命令就搞定sudo apt update sudo apt install libeigen3-dev libpcl-dev装完之后建议看一眼Eigen版本很多编译报错的根源其实是版本太低pkg-config --modversion eigen3 cat /usr/include/pcl-1.10/PCLConfig.cmake | grep PCL_VERSION重点来了livox_ros_driver必须单独装。FAST-LIO2的消息接口依赖livox_ros_driver里的livox_ros_driver/CustomMsg所以这一个包没编译好后面整个工程都编不过。推荐用源码方式在你准备放FAST-LIO2的同一工作区里克隆并先编译cd ~/catkin_ws/src git clone https://github.com/Livox-SDK/livox_ros_driver.git cd ~/catkin_ws catkin_make source devel/setup.bash注意这里有个很大的坑Livox官方后来推出了livox_ros_driver2和FAST-LIO2配套的driver1并不兼容。很多人机器上装了driver2结果编译FAST-LIO2时报找不到livox_ros_driver/CustomMsg.h排查半天发现是版本冲突。如果你不是搞Livox SDK二次开发老老实实只用driver1就好。另外Sophus这个库FAST-LIO2官方仓库的ThirdParty目录里自带编译时会一起编不需要你手动安装但如果你系统里恰好也装了一个Sophus偶发的不兼容问题偶尔会出现真遇到就先把系统里的Sophus卸了再编。2.3 源码编译与ROS工作区配置拿到源码并编译步骤如下mkdir -p ~/catkin_ws/src cd ~/catkin_ws/src git clone https://github.com/hku-mars/FAST_LIO.git cd ~/catkin_ws catkin_make source devel/setup.bash第一次编译会有点慢因为ThirdParty里的Sophus也要一起编。等看到类似[100%] Built target fast_lio的输出就说明编译成功了。如果中间有报错不要急着反复catkin_make去碰运气先把报错信息贴到搜索里搜一下九成以上是下面这几个问题Eigen版本低、livox_ros_driver没source、C标准冲突、PCL组件缺VTK依赖。还有一个容易被忽略的细节每次新开终端跑FAST-LIO2之前都要记得source ~/catkin_ws/devel/setup.bash或者直接写进~/.bashrc不然roslaunch fast_lio会提示找不到包。这个问题在不同终端里非常常见尤其你习惯多终端分别source不同环境的时候。2.4 launch文件里那些参数是什么意思编完以后先别急着跑数据集打开launch文件看看结构。以mapping_horizon.launch为例这里对应的是Livox Horizon雷达里面除了常规的bag_path、lid_topic、imu_topic之外还有几组参数值得你逐个看arg namebag_path default/path/to/your_dataset.bag / param namelid_topic typestring value/livox/lidar / param nameimu_topic typestring value/livox/imu / param nameextrinsic_T typestring value0.04165 0.02326 -0.0284 / param nameextrinsic_R typestring value1 0 0 0 1 0 0 0 1 / param namefilter_size_surf typedouble value0.5 / param namefilter_size_map typedouble value0.5 /bag_path数据集bag文件的绝对路径不改成自己的路径直接跑就会报找不到文件。lid_topic和imu_topic雷达点云和IMU的topic名不同数据集可能不一样跑之前用rosbag info xxx.bag确认一下。extrinsic_T和extrinsic_RIMU到雷达的外参。这个必须和数据集实际情况一致我见过只改路径不改外参、跑出来轨迹飞到天上的情况就是从一套传感器的配置直接套到另一套上。filter_size_surf和filter_size_map体素滤波的叶子大小。数值越小保留点云越密地图细节越多但计算量越大数值越大跑得越快但地图越糊。官方数据集默认0.5通常没问题真机点云密度不同可以再调。跑数据集的时候launch通常会调用rosbag play节点自动播放bag。如果没有自动播放也没关系后面我会单独说手动启动的流程。3. 用官方数据集把流程跑通3.1 数据集怎么下载、选哪一个FAST-LIO2官方README里放了几个测试数据集的下载地址百度网盘和Google Drive都有常见的有行空数据集、香港科技大学校园数据集、还有室内楼梯场景。我建议新手先跑行空数据集因为场景不大、运动速度适中、点云质量也好用默认参数就能出不错的效果。港科大校园那个场景大、走廊长能体验到大场景下ikd-Tree增量地图的优势但如果你外参配错或者IMU频率不对漂移起来也会更明显。下载的时候注意一下bag文件和launch的雷达型号要匹配。行空数据集应该是Horizon格式港科大校园数据集也有对应的版本别拿Avia的bag配Horizon的launchtopic名和点云格式都不一样跑出来基本是废的。下载完以后推荐先跑一句rosbag info xingkong.bag看一眼里面有哪几个topic、各有多少帧数据、时长多长。正常一个官方数据集通常就只有/livox/lidar和/livox/imu两个核心topic偶尔会有别的但这两个是FAST-LIO2必须的。3.2 启动与可视化rviz里该看哪些topic确认数据集没问题后修改launch里的bag_path为bag文件的绝对路径然后启动roslaunch fast_lio mapping_horizon.launch如果launch里没有自动播放bag新开一个终端手动播放rosbag play xingkong.bag想让它等launch先跑起来几秒、接收端就绪后再开始发数据可以加个延时参数rosbag play -d 5 xingkong.bagrviz可视化建议直接加载官方仓库提供的配置。FAST-LIO2源码的rviz目录下一般都带现成的配置例如rviz -d ~/catkin_ws/src/FAST_LIO/rviz/fastlio.rviz如果没有现成配置跟着topic列表手动添加也行。几个重要topic/Odometry里程计输出的位姿rviz里用Odometry显示能看到坐标系在动。/path轨迹线这个是评估建图效果最直观的东西路径是否平滑、是否闭合。/cloud_registered配准后并注册到全局地图的点云实时显示建图结果。/cloud_effected当前帧里真正参与配准的有效点。跑起来以后你会看到点云地图一帧一帧往外长轨迹线跟着雷达运动延伸。如果一切正常你就能体验到FAST-LIO2那种“没什么感知延迟”的流畅感。3.3 判断建图效果不是有地图就行很多人跑完数据一看有地图就觉得成功了这远远不够。判断一个LIO跑得好不好我认为看三件事。第一是轨迹回环是否闭合。如果数据集里有一段路是走回去的看/path在回到起点时是否和原来轨迹重合。闭合误差大说明状态估计在过程中累积了漂移可能原因包括外参不准确、IMU噪声参数没调好、地图降采样太粗等。第二看地图细节是否清晰。走廊里的墙面、地面边缘、柱子边缘应该干净利落。如果地图出现明显的双层墙、拖影大概率是运动畸变补偿没做好或者外参误差导致前后帧点云对不上。第三看CPU占用和帧率。用top观察fast_lio进程的CPU占用正常跑数据集应该在30%到80%之间取决于机器。如果CPU直接满载很可能是点云太密、地图增长过大或者线程参数不合适后面我会讲怎么调。3.4 调参入门从数据集到自己雷达的第一步官方数据集跑通只是第一步真正要做自己的采集数据需要把参数体系理解透。首先是IMU参数。FAST-LIO2默认按200Hz IMU频率来设计Livox雷达内置IMU通常是200Hz这个一般不用改。但如果你用外置IMUcommon.launch或mapping_*.launch里面的imu_rate、噪声密度、随机游走参数都要改成传感器手册上的真实值。很多人在自采数据上漂移严重其实就是噪声参数还是官方默认。其次是外参。自采数据前一定要做好IMU到雷达的外参标定别自己拿尺子量一下就写进去。港大MaRS Lab有开源的livox_camera_lidar_calibration等工具如果你只有雷达和IMU也可以找lidar_imu_calib这类方案。标定完成后再把外参写进launch或yaml里跑起来轨迹稳定得多。最后是降采样参数。室内小场景filter_size_surf0.3左右能保留更多细节室外大场景0.5~0.8更合适不然地图点太多内存会膨胀。具体看你的算力和场景规模没有绝对最优。4. 我记录下来的那些坑常见问题排查实录4.1 编译期报错Eigen/PCL/livox driver三座大山编译报错是最容易劝退新手的环节但其实八成的编译问题都集中在几个固定组合上。第一个是fatal error: livox_ros_driver/CustomMsg.h: No such file or directory。这个报错几乎可以确定是livox_ros_driver没编译成功或者没被source。检查方法很简单先单独编译并source livox_ros_driver然后再回到顶层工作区编译FAST-LIO2。另外确认自己装的是driver1而不是driver2。第二个是Eigen相关的报错比如Static assertion failed: YOU_MIXED_MATRICES_OF_DIFFERENT_SIZES或者各种aligned_allocator报错。这通常是因为Eigen版本低于3.3。Ubuntu 20.04的apt源里Eigen 3.3.7基本没问题如果是Ubuntu 18.04或者更老的系统建议源码编译安装最新Eigen并注意编译选项里的C标准要用C14或17。第三个是VTK相关的链接错误比如cannot find -lvtkCommonCore-7.1。这个一般出现在PCL安装不完整时。Ubuntu里重新把PCL相关依赖补齐sudo apt install libvtk6-qt-dev libvtk6-dev或者干脆sudo apt install libpcl-dev再装一遍把依赖拉全。4.2 launch报错与topic不匹配编译过了运行时又会出现一波新问题。最常见的表现是launch启动正常但rviz里没有点云、没有轨迹。先别急逐层排查。第一用rostopic list确认当前有没有/livox/lidar和/livox/imu两个topic。如果没有说明bag没播放成功或者topic名和launch里写的不一致。第二用rostopic echo /livox/lidar -n 1看一帧数据能不能正常输出确认数据类型匹配。第三检查launch里lid_topic和imu_topic的字符串值和bag里的实际topic名是否完全一致差一个斜杠都不行。还有一类问题是时间戳。如果你发现rviz里点云是有的但一直不更新或者轨迹卡住不动多半是sim time没设置。FAST-LIO2一般建议用数据集时间戳在播放bag前设置rosparam set use_sim_time true rosbag play --clock xingkong.bag不过这个操作有时候会带来新的困惑如果launch里某些节点的时间依赖有问题反而会造成奇怪的延迟现象。我的经验是先不设use_sim_time直接跑如果出现时间错乱再反过来加。4.3 运行卡顿、漂移、地图断层怎么办跑着跑着地图开始拖影、漂移这是状态估计不对不是显示问题。最常见的原因有三个。一个是外参不对。我见过有人拿Horizon的外参去跑Avia采集的数据结果地图完全飞掉。外参这个东西必须精确到厘米和度级别差个几厘米在远距离点云上就会被放大成巨大误差。第二个是降采样参数和地图维护参数不合理。地图点越堆越多ikd-Tree再高效也架不住几十GB的内存跑着跑着开始卡是正常的。FAST-LIO2在地图规模上做了一些控制但如果你把filter_size_map设得太小地图点密度爆炸实时性就会掉。建议先用默认参数跑通再根据自己场景调整。第三个是退化环境。长直走廊、空旷停车场、隧道这类场景缺少几何约束Y轴方向的位置漂移几乎是必然的。FAST-LIO2没有后端优化单纯靠前端的雷达和IMU约束退化场景下漂移是物理规律不是参数调不好。想解决这个问题就要考虑加入回环检测、融合其他传感器相机、UWB、GNSS或者做运动约束。这里也说句公道话任何纯LIO框架在退化场景都会漂不是只FAST-LIO2这样。4.4 部署到真机时的关键检查项如果你跑完数据集准备上真机下面的检查项是我实际部署时踩坑总结出来的。第一时间同步。雷达和IMU如果没有硬件同步至少要保证软件时间戳的延迟在几十毫秒以内。检查方法是rostopic echo分别看两个topic的header时间戳画出来看看延迟是否稳定。时间戳抖动大的话运动畸变补偿会不准地图会糊。第二坐标系和frame_id统一。真机上激光的frame_id、IMU的frame_id、base_link之间的关系要理清楚。FAST-LIO2输出的位姿是雷达坐标系到世界坐标系的变换下游导航和规划如果用的坐标系不一致定位结果再准也没用。第三雷达驱动参数。Livox雷达在纯自采时要注意点云频率、回波模式、扫描模式。不要用非重复扫描模式的数据去跑FAST-LIO2官方支持的是标准扫描模式。这个看着不起眼实际能折腾人一晚上。下面把我遇到的一些典型问题和直接对策整理成表方便你排查现象可能原因对策编译找不到CustomMsg.hlivox_ros_driver未编译或版本不对单独编译source driver1rviz没有点云topic名不匹配或bag未播放rosbag info核对topic名轨迹发散飞掉外参错误或IMU频率设置错核对launch外参和IMU频率地图拖影严重运动畸变补偿异常、时间戳不同步检查时间戳对齐、IMU参数卡顿、CPU飙满地图点过多、降采样太小调大filter_size_map和surf长走廊大漂移退化场景缺少约束加回环或融合其他传感器另外有一个很多人忽略的小问题launch里如果配置了pointcloud_to_pcd保存地图边跑边存点云会占用大量磁盘IO可能导致实时性下降。我通常会跑完后再用rosrun pcl_ros pointcloud_to_pcd /cloud_registered手动录一帧大地图而不是一边跑一边存。5. 跑通之后还能怎么玩5.1 从纯雷达到雷达相机多传感器融合怎么接FAST-LIO2跑通后很多人会想给它加个摄像头做视觉激光融合。这个方向港大MaRS Lab自己也有一系列工作从FAST-LIVO到FAST-LIVO2就是在FAST-LIO2的基础上加入视觉信息实现视觉-激光-惯性紧耦合的里程计和建图。如果你只是想简单接一个相机做展示级别的融合可以先把相机标定好把视觉特征投影到雷达坐标系里做叠加显示但这只是可视化不是真正的融合。真正合入视觉要做的事情比较多光外参标定就涉及相机-IMU、相机-雷达两套外参。我的建议是先别急着融把FAST-LIO2源码读透理解状态估计和点云配准的流程再去接触融合框架会顺很多。不然代码里一堆变量你根本分不清是雷达的还是视觉的排错会非常痛苦。5.2 在嵌入式平台RK3588/Jetson上部署的可行性最近特别多人问RK3588这类ARM平台能不能跑FAST-LIO2。我可以明确说能跑但不能拿它和x86台式机直接比。FAST-LIO2本身不依赖GPU核心计算是IKD-Tree搜索和卡尔曼滤波迭代这两个都是CPU密集型的。在RK3588上如果雷达点云频率不高、地图规模控制好实时性是可能的但你要做好几个准备系统用Ubuntu 22.04或Debian系装好ARM版本的ROS建议大家优先用ROS 2或基于ROS 1的解决方案PCL和Eigen的交叉编译要提前验证。相比之下Jetson Orin系列因为是Ubuntu桌面生态跑FAST-LIO2要顺滑很多。如果你有NVIDIA家的板子优先用它做SLAM落地方案RK3588更适合做视觉任务配合NPU使用。这里多说一句很多人被“嵌入式”三个字吓住其实FPGA和MCU才是嵌式的苦海RK3588和Jetson这种Linux板卡跟普通电脑编译没什么本质区别就是把x86换成aarch64依赖装好就行。真遇到问题先去确认uname -a是aarch64还是x86_64再决定下载哪个架构的预编译包。5.3 给新手的学习路线建议如果你是从零开始学SLAM我建议的路线是先把高翔的《视觉SLAM十四讲》啃到前七讲以上把李群李代数、状态估计、非线性优化这些底子打好再上手FAST-LIO2。不然你看到IESKF公式里的协方差矩阵更新可能连符号都看不懂。看完理论基础后源码阅读顺序推荐这样先读laserMapping.cpp的主线程流程搞清楚run()里怎么等待点云、怎么触发处理然后看IMU处理部分理解状态预测怎么做的再看点云预处理部分理解运动补偿和降采样最后再啃配准和更新的迭代过程。每一块代码都不长但信息密度很高。另外可以看看B站和知乎上许多人对FAST-LIO2的源码解析不过我的经验是视频看完是别人的只有自己把launch参数改了、把bug排了、把地图跑崩了再修好这些知识才是你的。5.4 一点个人体会我自己在跑FAST-LIO2的时候有一个印象很深的经历第一次把官方数据集跑出完整地图时觉得这框架太强了后来在自采的园区数据上反复调整参数发现地图偶尔还是会漂才意识到SLAM这个领域其实没有“跑通就完事”这回事。FAST-LIO2把前端的很多问题解决得很好但它不会替你解决传感器标定、时间同步、场景退化这些系统工程问题。所以如果你刚开始学不用追求一次把所有参数调到最优。先跑通再慢慢改一个参数观察地图变化比如把filter_size_surf从0.5改成0.2看地图细节变化把外参故意加个几厘米偏差看轨迹怎么飘。这种“故意破坏再修复”的练习比看任何教程都让你印象深刻。SLAM是一个魔鬼藏在细节里的方向动手去踩坑才是最快的路。