FAST-LIO源码精读：从李群李代数到ikd-tree调参排错

简介面向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接近单位阵时角度接近 0R接近 180° 旋转时sin(theta)接近 0这两处都要单独处理。FAST-LIO 的状态更新里大量调用的其实是ExpLog主要出现在误差定义和调试打印里所以如果你时间紧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/PointCloud2fields(name/offset/datatype)、point_step、row_step、data、header.stamp按字节解析出 xyz 和 intensitystamp是去畸变的时间基准sensor_msgs/PointCloud2is_dense、height/width判断有没有无效点无序点云height为 1sensor_msgs/Imulinear_acceleration、angular_velocity、orientation_covariance前向传播的输入协方差首元素常被拿来判断是否提供了姿态nav_msgs/Odometrypose.pose.position、pose.pose.orientation、twist.twist输出位姿和速度twist里的线速度就是状态量velPointCloud2的解析是最容易出事的一环。它不是float[3]数组而是一段二进制 blob需要靠fields里的offset和datatype自己去算偏移。比如x的offset是 0、datatype是FLOAT32intensity的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是点的反对称矩阵对外参的偏导结构类似但要再串一层。状态量和对应的维度我整理成一张表读代码时对着看状态量维度含义pos3雷达在世界系的位置rot3SO3雷达在世界系的姿态offset_R_L_I3SO3雷达到 IMU 的旋转外参offset_T_L_I3雷达到 IMU 的平移外参vel3世界系速度bg3陀螺零偏ba3加速度计零偏grav2重力方向模长固定只有 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.hppEigen 对齐、typedef 与那几个宏IMU_Processing.hpp是预处理的核心包含了前向传播和去畸变。这个文件里有几处 C 层面的细节单独看是语法理解之后能省掉一类段错误。EIGEN_MAKE_ALIGNED_OPERATOR_NEW这个宏出现在ImuProcess类里作用是重载new和delete保证对象内 Eigen 固定大小成员的内存按 SIMD 指令要求对齐。为什么需要SSE 要求 16 字节对齐、AVX 要求 32 字节编译器默认的new只保证到max_align_t在开启-O3 -marchnative时一个未对齐的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::esekfSTATE_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.8m/s²或者接近 1g不看不知道一看就知道该不该乘 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建议把它当成手边的手册读到哪查到哪别指望一遍看懂。希望帮到你。本文还有配套的精品资源点击获取