简介：围绕FAST-LIO论文与开源代码整理的这份docx学习笔记，面向已具备ROS与C++基础、希望吃透LiDAR-IMU紧耦合SLAM原理的开发者与研究生。内容从李群李代数、流形、点云与IMU数据格式等数学与工程准备讲起，延伸到velodyne VLP-16数据接入、ROS通信与C++实现要点，再按LiDAR预处理、IMU初始化、点云特征点、状态估计与ikd-tree增量建图逐层梳理算法框架，并对主程序main、IMU_Processing.hpp、laserMapping.cpp及esekfom::esekf等源码模块给出解读，附有状态量展开与旋转矩阵求导等附录。压缩包仅含1个docx文档，约5.09MB，篇幅集中、目录层级分明，便于按章节对照论文与代码检索。已有3161人学习，适合作为FAST-LIO入门到源码精读的辅助材料，帮助读者建立公式、框架与工程实现之间的对应关系，少走推导与调试弯路。
1. 一份 58 页的手写笔记：fast-lio 卡人的地方从来不是公式本身
我最早接触 fast-lio 的时候，手边摆着论文、源码、还有一份 58 页的笔记，翻来覆去读了整整两周，卡点却始终不在李代数那几行推导上。真正让我跑不出结果的，是雷达外参的旋转矩阵填反了、IMU 的单位是度每秒、点云里每个点的时间戳被我当成了一整帧的时间。这些坑论文不会写，源码注释也只是一笔带过。这份笔记的价值就在这儿：它把李群李代数、流形、PointCloud2 数据格式、ROS 收发、C++ 里那些容易忽略的宏，一直到 FAST-LIO2 的框架、ikd-tree、esekfom 的状态展开，按学习顺序摊开了一遍。适合已经能跑通基础雷达里程计、想把紧耦合方案吃透的人，也适合刚接手一份 FAST-LIO 工程、被编译错误和漂移折磨的新手。
2. 数学地基先立住：SO(3) 上的一次 ⊞ 到底改了哪几个数
很多人跳过前 20 页直接看状态估计，结果在后面读到boxplus的时候彻底懵掉。笔记把这一块放在最前面是对的，因为 FAST-LIO 的整个迭代卡尔曼滤波都建立在「状态在流形上、误差在切空间里」这件事之上，这个约定一旦没建立，后面每一个雅可比都看不懂。
2.1 用欧拉角还是用流形：为什么优化必须在切空间上做
姿态有三种常见表示：旋转矩阵 9 个参数但只有 3 个自由度，正交约束得额外维护；欧拉角 3 个参数但存在万向锁，在俯仰角接近 ±90° 的时候雅可比会炸；四元数 4 个参数带一个单位模约束，做加法之后得重新归一化。
流形的思路是绕开这三者的缺点。把姿态当作一个 3 维流形上的点，优化的时候不去直接加一个 3 维向量，而是先在对数映射下得到一个 3 维的扰动向量，用指数映射把扰动「乘」回流形上。这样优化的变量永远只有 3 个，约束天然满足，也不会出现奇异。
笔记里有一句话我觉得写得很准：流形上的运算就是把「加法」重新定义了。定义好新的加法，卡尔曼滤波的公式形式一个字都不用改，只是把+换成⊞，把-换成⊟。这也是 FAST-LIO 能把 ESKF 写成一个通用的esekfom::esekf模板的原因——滤波框架不关心你的状态是向量还是流形，它只要求你提供boxplus和boxminus。
2.2 指数映射、对数映射与三种 ⊞/⊟ 的具体写法
旋转向量 $\mathbf{r} = \theta \mathbf{a}$ 到旋转矩阵的映射，也就是指数映射，罗德里格斯公式给出：
$$\mathrm{Exp}(\mathbf{r}) = \mathbf{I} + \frac{\sin|\mathbf{r}|}{|\mathbf{r}|}\mathbf{r}^{\wedge} + \frac{1-\cos|\mathbf{r}|}{|\mathbf{r}|^2}(\mathbf{r}^{\wedge})^2$$
其中 $\mathbf{r}^{\wedge}$ 是反对称矩阵。对数映射 $\mathrm{Log}$ 是它的逆，从旋转矩阵的迹反解出旋转角，再从反对称部分解出旋转轴。有两个退化情形要单独处理：$|\mathbf{r}|$ 接近 0 时泰勒展开取前两阶，接近 $\pi$ 时旋转轴要从 $\mathbf{R}+\mathbf{I}$ 的列里挑模长最大的那一列，否则数值上会抖。
笔记把 ⊞ 和 ⊟ 分三种流形列了表，这个表建议直接抄在手边：
| 流形 | ⊞ 定义 | ⊟ 定义 |
|---|---|---|
| $SO(3)$ | $\mathbf{R} \boxplus \mathbf{r} = \mathbf{R},\mathrm{Exp}(\mathbf{r})$ | $\mathbf{R}_1 \boxminus \mathbf{R}_2 = \mathrm{Log}(\mathbf{R}_2^{\mathrm{T}}\mathbf{R}_1)$ |
| $\mathbb{R}^n$ | $\mathbf{a} \boxplus \mathbf{b} = \mathbf{a} + \mathbf{b}$ | $\mathbf{a} \boxminus \mathbf{b} = \mathbf{a} - \mathbf{b}$ |
| $SO(3)\times\mathbb{R}^n$ | ${\mathbf{R},\mathbf{a}} \boxplus {\mathbf{r},\mathbf{b}} = {\mathbf{R},\mathrm{Exp}(\mathbf{r}),\ \mathbf{a}+\mathbf{b}}$ | 逐分量对应相减 |
注意 $SO(3)$ 那个 ⊟ 里的顺序，是 $\mathrm{Log}(\mathbf{R}_2^{\mathrm{T}}\mathbf{R}_1)$ 而不是 $\mathrm{Log}(\mathbf{R}_1^{\mathrm{T}}\mathbf{R}_2)$，差一个符号。这个符号在残差计算里会直接影响雅可比的方向，写反了滤波器会朝反方向收敛，表现出来就是姿态越来越歪但协方差看着还挺正常，非常难查。
2.3 把 ⊞ 落到 state_ikfom：哪些量是向量，哪些量是流形
FAST-LIO 的状态结构体state_ikfom里混着两类量：位置、速度、零偏、外参平移是普通三维向量，姿态rot、雷达到 IMU 的外参旋转offset_R_L_I、重力方向grav是流形量。不同版本的源码里grav有时写成vec3、有时用球面参数化S2，这里按笔记里的展开方式给一份骨架，重点看结构而不是逐字对照。
// 状态定义：向量量与流形量混排，必须逐个声明清楚 struct state_ikfom { vec3 pos = vec3(0, 0, 0); // 世界系下的位置，普通向量 so3 rot = so3(); // 世界系到 IMU 系的旋转，SO(3) 流形 vec3 offset_R_L_I = vec3(0, 0, 0); // 雷达->IMU 外参旋转，以旋转向量形式存 vec3 offset_T_L_I = vec3(0, 0, 0); // 雷达->IMU 外参平移 vec3 vel = vec3(0, 0, 0); // 世界系速度 vec3 bg = vec3(0, 0, 0); // 陀螺零偏 vec3 ba = vec3(0, 0, 0); // 加计零偏 vec3 grav = vec3(0, 0, -1); // 重力方向，严格说是 2 自由度 }; // 流形加法：误差向量的第 0~2 位加位置，第 3~5 位是指态扰动 state_ikfom boxplus(const state_ikfom &s, const vec &d) { state_ikfom out = s; out.pos += d.segment<3>(0); out.rot = s.rot * Exp(d.segment<3>(3)); // 右乘，扰动在机体系 out.offset_R_L_I += d.segment<3>(6); out.offset_T_L_I += d.segment<3>(9); out.vel += d.segment<3>(12); out.bg += d.segment<3>(15); out.ba += d.segment<3>(18); out.grav += d.segment<3>(21); return out; } // 流形减法：残差就是两个状态在切空间下的差 vec boxminus(const state_ikfom &a, const state_ikfom &b) { vec d(24); d.segment<3>(0) = a.pos - b.pos; d.segment<3>(3) = Log(b.rot.inverse() * a.rot); // 注意顺序，反了就变号 // ... 其余分量同理逐段相减 return d; }这段骨架对应的逻辑很直白：误差状态向量的维数是 24，但真正的自由度不是 24 而是 23，因为grav只有 2 个自由度、rot只有 3 个。很多人在推协方差传播的时候对不上号，就是因为默认了「维度等于自由度」。
参数上要盯住两点。第一是rot右乘还是左乘：右乘表示扰动定义在机体系，此时雅可比里会出现 $\mathbf{R}$ 而不是 $\mathbf{R}^{\mathrm{T}}$，两者差一个旋转，混用会让协方差传播完全错位。第二是外参offset_R_L_I用旋转向量存储，而不是直接存so3，好处是加减零均值噪声的时候形式统一，坏处是每次用之前都得做一次Exp，这块在去畸变循环里是热点，别反复算。
提示：把这几个函数打印出来做一次数值自检，取任意
d，验证boxminus(boxplus(s, d), s)大约等于d。这一步花五分钟，能省掉后面几天的排查。
3. 数据入口的四个格式：PointCloud2、Imu、Odometry 和那个 30° 视场角
数学看完进入工程层，第一个拦路虎永远是数据。笔记把数据格式单独拎成一节，而且把sensor_msgs::PointCloud2的字段一个个拆开写，我觉得这是整份笔记里最实用的部分之一。因为一份跑不起来的工程，八成的锅在这里。
3.1 PointCloud2 的 fields、point_step 与 data 该怎么读
PointCloud2本身不存「点」，它存的是一段二进制 blob 加一份描述。理解三件事就够了：
fields描述每个字段（x、y、z、intensity、time 等）在单点结构里的偏移offset、数据类型datatype、元素个数count；point_step是单个点占多少字节，row_step是一行占多少字节；data是真正的数据，长度等于row_step * height。
雷达驱动不同，字段排列完全不同。有的把time放在最后当 float32，有的用两个 uint16 拼一个 double，还有的干脆只给一个timestamp的 uint32 纳秒偏移。读之前必须先把fields打印一遍，这一步不能省。
#include <sensor_msgs/PointCloud2.h> #include <sensor_msgs/point_cloud2_iterator.h> // 用迭代器读，避免手工算偏移，字段名写错会直接抛异常 void cloudHandler(const sensor_msgs::PointCloud2::ConstPtr &msg) { // 先确认字段存在，不存在就别硬读 sensor_msgs::PointCloud2ConstIterator<float> it_x(*msg, "x"); sensor_msgs::PointCloud2ConstIterator<float> it_y(*msg, "y"); sensor_msgs::PointCloud2ConstIterator<float> it_z(*msg, "z"); pcl::PointCloud<pcl::PointXYZI>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZI>()); cloud->reserve(msg->width * msg->height); for (; it_x != it_x.end(); ++it_x, ++it_y, ++it_z) { pcl::PointXYZI p; p.x = *it_x; p.y = *it_y; p.z = *it_z; // 距离过近的点是打在机体上的，一定要滤掉，否则会拖出一面"贴身墙" if (p.x * p.x + p.y * p.y + p.z * p.z < 0.5 * 0.5) continue; cloud->push_back(p); } cloud->width = cloud->size(); cloud->height = 1; cloud->is_dense = false; }逻辑说明：迭代器版本的好处是字段名和类型由库来校验，比手写reinterpret_cast安全得多。参数上，reserve用width * height是上界，实际会因为距离滤波少一些，问题不大。距离阈值0.5是经验值，16 线雷达装在车体上，最近有效回波一般在 0.3~0.5 米以外，具体要看你机体的实际尺寸，装得低的平台这个值要调大。
如果雷达点云带每点时间字段，还需要再开一个PointCloud2ConstIterator<float> it_t(*msg, "time")，把相对时间戳一并存进pcl::PointXYZI的自定义结构里。这一步不做，后面去畸变就是空转——因为去畸变必须知道每个点相对于帧头的时间偏移。
3.2 IMU 与里程计消息：单位、坐标系和重力这三件事
sensor_msgs::Imu里真正用到的只有两个字段：angular_velocity（三轴角速度，rad/s）和linear_acceleration（三轴线加速度，m/s²，含重力）。看着简单，但翻车点极多：有些驱动输出的是度每秒，需要在订阅后立刻乘 $\pi/180$；有些把加速度写成不含重力的「线性加速度」，那初始化时估计出来的重力方向永远是零；还有的坐标系是 NED 而不是 ENU，Z 轴朝下。
nav_msgs/Odometry是输出侧，主要给可视化和其他节点用，格式上注意header.frame_id写世界系、child_frame_id写机体系，两者写反在 RViz 里表现是坐标系整体反向旋转，很容易被误判成算法发散。
16 线旋转式雷达的参数也需要先算清楚，因为这决定了降采样和邻域搜索的量级：
| 参数 | 典型值 | 对算法的影响 |
|---|---|---|
| 线数 | 16 | 决定垂直方向的点密度 |
| 垂直视场 | 30°（-15°~+15°） | 上下楼、坡道场景先天的盲区 |
| 垂直角分辨率 | 2° | 相邻线间距，远处点面拟合的空间尺度 |
| 水平角分辨率 | 0.1°~0.4° | 单圈点数，10 Hz 下约 29 万点/秒 |
| 测距范围 | 数十米量级 | 决定det_range的上界 |
按 0.2° 的水平分辨率算，一圈 1800 点、16 线、每秒 10 帧，就是 28.8 万点。这个量级直接决定了不能对每帧所有点都做最近邻搜索，必须先降采样，笔记里写的「间隔 3 个点取一个」，本质就是把点数压到约三分之一。
3.3 ROS 收发骨架与 spinOnce 的隐含顺序
主循环的结构看起来平平无奇，但执行顺序错了会引入一整帧的延迟，表现出来就是「稍微一动就飘」。
int main(int argc, char **argv) { ros::init(argc, argv, "laserMapping"); // 节点名别和其他节点重名 ros::NodeHandle nh; // 发布器队列 1000，别写太小，建图节点偶尔会短暂阻塞 ros::Publisher pubOdom = nh.advertise<nav_msgs::Odometry>("/Odometry", 1000); ros::Publisher pubCloud = nh.advertise<sensor_msgs::PointCloud2>("/cloud_registered", 1000); ros::Subscriber subImu = nh.subscribe("/livox/imu", 200000, imu_cbk); ros::Subscriber subLidar = nh.subscribe("/points_raw", 10, standard_pcl_cbk); ros::Rate rate(5000); // 主循环按高频空转，保证处理及时 while (ros::ok()) { ros::spinOnce(); // 先吐回调，把队列里的数据消化掉 // 这里才是真正的处理逻辑：状态预测、残差构建、地图更新 rate.sleep(); } return 0; }三处参数值得说。IMU 订阅队列写到 20 万是刻意的，因为 IMU 频率高（常见 200 Hz）、处理一帧雷达点云时回调会积压，队列小了直接丢数据，丢帧之后前后向传播就不连续了。雷达订阅的队列写 10 就够，一帧处理完再取下一帧本来就是期望行为。主循环Rate(5000)不是处理频率，只是让循环空转得足够密，别让处理逻辑等太久。
ros::spinOnce()的位置必须在处理逻辑之前。反过来写，这一轮处理的是上一轮的数据，等效于人为增加一帧延迟，滤波器会觉得雷达和 IMU 对不上，然后就靠协方差去硬圆，结果就是轨迹在地图里缓慢打转。
3.4 16 线雷达的参数和三处时间戳
一帧点云从采集到进算法，会经过三处时间戳，必须对齐到同一时间基准：
- 雷达帧头时间
msg->header.stamp，代表这一帧第一个点被打出来的时刻； - 每点相对帧头的时间偏移，通常存在点云字段里；
- IMU 消息时间，用来和雷达做插值对齐。
常见做法是用一个time_offset_lidar_to_imu参数吸收硬件上两路传感器的固定时延，再配一个「是否启用时间同步」的开关。在没有硬件触发对齐的设备上，这两路时延几十毫秒是常态，不做补偿的话，车辆转向时 Z 轴和航向角会肉眼可见地漂。
注意：先用
rosbag info看一遍雷达和 IMU 的频率与时间跨度，如果两者起始时间差超过一秒，说明录制时没有同时启动，这种情况先别急着调参数。
4. 主循环逐段拆：从 IMU 前后向传播到 ikd-tree 增量建图
框架图看一眼就懂，代码看一天不懂。这一章按笔记里的顺序，把 FAST-LIO2 的主链路拆成四段：IMU 初始化、前后向传播、点面残差与 IEKF、ikd-tree 增量建图。每一段我都会写清楚输入是什么、输出是什么、失败时会看到什么现象。
4.1 IMU 初始化：静止判定、重力对齐与失败重试
初始化做的事情很朴素：把前若干帧 IMU 数据取均值，陀螺的均值就是零偏bg的初值，加计的均值方向就是重力方向的反方向。因为在静止时，加计测到的只有重力。
// 初始化判定：累计到一定帧数后做一次均值检查 void ImuProcess::IMU_init(const MeasureGroup &meas, int &N) { vec3 mean_acc(0, 0, 0), mean_gyr(0, 0, 0); for (const auto &m : meas.imu) { mean_acc += m.linear_acceleration; mean_gyr += m.angular_velocity; } mean_acc /= meas.imu.size(); mean_gyr /= meas.imu.size(); // 模长必须在 9.8 附近，偏太多说明不是静止，或者单位不是 m/s^2 if (std::fabs(mean_acc.norm() - G_m_s2) > 0.5) { ROS_WARN("IMU 静止检查失败，模长 %f，跳过这批数据", mean_acc.norm()); N = 0; // 直接重置，等下一次静止 return; } // 重力方向取反归一化，姿态初值由它构造 state.grav = -mean_acc / mean_acc.norm() * G_m_s2; state.rot = SO3::fromTwoVectors(vec3(0, 0, 1), -state.grav.normalized()); state.bg = mean_gyr; // 陀螺零偏初值，后面估计器还会继续修 init_flag = true; }逻辑上没什么花头，参数才是关键。模长容差0.5是相对宽松的值，说明注释里说「一般取 0.1~1.0」。取太小，稍微有点震动就初始化不过；取太大，把运动状态误判成静止，重力方向就估歪了，后面整条轨迹会有一个固定倾角。累计帧数N一般给 20~50 帧，按 200 Hz 算也就是 0.1~0.25 秒。
失败时的现象很好认：终端反复刷初始化警告，里程计不发布，RViz 里只有原始点云没有地图。如果你看到日志里打印的模长是个接近 1 的数，那基本可以确定 IMU 输出的加速度单位不是 m/s²，或者驱动已经替你扣掉了重力。
4.2 前向传播与反向传播：去畸变到底用了哪一段 IMU
一帧雷达不是瞬间拍下来的，旋转式雷达扫完一圈要 100 毫秒，这期间载体在动，同一帧里的点其实是在不同位姿下测到的。不去畸变直接拿去和地图匹配，转个弯墙面就成弧形了。
FAST-LIO 的做法是两趟：
- 前向传播：从上一帧的状态出发，用这帧内所有 IMU 数据逐个积分，递推出当前帧末尾的位姿和协方差，这部分就是 EKF 的预测步；
- 反向传播：从帧末尾的状态往回走，把每个点的时间戳对应的位姿算出来，然后把点从测量时刻的机体系变换到帧头机体系，完成去畸变。
// 反向传播：从帧尾往帧头推，给每个点的补偿量 void ImuProcess::UndistortPcl(const MeasureGroup &meas, state_ikfom &state) { // 这里用的是缓存的 IMU 序列，注意要和雷达帧的时间区间严格对齐 for (int i = (int)meas.imu.size() - 1; i >= 0; --i) { double dt = ...; // 由相邻 IMU 时间戳算出 // 用角速度和零偏做旋转积分，方向是"回退" state.rot = state.rot * Exp(-(meas.imu[i].angular_velocity - state.bg) * dt); // 加速度里要扣掉零偏，并转到世界系后扣重力 vec3 acc_world = state.rot * (meas.imu[i].linear_acceleration - state.ba) + state.grav; state.pos -= state.vel * dt + 0.5 * acc_world * dt * dt; state.vel -= acc_world * dt; } }两个细节必须说清。第一，反向传播里的积分方向是负的，Exp里的符号、速度的减号都不能少，写错了点云会被「反向拉长」，表现出来是墙壁边缘出现拖影。第二，扣重力必须在世界系下做，如果在机体系下直接减grav，姿态一变符号就乱。
去畸变之后的点云还有个隐含要求：每点的时间戳必须单调且落在这一帧的时间区间内。有些雷达驱动给出的时间字段是设备上电以来的纳秒数，需要先减去帧头时间再归一化，否则算出来的补偿系数会是几万倍。
4.3 点面残差与 IEKF：最近邻五个点怎么拟合平面
去畸变之后就是匹配。对降采样后的每个点，在局部地图里找最近的 5 个点，用这 5 个点做一次 PCA，最小特征值对应的特征向量就是平面法向量 $\mathbf{n}$，点到平面的距离就是残差：
$$r = \mathbf{n}^{\mathrm{T}}(\mathbf{p} - \mathbf{q}_0)$$
其中 $\mathbf{q}_0$ 是 5 个近邻的质心。这个残差对状态求导，就得到观测雅可比。
迭代卡尔曼滤波（IEKF）在每个雷达帧内会跑若干轮：用当前状态把点云变换到世界系，重新找近邻、重新算残差，然后做一次更新。收敛判据是状态增量足够小，或者迭代次数到上限。
// IEKF 单帧迭代骨架，NUM_MAX_ITERATIONS 常见取 4 for (int iter = 0; iter < NUM_MAX_ITERATIONS; ++iter) { buildResidual(state); // 用当前状态重新找近邻、算残差与雅可比 H // 卡尔曼增益：注意这里用的是流形上的协方差，不是普通向量 mat K = (H^T * R_inv * H + P_inv).inverse() * H^T * R_inv; vec delta = K * residual; // 残差是流形差，不是普通减法 state = boxplus(state, delta); // 用 ⊞ 更新，保证姿态仍在 SO(3) 上 if (delta.norm() < 1e-4) break; // 收敛就提前退出 P = (I - K * H) * P; }参数上有三处值得调：
- 迭代次数：4 次是常见值，降到 2 能提速度但噪声场景下精度掉得明显，加到 6 以上收益很小；
- 近邻数量：5 个是平面拟合的最小够用值，取 3 个对噪声太敏感，取 10 个会把墙面拐角处的点也算进来，平面拟合反而失准；
- 降采样尺寸：FAST-LIO2 里对应
filter_size_surf，取值和雷达的角分辨率、det_range相关，通常 0.3~0.6 米。
这里有个反直觉的点：迭代次数不是越多越好。因为近邻搜索是在当前估计的地图上做的，迭代太多次，状态会被「自己的估计」反复拉过去，观测噪声被过度平滑，表现出来是轨迹看着很光滑，但闭环误差反而更大。
4.4 ikd-tree 的三个参数：cube_len、降采样尺寸与增量插入
普通 KD 树只能静态建树，地图每帧更新一次就得整棵重建，几十万点的规模根本扛不住。ikd-tree 的核心是三点：增量插入、懒惰删除、按不平衡度局部重建。
- 增量插入：新点沿着树往下走，插到对应叶节点，再向上更新子树包围盒和点数；
- 懒惰删除：被地图淘汰的点只打删除标记，不真的从树里摘掉；
- 局部重建：一旦某棵子树的删除比例或不平衡比例超过阈值，就把这棵子树摘下来当点集，重新建一棵平衡树挂回去。
构造函数有三个参数，常见取值在这一档：删除比例阈值约 0.2~0.3，不平衡阈值约 0.6~0.7，盒子边长约 0.2 米。前两个决定重建频率，太小会频繁重建拖慢速度，太大会让树退化成链表，最近邻搜索变慢。盒子边长是「按体素去重」的尺度，和点云的密度直接相关。
地图范围由cube_len控制，只有在当前位姿周围这个立方体内的点才保留，超出的被删除。这个值要大于det_range，否则雷达看得见的地方地图里没有，匹配会失败——现象是走得越远、匹配上的点越少、最后突然发散。det_range一般取雷达可靠测距的 80% 左右，cube_len取它的 1.5~2 倍。
提示：ikd-tree 的删除是懒惰的，内存不会立刻释放。跑长时间序列时看
top里 RSS 缓慢上涨是正常现象，涨到某个平台期会稳定下来，不必惊慌。
5. 避坑清单：五种跑歪的现象、原因和补救
这一章是血泪经验最集中的地方。下面五条都是我自己或者身边人真实踩过的，按「现象 → 原因 → 解决」写，遇到问题时可以对号入座。
5.1 点云整体飘走：先查外参旋转矩阵的方向
现象：启动后点云能建出地图，但地图是斜的，走直线时机体系整体向下沉或者向一侧偏，RViz 里看地图和轨迹的相对关系明显不对。
原因：雷达和 IMU 之间的外参extrinsic_R填错了。最常见的是把「雷达到 IMU」的旋转矩阵写成了「IMU 到雷达」，也就是忘了转置；其次是欧拉角转矩阵时把角度单位当成了度，实际要弧度；还有一种是行主序和列主序混了，在 yaml 里按行写进去，代码按列读。
解决：拿一个已知的静止场景验证——把设备水平静置，输出的姿态应该接近单位阵，重力方向应该接近(0,0,-9.8)。如果不满足，先别动别的参数，把外参打印出来，用它的逆乘回去看看是否更合理。外参的旋转部分行列式必须等于 1，写错的矩阵经常连这个都过不了。
5.2 初始化一直不过：g 的模长和 IMU 单位
现象：终端反复打印静止检查失败的警告，或者初始化能过但估计出来的重力方向明显不对，跑起来 Z 轴一路往下掉。
原因：四种可能——设备没静止就开始跑；IMU 的加速度单位不是 m/s²（有些驱动输出以 g 为单位，模长会是 1 左右）；驱动已经扣掉了重力，输出的是纯线性加速度；IMU 频率不稳定，均值统计的样本太少。
解决：先单订阅 IMU 话题，用rostopic echo抓十秒数据肉眼看模长。如果是 1 附近，乘 9.8；如果是运动状态下的抖动量级，说明驱动扣了重力，得改初始化逻辑。频率用rostopic hz确认，常见是 200 Hz，如果只有几十赫兹，初始化累计帧数要相应加大。
5.3 墙面出现双层重影：时间戳和去畸变
现象：静止时地图很干净，一运动墙面就分成两层，间距和速度正相关；转弯时走廊的墙壁弯成弧形。
原因：主要是去畸变没生效或者时间戳不对。点云里没有每点时间字段、时间字段单位是微秒而不是秒、时间字段是绝对时间没有减去帧头，这三种都会让补偿系数算错。另一个可能是雷达和 IMU 之间的固定时延没有补偿，速度越快偏差越明显。
解决：先把每点时间字段打印出来，看它的取值范围是否落在 0 到帧周期（约 0.1 秒）之间。如果不落在这个区间，就是单位或者基准问题。确认时间戳没问题之后，再去调time_offset_lidar_to_imu，从 0 开始每次加 5 毫秒试，看哪一档重影最轻。
5.4 跑几分钟就卡死：内存与降采样
现象：短序列跑得很顺，跑长序列几分钟后帧率骤降，最后卡住不再输出；或者内存占用一路涨到把机器吃满。
原因：地图降采样参数给得太小，或者没有开地图降采样，点云无限累积；cube_len设得过大，局部地图永远不裁剪；ikd-tree 的删除阈值给得太大，被删除的点长期不释放；RViz 里同时显示原始点云和全局地图，渲染端先撑不住。
解决：先把 RViz 的显示项砍到只剩地图和一个坐标系，很多时候是显示端的问题。然后检查filter_size_map_min和cube_len的搭配，前者是地图点的体素尺寸，后者是地图范围，两者比值决定了地图里的点数上限。跑长序列时把cube_len压到det_range的 1.5 倍左右，内存就能稳在一个平台。
5.5 编译期 Eigen 对齐报错：那个宏和编译选项
现象：编译能过但运行时随机崩溃，报错信息里带Eigen和unaligned；或者编译直接报某个结构体大小不是 16 的倍数。
原因：Eigen 的定长向量化类型要求内存按 16 字节（SSE）或 32 字节（AVX）对齐。new出来的内存默认只保证 8 字节对齐，如果类里含有 Eigen 的定长成员，就必须重载这个类的new和delete。FAST-LIO 里ImuProcess这类含大量 Eigen 成员的类都需要处理。
解决：在类的私有区加一行宏即可，位置很关键，必须写在类内部。
class ImuProcess { public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW // 重载 new/delete，保证对齐 // 含 Eigen 定长成员的类都必须加这一行 private: Eigen::Matrix<double, 24, 24> cov_; // 定长矩阵，需要对齐 };同时编译选项里开-O3 -march=native，让 Eigen 有能力用上 SIMD。如果用容器存这类对象，得用std::vector<Obj, Eigen::aligned_allocator<Obj>>，普通std::vector在扩容时会重新分配内存，对齐又丢了，这种崩溃非常随机，属于典型的黑匣子问题。
6. 从能跑到能改：三条验证线和一组必须会调的参数
跑通只是起点，真正决定这套东西能不能用在项目里的，是你有没有一套稳定的验证方法。我一般按三条线走。
第一条是静止漂移线。设备静置一到两分钟，看位置输出的漂移量。正常情况下应该在厘米级以内，同时零偏估计会缓慢收敛。如果漂移是线性增长的，多半是零偏没估出来；如果是阶跃式的，检查 IMU 有没有丢帧。这条线最便宜，却最能暴露初始化和时间同步的问题。
第二条是闭环重合线。走一个回到起点的矩形或 8 字，看终点和起点的位置差、以及两段地图的重合度。位置误差可以接受，但地图如果明显错层，说明位姿估计里存在系统性偏差，通常是外参或者时间偏移没调好。
第三条是 Z 轴高程线。上楼、下坡各走一遍，看高度变化是否连续。16 线雷达的垂直视场只有 30°，上下楼时天空方向几乎没有点，纯几何匹配会退化成靠 IMU 积分，高度漂移是这类硬件的先天短板，别指望算法能完全消除。
参数调整上有个顺序，我按优先级排：
| 优先级 | 参数 | 典型值 | 调整方向与后果 |
|---|---|---|---|
| 先 | extrinsic_R/extrinsic_T | 标定值 | 错一点全错，先确认无误再动别的 |
| 先 | time_offset_lidar_to_imu | 0~0.05 秒 | 重影、转弯弧形先调它 |
| 先 | filter_size_surf | 0.3~0.6 米 | 太小噪声敏感，太大特征丢失 |
| 后 | filter_size_map_min | 与上一项同量级 | 影响地图密度和内存 |
| 后 | cube_len | det_range的 1.5~2 倍 | 太小匹配失败，太大内存暴涨 |
| 后 | 迭代次数 | 4 | 降到 2 提速，加到 6 收益很小 |
调参时每次只动一个，跑同一段数据，把轨迹存下来对比，别凭肉眼在 RViz 里看。存轨迹的方法很简单，把每帧里程计按时间戳、位置、四元数的格式写成文本，然后拿两次结果叠在一起看偏差，比反复回放高效得多。
# 录制时只留必要话题，别把相机和原始点云全录进去，否则回放会掉帧 rosbag record -O seq_test.bag /points_raw /imu/data /Odometry # 回放限速，保证处理节点跟得上，这一点很多人忽略 rosbag play seq_test.bag -r 0.5 --clock回放限速这件事我必须单独强调。rosbag play默认按原速播放，如果你的建图节点处理一帧要 150 毫秒而雷达是 10 Hz，队列立刻爆掉，表现出来的就是「跑起来卡、漂得厉害」，但换个机器又好了。用-r 0.5慢放，能区分是算法问题还是算力问题——这个判断不做，后面所有调参都是在错误的前提上折腾。
最后说个我自己的习惯。每次改完参数，我会先把外参、时间偏移、降采样这三个值连同这次跑出来的终点误差记在同一张表里，跑够五组再下结论。有一回我连续调了三天降采样尺寸，最后发现根因是外参矩阵转置写反了，那张表让这个错误在第四组就露了馅。从那以后我每次动参数之前，都强制先走一遍静止漂移和闭环重合这两条线，确认基线没问题再谈优化。希望帮到你。
本文还有配套的精品资源，点击获取
