简介：面向FAST-LIO论文阅读与源码剖析的中文学习文档，适合已具备ROS、C++和SLAM基础，希望打通激光惯性里程计理论推导与工程实现的读者，也可供初学激光雷达惯性建图者作查漏补缺之用。内容先从李群李代数、流形、点云数据格式及VLP-16、ROS、C++等前置知识切入，再梳理LiDAR数据预处理、IMU初始化、点云特征点、状态估计与ikd-tree等核心模块，帮助建立论文公式、算法流程与代码落点之间的对应关系。源码部分围绕主程序、IMU处理、激光建图及滤波器封装展开，并记录常见问题、旋转矩阵求导、状态量展开等个人理解，适合边读论文边对照代码做笔记。包内为1个docx文档，压缩包约5.09MB，目录层级较清晰，可按章节检索与专题复盘。已有3161人学习，可作为FAST-LIO入门梳理与源码阅读的辅助材料。
1. 跑通只花了二十分钟，读懂花了两个月
第一次把 FAST-LIO 的代码拉下来、编译、rosbag play，看着 rviz 里点云一帧帧对齐，前后不到二十分钟。那时候我以为自己会了。真正的麻烦从第二天开始：esekfom::esekf那堆模板参数让我完全不知道实例化的是个什么东西，state_ikfom里的⊞运算看着像重载了加号又不像，H_SHARE_MODEL里一串链式求导更是直接劝退。我花了将近两个月，才把「跑通」和「读懂」之间的那道坎填平。这篇不打算复述论文公式，而是按我自己的阅读顺序，把 FAST-LIO 从数学地基到源码调用链重新串一遍，顺带把六次真实翻车记录下来。适合两类人：刚接触 LiDAR 惯性里程计、想少走弯路的新手；跑通了但说不清自己在跑什么、想拿到一句判断依据的熟手。
2. 数学地基不要跳过：李群李代数与流形最终落在哪几行代码
我一开始的想法是「数学那章先扫一眼，直接看代码」。结果在laserMapping.cpp里看到一个state.boxplus(...)，又看到一个SO3::exp()，来回查了两天才明白它们不是装饰。这一章的定位很明确：把论文第一章的抽象符号，逐个对应到源码里你能Ctrl+左键跳过去的行。凡是能和代码对上的数学，就不用死记；对不上的，基本都是推导过程，先跳过不影响读主干。
2.1 旋转向量、旋转矩阵与指数映射：三个入口函数
李群和 Lie Algebra（李代数）在 FAST-LIO 里的唯一用途，是把「旋转」这个不能直接做加减的量，搬到一个能加减的切空间里。具体到代码，只有三件事：Exp（旋转向量 → 旋转矩阵）、Log（旋转矩阵 → 旋转向量）、反对称矩阵skew。罗德里格斯公式是这一切的底子，手写一遍比看十遍推导有用。
// 罗德里格斯公式：旋转向量 r -> 旋转矩阵 R // r 的方向是旋转轴，模长是旋转角 inline Eigen::Matrix3d expSO3(const Eigen::Vector3d& r) { const double theta = r.norm(); // 旋转角 const Eigen::Matrix3d K = skew(r); // 反对称矩阵 [r]x if (theta < 1e-8) { // 小角度退化为 I + [r]x，避免 sin(theta)/theta 除零 return Eigen::Matrix3d::Identity() + K; } return Eigen::Matrix3d::Identity() + std::sin(theta) / theta * K + (1.0 - std::cos(theta)) / (theta * theta) * K * K; }这里有个容易被忽略的细节：theta < 1e-8这个分支不是可选的优化，而是必须的。IMU 静止时角速度接近零，连续很多帧的旋转增量都在 1e-9 量级，没有这个分支就会算出 NaN，而 NaN 一旦污染协方差矩阵P，后面所有帧全废，日志里还看不出原因，只会看到位姿慢慢变成nan。这是我在第一周就踩过的坑，排查了两小时才定位到一行除法。
Exp和Log的对称性也值得亲手写一次。Log是从旋转矩阵反解旋转向量，代码里通常用acos((trace(R)-1)/2)拿角度，再从一个非对角元素组合里恢复轴方向。注意trace在接近 ±1 时数值条件极差，R接近单位阵时角度接近 0，R接近 180° 旋转时sin(theta)接近 0，这两处都要单独处理。FAST-LIO 的状态更新里大量调用的其实是Exp，Log主要出现在误差定义和调试打印里，所以如果你时间紧，Log可以先看个大概，但Exp的小角度分支逻辑必须吃透。
2.2 流形上的 ⊞ 和 ⊟：误差状态为什么不能直接相减
这是整个 FAST-LIO 里最反直觉的一处。旋转矩阵是 3×3 的 9 个数，但它的自由度只有 3，所以你不能写R_new = R_old + dR，加出来的东西已经不是旋转矩阵了。流形就是用来处理这件事的：状态本身留在流形上，误差和增量放在切空间里，两者之间用⊞（boxplus）和⊟（boxminus）来回搬。
// 流形上的广义加法：状态在切空间叠加扰动 // 注意：这里的右乘约定必须和雅可比矩阵的推导约定一致 inline Sophus::SO3d boxplus(const Sophus::SO3d& R, const Eigen::Vector3d& dtheta) { return R * Sophus::SO3d::exp(dtheta); // 右乘扰动 } // 流形上的广义减法：两个状态之差，结果是切空间向量 inline Eigen::Vector3d boxminus(const Sophus::SO3d& R1, const Sophus::SO3d& R2) { return Sophus::SO3d::exp(R2.inverse() * R1).log(); // Log(R2^T * R1) }我一般会这么比：左乘扰动Exp(dθ) * R和右乘扰动R * Exp(dθ)都能让等式成立，但两者推出雅可比差一个R，混用之后滤波器不一定立刻炸，而是先表现为「跑一段路之后航向角慢慢偏」，等发现时已经跑了半小时的 bag。判断方法是看esekfom里雅可比和boxplus用的是不是同一套约定——如果是右乘更新、左乘求导，那输出慢漂就是必然的，不是标定问题。
流形的另一层价值是效率。SO(3) 只需要存 4 个数（四元数）参与优化，而不是 9 个；状态维数从矩阵尺寸降下来，协方差矩阵的规模也跟着降。论文里反复说降维提高运算效率，落到代码上就是状态量定义成 23 维而不是更大，这件事在第 4 章展开。
2.3 三种消息格式：PointCloud2、Imu、Odometry 里哪些字段真被用到
读源码前先把数据格式对齐，比读算法本身更省时间。ROS 的消息定义字段很多，但 FAST-LIO 真正读的只有几个，知道哪几个，调试时就知道去哪看。
| 消息类型 | 常用字段 | 在算法里的作用 |
|---|---|---|
sensor_msgs/PointCloud2 | fields(name/offset/datatype)、point_step、row_step、data、header.stamp | 按字节解析出 xyz 和 intensity；stamp是去畸变的时间基准 |
sensor_msgs/PointCloud2 | is_dense、height/width | 判断有没有无效点；无序点云height为 1 |
sensor_msgs/Imu | linear_acceleration、angular_velocity、orientation_covariance | 前向传播的输入；协方差首元素常被拿来判断是否提供了姿态 |
nav_msgs/Odometry | pose.pose.position、pose.pose.orientation、twist.twist | 输出位姿和速度；twist里的线速度就是状态量vel |
PointCloud2的解析是最容易出事的一环。它不是float[3]数组，而是一段二进制 blob，需要靠fields里的offset和datatype自己去算偏移。比如x的offset是 0、datatype是FLOAT32，intensity的offset可能是 16，中间还夹着ring和time字段。不同雷达驱动打包顺序不一样，照抄别人的解析代码，最常见的后果是点云看起来「有」，但全挤在一个平面上——因为 y、z 读到了 padding 字节。
时间戳这一栏单独强调。header.stamp是整帧点云的参考时刻，而雷达每个点自己的相对时间（time字段，单位通常是秒或纳秒）决定去畸变时插值到哪个位姿。如果time字段缺了，或者单位判断错了（把纳秒当秒），去畸变就等于没做，直道上看不出来，转弯时点云会明显拖尾。判断依据很直接：把车原地转 90 度，看地图有没有出现扇形拖影。
3. 一帧点云的旅程：预处理、IMU 传播、点面残差与增量建图
这一章按数据的流动顺序走一遍，从一帧原始点云进来到最终变成状态更新，中间每一步都在做什么、参数在哪改、改错了会怎样。我更愿意把 FAST-LIO 看成一个流水线而不是一个算法：预处理、传播、配准、建图四个工位，每个工位的输入输出都很清楚，出问题时按工位切分排查，比盯着整个系统看有效得多。
3.1 预处理：间隔降采样、盲区过滤与运动补偿
FAST-LIO2 跟 FAST-LIO 最大的差别之一就在这里：默认不提取特征点。原始点云直接按固定间隔取样，比如每隔 3 个点取 1 个。这个选择一开始我不理解——LOAM 系列都是先分角点面点再匹配的。看到 ikd-tree 那一章才想通：点到面残差不需要事先知道这个点是角点还是面点，找 5 个近邻直接拟合平面就行，平面拟合得好不好由残差和近邻距离共同筛选，所以特征提取这一步可以省。省掉之后，预处理的计算量从「每个点算曲率 + 排序 + 分类」降到「按固定步长跳着取」，在 64 线雷达上差一个数量级。
几组常见参数，我按「改错了会怎样」的方式记：
| 参数 | 常见取值含义 | 调错后的现象 |
|---|---|---|
point_filter_num | 每隔几个点取一个，3 表示 1/3 采样 | 设为 1 时 CPU 占满、帧率掉；设太大（≥6）时近处细节丢失 |
blind | 盲区半径（米），过滤掉雷达附近的点 | 设太小，车体自身反射点进入地图，形成一圈「鬼影」 |
scan_line | 雷达线数 | 填错（16 线填 64）导致按线号解析越界，点云错乱 |
feature_extract_enable | 是否提取特征点 | 打开但外参不准时，面点拟合失败率飙升 |
filter_size_surf | 当前帧降采样的体素边长（米） | 太小保留噪声，太大平面参数估不准 |
lidar_type | 雷达驱动类型枚举 | 选错等于点云解析全错，直接表现为「地图是散的」 |
去畸变这一步值得单独说。一帧点云不是同一时刻采到的，16 线雷达转一圈大概 0.1 秒，车在这 0.1 秒里动了。做法是：把 IMU 在这一帧时间范围内的积分结果缓存成若干个位姿，然后按每个点自己的相对时间戳，在缓存里插值出一个位姿，把这个点变换到帧尾（或帧首）坐标系。代码里的实现是先做一遍反向传播，把点都对齐到帧尾，再做配准；配准完成后再用更新后的状态重算一遍，把点变换到世界系。第一遍是为了让配准有个合理的初值，第二遍才是最终结果。
这段逻辑的常见误解是「去畸变只在高速时才有用」。实际上低速时也有用，只是量级小到肉眼看不出。真正会翻车的是时间字段单位搞错——雷达驱动给的time是相对这一帧起点的偏移，单位可能是秒，也可能是纳秒，甚至是「这一圈里第几个点」。看到time值普遍大于 1 时就要警惕，多半是纳秒没除 1e9。这个判断很土但很好用：一帧 0.1 秒，相对时间不可能超过 0.1。
3.2 IMU 初始化与前向传播：重力和零偏是怎么被估出来的
IMU 初始化决定了整条流水线能不能起步。做法并不复杂：假定上电时静止，取最初若干帧加速度计读数的平均值，归一化之后就是重力方向在世界系里的指向；陀螺仪读数的平均值直接作为陀螺零偏初值bg；加速度计零偏ba初值一般设为 0，靠后面的滤波慢慢收敛。
这里有个我踩过的坑：如果上电时手还在动，重力方向估出来是歪的，整个地图会以一个固定倾角斜着长出来，而且这个倾角不会自己修正回来——因为重力方向在状态里虽然参与优化，但收敛得很慢，前几十秒基本不动。判断依据是静止时看输出的 roll/pitch，应该接近 0，如果稳定在某个几度的值，基本就是初始化没做好。
前向传播本身是标准的 IMU 积分：用角速度积分出旋转，用旋转把加速度转到世界系、扣掉重力、积分出速度和位置。每一帧 LiDAR 到来时，会把上一帧到这一帧之间的 IMU 数据全部积一遍，得到一个先验状态和先验协方差。协方差传播用的是误差状态的线性化模型，里面会用到 IMU 的噪声参数（陀螺噪声、陀螺随机游走、加速度计噪声、加速度计随机游走）。这四个参数在配置文件里通常写得很小，改大改小直接影响滤波器对 IMU 的信任程度。
我一般会这么比：把陀螺噪声调大，滤波器更信 LiDAR，转弯时点云贴合更好但可能抖；调小则更信 IMU，短时间遮挡能扛住，长时间遮挡后一旦恢复会猛拉一下。这四个参数没有普适值，跟具体 IMU 型号和安装方式强相关，照抄别人的配置往往效果一般。
3.3 点面残差与迭代卡尔曼：H 矩阵的每一块对应哪个状态
配准这一步是 FAST-LIO 的核心。对当前帧的每一个点，先按先验位姿变换到世界系，然后在 ikd-tree 里找最近的 5 个点，用这 5 个点拟合一个平面，得到法向量n和平面上的一个点q。如果 5 个点不共面（用特征值判断）或者最近点距离超过阈值，这个点就丢掉。保留下来的点构造残差r = n^T (p_w - q)，理想情况下这个值应该等于 0。
然后要对这个残差求雅可比。这是整个代码里最难读的部分，因为链式求导写得很长。但结构其实很规整：残差只跟当前点的世界坐标p_w有关，而p_w由当前状态里的位姿、外参决定，所以H只有几块非零，其余全是零。列数等于状态维数（23），行数等于这一帧保留下来的点数（几千到上万）。手算一遍n^T对pos的偏导就是n^T本身，对rot的偏导是-n^T R [p]_x（[p]_x是点的反对称矩阵），对外参的偏导结构类似但要再串一层。
状态量和对应的维度，我整理成一张表，读代码时对着看：
| 状态量 | 维度 | 含义 |
|---|---|---|
pos | 3 | 雷达在世界系的位置 |
rot | 3（SO3） | 雷达在世界系的姿态 |
offset_R_L_I | 3（SO3） | 雷达到 IMU 的旋转外参 |
offset_T_L_I | 3 | 雷达到 IMU 的平移外参 |
vel | 3 | 世界系速度 |
bg | 3 | 陀螺零偏 |
ba | 3 | 加速度计零偏 |
grav | 2 | 重力方向（模长固定，只有 2 个自由度） |
加起来 23 维。重力用 2 维而不是 3 维，是因为它的模长已知，只有方向在变，少一个自由度就少一次矩阵求逆的规模。这个细节在论文里一句话带过，但读代码时如果不知道，会疑惑为什么grav的类型和别的三维量不一样。
迭代部分还有一个很值得注意的设计。标准 EKF 的增益是K = P H^T (H P H^T + R)^{-1}，要对一个行数上万的矩阵求逆，代价极高。FAST-LIO 把公式等价变形为K = (H^T R^{-1} H + P^{-1})^{-1} H^T R^{-1}，求逆的对象变成 23×23 的矩阵，规模固定且很小。这是它能跑实时的重要原因之一。我第一次看到这个变形时以为是数学技巧，实际是工程上的关键取舍——同样的估计结果，计算量差了两个数量级。
迭代次数在代码里是个宏定义的常量，典型值是 4 左右。迭代的好处是每次都用更新后的状态重新投影点、重新找近邻，相当于重线性化，比只做一次线性化精度高。但不是越多越好：迭代到后面，近邻搜索用的地图还是上一轮建的那棵，残差关联已经固定，继续迭代容易在错误的关联上过拟合，表现为位姿小幅来回跳。我见过有人把它调到 10，结果帧率掉一半、精度反而更差。
3.4 ikd-tree：增量插入和惰性删除把重建开销摊掉了
ikd-tree 是 FAST-LIO2 相对前作最大的工程贡献。普通的 k-d 树每帧都要重建，因为新点要进来、旧点要淘汰。百万级点云重建一次是几十毫秒，帧率直接被卡在 10Hz 以下。
增量式 k-d 树的思路是：新点直接插到树里，删除的点只打标记不真的改结构，等某棵子树里的删除标记累积到一定比例（论文里叫 scape factor）再局部重建，而且重建可以放到独立线程里并行做。这样每帧的开销从「全量重建」变成「插入新点 + 少量局部重建」，摊还下来是O(log n)级别。
几个实操层面的点。一是cube_side_length这个参数，决定局部地图盒子的边长，超出盒子的点会被丢弃。设得大，地图保留得多，远处结构更完整，但内存和查找时间都涨；设得小，近处更精细，但长走廊场景容易丢历史结构。二是filter_size_map决定插进树之前的体素降采样粒度，和filter_size_surf是两个独立参数，混用会出问题——前者控制地图稠密度，后者控制当前帧点数。三是树的删除是惰性的，所以size()返回的数量包含已删除的点，调试时如果发现「点数一直涨但地图没变多」，多半是这个原因，不是内存泄漏。
我一开始把 ikd-tree 当成「更快的 k-d 树」理解，后来发现它的关键不在快，而在「数据结构的生命周期管理」——什么时候插、什么时候删、什么时候重建，这套策略才是核心。理解了这一层，再看代码里那几个flatten、rebuild调用，就知道它们不是随手写的清理逻辑。
4. 源码地图：main、IMU_Processing.hpp、laserMapping.cpp 与 esekfom::esekf 的调用顺序
前面三章讲的是「应该发生什么」，这一章讲「代码里实际怎么串起来」。我读源码的习惯是先画出调用层级，再钻进最复杂的那一个文件，因为知道上下文之后再读细节，比从入口一行行往里陷要快得多。FAST-LIO 的调用层级本身不复杂，复杂的是中间那个滤波器模板。
4.1 main 里的初始化顺序决定了后面所有调试方式
main函数里真正的逻辑只有几行：初始化 ROS、创建节点句柄、读取参数、订阅话题、进入ros::spin()。但恰恰是这几行决定了后面调试的难易。
int main(int argc, char** argv) { ros::init(argc, argv, "laserMapping"); // 节点名，重复会互相踢出 ros::NodeHandle nh; ros::NodeHandle private_nh("~"); // 私有命名空间，读 yaml 参数用 // 订阅点云和 IMU：队列长度别太小，否则回放 bag 时丢帧 ros::Subscriber sub_pcl = nh.subscribe(lid_topic, 200000, livox_pcl_cbk); ros::Subscriber sub_imu = nh.subscribe(imu_topic, 200000, imu_cbk); ros::spin(); // 主循环只是个事件循环，逻辑都在回调里 return 0; }这里有两个常被忽视的点。订阅队列长度我一般会设得很大（十万量级），因为回放 bag 时消息是「突然涌进来」的，队列小了会丢帧，而丢帧的表现是「跑着跑着 IMU 断了」，看日志又找不到错误，很像传感器故障。另一个是节点名重复：如果同时起了两个实例，ROS 会把先起的踢出网络，表现是「程序还在跑但不出结果」，ros::ok()返回 false。这个问题我在第一次双开对比实验时撞上过，排了半小时。
还要注意回调里不要做重活。imu_cbk只负责把数据塞进缓冲区，livox_pcl_cbk只负责把点云塞进队列，真正的计算在主循环或者单独的线程里。如果哪天你把配准逻辑写进回调，会发现看门狗报警但程序看着正常。
4.2 IMU_Processing.hpp：Eigen 对齐、typedef 与那几个宏
IMU_Processing.hpp是预处理的核心，包含了前向传播和去畸变。这个文件里有几处 C++ 层面的细节，单独看是语法，理解之后能省掉一类段错误。
EIGEN_MAKE_ALIGNED_OPERATOR_NEW这个宏出现在ImuProcess类里，作用是重载new和delete，保证对象内 Eigen 固定大小成员的内存按 SIMD 指令要求对齐。为什么需要：SSE 要求 16 字节对齐、AVX 要求 32 字节，编译器默认的new只保证到max_align_t，在开启-O3 -march=native时一个未对齐的Eigen::Vector4d成员就可能触发段错误。这个宏要写在能被operator new找到的地方，Eigen 文档建议放public，社区里有些代码放private也能编过，但换编译器可能就挂。我一般无脑放public。
对齐问题最阴的地方在于它不一定复现。同样的代码，在你的机器上编译过了、跑得好好的，换一台 CPU 指令集不同的机器就崩，而且崩的位置看起来跟 Eigen 毫无关系。这类问题基本没有「后悔药」可吃，只能在工程规范上提前卡住：只要类里有 Eigen 固定大小的向量或矩阵成员，先把这个宏写上，别等出事再补。
typedef在这个文件里出现得很密集，主要是把一长串模板类型起短名字。它的价值在跨平台和可读性：比如定义一个浮点别名，某个平台支持高精度就映射到long double，不支持就映射到double，改一行 typedef 就行，不用动整个文件。但也别滥用，typedef的可读性是隐式的，代码里看到V3D要反应过来它是三元素向量，新人接手会有成本。我的经验是：跨多处使用的类型才值得起别名，只在两三个地方用的，写全了反而清楚。
inline在这个文件里用得很多，小函数前面都带。作用是建议编译器把函数体直接展开到调用处，省掉函数调用的栈操作开销，代价是二进制体积变大。这部分本质上是编译器优化的提示，写在头文件里还需要注意重定义问题。#ifdef则常用来做编译期开关，比如某个调试输出只在定义了宏时编译进去，避免运行时的分支判断。这两处不用深究，知道它们在控制什么就行。
Eigen::Dynamic值得提一句。矩阵维度有静态和动态两种，静态在编译期确定，动态在运行期确定。处理未知长度的点云列表时只能用动态维度，代价是有运行期内存分配和动态调整的开销。性能敏感的内层循环里尽量用固定维度，比如位姿是 4×4 就写死，别用动态。看到有人把整条流水线的矩阵全写成动态维度，然后抱怨帧率低，这类问题往往就出在这里。
4.3 laserMapping.cpp 与 esekfom::esekf：STATE_IKFOM 展开怎么看
这是整个项目里最厚的一个文件。laserMapping.cpp里定义了state_ikfom和input_ikfom两个结构体，然后把它们喂给esekfom::esekf这个模板类，实例化出一个针对 23 维状态的迭代卡尔曼滤波器。
// 状态结构体（按我手头这份代码整理的核心字段，顺序会影响矩阵分块） struct state_ikfom { V3D pos; // 3 位置 SO3 rot; // 3 姿态（流形，存储是四元数） SO3 offset_R_L_I; // 3 雷达到 IMU 旋转外参 V3D offset_T_L_I; // 3 雷达到 IMU 平移外参 V3D vel; // 3 速度 V3D bg; // 3 陀螺零偏 V3D ba; // 3 加速度计零偏 V2D grav; // 2 重力方向 }; // 输入结构体：只包含 IMU 观测量 struct input_ikfom { V3D acc; // 加速度计原始读数 V3D gyro; // 陀螺仪原始读数 };esekfom::esekf这个模板类第一次看会很怪：它把状态、输入、过程模型、观测模型全用模板参数传进去，内部通过模板特化来调用对应的雅可比函数。好处是不用虚函数，编译期就能展开；坏处是报错信息极长，一个括号错了能刷出几百行。我遇到过的典型情况是改状态字段顺序之后忘了同步改雅可比里对应的分块偏移，编译能过，跑起来精度特别差。判断依据是静止时协方差里的姿态块不收敛，一直在抖。
附录里对STATE_IKFOM和INPUT_IKFOM的展开、以及对H_SHARE_MODEL函数逐行的注释，是这份资源里最有价值的部分之一。原因很实在：网上讲 FAST-LIO 的文章大多停在框架图，真正把H矩阵每一块对应哪个状态、每个偏导为什么长那样写清楚的并不多。我读那部分时的用法是：先自己推一遍pos和rot那两块，再对照，对上了说明理解到位，对不上就说明左乘右乘或符号用错了。
读这个文件我推荐一个顺序：先只看状态结构体定义和它的operator+，再看过程模型的雅可比，最后看观测模型。中间那个H_SHARE_MODEL函数可以拆成三段看——点坐标对位姿的偏导、对速度零偏的偏导、对外参的偏导，其中速度零偏那部分是不是零块取决于残差只跟位置有关，想清楚这一点就能少看一半代码。
5. 六次翻车与排查手册：从时间戳对不齐到点云全飘
这一章是代价最高的一章。下面五条都是我实际撞过的，按「现象 → 原因 → 解决」写。新手可以直接当检查清单用，熟手可以对照看看有没有遗漏的排查维度。
5.1 点云整体糊成一片，地图有重影
现象：rviz 里点云能显示，但相邻两帧没有清晰对齐，走廊场景出现双层墙，越跑越歪。
原因：几乎都是时间戳问题。常见三种：一是 LiDAR 和 IMU 用了不同的时间基准，一个来自驱动、一个来自系统时钟；二是回调里用了ros::Time::now()而不是消息头里的header.stamp，回放 bag 时这个差异会被放大；三是雷达驱动配置了时间偏移量，而算法里没同步。还有一种更隐蔽的：点云内部的相对时间字段单位判断错了，去畸变插值到错误的位姿上。
解决：先打印两路消息的header.stamp，看差值是不是恒定；再看是否单调递增。如果差值恒定，在算法里加一个固定的时间偏移参数，把 LiDAR 时间平移过去。如果是相对时间单位问题，把time字段的数值分布打出来，最大值超过一帧时长就是单位错了。我一般会写个小脚本离线扫一遍 bag 的时间戳，比边跑边猜快得多。
5.2 启动瞬间位姿直接飞掉，点云散到几公里外
现象：一开始跑，输出的位置坐标就是几百甚至上千，rviz 里什么都看不到。
原因：两类。一是 IMU 初始化没做好，上电未静止，重力方向估错，导致后续所有加速度都被错误地扣掉重力，积分出来的速度爆炸。二是加速度计量纲不对，有的驱动输出单位是 g，有的是 m/s²，差 9.8 倍，算法里没做转换。这两种在日志上都表现为「第二帧就飞了」。
解决：先确认上电静置时间，至少两到三秒再动。再检查静止时加速度计读数的模长，应该接近 9.8（m/s²）或者接近 1（g），不看不知道，一看就知道该不该乘 9.8。初始化结束后打印估计出的重力向量，模长应该接近 9.8、方向应该接近竖直向上对应的那根轴，如果不是，说明坐标系定义对不上，得先解决外参或坐标系约定问题。
5.3 编译通过但一运行就段错误，换台机器又好了
现象：程序启动后立刻崩，core dump 位置在 Eigen 的某个赋值或者拷贝上，看起来跟业务逻辑无关。
原因：Eigen 固定大小成员的内存对齐问题。开了高优化等级和 SIMD 指令后，未对齐的向量化访存会触发非法指令或者访问越界。这类问题高度依赖 CPU 指令集和编译选项，所以会「换台机器就好了」，表现为玄学。
解决：凡是类里有 Eigen 固定大小向量或矩阵成员，加上EIGEN_MAKE_ALIGNED_OPERATOR_NEW；用std::vector装这类类型时，注意分配器也要指定对齐。验证方式很简单：用-O0编译跑一遍，如果不崩，基本就是对齐或者优化相关。定位到之后再逐步加回-O3和-march。这类问题我建议提前防，不要等崩了再补，因为它复现条件太苛刻。
5.4 近处建图正常，跑远了整体漂移
现象：前几十米地图很干净，跑出几百米后回环位置明显错开，但近处依然清晰。
原因：大概率是外参没用上或者用错了。雷达到 IMU 的旋转和平移外参如果保持单位阵和零向量，短距离内误差积累不明显，长距离会因为杠杆臂效应放大。另一个可能是filter_size_map设得太大，地图稠密度不够，远处点找不到可靠的近邻平面，残差关联质量下降。
解决：先确认外参是不是真的被写进了配置文件并被读取，最简单的验证是打印状态里offset_R_L_I的初值。外参标定如果有条件就实测一遍，没条件至少量一下雷达和 IMU 之间的物理距离，把平移填对。然后检查远处点的近邻距离分布，如果大量点因为超阈值被丢弃，说明地图太稀，把filter_size_map调小一档再试。
5.5 跑十几分钟后内存持续上涨，最后被杀
现象：top里看内存一路涨，十几分钟从几百兆涨到几个 G。
原因：常见两个。一是点云保存功能开着（pcd_save_en之类），每帧都往内存里累积点，跑久了必然爆；二是局部地图没有裁剪，cube_side_length设得很大或者根本没生效，点只增不减。
解决：先关掉保存开关再跑一遍，如果内存不再涨，问题就定位了。如果还涨，就把 ikd-tree 的点数打出来看趋势，正常应该在某个量级附近波动而不是单调上涨。注意惰性删除会导致size()偏大，看趋势比看绝对值靠谱。我一般会在主循环里每隔几百帧打一行统计，不看的话这类问题只能等它自己爆出来。
6. 把读法固化成一个流程：三张表和一个时间戳检查脚本
两个月的阅读过程，最后沉淀下来的不是笔记，而是一个固定动作：拿到任何一份新的 LiDAR 惯性里程计工程，先不看算法，先把三张表填出来——状态量表、消息表、参数表。状态量表回答「它到底在估什么」，消息表回答「数据从哪来到哪去」，参数表回答「哪些旋钮会影响我调试」。三张表填完，代码结构基本就透明了，剩下的只是细节。
具体到 FAST-LIO，我给的顺序是：先看state_ikfom的字段和总维数，确认是 23 维；再看两位回调里消息是怎么被塞进缓冲区的；最后看配置文件里point_filter_num、filter_size_surf、filter_size_map、cube_side_length这几个参数。这四个参数覆盖了八成以上的调参场景，剩下的多是外参和噪声参数。
然后是那个我每次都会跑一遍的脚本。它的作用是在启动算法之前，先把数据层面的问题排掉——时间戳对不齐、频率异常、字段缺失，这些问题在算法里排查极难，在数据层面排查极快。
# 离线检查 rosbag 里 LiDAR 与 IMU 的时间戳是否同一时钟、频率是否合理 # 依赖 rosbag，仅在数据排查阶段使用，不参与算法运行 import rosbag bag = rosbag.Bag("your_data.bag") lidar_ts, imu_ts = [], [] for topic, msg, t in bag.read_messages(topics=["/livox/lidar", "/livox/imu"]): # 用消息头时间戳，不用 bag 记录时间，两者可能差一个固定偏移 if topic == "/livox/lidar": lidar_ts.append(msg.header.stamp.to_sec()) else: imu_ts.append(msg.header.stamp.to_sec()) def report(name, ts): if len(ts) < 2: print(name, "数据量不足") return diffs = [b - a for a, b in zip(ts, ts[1:])] print(f"{name}: 帧数={len(ts)}, 平均间隔={sum(diffs)/len(diffs):.4f}s, " f"最大间隔={max(diffs):.4f}s, 单调递增={all(d > 0 for d in diffs)}") report("LiDAR", lidar_ts) report("IMU", imu_ts) # 判断两路是否同一时间基准：取各自起始时间，差值过大说明时钟不同源 if lidar_ts and imu_ts: print("起始时间差(s):", abs(lidar_ts[0] - imu_ts[0])) bag.close()脚本里三个判断点值得说清楚。单调递增为假，说明有乱序消息，算法里按时间排序的逻辑会被打乱；最大间隔远超平均值，说明中间丢帧，回放时可能表现为 IMU 数据断裂；起始时间差如果是个很大的常数，两路就不同源，必须加固定偏移。这三条任意一条不满足，先解决数据问题再谈算法调参，否则后面所有调试都是在错误的前提下做的。
回过头看，我最大的教训不是某个公式没推明白，而是太早开始调参数。前面那两个月里，有一半时间花在「改一个参数、跑一遍、看效果」的循环上，而其中大部分尝试在数据层面就已经注定失败。从那以后我每次拿到新的数据集，都强制先跑一遍时间戳检查，三张表填完再动参数，宁可开局慢半小时，也不要在错误的方向上试一天。
这份笔记里对我帮助最大的其实是附录那部分对状态量和观测矩阵的逐行展开，很多卡住的地方都是在对照它的时候突然通的。如果你也在读 FAST-LIO，建议把它当成手边的手册，读到哪查到哪，别指望一遍看懂。希望帮到你。
本文还有配套的精品资源，点击获取
