★ FEATURED ARTICLE 简介面向激光雷达SLAM学习者的FAST-LIO论文与源码阅读笔记从李群李代数、流形等数学基础出发系统梳理了数据格式、VLP-16传感器、ROS与C实现等前置知识并对FAST-LIO整体框架、LiDAR预处理、IMU初始化、点云特征与状态估计展开说明尤其着重解读ikd-tree高效邻域搜索及源码主程序、laserMapping等模块帮助读者打通从论文理论到代码实践的路径。整份笔记以单个docx文档形式打包压缩包大小约5.09MB目录结构层次清晰便于按需跳转。这份学习笔记没有堆砌公式而是将个人理解与代码位置对应起来并附有参考文献和附录推导适合研究生、工程师在调通FAST-LIO前快速建立整体认识。资源已有3139人浏览学习适合希望从能跑通走向看懂并修改的读者收藏参考。1. FAST-LIO 为什么能成为 LiDARIMU 紧耦合的标杆先给结论先抛一个反直觉的事实FAST-LIO 在点云前端几乎不提取特征而是一股脑把降采样后的原始点云拿去做配准却能做到比很多「先提特征再匹配」的激光 SLAM 更稳。它的底气来自两件事一是把状态估计建模成「流形上的迭代误差状态卡尔曼滤波」二是用 ikd-tree 维护一个可增量增删的局部地图。前者让 LiDAR 的观测残差能反复迭代进 IMU 递推出来的状态里后者让地图更新不再每帧全量重建。理解 FAST-LIO本质上是理解「为什么旋转增量不能直接做加减法」以及「动态地图怎么做到删除点还能维持查询效率」这两层机制。这篇笔记带你从李群基本运算一直拆到源码里的滤波更新和树结构每一步都回答「为什么这样设计、怎么实现的」。2. 流形上的状态估计为什么旋转增量不能直接加在矩阵上2.1 李群与李代数旋转变量不在欧氏空间里看 FAST-LIO 源码时凡是在state_ikfom里看到旋转矩阵R都要意识到一件事它不是普通向量。旋转矩阵构成特殊正交群 SO(3)它满足R^T R I且 det(R)1。这两个约束意味着你在更新旋转时不能直接写R R ΔR因为两个旋转矩阵相加的结果几乎肯定不满足正交性也丢掉了行列式为 1 的性质。李代数才是旋转的「切空间」表示。每个旋转矩阵都对应一个反对称矩阵的指数映射R Exp(r) I (sin‖r‖ / ‖r‖) r^ ((1 - cos‖r‖) / ‖r‖^2) (r^)^2其中r^是由旋转向量r张成的反对称矩阵。FAST-LIO 代码里常见的Exp()函数就是在做这个泰勒展开的闭合形式。反过来Log(R)把旋转矩阵映射回旋转向量用于计算两个姿态之间的差值。这里最关键的设计含义是误差状态更新时旋转的增量只在切空间里做加法然后通过指数映射投影回流形。代码里你会看到类似这样的模式// 流形更新R_new R * Exp(dr) state.rot state.rot * Exp(dr); // 位置更新p_new p dp因为平移在欧氏空间 state.pos state.pos dp;注意旋转的更新是右乘Exp(dr)而不是R dr。这样更新出来的 R 依然满足正交约束。如果把旋转矩阵当成普通矩阵做线性加两个迭代周期后行列式就会偏离 1误差会不可控地累积。这是我最初读代码时最容易忽略的点同一个号在不同流形上的含义完全不同。2.2 流形运算 ⊞ 和 ⊟把不同状态统一成一套操作FAST-LIO 的数学推导里到处是⊞和⊟不是花哨的记号。它们定义了「状态在流形上的加法和减法」。对于 SO(3)有R1 ⊞ r R1 * Exp(r) R1 ⊟ R2 Log(R2^T * R1)对于欧氏空间里的位置和速度⊞退化成普通加法。代码里继承自esekfom的State类型正是用这套运算把姿态、位置、速度、零偏统一成可迭代的变量。为什么要搞这套抽象因为迭代卡尔曼滤波的更新公式里会出现「状态增量 当前状态」的表达式。如果不区分流形和切空间直接用x dx姿态部分就崩了。FAST-LIO 把⊞重载进状态操作里让卡尔曼增益计算和状态更新可以统一写// 状态更新x_new x ⊞ dx state state.oplus(dx); // 等价于 x ⊞ dx在源码的实现里oplus会对旋转部分调用Exp对平移和速度部分直接相加。这就是「流形上的加法」落到代码的样子。我一般会这么理解这相当于把滤波器的状态空间从「一个大的欧氏向量」改成了「一个带几何结构的流形」而滤波器在每个迭代周期内其实是在切空间里做高斯近似。2.3 零偏随机游走与切空间里的协方差更新IMU 的陀螺零偏和加速度计零偏通常建模为随机游走它们本身也是欧氏向量但问题在于零偏与姿态强耦合零偏的观测影响会通过状态转移矩阵传播到姿态上。如果姿态在流形上那么雅可比矩阵F里与旋转相关的部分实际上是「状态在切空间上的线性近似」。FAST-LIO 的预测步骤中协方差P的传播沿用了标准 EKF 的形式P_pred F P F^T Q但这里的F是在误差状态下推导的而不是世界坐标系下的直接导数。这个误差状态是δx [δθ, δp, δv, δbg, δba]注意 δθ 是旋转误差的切空间表示它对应的是「真实旋转与预测旋转之间的微小旋转向量」。协方差更新只在这个切空间里做所以 P 的大小始终是固定的比如 18x18 或类似不会因为旋转表示形式不同而改变。理解了这一点再回头看 FAST-LIO 源码里build_propagate_matrix这类函数你会发现它不是在算一个神秘矩阵而是在把「IMU 测量驱动的运动学 误差状态转移」线性化。这几个理解合在一起才能看懂为什么 FAST-LIO 的状态估计在高动态运动下依然不发散因为它的协方差描述的是切空间上的局部不确定性而不是把姿态强按在欧氏空间里「假装它是线性的」。3. 迭代卡尔曼滤波FAST-LIO 状态估计的核心机制3.1 前向传播IMU 递推与协方差预测FAST-LIO 是不折不扣的紧耦合LiDAR 点云只是在提供观测而系统的预测完全由 IMU 驱动。在两次 LiDAR 帧之间IMU 测量以远高于点云的频率送入滤波器做前向传播。传播公式就是一个带零偏补偿的惯性运动学p_{k1} p_k v_k dt 0.5 * (R_k * (a_m - ba) g) * dt^2 v_{k1} v_k (R_k * (a_m - ba) g) * dt R_{k1} R_k * Exp((w_m - bg) * dt)这里a_m、w_m是原始测量ba、bg是当前估计的零偏。代码里IMU_Processing.hpp中的get_f、get_Jac等函数本质上就是这条运动学和它对应的状态转移雅可比。get_Jac返回的矩阵会被用来更新协方差P这一步和标准 EKF 没有本质区别。但有一个坑IMU 数据的到达时间与 LiDAR 帧的时间戳并不对齐。FAST-LIO 的做法是把一帧点云对应的所有 IMU 测量缓存起来在点云时间戳到达时一次性完成积分并在积分过程中同步传播协方差。这也是为什么代码里会有Measures结构体和last_lidar_end_time_这类成员——它们在管理「从上一帧点云到这一帧点云之间」的 IMU 数据队列。3.2 反向传播去畸变与点面残差的构建LiDAR 扫描不是瞬时完成的一个 10Hz 的旋转雷达每帧点云里不同点的时间戳可能相差 100ms。如果直接用整帧点云去匹配等于把运动畸变当成了环境特征地图会糊。FAST-LIO 的处理方式是在拿到一帧点云所有点的时间戳后用已经预测好的状态把点云从各自的测量时刻补偿到帧尾或帧首的统一时刻。这个补偿也就是常说的「去畸变」。在源码里PointCloudXYZI::Ptr中的每个点都带一个相对时间relative_time通过 IMU 姿态插值把每个点旋转到位姿一致的时间点。做完这一步点云才被允许用来构建残差。残差的构建不是 ICP 式的点对点而是点对面point-to-plane。对当前点p_i在 ikd-tree 里搜索最近的若干邻域点用这些邻域点拟合一个平面得到平面法向量n和平面位置。残差定义为点到平面的距离r_i n^T * (T * p_i - q)其中T是当前估计的位姿变换q是平面上的一点。FAST-LIO 源码laserMapping.cpp中的point_plane相关逻辑就是在做「搜索邻居 → 求平面 → 算残差 → 组合成自适应残差」。注意它不是简单地把残差堆在一起而是把所有点到平面的距离构成一列残差向量后面交给迭代卡尔曼滤波器同时优化位姿和地图里的点关系。3.3 迭代更新为什么一次 EKF 不够要迭代标准 EKF 的更新是「一步到位」算卡尔曼增益更新状态结束。但问题在于点面残差对状态的依赖是非线性的——同一个点位姿不同它对应的邻域平面也不同。一次更新算出来的增益只在一个局部近似的雅可比下最优。FAST-LIO 的做法是让更新步骤重复多次每次用当前估计的位姿重新找最近邻、重新拟合平面、重新计算残差和雅可比再计算一次卡尔曼增益并修正状态。这就是「迭代误差状态卡尔曼滤波」名字里「迭代」的由来。伪代码层面更新循环是这样的# 迭代卡尔曼滤波更新 for i in range(num_iterations): H build_jacobian(state) # 基于当前状态计算点面残差的雅可比 z compute_residual(state) # 当前状态下的残差 K P_pred * H^T * (H * P_pred * H^T R)^-1 dx K * z # 切空间增量 state state.oplus(dx) # 流形更新 if norm(dx) eps: break代码里esekfom::esekf模板的update函数接收两个可调用对象一个是h残差模型一个是h_x残差雅可比。迭代的终止条件是状态增量足够小。这才是整个滤波器最核心的机制它把「配准」和「滤波」揉成了一个相互促进的迭代过程每次迭代都在改善配准配准变好又让滤波更新更准。我当初看代码时有个困惑为什么明明卡尔曼滤波是线性最优估计还要迭代原因就在于观测模型不是线性的点面残差依赖状态又依赖地图。一次更新只是从当前状态做了一次展开而迭代相当于在同一帧点云上不断重新线性化直到收敛。这比直接做一次 EKF 更接近非线性优化的效果又比纯优化保留了点云地图的不确定性建模。4. ikd-tree动态局部地图如何做到增量更新4.1 从 KD-tree 到 ikd-tree删除点是一个大麻烦FAST-LIO 的局部地图不是建好后就不动了。激光雷达每扫描出一帧新点地图里都可能加入新点而视角移动旧点离开传感器视场后就应该被删掉。如果每次都把整棵 KD-tree 重建10Hz 的频率下计算量和内存分配都扛不住。原来的 KD-tree 只支持静态构建和查询插入点会导致树退化删除点更是麻烦。ikd-tree 的关键在于把「增量插入」「延迟删除」「自动重建」三者结合成一套机制。它维护的是一棵「能被单个点级更新」的树而不是每帧重新灌数据。4.2 盒式操作与删除策略ikd-tree 中的删除不是物理删除而是标记删除。每个节点有一个deleted标志同时子树维护一个invalid_num计数记录该子树下有多少被标记删除的节点。查询时会跳过这些点但树结构仍然保留它们占用的位置。这种延迟删除的代价是删除点不会立刻释放内存也不会立刻让树变得不平衡。ikd-tree 之所以能保持效率是因为它在删除数量超过一定比例后触发重建重建时把保留下来的点重新建树物理释放被删点的内存。这个机制写进代码里你会看到类似这样的节点定义思路struct ikdTree_node { PointType point; ikdTree_node *left, *right; int tree_size; // 子树总节点数 int invalid_num; // 被标记删除的数量 bool deleted; // 当前点是否已删除 bool is_leaf; };查询时如果某个子树里invalid_num tree_size那整个子树都可以剪枝跳过。这个设计让删除操作在统计意义上接近 O(log n)而不是 O(n) 重建。4.3 并行重建与平衡策略增量插入会让树慢慢失衡。如果任其发展最坏情况会退化成一个链表查询从 O(log n) 变成 O(n)。ikd-tree 的平衡策略是基于子树大体重建当左右子树的大小比例超过阈值或者被标记删除的节点占比过高就触发该子树的重新平衡。更巧妙的是重建操作和查询操作是并发安全的。ikd-tree 会用双缓冲或加锁的方式让重建不阻塞主流程。源码里常见的用法是在一帧点云处理完后用kd_tree-AddPoints插入新点用kd_tree-DeletePoint或盒式删除去掉远离当前位置的点。地图的规模因此被控制在一个局部范围内查询最近邻的速度也稳定在 log 量级。如果你去看 FAST-LIO2 的实现会发现它维护的树不是全图而是以当前点为中心的局部地图。点云帧与帧之间重合的区域不用重复建树这是算法能跑到较高实时性的重要原因。ikd-tree 的贡献不只是「数据结构的优化」它实际上让「地图可以像滑窗一样流动」这件事变成了工程现实。5. FAST-LIO 源码阅读与运行常见问题排查5.1 现象编译报错提示 Eigen 对齐相关的问题原因FAST-LIO 的类成员里包含Eigen::Matrix等类型而 Eigen 为了 SIMD 加速要求一定内存对齐。部分编译器或结构体布局条件下new出来的对象地址不满足对齐要求于是出现static assertion failed或运行时不时崩溃。解决在包含 Eigen 成员的类定义里加上宏EIGEN_MAKE_ALIGNED_OPERATOR_NEW并保证用std::make_shared或new创建对象时走的是重载过的运算符。源码里ImuProcess等类使用了这个宏但如果你自己扩展了状态类或观测类也要记得加。否则调试起来非常诡异时而崩时而不崩。5.2 现象系统启动后一直停在初始化不出里程计原因FAST-LIO 初始化依赖 LiDAR 与 IMU 数据时间同步且需要累积足够多帧有效点云。常见的问题是把 IMU 话题名或点云话题名配错了或者 IMU 频率太低比如低于 100Hz导致前向传播的协方差涨得过大初始化条件不满足。解决先看config里的 yaml 参数确认lid_topic、imu_topic与实际发布的话题一致。再用rosbag info检查 IMU 的频率和点云频率建议 IMU 至少 200Hz。时间戳不同步也在初始化阶段会直接卡住可以用rosbag回放时检查第一帧点云和第一帧 IMU 的时间差通常需要先发 IMU 的数据让滤波器预热。5.3 现象里程计跳变地图局部出现重影或漂移原因局部地图的范围参数filter_size_surf或filter_size_map设置不合理。降采样体素太大点面残差过于粗糙体素太小地图点太多局部地图窗口里删除点的频率过高容易造成匹配到不稳定的点。解决先按默认参数运行确认「里程计跳变」是出现在快速旋转还是直线运动。快速旋转时跳变多半是 IMU 零偏未收敛直线运动漂移多半是点云退化场景长走廊、空旷场地导致点面残差约束不足。此时不该盲目调滤波器参数而是观察残差向量的大小——残差过大说明数据关联错残差过小且状态还在漂说明约束不够需要提高体素分辨率或调整地图窗口。5.4 现象编译时出现模板错误集中在esekfom::esekf原因esekfom是一个高度模板化的迭代卡尔曼滤波实现状态类型、输入类型、观测模型全部用模板参数传入。如果你自己改了状态维度或残差定义模板实例化时的类型不匹配会在编译期抛出一长串报错。最常见的是State里缺少oplus、boxplus等流形运算或者残差模型函数返回的向量维度与雅可比矩阵不一致。解决不要直接改模板先看懂STATE_IKFOM和INPUT_IKFOM展开后的结构。源码的附录里常把这两类展开成长表达式新手可以直接把展开结果从左往右读确认每个成员的类型和顺序。修改状态时要同步修改esekf模板里对应的维度常量否则协方差矩阵尺寸对不上编译期报错属于正常更多时候是运行时内存越界表现为随机崩溃。5.5 现象里程计输出正常但地图点一直增长内存越来越大原因局部地图没有正确执行盒式删除。ikd-tree 支持删除点但你必须显式调用删除接口而不是只插入新点。FAST-LIO 用当前位姿与历史位姿的距离来判断哪些点离开视野如果这个判断条件被跳过树的节点数就会无限增长。解决确认局部地图范围参数比如feat_length它控制盒式删除的边界。检查 Log 里是否周期性地出现Delete Points相关的输出如果没有说明删除逻辑没触发。此时优先排查树节点的delete_point是否被调用以及点云来源是否每帧都在同一个区域产生大量重复点导致树无法平衡。6. 设计取舍思考滤波与优化、特征与原始点的边界FAST-LIO 的核心选择有两个。第一个是「用滤波而不是图优化」。图优化每次迭代需要维护整个历史位姿和所有关键帧的关联实时性很难保证滤波用协方差压缩历史信息相当于放弃了全局优化的一致性换来了单帧状态更新 O(n) 或接近线性的代价。ikd-tree 的出现让局部地图规模可控进一步压缩了滤波更新里匹配部分的耗时。第二个是「用原始点云计算而不是特征提取」。早期激光 SLAM 会专门提取角点和平面点因为当时认为特征能降低计算量、减少误匹配。但特征提取会丢信息在植被、反射弱的环境中可靠的角点平面点数量显著下降。FAST-LIO 选择直接对原始点云做间隔降采样再配合点面残差实际是赌了一件事——「只要地图密度足够大点面残差对状态的约束力能超过稀疏特征约束」。这个赌注在现代激光雷达驱动下是成立的但也带来一个隐忧对地图点密度的依赖更强体素分辨率设置不当性能会明显拉开差距。还有一个值得琢磨的取舍是「迭代卡尔曼滤波中的迭代到底收敛到哪」。它并不是在做全局非线性优化而是在当前状态附近做多次重新线性化。如果初始状态差得远迭代可能只在局部收敛这也就是为什么 IMU 初始化质量会直接影响最终精度。FAST-LIO 把这个问题丢给系统给用户的直觉是IMU 的零偏估不准后面怎么迭代都救不回来。我那段时间调试一个退化场景时发现残差总量始终降不下去最后把 IMU 的回传频率从 100Hz 提到 200Hz零偏收敛明显变快整个系统就不再跳了。从那以后我每次搭建类似系统都会先做一遍「时间同步 频率清单 初始化表现」三项检查再下场调滤波参数。希望帮到你。本文还有配套的精品资源点击获取
阅读完成 · 觉得有帮助？
