简介面向FAST-LIO论文与源码学习者的理解笔记适合具备机器人操作系统、C与激光惯性里程计基础希望打通公式推导和代码实现的读者。文档先从李群李代数、流形、点云与惯性测量数据格式、十六线激光雷达等前置知识切入再梳理激光数据预处理、惯性测量单元初始化、特征提取、状态估计和增量式K-D树邻域搜索并对源码主流程、惯性数据处理、激光建图及误差状态卡尔曼滤波相关问题做阅读记录。附录整理旋转矩阵求导、状态量展开和参考文献并标注常见疑问与阅读建议方便对照论文查漏补缺。包内为1个docx文件压缩包约5.09MB目录按基础准备、算法框架、源码解析和问题记录组织。已有3155人学习适合作为入门梳理与源码复盘参考。1. 从一份 79 页笔记说起读 FAST-LIO 该先啃哪块骨头第一次打开 FAST-LIO 源码我盯着esekfom::esekf那一层模板参数看了半小时最后还是靠一份 79 页的个人笔记把整条链路串起来的。那份笔记从李群李代数一路写到H_SHARE_MODEL的逐行解析中间夹着 ROS 消息格式、C 内存对齐这些看起来「跟算法没关系」的内容——恰恰是这些卡住了大部分想复现 FAST-LIO 的人。这篇把我读这份资料的顺序和自己踩过的坑摊开讲如果你正在纠结先啃论文还是先啃代码或者跑通了 demo 却说不清状态量 23 维是怎么来的、去畸变到底以哪个时刻为基准下面几章应该能帮你省掉几天瞎试的时间。2. 地基没打牢李群、流形与四个消息格式怎么和代码对上读这类紧耦合 LIO 的开源代码我的判断依据很简单能不能把论文里的一个公式指到某个.hpp的某一行上。指不到说明地基还没打牢继续往下读只会越读越虚。笔记第一章花了不少篇幅讲李群、流形、ROS 消息和 C 特性很多人会跳过我第一遍也跳了第二遍回来补的时候才发现跳过的那部分正是后面读esekfom卡住的直接原因。2.1 李群李代数SO(3) 上的 ⊞ 和 ⊟ 到底在算什么旋转矩阵作为状态量有个麻烦它自己是 3×3 的九个元素但真正独立的自由度只有 3 个直接做加减法会破坏正交性。笔记里给的解法是在旋转向量上做增量再指数映射回旋转矩阵——旋转向量 r θa绕轴 a 转 θ 角exp(r∧)就是对应的旋转矩阵。代码层面要记住的只有两个运算符⊞是「在流形上加一个小量」⊟是「两个流形元素之间的差用李代数表示」。放到 SO(3) 上就是R ⊞ r R · Exp(r)R1 ⊟ R2 Log(R2ᵀ · R1)为什么要这么绕因为卡尔曼滤波的协方差传播和雅可比推导都建立在向量空间上你必须把旋转这个非线性对象「局部线性化」到切空间才能用矩阵乘法算。⊞/⊟就是把「流形上的运动」翻译成「切空间上的加减」的那座桥。我当时一直不理解为什么更新量是 3 维而不是 9 维想通这一点就通了滤波器维护的是误差状态误差状态一直在切空间里天生 3 维。这里有个容易翻车的地方Exp和Log的展开式有好几种写法罗德里格斯公式、泰勒展开近似、以及I sin/θ · r∧ (1-cos)/θ² · (r∧)²这种形式不同的写法在小角度下数值行为不一样。看源码时不要纠结用哪个公式去看它实际调的是SO3::exp()还是手写的近似——FAST-LIO 里对重力这类二维流形S²也做了同样的处理只是切空间是 2 维。2.2 状态量到底是几维23 维的由来和流形里的「2」这个问题我被问过好几次也是读源码时第一个必须搞明白的点。误差状态协方差矩阵是 23×23为什么不是 24 也不是 21答案在state_ikfom的成员里// 摘自 state_ikfom 的结构定义整理后的字段说明 struct state_ikfom { Vector3d pos; // 位置3 维 SO3 rot; // 姿态流形上 3 维 SO3 offset_R_L_I; // 雷达到 IMU 的旋转外参3 维 Vector3d offset_T_L_I; // 雷达到 IMU 的平移外参3 维 Vector3d vel; // 速度3 维 Vector3d bg; // 陀螺零偏3 维 Vector3d ba; // 加计零偏3 维 S2 grav; // 重力方向二维球面流形2 维 };33333332 23。关键在于最后那个grav重力向量本身是 3 维的但它的模长已知约 9.8剩下的自由度只有方向方向落在单位球面上而球面是 2 维流形。所以滤波器里只估计 2 维的扰动而不是 3 维。这就是流形概念在工程里的直接落点不是数学课上的抽象名词。我一般会这么判断自己有没有真读懂这一章能不能不看代码白板画出「位姿误差 6 维 外参误差 6 维 速度 3 维 两个零偏 6 维 重力 2 维 23 维」并且解释清楚每一维为什么要存在。做不到就回去重看流形那节别急着读esekf。提示不同版本的状态量可能包含时间偏移项维度会变成 24。看源码时以实际结构体为准不要背死数字。2.3 四个 ROS 消息数据从传感器进到算法里长什么样点云不是数组是带步长的二进制块。这一点如果不清楚读预处理代码时会对着一堆 offset 发懵。sensor_msgs/PointCloud2的核心字段是fields、point_step、row_step、data。fields描述每个点有哪些通道x/y/z/intensity/ring/time以及各自的offset相对点起始位置的字节偏移和datatypeFLOAT32、UINT16 等枚举值。真正遍历点时对第 i 个点取data中i * point_step field.offset处按datatype读出来的值——这就是为什么要用point_step做步长而不是sizeof(PointType)。std_msgs/Header里的stamp是秒 纳秒两段整数frame_id标定坐标系。这两个字段在所有带时间戳的消息里都会出现是后面做时间对齐的依据。sensor_msgs/Imu提供角速度和线加速度、以及各自的协方差协方差直接喂给滤波器的过程噪声矩阵噪声参数没调好轨迹抖得像筛糠。nav_msgs/Odometry是输出位姿用四元数存看着直观但算法内部用的是旋转矩阵中间要做一次Quaterniond(R)转换这一步在线程里频繁调用不算便宜。理解这四个消息的价值在于看到velodyne回调里头几行在做 offset 计算你能立刻知道那是在解包原始点看到ImuConstPtr里取linear_acceleration你知道该字段已经在驱动层做过什么处理、要不要再减重力。2.4 C 的三个小坎对齐宏、typedef、动态维度EIGEN_MAKE_ALIGNED_OPERATOR_NEW这个宏在ImuProcess类里出现第一眼不知道干嘛的。原因很实在Eigen 的向量化运算SSE/AVX要求数据地址按 16 或 32 字节对齐而new默认不保证。类里放了 Eigen 定长成员又没加这个宏程序在优化编译下可能直接崩报错还奇奇怪怪。加在类的 public 段或 private 段顶部都行效果是重载该类的new/delete。typedef那一节笔记写得很细其实对读 LIO 源码有用的就一条复杂函数指针声明可以一层层替换成别名。像esekf里大量的回调、模板参数本质都是这个套路看不惯就顺着 typedef 往外剥。Eigen::Dynamic则是运行期定尺寸的矩阵。点云每帧点数不一样局部地图大小动态变化这类地方必须用动态维度。代价是堆内存分配所以内层循环里该用固定尺寸的地方千万别偷懒。3. FAST-LIO2 的六步流水线从原始点云到点面残差框架图上的箭头谁都会画能讲清「为什么这一步要放在这一步之前」才算读进去。我的判断标准是任意抽掉流水线中间的一步你能不能说出它会造成什么后果。下面按数据流顺序拆。3.1 预处理为什么默认不提取特征点这一条和大多数 LOAM 系方案反着来也是 FAST-LIO2 提速的关键。因为地图用的是逐点的增量式 kd 树ikd-tree不需要预先聚出角点、平面点再匹配每个原始点都能直接进最近邻搜索。所以预处理的活儿变成剔除无效点NaN、超距离、过近按ring和时间戳排序间隔采样降采样笔记里写的是间隔 3 个点取 1 个把点从雷达坐标系换到 IMU 坐标系用外参offset_R_L_I、offset_T_L_I。第 4 步是最容易出事的一步。外参写反了点云在地图里会整体错位而这种错位不会报错只会让轨迹慢慢发散看起来像滤波器调参问题其实是坐标变换错。我一般会在预处理后把一帧点云直接发出来看一眼形状——如果墙面是弯的先查外参再查去畸变不要一上来就怀疑噪声矩阵。3.2 IMU 初始化重力方向、零偏和第一批协方差IMU 初始化做几件事估计重力方向和初始姿态、估计陀螺零偏、设置初始协方差。初始化期间也就是没收到有效点云、还没建立地图的时候。代码里通常判断「已经积累了多少秒的 IMU 数据」够了才切到建图状态。这里有个隐蔽点初始化估计的重力方向直接决定了世界坐标系的朝向后面所有状态都挂在这个系下。如果初始化时机器人在缓慢移动估计出的重力会偏整个地图跟着歪。我的习惯是启动时让设备静止三到五秒等初始化标志位切过去再动。前向传播部分用的是标准 IMU 运动学积分位置由速度积分、速度由加速度减掉重力、减掉加计零偏积分、姿态由角速度减掉陀螺零偏积分。协方差同步按状态转移矩阵传播同时叠加过程噪声。零偏的随机游走也要建模否则长时间运行后姿态会慢慢漂。3.3 去畸变每个点的时间戳从哪来机械式 16 线雷达一圈转下来约 0.1 秒转到 0° 和转到 180° 时设备自身已经移动了一段距离。如果拿整帧统一的一个时刻去算位姿几百个点会散成「重影」表现为墙面出现双线或拖尾。做法是为每个点确定它在扫描周期内的相对时刻用 IMU 积分插值出该时刻相对参考时刻的位姿增量把点变换到统一的参考时刻。参考时刻一般取帧尾或者帧头代码里要看清用的是哪个两种都能工作但配错会引入半帧的系统性偏移。这一步依赖两个前提点的时间戳字段存在且单位正确秒还是微秒、雷达和 IMU 做过时间同步。这两个前提任意一个不成立去畸变都会「看起来在跑」但结果变差。血泪经验先拿一帧点云在地图里叠两次看厚度再调别的。3.4 ikd-tree 与局部地图最近邻和平面拟合普通 kd 树插入删除要重建一帧点云上万点重建开销扛不住。ikd-tree 做的是增量更新插入、删除、降采样都在树上局部操作按需重建子树。同时在滑动窗口之外的点会被删除保证内存不炸。建图时对每个待配准点在树上找最近的若干个点通常是 5 个用这些点拟合一个平面然后计算点到面的距离作为残差。残差公式本身不复杂麻烦的是要在残差对状态量的雅可比里把「点在世界系下的位姿」这一层链式关系展开对——这就是H_SHARE_MODEL干的活。我读这块的经验先把残差函数的输入输出列清楚输入是状态、点、近邻点集输出是 1 维残差以及它对 23 维状态的雅可比再去看代码否则会被一堆临时变量绕晕。3.5 迭代卡尔曼滤波为什么不是一次更新就完事点云和地图的匹配是非线性的一次线性化误差很大。所以用迭代的方式IEKF在当前状态点附近线性化求一次更新量把状态更新到新点再在新点重新线性化和求残差直到更新量的范数小于阈值收敛判据或者达到最大迭代次数。工程上要注意三件事收敛阈值设太小迭代次数暴涨、设太大会提前退出精度下降最大迭代次数是保底正常应该几步内收敛迭代过程中协方差只在最后一次更新前面几步只更新状态。这几条在源码里体现为update()内部的一层循环读的时候盯着「哪些量在循环内更新、哪些在循环外」就不会乱。4. 源码走读main、IMU_Processing、laserMapping 的调用链框架清楚了接下来要能顺着调用栈走一遍完整的帧。我的要求是能说出任意一行日志是哪一层打的。4.1 main参数、订阅和主循环main里没有算法全是粘合从参数服务器读话题名、外参、噪声参数创建订阅者订阅点云和 IMU创建发布者发布里程计、点云、路径然后ros::spin()或者自建循环。看这段只需要确认三件事点云和 IMU 的回调是不是分开的分开就说明有缓存和同步逻辑、主循环是定时驱动还是数据驱动、有没有做初始化状态判断。这三件事决定后面读同步代码时的前提。4.2 IMU_Processing.hppprocess()和undistortPcl()IMU 缓存在这里是主角。process()接收一批 IMU 数据和新的一帧点云做前向传播把状态和协方差从上一帧时刻推到当前帧时刻undistortPcl()负责按点时间戳做插值去畸变。// IMU 前向传播的骨架整理后的逻辑非原始代码 for (auto imu : imu_buf) { // 按时间顺序遍历缓存中的 IMU double dt imu.stamp - last_stamp; // 1. 用上一时刻的零偏和角速度积分姿态 // 2. 用上一时刻的姿态把加速度转到世界系扣除重力与加计零偏后积分速度与位置 // 3. 按状态转移矩阵推进协方差并叠加过程噪声 predict(dt, imu); last_stamp imu.stamp; }dt的取法直接决定数值精度用相邻 IMU 时间差还是用帧间固定值结果差得很明显。缓存区的清理逻辑也要看——如果不清或者清理早了会出现重复积分或数据缺失表现为轨迹偶尔跳一下。噪声参数acc_n、gyr_n、零偏随机游走都在这个阶段进入协方差传播参数大一点轨迹更平滑但响应变慢小一点响应快但容易抖这是最常见的调参点。4.3 laserMapping.cpph_share_model如何计算残差与雅可比这是全文最重的一个函数。输入是当前状态、当前帧点云、ikd-tree输出是残差向量和雅可比矩阵。流程可以拆成四段把每个点用当前状态位姿外参变换到世界系在世界系下做近邻搜索取最近的若干点用这些点拟合平面判断平面是否有效点太少、退化、距离太大就跳过该点计算点到面距离作为残差并展开它对 23 维状态的雅可比。// 单个点的残差构建整理后的伪代码用于说明逻辑层次 for (auto pt : feats_undistort) { PointType world_pt trans_extrinsic_and_state(pt); // 点转到世界系 if (!ikdtree.nearest_search(world_pt, nearest_points)) continue; // 近邻不够直接跳过 if (!fit_plane(nearest_points, normal, d)) continue; // 退化或共线则跳过 double residual normal.dot(world_pt) d; // 点到面距离 // 按链式法则展开 d(residual)/d(state)依次对位姿、外参、速度、零偏、重力求偏导 fill_jacobian_row(jacobian, residual, normal, pt, state); }有三处值得专门停下来看。跳过机制近邻不够或拟合退化时直接 continue这些点不产生残差如果一帧里被跳过的点比例过高说明建图质量或初始外参有问题而不是滤波器问题。平面有效性判断近邻点如果几乎共线拟合出的法向量不稳定残差方向会乱。雅可比的方向残差是标量雅可比是一行如果发现某几维的偏导恒为零很可能是那部分状态在当前帧里没被约束到例如静止时速度、零偏的观测很弱。4.4STATE_IKFOM和INPUT_IKFOM的展开宏里的东西这两个宏是把状态量和输入量的字段「铺开」让esekf模板能在编译期知道有多少个分量、各自是什么流形类型。展开之后你能看到每个字段对应的流形标注——标量、向量用 ℝⁿ旋转用 SO(3)重力用 S²。读这部分的正确姿势不是逐行看宏展开而是去找「流形标注和状态维度的对应关系」校验它和 2.2 节算出来的 23 维是否一致。不一致就说明你看的版本改过状态定义后续所有雅可比维度都要跟着改。esekfom::esekf那一层之所以难读就是把「状态是什么、怎么加、怎么减、怎么传播」全做成了模板参数剥掉模板外壳之后剩下的就是标准的误差状态迭代卡尔曼滤波。5. 踩坑记录读 FAST-LIO 源码最容易翻车的五处这一节按「现象 → 原因 → 解决」写都是我自己或者身边人真碰过的。5.1 编译零错误跑起来位姿慢慢发飘现象是前几十秒正常之后轨迹开始缓慢偏离回环对不上也没有报错。原因大概率在噪声参数或者零偏建模。过程噪声给得太小滤波器过度相信 IMU 预测点云观测的权重被压低误差积累后没人纠。另一个常见原因是初始化阶段的零偏估计不准之后又没有足够观测把它拉回来。解决顺序先把 IMU 数据静置录一段用几十秒的静止数据反推零偏和噪声水平别直接用别人的参数再把点云残差数量打日志如果每帧参与优化的点数远低于点数总量问题在建图匹配而不是滤波参数。5.2 墙面出现重影越走越糊现象是点云地图里平面变成两层甚至三层近距离看明显拖尾。原因基本都是去畸变的时间基准不对点的相对时间戳单位搞错当成了秒实际是微秒或者反过来、参考时刻选了帧头而代码按帧尾处理、IMU 和雷达时间没同步。也有一种情况是外参平移写错量级把小位移放大成了大错位。解决先把去畸变关掉跑一遍对比开关前后的差异能明确是不是这一步的问题再把一帧点云单独发布出去静止状态下看厚度静止时有厚度说明不是运动畸变而是外参或标定问题。注意静止状态下点云还有厚度别急着调滤波器先查标定和时间戳。5.3 换了一台雷达就彻底跑不动现象是代码能启动点云也能收到但地图完全乱掉或者根本配准不上。原因通常是点云字段假设。原始代码对点结构体有隐含假设有哪些字段、字段顺序、ring是否存在、时间字段叫什么名字。换个型号字段名变了、时间字段没了、点云密度变了预处理里的排序和降采样全部失效。解决拿到新雷达后第一件事不是改算法是把点云回调里的字段名和point_step打印出来逐个核对时间字段缺失的话要么在驱动层补上要么退化成按ring和方位角估算相对时间。5.4 优化编译下随机崩溃Debug 下又正常现象是 Release 编译跑几分钟就段错误Debug 从不复现日志也看不到异常。原因几乎都是 Eigen 的内存对齐。结构体或类里放了定长的 Eigen 类型Vector3d、Matrix3d又用了new分配地址没对齐向量化指令一读就崩。解决凡是含有定长 Eigen 成员的类都加上对齐宏用std::vector存这类对象时要么换成Eigen::aligned_allocator要么干脆存指针。这类问题查起来最耗时间因为崩溃点和真正的错误点往往隔着好几层调用。5.5 附录 A 那段旋转矩阵求导看了三遍还是不懂现象很真实正文都跟下来了到附录的矩阵求导卡死怀疑自己数学不行。原因不是数学不行是缺少「扰动模型」这个前提。求导不是在旋转矩阵的九个元素上求而是在右乘一个微小扰动Exp(δθ)之后看结果对 δθ 的变化率。绕开这个前提直接看推导必然看不懂。解决先用一个具体的小角度例子手推一遍比如绕 z 轴转很小的角看R·Exp(δθ)展开后的一阶项再回去看符号推导。实在推不动就先跳过把h_share_model里雅可比的结构看懂——哪几块对应位姿、哪几块对应外参用的是不是同一个扰动模型——结构对了细节可以后面补。6. 落到能复现一套我每次换设备都会走一遍的验证流程读到这一步判断自己有没有真读懂不看能不能复述公式看能不能在一台新设备上把它跑对。我的做法是把验证拆成前后有依赖的五步任何一步不过就不往下走步骤检查对象通过标准不过时先查1点云字段字段名与偏移全部核对一致无 NaN 大范围出现驱动层字段定义、点结构体2静止点云静止五秒墙面单层无厚度外参、标定、时间戳3时间同步点时间戳单调且落在帧区间内驱动时间源、同步方式4慢速直行十米直行轨迹无明显弯折Z 轴不漂重力初始化、零偏5回环闭合绕一圈回到起点误差在可接受范围噪声参数、残差有效性这套流程的价值在于把「调参」压缩到最后一步。前四步都是确定性问题有明确的对错只有第五步才需要凭经验调数值。我见过太多人第一步没过就开始改噪声矩阵改了两天问题其实是一个字段名。具体到代码层面我会在预处理之后、配准之前插一段临时输出把当前帧点云和局部地图各发一个话题出来用可视化工具叠着看// 临时调试代码确认预处理结果和地图是否对齐验证完记得删掉 pcl::PointCloudPointType::Ptr dbg(new pcl::PointCloudPointType()); *dbg *feats_undistort; // 当前帧已去畸变、已转到世界系 publish_cloud(dbg, dbg_current_frame); // 与局部地图叠加观察厚度 publish_map(ikdtree, dbg_local_map); // 局部地图点云这段代码不参与算法只回答一个问题当前帧和地图是不是贴合的。贴合而轨迹还飘问题在状态估计不贴合问题在预处理或外参。把这两类问题分开排查时间能从几天缩到几小时。最后说一个习惯。从那以后我每次读这类紧耦合的源码都会先在纸上写一行「这一帧从进入到输出状态被改了几次、每次改的是哪几维」写完再去读代码对不上的地方逐个查。这个动作花十分钟能省掉后面一整天在模板和宏里迷路。那份 79 页的笔记帮我把这条线第一次捋顺后面每次重读我都会拿它对照着自己那行字核一遍看是哪一步又想当然了。希望帮到你。本文还有配套的精品资源点击获取
简介面向FAST-LIO论文与源码学习者的理解笔记适合具备机器人操作系统、C与激光惯性里程计基础希望打通公式推导和代码实现的读者。文档先从李群李代数、流形、点云与惯性测量数据格式、十六线激光雷达等前置知识切入再梳理激光数据预处理、惯性测量单元初始化、特征提取、状态估计和增量式K-D树邻域搜索并对源码主流程、惯性数据处理、激光建图及误差状态卡尔曼滤波相关问题做阅读记录。附录整理旋转矩阵求导、状态量展开和参考文献并标注常见疑问与阅读建议方便对照论文查漏补缺。包内为1个docx文件压缩包约5.09MB目录按基础准备、算法框架、源码解析和问题记录组织。已有3155人学习适合作为入门梳理与源码复盘参考。1. 从一份 79 页笔记说起读 FAST-LIO 该先啃哪块骨头第一次打开 FAST-LIO 源码我盯着esekfom::esekf那一层模板参数看了半小时最后还是靠一份 79 页的个人笔记把整条链路串起来的。那份笔记从李群李代数一路写到H_SHARE_MODEL的逐行解析中间夹着 ROS 消息格式、C 内存对齐这些看起来「跟算法没关系」的内容——恰恰是这些卡住了大部分想复现 FAST-LIO 的人。这篇把我读这份资料的顺序和自己踩过的坑摊开讲如果你正在纠结先啃论文还是先啃代码或者跑通了 demo 却说不清状态量 23 维是怎么来的、去畸变到底以哪个时刻为基准下面几章应该能帮你省掉几天瞎试的时间。2. 地基没打牢李群、流形与四个消息格式怎么和代码对上读这类紧耦合 LIO 的开源代码我的判断依据很简单能不能把论文里的一个公式指到某个.hpp的某一行上。指不到说明地基还没打牢继续往下读只会越读越虚。笔记第一章花了不少篇幅讲李群、流形、ROS 消息和 C 特性很多人会跳过我第一遍也跳了第二遍回来补的时候才发现跳过的那部分正是后面读esekfom卡住的直接原因。2.1 李群李代数SO(3) 上的 ⊞ 和 ⊟ 到底在算什么旋转矩阵作为状态量有个麻烦它自己是 3×3 的九个元素但真正独立的自由度只有 3 个直接做加减法会破坏正交性。笔记里给的解法是在旋转向量上做增量再指数映射回旋转矩阵——旋转向量 r θa绕轴 a 转 θ 角exp(r∧)就是对应的旋转矩阵。代码层面要记住的只有两个运算符⊞是「在流形上加一个小量」⊟是「两个流形元素之间的差用李代数表示」。放到 SO(3) 上就是R ⊞ r R · Exp(r)R1 ⊟ R2 Log(R2ᵀ · R1)为什么要这么绕因为卡尔曼滤波的协方差传播和雅可比推导都建立在向量空间上你必须把旋转这个非线性对象「局部线性化」到切空间才能用矩阵乘法算。⊞/⊟就是把「流形上的运动」翻译成「切空间上的加减」的那座桥。我当时一直不理解为什么更新量是 3 维而不是 9 维想通这一点就通了滤波器维护的是误差状态误差状态一直在切空间里天生 3 维。这里有个容易翻车的地方Exp和Log的展开式有好几种写法罗德里格斯公式、泰勒展开近似、以及I sin/θ · r∧ (1-cos)/θ² · (r∧)²这种形式不同的写法在小角度下数值行为不一样。看源码时不要纠结用哪个公式去看它实际调的是SO3::exp()还是手写的近似——FAST-LIO 里对重力这类二维流形S²也做了同样的处理只是切空间是 2 维。2.2 状态量到底是几维23 维的由来和流形里的「2」这个问题我被问过好几次也是读源码时第一个必须搞明白的点。误差状态协方差矩阵是 23×23为什么不是 24 也不是 21答案在state_ikfom的成员里// 摘自 state_ikfom 的结构定义整理后的字段说明 struct state_ikfom { Vector3d pos; // 位置3 维 SO3 rot; // 姿态流形上 3 维 SO3 offset_R_L_I; // 雷达到 IMU 的旋转外参3 维 Vector3d offset_T_L_I; // 雷达到 IMU 的平移外参3 维 Vector3d vel; // 速度3 维 Vector3d bg; // 陀螺零偏3 维 Vector3d ba; // 加计零偏3 维 S2 grav; // 重力方向二维球面流形2 维 };33333332 23。关键在于最后那个grav重力向量本身是 3 维的但它的模长已知约 9.8剩下的自由度只有方向方向落在单位球面上而球面是 2 维流形。所以滤波器里只估计 2 维的扰动而不是 3 维。这就是流形概念在工程里的直接落点不是数学课上的抽象名词。我一般会这么判断自己有没有真读懂这一章能不能不看代码白板画出「位姿误差 6 维 外参误差 6 维 速度 3 维 两个零偏 6 维 重力 2 维 23 维」并且解释清楚每一维为什么要存在。做不到就回去重看流形那节别急着读esekf。提示不同版本的状态量可能包含时间偏移项维度会变成 24。看源码时以实际结构体为准不要背死数字。2.3 四个 ROS 消息数据从传感器进到算法里长什么样点云不是数组是带步长的二进制块。这一点如果不清楚读预处理代码时会对着一堆 offset 发懵。sensor_msgs/PointCloud2的核心字段是fields、point_step、row_step、data。fields描述每个点有哪些通道x/y/z/intensity/ring/time以及各自的offset相对点起始位置的字节偏移和datatypeFLOAT32、UINT16 等枚举值。真正遍历点时对第 i 个点取data中i * point_step field.offset处按datatype读出来的值——这就是为什么要用point_step做步长而不是sizeof(PointType)。std_msgs/Header里的stamp是秒 纳秒两段整数frame_id标定坐标系。这两个字段在所有带时间戳的消息里都会出现是后面做时间对齐的依据。sensor_msgs/Imu提供角速度和线加速度、以及各自的协方差协方差直接喂给滤波器的过程噪声矩阵噪声参数没调好轨迹抖得像筛糠。nav_msgs/Odometry是输出位姿用四元数存看着直观但算法内部用的是旋转矩阵中间要做一次Quaterniond(R)转换这一步在线程里频繁调用不算便宜。理解这四个消息的价值在于看到velodyne回调里头几行在做 offset 计算你能立刻知道那是在解包原始点看到ImuConstPtr里取linear_acceleration你知道该字段已经在驱动层做过什么处理、要不要再减重力。2.4 C 的三个小坎对齐宏、typedef、动态维度EIGEN_MAKE_ALIGNED_OPERATOR_NEW这个宏在ImuProcess类里出现第一眼不知道干嘛的。原因很实在Eigen 的向量化运算SSE/AVX要求数据地址按 16 或 32 字节对齐而new默认不保证。类里放了 Eigen 定长成员又没加这个宏程序在优化编译下可能直接崩报错还奇奇怪怪。加在类的 public 段或 private 段顶部都行效果是重载该类的new/delete。typedef那一节笔记写得很细其实对读 LIO 源码有用的就一条复杂函数指针声明可以一层层替换成别名。像esekf里大量的回调、模板参数本质都是这个套路看不惯就顺着 typedef 往外剥。Eigen::Dynamic则是运行期定尺寸的矩阵。点云每帧点数不一样局部地图大小动态变化这类地方必须用动态维度。代价是堆内存分配所以内层循环里该用固定尺寸的地方千万别偷懒。3. FAST-LIO2 的六步流水线从原始点云到点面残差框架图上的箭头谁都会画能讲清「为什么这一步要放在这一步之前」才算读进去。我的判断标准是任意抽掉流水线中间的一步你能不能说出它会造成什么后果。下面按数据流顺序拆。3.1 预处理为什么默认不提取特征点这一条和大多数 LOAM 系方案反着来也是 FAST-LIO2 提速的关键。因为地图用的是逐点的增量式 kd 树ikd-tree不需要预先聚出角点、平面点再匹配每个原始点都能直接进最近邻搜索。所以预处理的活儿变成剔除无效点NaN、超距离、过近按ring和时间戳排序间隔采样降采样笔记里写的是间隔 3 个点取 1 个把点从雷达坐标系换到 IMU 坐标系用外参offset_R_L_I、offset_T_L_I。第 4 步是最容易出事的一步。外参写反了点云在地图里会整体错位而这种错位不会报错只会让轨迹慢慢发散看起来像滤波器调参问题其实是坐标变换错。我一般会在预处理后把一帧点云直接发出来看一眼形状——如果墙面是弯的先查外参再查去畸变不要一上来就怀疑噪声矩阵。3.2 IMU 初始化重力方向、零偏和第一批协方差IMU 初始化做几件事估计重力方向和初始姿态、估计陀螺零偏、设置初始协方差。初始化期间也就是没收到有效点云、还没建立地图的时候。代码里通常判断「已经积累了多少秒的 IMU 数据」够了才切到建图状态。这里有个隐蔽点初始化估计的重力方向直接决定了世界坐标系的朝向后面所有状态都挂在这个系下。如果初始化时机器人在缓慢移动估计出的重力会偏整个地图跟着歪。我的习惯是启动时让设备静止三到五秒等初始化标志位切过去再动。前向传播部分用的是标准 IMU 运动学积分位置由速度积分、速度由加速度减掉重力、减掉加计零偏积分、姿态由角速度减掉陀螺零偏积分。协方差同步按状态转移矩阵传播同时叠加过程噪声。零偏的随机游走也要建模否则长时间运行后姿态会慢慢漂。3.3 去畸变每个点的时间戳从哪来机械式 16 线雷达一圈转下来约 0.1 秒转到 0° 和转到 180° 时设备自身已经移动了一段距离。如果拿整帧统一的一个时刻去算位姿几百个点会散成「重影」表现为墙面出现双线或拖尾。做法是为每个点确定它在扫描周期内的相对时刻用 IMU 积分插值出该时刻相对参考时刻的位姿增量把点变换到统一的参考时刻。参考时刻一般取帧尾或者帧头代码里要看清用的是哪个两种都能工作但配错会引入半帧的系统性偏移。这一步依赖两个前提点的时间戳字段存在且单位正确秒还是微秒、雷达和 IMU 做过时间同步。这两个前提任意一个不成立去畸变都会「看起来在跑」但结果变差。血泪经验先拿一帧点云在地图里叠两次看厚度再调别的。3.4 ikd-tree 与局部地图最近邻和平面拟合普通 kd 树插入删除要重建一帧点云上万点重建开销扛不住。ikd-tree 做的是增量更新插入、删除、降采样都在树上局部操作按需重建子树。同时在滑动窗口之外的点会被删除保证内存不炸。建图时对每个待配准点在树上找最近的若干个点通常是 5 个用这些点拟合一个平面然后计算点到面的距离作为残差。残差公式本身不复杂麻烦的是要在残差对状态量的雅可比里把「点在世界系下的位姿」这一层链式关系展开对——这就是H_SHARE_MODEL干的活。我读这块的经验先把残差函数的输入输出列清楚输入是状态、点、近邻点集输出是 1 维残差以及它对 23 维状态的雅可比再去看代码否则会被一堆临时变量绕晕。3.5 迭代卡尔曼滤波为什么不是一次更新就完事点云和地图的匹配是非线性的一次线性化误差很大。所以用迭代的方式IEKF在当前状态点附近线性化求一次更新量把状态更新到新点再在新点重新线性化和求残差直到更新量的范数小于阈值收敛判据或者达到最大迭代次数。工程上要注意三件事收敛阈值设太小迭代次数暴涨、设太大会提前退出精度下降最大迭代次数是保底正常应该几步内收敛迭代过程中协方差只在最后一次更新前面几步只更新状态。这几条在源码里体现为update()内部的一层循环读的时候盯着「哪些量在循环内更新、哪些在循环外」就不会乱。4. 源码走读main、IMU_Processing、laserMapping 的调用链框架清楚了接下来要能顺着调用栈走一遍完整的帧。我的要求是能说出任意一行日志是哪一层打的。4.1 main参数、订阅和主循环main里没有算法全是粘合从参数服务器读话题名、外参、噪声参数创建订阅者订阅点云和 IMU创建发布者发布里程计、点云、路径然后ros::spin()或者自建循环。看这段只需要确认三件事点云和 IMU 的回调是不是分开的分开就说明有缓存和同步逻辑、主循环是定时驱动还是数据驱动、有没有做初始化状态判断。这三件事决定后面读同步代码时的前提。4.2 IMU_Processing.hppprocess()和undistortPcl()IMU 缓存在这里是主角。process()接收一批 IMU 数据和新的一帧点云做前向传播把状态和协方差从上一帧时刻推到当前帧时刻undistortPcl()负责按点时间戳做插值去畸变。// IMU 前向传播的骨架整理后的逻辑非原始代码 for (auto imu : imu_buf) { // 按时间顺序遍历缓存中的 IMU double dt imu.stamp - last_stamp; // 1. 用上一时刻的零偏和角速度积分姿态 // 2. 用上一时刻的姿态把加速度转到世界系扣除重力与加计零偏后积分速度与位置 // 3. 按状态转移矩阵推进协方差并叠加过程噪声 predict(dt, imu); last_stamp imu.stamp; }dt的取法直接决定数值精度用相邻 IMU 时间差还是用帧间固定值结果差得很明显。缓存区的清理逻辑也要看——如果不清或者清理早了会出现重复积分或数据缺失表现为轨迹偶尔跳一下。噪声参数acc_n、gyr_n、零偏随机游走都在这个阶段进入协方差传播参数大一点轨迹更平滑但响应变慢小一点响应快但容易抖这是最常见的调参点。4.3 laserMapping.cpph_share_model如何计算残差与雅可比这是全文最重的一个函数。输入是当前状态、当前帧点云、ikd-tree输出是残差向量和雅可比矩阵。流程可以拆成四段把每个点用当前状态位姿外参变换到世界系在世界系下做近邻搜索取最近的若干点用这些点拟合平面判断平面是否有效点太少、退化、距离太大就跳过该点计算点到面距离作为残差并展开它对 23 维状态的雅可比。// 单个点的残差构建整理后的伪代码用于说明逻辑层次 for (auto pt : feats_undistort) { PointType world_pt trans_extrinsic_and_state(pt); // 点转到世界系 if (!ikdtree.nearest_search(world_pt, nearest_points)) continue; // 近邻不够直接跳过 if (!fit_plane(nearest_points, normal, d)) continue; // 退化或共线则跳过 double residual normal.dot(world_pt) d; // 点到面距离 // 按链式法则展开 d(residual)/d(state)依次对位姿、外参、速度、零偏、重力求偏导 fill_jacobian_row(jacobian, residual, normal, pt, state); }有三处值得专门停下来看。跳过机制近邻不够或拟合退化时直接 continue这些点不产生残差如果一帧里被跳过的点比例过高说明建图质量或初始外参有问题而不是滤波器问题。平面有效性判断近邻点如果几乎共线拟合出的法向量不稳定残差方向会乱。雅可比的方向残差是标量雅可比是一行如果发现某几维的偏导恒为零很可能是那部分状态在当前帧里没被约束到例如静止时速度、零偏的观测很弱。4.4STATE_IKFOM和INPUT_IKFOM的展开宏里的东西这两个宏是把状态量和输入量的字段「铺开」让esekf模板能在编译期知道有多少个分量、各自是什么流形类型。展开之后你能看到每个字段对应的流形标注——标量、向量用 ℝⁿ旋转用 SO(3)重力用 S²。读这部分的正确姿势不是逐行看宏展开而是去找「流形标注和状态维度的对应关系」校验它和 2.2 节算出来的 23 维是否一致。不一致就说明你看的版本改过状态定义后续所有雅可比维度都要跟着改。esekfom::esekf那一层之所以难读就是把「状态是什么、怎么加、怎么减、怎么传播」全做成了模板参数剥掉模板外壳之后剩下的就是标准的误差状态迭代卡尔曼滤波。5. 踩坑记录读 FAST-LIO 源码最容易翻车的五处这一节按「现象 → 原因 → 解决」写都是我自己或者身边人真碰过的。5.1 编译零错误跑起来位姿慢慢发飘现象是前几十秒正常之后轨迹开始缓慢偏离回环对不上也没有报错。原因大概率在噪声参数或者零偏建模。过程噪声给得太小滤波器过度相信 IMU 预测点云观测的权重被压低误差积累后没人纠。另一个常见原因是初始化阶段的零偏估计不准之后又没有足够观测把它拉回来。解决顺序先把 IMU 数据静置录一段用几十秒的静止数据反推零偏和噪声水平别直接用别人的参数再把点云残差数量打日志如果每帧参与优化的点数远低于点数总量问题在建图匹配而不是滤波参数。5.2 墙面出现重影越走越糊现象是点云地图里平面变成两层甚至三层近距离看明显拖尾。原因基本都是去畸变的时间基准不对点的相对时间戳单位搞错当成了秒实际是微秒或者反过来、参考时刻选了帧头而代码按帧尾处理、IMU 和雷达时间没同步。也有一种情况是外参平移写错量级把小位移放大成了大错位。解决先把去畸变关掉跑一遍对比开关前后的差异能明确是不是这一步的问题再把一帧点云单独发布出去静止状态下看厚度静止时有厚度说明不是运动畸变而是外参或标定问题。注意静止状态下点云还有厚度别急着调滤波器先查标定和时间戳。5.3 换了一台雷达就彻底跑不动现象是代码能启动点云也能收到但地图完全乱掉或者根本配准不上。原因通常是点云字段假设。原始代码对点结构体有隐含假设有哪些字段、字段顺序、ring是否存在、时间字段叫什么名字。换个型号字段名变了、时间字段没了、点云密度变了预处理里的排序和降采样全部失效。解决拿到新雷达后第一件事不是改算法是把点云回调里的字段名和point_step打印出来逐个核对时间字段缺失的话要么在驱动层补上要么退化成按ring和方位角估算相对时间。5.4 优化编译下随机崩溃Debug 下又正常现象是 Release 编译跑几分钟就段错误Debug 从不复现日志也看不到异常。原因几乎都是 Eigen 的内存对齐。结构体或类里放了定长的 Eigen 类型Vector3d、Matrix3d又用了new分配地址没对齐向量化指令一读就崩。解决凡是含有定长 Eigen 成员的类都加上对齐宏用std::vector存这类对象时要么换成Eigen::aligned_allocator要么干脆存指针。这类问题查起来最耗时间因为崩溃点和真正的错误点往往隔着好几层调用。5.5 附录 A 那段旋转矩阵求导看了三遍还是不懂现象很真实正文都跟下来了到附录的矩阵求导卡死怀疑自己数学不行。原因不是数学不行是缺少「扰动模型」这个前提。求导不是在旋转矩阵的九个元素上求而是在右乘一个微小扰动Exp(δθ)之后看结果对 δθ 的变化率。绕开这个前提直接看推导必然看不懂。解决先用一个具体的小角度例子手推一遍比如绕 z 轴转很小的角看R·Exp(δθ)展开后的一阶项再回去看符号推导。实在推不动就先跳过把h_share_model里雅可比的结构看懂——哪几块对应位姿、哪几块对应外参用的是不是同一个扰动模型——结构对了细节可以后面补。6. 落到能复现一套我每次换设备都会走一遍的验证流程读到这一步判断自己有没有真读懂不看能不能复述公式看能不能在一台新设备上把它跑对。我的做法是把验证拆成前后有依赖的五步任何一步不过就不往下走步骤检查对象通过标准不过时先查1点云字段字段名与偏移全部核对一致无 NaN 大范围出现驱动层字段定义、点结构体2静止点云静止五秒墙面单层无厚度外参、标定、时间戳3时间同步点时间戳单调且落在帧区间内驱动时间源、同步方式4慢速直行十米直行轨迹无明显弯折Z 轴不漂重力初始化、零偏5回环闭合绕一圈回到起点误差在可接受范围噪声参数、残差有效性这套流程的价值在于把「调参」压缩到最后一步。前四步都是确定性问题有明确的对错只有第五步才需要凭经验调数值。我见过太多人第一步没过就开始改噪声矩阵改了两天问题其实是一个字段名。具体到代码层面我会在预处理之后、配准之前插一段临时输出把当前帧点云和局部地图各发一个话题出来用可视化工具叠着看// 临时调试代码确认预处理结果和地图是否对齐验证完记得删掉 pcl::PointCloudPointType::Ptr dbg(new pcl::PointCloudPointType()); *dbg *feats_undistort; // 当前帧已去畸变、已转到世界系 publish_cloud(dbg, dbg_current_frame); // 与局部地图叠加观察厚度 publish_map(ikdtree, dbg_local_map); // 局部地图点云这段代码不参与算法只回答一个问题当前帧和地图是不是贴合的。贴合而轨迹还飘问题在状态估计不贴合问题在预处理或外参。把这两类问题分开排查时间能从几天缩到几小时。最后说一个习惯。从那以后我每次读这类紧耦合的源码都会先在纸上写一行「这一帧从进入到输出状态被改了几次、每次改的是哪几维」写完再去读代码对不上的地方逐个查。这个动作花十分钟能省掉后面一整天在模板和宏里迷路。那份 79 页的笔记帮我把这条线第一次捋顺后面每次重读我都会拿它对照着自己那行字核一遍看是哪一步又想当然了。希望帮到你。本文还有配套的精品资源点击获取
