简介面向SLAM学习者的FAST-LIO论文与代码理解笔记适合已有LiDAR、IMU基础或正在研读源码的读者。资源共包含1个docx文档压缩包大小约为5.09MB结构完整便于按章研读。目前已有3100余人学习下载关注度较高。笔记从李群、李代数、流形等数学基础切入逐步讲解LiDAR与IMU的数据格式、velodyne VLP-16传感器的选型要点、ROS通信机制、C编程预备知识随后梳理FAST-LIO框架中的LiDAR数据预处理、IMU初始化、点云特征点提取、状态估计与ikd-tree等核心模块再结合源码详细分析主程序、IMU_Processing.hpp、laserMapping.cpp以及esekfom库中的迭代扩展卡尔曼滤波实现拆解算法公式与代码组织方式附录还额外列出了STATE_IKFOM、INPUT_IKFOM的完整展开整理并给出了参考文献等补充资料。整份笔记逻辑清晰覆盖从数学基础到工程实现的完整链路能帮助读者更高效地掌握FAST-LIO的定位与建图原理。 做 LiDAR-Inertial SLAM 的人大概率都翻过 FAST-LIO 的论文和源码。它把 LiDAR 点云和 IMU 数据通过迭代卡尔曼滤波融合在一起在普通工控机上就能跑到实时精度和鲁棒性在同类方案里属于第一梯队。这篇是某开发者读 FAST-LIO 论文和代码时留下的逐章理解从李群李代数的数学准备、ROS 消息格式、VLP-16 点云结构一路拆到 esekf 迭代卡尔曼滤波和 ikd-tree 增量地图。如果你正卡在“论文公式能看懂、代码不知道从哪读起”或者想复现却总在编译和初始化阶段翻车这份笔记能帮你把论文和代码一一对应起来。新手按章节顺序读熟手可以直接跳到第 5 章看踩坑记录。2. 数学地基李群李代数与流形运算在状态估计里的实际落点2.1 为什么 FAST-LIO 不在旋转矩阵上直接加增量旋转矩阵 R 有三个硬约束行列式为 1、逆等于转置、行列向量都是单位向量且相互正交。如果你在滤波或优化里对 R 直接加一个扰动矩阵 δR得到的 R δR 几乎必然不满足这些约束下一步迭代就会数值发散。常见的处理思路是保持 R 本体不变把扰动定义在切空间 so(3) 上也就是旋转向量那一套。FAST-LIO 的状态估计走的是误差状态迭代卡尔曼滤波误差状态里的姿态增量就放在李代数上最后通过指数映射回到旋转矩阵。这一下就把“加法”从非线性的旋转矩阵空间挪到了线性的李代数空间协方差更新、状态叠加全都可以用常规线性代数操作不用每次做正交化。笔记里还强调了一个容易被忽略的性质旋转矩阵的特征向量代表旋转轴特征值代表旋转大小。这意味着你从 R 还原旋转向量时可以直接用特征分解但工程上一般用对数映射或者 AngleAxis 提取计算更快也更稳。2.2 指数映射与对数映射的代码形式和参数笔记里给出的指数映射公式是exp(r∧) Exp(r) I (r / ‖r‖) sin(‖r‖) (r² / ‖r‖²) (1 - cos(‖r‖))第一项是单位阵第二项是一阶近似第三项是二阶修正。这个公式在角度很小时会出现 0/0 的问题所以工程实现都要加小角度阈值。我之前在项目里写过一个简化版// 旋转向量转旋转矩阵对应 so(3) - SO(3) 的指数映射 Eigen::Matrix3d Exp(const Eigen::Vector3d r) { double angle r.norm(); Eigen::Matrix3d R; if (angle 1e-9) { // 小角度近似R ≈ I r^避免除以零 R Eigen::Matrix3d::Identity() Sophus::SO3d::hat(r); } else { Eigen::Vector3d axis r / angle; R Eigen::AngleAxisd(angle, axis).toRotationMatrix(); } return R; }逻辑说明先算出旋转角angle小于阈值时直接做一阶近似大于阈值时归一化得到单位轴再用AngleAxis构造旋转矩阵。Sophus::SO3d::hat的作用是把三维向量转成反对称矩阵也就是公式里的 r∧。参数说明1e-9 这个阈值在工程里够用再小的话浮点精度已经分不清 0 和 1e-12 的区别。如果你手写一个自研的Exp一定要注意angle恰好为 0 时r / angle是 NaN而不是无穷大。对数映射 Log 是从 R 提取旋转向量正好是 Exp 的逆过程。FAST-LIO 源码里大量用到这两个映射尤其是状态更新那一步。读代码时看到Exp、Log的调用不要跳过去它们决定了整个状态估计的坐标系定义。2.3 流形运算 ⊞ 和 ⊟状态更新那行代码在做什么流形Manifold这个概念在笔记里解释得很直白n 维数据可以用更低维的数据表示和运算自由度小于 n目的是提高运算效率。FAST-LIO 里把状态向量拆成两块旋转部分在 SO(3) 流形上其余部分在欧氏空间。笔记里的定义SO(3)R ⊞ r R Exp(r)R1 ⊟ R2 Log(R2ᵀ R1)ℝⁿa ⊞ b a ba ⊟ b a − b复合流形 SO(3) × ℝⁿ分块各自运算后组合代码里对应的就是 boxplus 操作。我在复现某个跨平台系统时写过类似的函数// 状态增量叠加x ⊞ dx用于迭代卡尔曼滤波里的状态更新 using state_ikfom Eigen::Matrixdouble, 18, 1; state_ikfom boxplus(state_ikfom x, const Eigen::Matrixdouble, 18, 1 dx) { // 姿态部分走 SO(3) 乘法R_new R * Exp(增量) x.block3, 1(0, 0) x.block3, 1(0, 0) * Exp(dx.block3, 1(0, 0)); // 位置、速度、零偏直接加法 x.block3, 1(3, 0) dx.block3, 1(3, 0); // p x.block3, 1(6, 0) dx.block3, 1(6, 0); // v x.block3, 1(9, 0) dx.block3, 1(9, 0); // bg x.block3, 1(12, 0) dx.block3, 1(12, 0); // ba x.block3, 1(15, 0) dx.block3, 1(15, 0); // g return x; }逻辑说明block 操作按状态向量排布取出子块姿态子块不能直接加必须左乘 Exp位置、速度、零偏都在欧氏空间直接加法即可。这个 boxplus 是迭代卡尔曼滤波里每次迭代都要调用的核心函数。参数说明状态向量维度这里是 18排布顺序是姿态、位置、速度、陀螺零偏、加速度计零偏、重力。实际源码里用的是 struct不是裸的 Eigen 向量但展开后的操作完全一致。2.4 复合流形在状态向量里的具体排布FAST-LIO 的迭代卡尔曼状态由姿态、位置、速度、两个零偏和重力组成。笔记附录 B 把 STATE_IKFOM 和 INPUT_IKFOM 做了完整展开看懂了才知道代码里为啥要分 segment。字段维度所属空间更新方式R姿态3SO(3)⊞ Exp 增量p位置3ℝ³直接加法v速度3ℝ³直接加法bg陀螺零偏3ℝ³直接加法ba加速度计零偏3ℝ³直接加法g重力3ℝ³直接加法这个表看起来简单但决定了协方差矩阵里哪些行做线性更新、哪些行做流形更新。如果你自己扩展状态向量比如加一个时间偏移量必须同步改 boxplus 和雅可比否则协方差预测那一步就会对不上。我见过有人没改 boxplus结果滤波发散排查半天才发现是状态维度和更新函数不匹配。2.5 推导旋转矩阵导数时的个人笔记态度笔记作者在附录 A 里写“旋转矩阵的导数看不懂暂时放弃吧ChatGPT 回答得也不明不白”这个态度其实很真实。旋转矩阵的导数本质上是 R′ R ω∧理解它需要把李代数左乘右乘分清楚左扰动和右扰动差一个正负号。FAST-LIO 的 IMU 处理用的扰动模型直接决定雅可比是左乘还是右乘。你读代码时如果发现 Jacobian 和自己推的对不上先别怀疑代码错检查一遍扰动模型是不是选反了。这算是我读过最多次的一个翻车点每次换传感器重新标定后我都会强制把外参旋转方向再过一遍这个流程。3. 数据链路ROS 消息、VLP-16 点云格式与 Eigen 对齐内存的四个细节3.1 PointCloud2、Imu、Odometry 三种消息的字段与时间戳对齐FAST-LIO 的输入是 ROS 标准消息点云是 sensor_msgs/PointCloud2IMU 是 sensor_msgs/Imu输出里程计是 nav_msgs/Odometry。三种消息的时间戳对齐是整个系统能跑起来的前提。PointCloud2 的关键字段不多但容易看晕字段含义注意事项height有序点云的行数VLP-16 一帧是 16 行width每行点数或无序点云总点数16 × 1800 28800point_step单个点的字节数包含 xyz intensity ring 等row_step一行的字节数point_step × widthfields每个字段的偏移和类型x 不一定从 offset 0 开始data实际二进制数据大小是 row_step × heightis_dense是否有无效点注意是 true 才没有 NaNIMU 消息里的orientation在 FAST-LIO 里基本不用用的是angular_velocity和linear_acceleration。千万别搞反我见过有人把四元数当角速度喂进去轨迹直接飞了。时间戳对齐的常见做法是在雷达回调里取出当前帧的起始时间和结束时间把落在该时间段内的所有 IMU 数据按时间插值或用原始值参与前向传播。IMU 频率一般是 200 Hz 左右雷达 10 Hz所以一帧雷达窗口内大约有 20 条 IMU 数据够用。3.2 VLP-16 的结构与降采样VLP-16 是 16 线激光雷达垂直视场约 30 度从 15 度到 −15 度水平 360 度角分辨率在 0.1 到 0.4 度之间。它的点云天然按线束排列这在处理时很有用因为同一线束上的点畸变模式相似。FAST-LIO2 默认不提取特征点对原始点云做间隔降采样。为什么用间隔采样而不是随机采样因为时间上相邻的激光点畸变模式相似等间隔采样能保住点云结构随机采样容易破坏平面分布。我在模拟项目 X 里对比过两种方式间隔采样在走廊场景下的配准残差明显更稳定。// 间隔采样每 4 个点取 1 个同时过滤近处和远处的点 for (size_t i 0; i pc_in-points.size(); i 4) { pcl::PointXYZINormal pt pc_in-points[i]; double dist std::sqrt(pt.x * pt.x pt.y * pt.y pt.z * pt.z); if (dist 0.5 || dist 100.0) continue; pc_out-points.push_back(pt); }逻辑说明步长 4 等效于把一帧 VLP-16 点云从约 28800 点降到 7200 点左右既保留了环境结构又把配准计算量压到实时可跑的范围。距离阈值过滤掉雷达自身近场噪声和远场低置信度点。参数说明0.5 米的下限在室内机器人上够了如果是车载可以放到 1.0 米防止车体自身反射点混入。100 米的上限按场景调室内可以缩到 50 米计算量还能再降一截。3.3 EIGEN_MAKE_ALIGNED_OPERATOR_NEW不写就崩的玄学FAST-LIO 代码里有一行很容易被新手忽略EIGEN_MAKE_ALIGNED_OPERATOR_NEW。它是一个宏作用是重载类的new和delete确保 Eigen 固定大小类型比如Eigen::Matrix3d、Eigen::Vector4d在堆上分配内存时满足 16 字节甚至 32 字节对齐。为什么要对齐因为 SSE 和 AVX 指令要求数据按特定边界对齐不对齐时处理器可能要多访问一次内存严重时直接段错误。我遇到过的情况是类里有一个Eigen::Matrixdouble, 3, 3成员编译能过一运行就崩加了这行宏就好了。class ImuProcess { EIGEN_MAKE_ALIGNED_OPERATOR_NEW // 不加这一行含固定大小 Eigen 成员时可能崩溃 public: ImuProcess() default; // 固定大小 Eigen 类型成员 Eigen::Matrix3d R_ Eigen::Matrix3d::Identity(); Eigen::Vector3d v_ Eigen::Vector3d::Zero(); };逻辑说明宏放在类定义的任意位置都可以惯例放 public 区因为外部代码要能直接用new创建该类的对象。它内部把 operator new 替换成 aligned 版本。参数说明如果你的对象是用工厂函数或std::make_shared构造的也需要这个宏因为底层还是走new。唯一的例外是对象本身在栈上但栈上数组如果用了Eigen::Matrix固定大小仍然要小心对齐问题。3.4 ROS 发布订阅骨架与回调的代码形态FAST-LIO 的主程序入口是一个标准的 ROS 节点核心就是把两个订阅和一个发布组织好。我一般会先把话题名和队列大小定下来再写回调int main(int argc, char** argv) { ros::init(argc, argv, fast_lio_node); ros::NodeHandle nh; // 点云话题队列小因为点云大且频率低缓存太多没有意义 ros::Subscriber lidar_sub nh.subscribesensor_msgs::PointCloud2( /velodyne_points, 5, LidarCallback); // IMU 话题队列要大IMU 频率高回调密集 ros::Subscriber imu_sub nh.subscribesensor_msgs::Imu( /imu/data, 200, ImuCallback); ros::Publisher odom_pub nh.advertisenav_msgs::Odometry( /Odometry, 100); ros::Publisher path_pub nh.advertisenav_msgs::Path( /path, 10); ros::spin(); return 0; }逻辑说明init的第一个参数是节点名ROS master 靠它识别节点subscribe的第二个参数是队列长度IMU 队列给 200 是因为 200 Hz 下 1 秒就产生 200 条消息队列小了会丢数据。发布器的第二个参数同理是缓冲区大小。参数说明点云队列给 5 就够了因为每帧点云处理时间大约在几十毫秒缓存 5 帧足够应对调度抖动。路径话题队列给 10显示用不需要缓存太多历史。4. 核心框架从 IMU 前向传播到迭代卡尔曼滤波与 ikd-tree 地图4.1 FAST-LIO2 系统架构与数据流从笔记的框架图看FAST-LIO2 的数据流分成两路一路是 LiDAR 点云预处理一路是 IMU 数据。点云预处理对原始点云做降采样IMU 数据做前向传播和状态预测两路在迭代卡尔曼滤波里汇合用点面残差更新状态最后把新点插入 ikd-tree 维护的局部地图。这个架构和传统 LiDAR SLAM 的最大区别是没有单独的视觉里程计前端也没有 TF 树广播。状态估计完全由滤波驱动地图只作为残差计算的参考。这意味着它依赖好的初始化和稳定的外参但不需要像图优化那样维护庞大的位姿图。4.2 LiDAR 预处理间隔降采样与特征点的取舍FAST-LIO 早期版本用角点和平面点做特征提取FAST-LIO2 改成直接对原始点云降采样。笔记里专门提到“默认不提取特征对原始数据间隔式间隔 3 个点降采样提取”。这个改动背后的逻辑是特征点提取会丢掉大量环境信息在退化场景长走廊、空旷场地下匹配质量下降而全点云配准虽然计算量大但提供了更多的几何约束。工程上的取舍是如果你在室内结构化环境保留特征点方案可以显著降低计算量如果在户外或走廊场景建议走 FAST-LIO2 的全点云路线。我自己在模拟项目 X 里两种都试过室外大场景下全点云的鲁棒性明显更好。4.3 IMU 初始化与前向传播的代码形态IMU 初始化在 FAST-LIO 里是很关键的一步。笔记里的流程是激光雷达数据到达后先判断 IMU 是否完成静止初始化否则等待。静止初始化主要做两件事估计重力方向估计陀螺零偏。初始化没完成后面的前向传播就是错的。前向传播的常见做法是中值积分对相邻两帧 IMU 数据的加速度和角速度取平均再更新状态。代码形态如下// IMU 前向传播中值积分imu1 和 imu2 是相邻两帧 IMU 数据 void forwardPropagate(const ImuData imu1, const ImuData imu2, state_ikfom s) { double dt imu2.t - imu1.t; // 相邻两帧取中值减少噪声尖峰影响 Eigen::Vector3d acc_mid 0.5 * (imu1.acc imu2.acc); Eigen::Vector3d gyr_mid 0.5 * (imu1.gyr imu2.gyr); // 补偿零偏并把重力从测量值里加回去注意符号约定 Eigen::Vector3d acc acc_mid - s.ba s.g; // 姿态更新R_new R * Exp(gyr * dt)右乘是 IMU 本体坐标系 s.R s.R * Exp(gyr_mid * dt); // 速度、位置更新 s.v acc * dt; s.p s.v * dt 0.5 * acc * dt * dt; }逻辑说明acc_mid和gyr_mid是相邻两帧 IMU 的均值中值积分比欧拉积分更稳。加速度测量值减去加速度计零偏ba然后再加上重力s.g是因为加速度计的测量模型是“比力”静止时测的是 −g这里要把重力加回来才能得到真正的加速度。参数说明dt是两帧 IMU 的时间差单位秒。IMU 频率 200 Hz 时 dt 约 0.005 秒中值积分在这个步长下精度足够。如果你的 IMU 只有 100 Hz建议换四阶龙格库塔积分中值积分的误差会明显变大。4.4 迭代卡尔曼滤波残差构建与状态更新FAST-LIO 用的是误差状态迭代卡尔曼滤波。先预测状态和协方差然后用 LiDAR 点面残差反复迭代更新状态直到收敛。点面残差的构建在代码里是一大块从 ikd-tree 找到最近邻的 5 个点用最小二乘拟合平面计算当前点到平面的距离。笔记里提到“构建局部地图提供最近点索引最小二乘方法拟合平面构建点面残差”这个流程对应的是代码里的 h_share_model 函数。迭代卡尔曼的核心调用形态我在复现时是这样组织的// 预测把 IMU 前向传播结果喂给滤波器 kf.predict(process_model, input); // 更新用当前帧的所有 LiDAR 点做迭代更新 kf.update_iterated_dyn_share(measurement_model, lidar_points);逻辑说明predict接收 IMU 输入执行状态和协方差预测update_iterated_dyn_share接收激光雷达点反复计算残差和雅可比迭代修正状态。迭代次数不是固定值而是看残差变化一般 2 到 3 次就收敛。参数说明迭代更新的一个重要参数是迭代收敛阈值太小会多算几次太大则精度下降。我一般取残差变化小于 0.1 米就停止迭代这个值在室内外通用。4.5 ikd-tree增量式维护局部地图ikd-tree 是 FAST-LIO2 的核心数据结构它是一个支持增量插入和删除的 KD 树。普通 KD 树每帧都要重建ikd-tree 只更新变化的部分大幅降低地图维护开销。局部地图的维护逻辑是新点插入远离当前位置的点删除树结构动态平衡。笔记里特别强调了 ikd-tree 的作用是实现高效的邻域搜索没有它点面残差计算会慢一个数量级。实际使用中ikd-tree 的调参重点是地图范围。范围太大内存和时间浪费范围太小回环和多视角约束不足。我一般把局部地图边长设在 100 米左右配合降采样分辨率 0.5 米在室内外场景都能保持实时。5. 源码拆解主循环、esekf 调用链与五条踩坑记录5.1 主循环总流程main 到 laserMapping 的调用链FAST-LIO 的主循环不是经典 ROS 回调驱动而是等雷达数据攒够一帧再处理。第一次运行时先初始化局部地图之后每来一帧执行完整的“预处理 → 前向传播 → 迭代更新 → 地图更新”流程。伪代码大致是这样// 主循环攒够一帧点云 对应 IMU 数据后开始处理 while (ros::ok()) { if (hasFrame) { // 1. 激光雷达数据预处理降采样 preprocess(pc_raw, pc_down); // 2. 如果 IMU 还没初始化做静止初始化 if (!imuInitialized) { tryInitIMU(); continue; } // 3. 用对应时间窗口的 IMU 数据做前向传播 imuProcess.pushData(imu_buf); imuProcess.predict(dt); // 4. 迭代卡尔曼更新 kf.update_iterated_dyn_share(measurement_model, pc_down); // 5. 更新 ikd-tree 局部地图 map.addCloud(pc_down); // 6. 发布里程计和地图 publishOdometry(); } ros::spinOnce(); }逻辑说明hasFrame是回调里置位的标志表示新一帧点云已经完整接收。初始化没完成时不会进入主处理流程这是为了避免用错误的初始状态跑滤波。参数说明初始化大概需要 1 到 2 秒静止数据。如果你发现初始化后轨迹一开始就偏多半是静止时间不够陀螺零偏没收敛。5.2 IMU_Processing.hpp 与 esekfom::esekf 的接口边界IMU_Processing.hpp 是 FAST-LIO 里专门处理 IMU 的类负责缓存 IMU 数据、做前向传播并把预测结果交给 esekf。esekf 是误差状态卡尔曼滤波的具体实现接口只有几个函数但内部逻辑很深。常见做法是// esekf 的调用接口predict 和 update_iterated_dyn_share kf.predict(process_model, input); // 状态预测 kf.update_iterated_dyn_share(measurement_model, points); // 迭代更新 // process_model 定义输入 IMU 数据输出状态导数 Eigen::Matrixdouble, 18, 1 process_model( const state_ikfom s, const input_ikfom u) { // 返回状态导数姿态导数、速度导数、位置导数、零偏导数、重力导数 }逻辑说明process_model返回的不是新状态而是状态的时间导数esekf 内部做离散积分。measurement_model返回的是残差和残差对状态的雅可比。这两个函数是用户必须实现的回调spirit 就是靠它们把具体问题接入滤波框架。参数说明雅可比矩阵的维度取决于残差维度。点面残差每个点一个残差一帧 7000 个点就是 7000 维的残差向量但雅可比是稀疏的esekf 内部会做压缩。5.3 避坑从编译到运行的五条踩坑记录坑 1编译报错提示类没有名为某个成员。现象编译到 laserMapping.cpp 时报错说找不到StatesGroup的某个成员。原因Eigen 版本太老或者 C 标准不是 C14。解决把 Eigen 升到 3.4 以上CMake 里显式set(CMAKE_CXX_STANDARD 14)。坑 2运行时突然段错误没有任何报错。现象程序跑几秒后直接 Segfaultgdb 看不出位置。原因某个类含固定大小 Eigen 成员但没加EIGEN_MAKE_ALIGNED_OPERATOR_NEW。解决给所有含Eigen::Matrix3d、Eigen::Vector4d这类固定大小成员的类补上宏。坑 3初始化阶段状态发散。现象跑数据包时前几秒里程计输出乱跳。原因初始化时传感器没有静止或者静止时间太短陀螺零偏估计不准。解决确保数据包一开始有至少 2 秒静止数据如果是真机启动后让设备静止 3 秒再动。坑 4里程计轨迹整体旋转。现象轨迹、地图整体相对于真实方向转了固定角度。原因雷达和 IMU 之间的外参旋转没标好或者标定矩阵正负号写反。解决用标定工具重新估计外参重点检查旋转矩阵的符号和坐标轴顺序。这个问题在 FAST-LIO 里最隐蔽因为它是全局一致错误不容易看出来。坑 5IMU 数据时间戳单位混淆。现象轨迹出现周期性抖动或者预处理时 IMU 数据对不上点云时间。原因bag 里 IMU 时间戳是纳秒代码里当微秒用或者反过来。解决先rostopic echo看时间戳大小判断单位再统一转成秒。这个坑我犯过一次从那以后每次换数据包第一件事都是查时间戳单位。6. 验证与进阶跑通数据包后这样检查状态估计拿到一份 FAST-LIO 的复现资源先别急着改代码按下面三步把状态估计验证一遍确认系统是好的再动手。第一步跑数据包同时打印或录制输出轨迹rosbag play your_bag.bag rostopic echo -n 1 /Odometry检查/Odometry输出的pose.pose.position是否有跳变。正常情况是连续平缓变化的如果某两帧之间位置突变超过几米说明初始化或外参有问题。第二步把路径话题画出来看形状rostopic echo /path路径话题是 nav_msgs/Path 数组存了全部历史位姿。把它画出来看轨迹是否闭合、是否有明显折返。如果轨迹绕大圈或者原地打转优先怀疑外参旋转符号。第三步检查 IMU 零偏是否收敛。在初始化完成后的 10 秒左右陀螺零偏应该稳定在一个接近常数的值数量级通常在 0.001 rad/s 附近。如果零偏持续漂移说明初始化静止时间不够或 IMU 数据本身有问题。进阶技巧是调降采样分辨率。FAST-LIO 对点云密度不敏感把降采样分辨率从 0.5 米调到 1.0 米计算量能降三成在室外大场景下轨迹精度几乎不变。室内小场景建议保持 0.5 米因为近距离点云密度高太粗会丢细节。有一次我在模拟项目 X 上跑室外数据轨迹整体绕了一个大圈排查了两天才发现是外参文件里雷达相对 IMU 的旋转写反了。从那以后我每次换传感器都强制先做外参标定再跑 FAST-LIO。希望帮到你。本文还有配套的精品资源点击获取