企业官网建设流程全解析

简介：面向 FAST-LIO 学习者与 LiDAR-IMU 融合 SLAM 入门者的个人理解文档，围绕论文与源码梳理算法脉络，适合在具备 ROS、C++ 与点云基础后对照研读。压缩包共 1 个 docx 文件，约 5.09MB，以图文笔记形式组织，目录按基础准备、框架解析、源码阅读和附录问题分区，便于按章节检索、批注与复盘。内容从李群李代数、流形、数据格式等基础准备切入，延伸到 LiDAR 数据预处理、IMU 初始化、点云特征点、状态估计与 ikd-tree 等核心环节，并落到主程序、IMU 处理、laserMapping 以及 esekf 等源码模块，附有旋转矩阵导数、状态量展开、问题记录与参考文献，并结合个人阅读体会重点说明各模块的输入输出、代码结构和推导思路，方便后续复现与扩展。已有 3155 人学习，可作为理解 FAST-LIO 框架、建立论文公式到工程实现对应关系、排查阅读源码时常见疑问的参考笔记。

1. 从一帧 10 毫秒说起：fast-lio 快的不是特征提取，而是“少做两步”

第一次把 fast-lio 的代码跑通之后，最容易冒出来的疑问其实并不是“它为什么准”，而是“它凭什么这么快”。直觉上，激光 SLAM 的算力大头在特征提取和近邻搜索上，想让前端跑进 10 毫秒一帧，最该做的就是把角点、面点提取写得更快、更省。fast-lio 的做法正好相反：特征提取整段砍掉，原始点云按固定间隔抽稀后直接丢进滤波器；近邻搜索也不每帧重来，而是挂在一棵能原地增删的增量 KD 树上。把这两件事拆开看，实时性才讲得通——它并没有把某一步优化得更快；它直接把原本需要反复做的整段工作变成了增量。真正值得搞懂的是：为什么去掉特征提取之后精度没塌，以及那棵能增删的树到底是怎么绕开重建代价的。

2. 李群与流形：为什么姿态不能在向量空间里直接加减

2.1 从旋转向量到指数映射，SO(3) 上的两种运算

姿态用三个数表示看起来很自然：绕 x 转多少、绕 y 转多少、绕 z 转多少。但这三个数拼出来的空间不是平的——你用 350° 和 10° 做加权平均，得到的不是 180°，而是 180° 附近的另一条路径；两个旋转矩阵相加，结果连正交性都不满足，行列式也不再是 1。卡尔曼滤波的每一步都在做加减和线性组合，如果状态里塞进去的是旋转矩阵本身，第一步就已经不成立了。

李代数就是给这个弯曲空间找的一块局部平面。旋转向量r = θa是李代数里的元素，通过指数映射变回旋转矩阵：

Exp(r) = I + (sin‖r‖ / ‖r‖) · r^ + ((1 - cos‖r‖) / ‖r‖²) · (r^)²

这个式子的几何含义很直白：把“绕轴 a 转 θ”这件事，写成矩阵形式。反过来，对数映射把矩阵拉回旋转向量。卡尔曼滤波真正需要的是一个“局部加法”，在 FAST-LIO 里被定义成两个算子：

// ⊞：在流形上叠加一个切空间扰动，δ 是 3 维的 // 注意这里是右乘，FAST-LIO 全文的约定都是 R ⊞ δ = R * Exp(δ) inline SO3 SO3::boxplus(const Vec3 &delta) const { return SO3(exp(delta) * R_); // R_ 是内部存的 3x3 旋转矩阵 } // ⊟：两个流形元素之间的差，结果落在切空间，仍是 3 维 inline Vec3 SO3::boxminus(const SO3 &other) const { return log(other.R_ * R_.transpose()); }

boxplus和boxminus必须互为逆运算，这是整个滤波器自洽的前提。参数delta就是误差状态 δθ，它的维度恒为 3，与旋转矩阵的 9 个元素、四元数的 4 个元素都无关。这一点是后面所有雅可比能写成 3 列的原因。

2.2 23 维状态里，哪几个维度天生“不在平面上”

FAST-LIO 的状态向量名义上 23 维，但它是拼起来的：位置 3、旋转 3、雷达到 IMU 的旋转外参 3、平移外参 3、速度 3、陀螺零偏 3、加计零偏 3、重力方向 2。看似 23 个 double，实际上每一段的“加法规则”都不一样。

struct state_ikfom { vec3 pos; // 3：普通欧式向量，直接加 SO3 rot; // 3：长在 SO(3) 上，走 boxplus SO3 offset_R_L_I; // 3：外参旋转，同样在 SO(3) 上 vec3 offset_T_L_I; // 3：外参平移，直接加 vec3 vel; // 3：速度，直接加 vec3 bg; // 3：陀螺零偏，直接加 vec3 ba; // 3：加计零偏，直接加 S2 grav; // 2：重力方向，模长固定，只在球面切空间扰动 };

最容易看漏的是最后两维。重力向量不是自由的三维向量，它的模长被固定在大约 9.81，真正可变的只有方向，方向有两个自由度，所以它活在一个二维球面 S2 上。给它加扰动的方式是：在球面上取两个正交切向量 b1、b2，g ⊞ δ = g + b1·δ₁ + b2·δ₂，加完再归一化回模长。如果硬把重力当成三维向量去估，滤波器会多出一个“重力大小”的不可观方向，协方差在那条方向上会一直涨，跑久了就容易发散——这是一条典型的翻车路径。

外参也放进状态里一起估，是个挺有意思的取舍。理论上雷达和 IMU 的相对位姿应该提前标好、当常量用；但只要标定有零点几度的残差，长时间积分后就会体现在地图重影上。把它塞进状态，等于让滤波器在线微调标定结果，代价是状态维度变大、可观性变差——外参只有在机器人有充分的旋转激励时才可观测，原地直行是估不出来的。

2.3 为什么不用“四元数相加再归一化”硬凑

一个常见的替代思路是：状态里存四元数，加法就做普通四元数加法，加完归一化。工程上确实能跑，但有三个隐患。

第一，归一化是非线性操作，它会把协方差矩阵的传播假设打破。卡尔曼滤波的协方差更新建立在“状态更新是线性的”这个前提上，你在更新之后偷偷做一次归一化，等于在协方差上叠了一个没有建模的投影，长期下来 P 矩阵会低估不确定性，滤波器变得过度自信。

第二，误差状态会退化成 4 维，但实际自由度只有 3，H 矩阵和卡尔曼增益里会多出一个零空间方向，数值上容易出问题。有些实现会在归一化之后再强行把协方差投影到切空间，代码复杂度立刻上去。

第三，切空间扰动这套写法的好处是 δθ 与“旋转了多少角度”量纲一致，雅可比推导干净，收敛判据也好写——直接看dx.norm()是否小于阈值即可。FAST-LIO 的迭代卡尔曼每一轮都在这个 3 维切空间上解方程，23 维状态对应 23 维增量，不多不少。

2.4 代码里怎么落地：内存对齐、固定尺寸和类型别名

流形的数学再漂亮，落到 C++ 上还是绕不开几个工程细节，而这些细节恰恰是跑不起来时最先怀疑的地方。

class ImuProcess { public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW // 重载 new/delete，保证 16/32 字节对齐 private: // 固定尺寸的矩阵在编译期就能展开，比动态尺寸快一个档次 typedef Eigen::Matrix<double, 23, 23> Matrix23d; Matrix23d F_x; // 状态转移雅可比 Matrix23d P; // 协方差 };

EIGEN_MAKE_ALIGNED_OPERATOR_NEW这个宏做的事，是给类重载operator new和operator delete，把分配出来的地址对齐到 16 字节（SSE）或 32 字节（AVX）。Eigen 里固定大小的向量化类型（比如Eigen::Vector4d、Matrix4d）内部按 SIMD 指令的要求排布数据，地址没对齐时，加载指令要么性能打折，要么直接触发总线错误。这个宏必须写在类的public区，而且要写在所有 Eigen 成员之前的位置习惯上——漏写不会编译报错，只会在运行到某个特定对象时莫名其妙地崩。

typedef在这里不是为了好看。Eigen::Matrix<double, 23, 23>写十遍容易错，起个别名之后，改维度只要改一处。更重要的是，固定尺寸（编译期已知）和动态尺寸（运行时才知道）在 Eigen 里是两套完全不同的实现：前者把循环全部展开、栈上分配，后者走堆分配加运行时判断。23 维状态是编译期确定的，用固定尺寸；而点云的点数是运行时才知道的，用Eigen::Matrix<double, Eigen::Dynamic, 1>这类动态类型才合理。这个区分在点云规模变化时对性能影响非常明显。

3. 两次激光之间的 100 毫秒：IMU 传播、去畸变与迭代卡尔曼的底层机制

3.1 前向传播：23×23 的协方差是怎么滚起来的

16 线机械雷达转一圈大约 100 毫秒，这期间 IMU 可能输出 100 到 200 组数据（100Hz 到 200Hz 很常见）。滤波器不能等到激光来了才算，必须靠 IMU 把状态一步一步推过去，激光帧到达时只做一次量测更新。这就是所谓“紧耦合”的实际含义：IMU 不是辅助，它是主循环。

// 一次 IMU 采样推一步，dt 是两个相邻 IMU 时间戳之差，典型 5~10 ms void ImuProcess::UndistortPcl(...) { for (size_t i = 0; i < imu_buf.size(); ++i) { double dt = (imu_buf[i].time - imu_buf[i-1].time); // 1. 用当前零偏扣掉原始测量 Vec3 acc = imu_buf[i].linear_acceleration - state.ba; Vec3 gyr = imu_buf[i].angular_velocity - state.bg; // 2. 状态递推：旋转走流形乘法，速度位置是普通积分 state.rot = state.rot * Exp(gyr * dt); state.vel += (state.rot * acc + state.grav) * dt; state.pos += state.vel * dt; // 3. 协方差跟着状态转移雅可比 F 走，Q 是 IMU 噪声阵 state.P = F * state.P * F.transpose() + Q; } }

F是 23×23 的状态转移雅可比，它对状态的每个分量求偏导。里面的关键项是旋转对速度的影响：旋转变了 δθ，加速度在世界系下的方向就变了-R·a^。这一项是协方差能正确“传递不确定性”的核心，写错符号会让滤波器在转弯时突然发散。Q是过程噪声，由 IMU 的陀螺噪声密度、零偏随机游走密度算出来，通常把连续时间噪声按dt离散化后累加。

这段代码还有个容易被忽略的点：传播用的是 IMU 自身的采样时刻，但激光帧的时间戳未必和某个 IMU 采样点对齐。所以传播的最后一步通常要做一次时间插值，把状态正好推到激光帧的起始时刻，否则后面的去畸变会整体偏掉几毫秒。

3.2 去畸变：把一帧点云“倒带”回同一个瞬间

一帧点云不是在同一时刻采集的。16 线雷达水平 360°、角分辨率 0.2° 到 0.4°、垂直视场 30°，扫描一周 100 毫秒里，机器人可能已经转过 10°、走出去十几厘米。如果直接把这 1800 个点当成同一时刻的观测，快速旋转时地图会明显“分层”。

去畸变的做法，是用 IMU 把每个点从它自己的采集时刻变换到帧尾时刻。思路是这样：滤波器在传播阶段已经记录了一串位姿（每个 IMU 时刻一个），对第 i 个点，按它的相对时间在相邻两帧位姿之间插值，得到 T_i；再把点从雷达系变到 IMU 系、再从 IMU 系变到世界系，最后统一变换回帧尾的雷达坐标系：

// 每个点带着自己的相对时间 rel_time，范围是 0 到这一帧的时长 Vec3 P_compensated = T_end.inverse() * T_i * T_L_I * P_raw; // T_L_I 是雷达到 IMU 的外参，T_end 是帧尾时刻的位姿

这里对时间精度的要求其实很高。雷达的点时间戳精度通常在微秒级，但 IMU 和雷达如果用的是两个时钟源、又没有做时间同步，偏差可能到十几毫秒。十几毫秒在静止时无所谓，在高速运动时就是几厘米的地图重影，而且这种重影不会随着建图时间变长而收敛——因为它不是随机噪声，是系统误差。代码里那个time_offset_lidar_to_imu参数就是干这个的，填错方向会越跑越飘。

3.3 IEKF：为什么要在同一个残差上反复重线性化

标准扩展卡尔曼滤波只做一次线性化：在当前估计点算雅可比，解一个线性方程，结束。问题在于点到面的残差对姿态是非线性的，尤其初始位姿和真值差得比较远时，一次线性化得到的梯度和真实曲面差很多，更新后的位姿可能反而更差。

FAST-LIO 用的是迭代扩展卡尔曼（IEKF），做法是把“算残差、算雅可比、解方程、更新状态”这几步循环若干次，每次都在新的估计点上重新线性化。

for (int i = 0; i < NUM_MAX_ITERATIONS; ++i) { // 1. 用当前迭代的位姿，把这一帧点重新投影到世界系 // 2. 在 ikd-tree 里找近邻，拟合平面，算点到面残差和雅可比 H // 3. 解增益，得到切空间上的增量 dx Vec23d dx = K * residual; state = state.boxplus(dx); // 流形上的加法，注意不是直接相加 if (dx.norm() < 1e-3) break; // 增量足够小就提前退出，省算力 }

循环退出有两个条件：增量范数小于阈值（说明已经收敛到线性化点附近），或者达到最大迭代次数。阈值设太小会让迭代次数暴增，设太大又失去了迭代的意义。工程上常见的经验是，收敛时通常两到三次迭代就够，退化场景会打满上限。

迭代带来的收益不只是精度。因为每次都用最新的位姿去投影点、重新找近邻，残差本身也在变——那些第一次投影时找错了近邻的点，后续迭代有机会纠正。这也是为什么 FAST-LIO 敢不做特征提取：只要点足够多、迭代足够充分，原始点的点到面约束和“面点”提取后的约束在信息量上是接近的，而提取特征那一步的时间和阈值调参全被省掉了。

4. ikd-tree：为什么地图不能每帧重建一棵 KD 树

4.1 静态 KD 树在 SLAM 里的三个致命操作

标准 KD 树是为静态数据设计的：一次建好，多次查询。把它塞进 SLAM 会立刻撞上三个操作。

第一是增。每帧都有几百上千个新点要进地图，静态 KD 树的插入只能沿着已有的划分往下走，走到叶子发现满了就分裂，分裂会破坏平衡，最坏情况退化成链表。第二是删。地图不能无限大，远处的点必须清掉，而标准 KD 树没有原地删除的接口，要么标记后定期重建，要么整体推倒重来。第三是降采样。点云密度不均匀是常态，近处密集、远处稀疏，如果没有按空间体积的降采样，地图会越来越稠，近邻搜索越来越慢。

最粗暴的方案是每帧把地图清空重建一棵树。复杂度是 O(n log n)，n 在跑几分钟后就是几十万点，这个开销直接吃掉实时性。

4.2 懒删除、子树下放与后台重建

增量 KD 树（ikd-tree）的核心思路，是把这三个操作都变成“摊还代价”。它维护一个按空间划分的盒子（box）结构，每个盒子记录自己范围内的点数，同时维护两个计数器：树里的总点数、被标记删除的点数。

插入新点时，沿着盒子的划分往下走，如果某个盒子里的点数超过阈值，就沿着最长的那一维从中间劈开，把子节点重新分配。这一步是典型的子树下放（push down）：老的点不会删掉重建，只是被挪到下一层的子盒子里。

删除不做物理删除，只打标记。

// 删除一个空间范围内的点，只做惰性标记，不真的从树里摘掉 void KD_TREE::Delete_Point_Boxes(const BoxPointType &range) { for (auto &box : range) { if (inside(box, tree_range)) { // 只登记“这个区域要删”，真正的清理留给重建 points_deleted_.push_back(box); tree_size_ -= count_in_box(box); } } // 删除比例超过约 20%~30% 才触发后台重建线程 if (need_rebuild() && allow_rebuild) { rebuild_thread_ = std::thread(&KD_TREE::Rebuild, this); } }

重建触发条件是这个设计里最值得看的参数。阈值设太低，重建太频繁，CPU 白烧；设太高，树里全是已被标记删除、却在近邻搜索时被跳过的无效点，查询退化。常见的实现把删除比例压在 20% 到 30% 之间，重建在独立线程里做，主线程继续用老树应答查询，做完再原子地把指针换过去。这就是为什么你会在代码里看到两个指针、一个标志位——读写分离是为了不阻塞前端。

还有一节是并行重建时的“子树复用”：如果某棵子树在这次重建里完全没有被改动，直接把它挂到新树上，不重新插入。地图大部分区域在相邻几帧之间是不变的，这个复用能省掉相当一部分开销。

4.3 五个最近邻拟合一个平面：点到面残差怎么来的

ikd-tree 的查询接口不算复杂，真正决定精度的是拿到近邻之后干什么。FAST-LIO 对每个当前帧的点，在地图里找最近的 5 个点，用它们拟合一个局部平面：

// 用最近邻拟合平面 ax+by+cz+d=0，并给出拟合质量 bool esti_plane(Vec4 &pabcd, const PointVector &points, float thresh) { Vec3 mean = Vec3::Zero(); for (auto &p : points) mean += p.get_vec3f(); mean /= points.size(); Mat3 cov = Mat3::Zero(); for (auto &p : points) { Vec3 d = p.get_vec3f() - mean; cov += d * d.transpose(); // 3x3 协方差 } SelfAdjointEigenSolver<Mat3> es(cov); Vec3 normal = es.eigenvectors().col(0); // 最小特征值对应的就是法向量 pabcd = Vec4(normal.x(), normal.y(), normal.z(), -normal.dot(mean)); // 每个近邻到平面的距离都要小于 thresh，否则这块不是平面，直接丢弃 for (auto &p : points) if (fabs(normal.dot(p.get_vec3f()) + pabcd[3]) > thresh) return false; return true; }

为什么是 5 个点而不是 3 个？3 个点唯一确定一个平面，但完全没有冗余，任何一个点的测量噪声都会直接变成法向量的误差。5 个点做最小二乘，多出来的两个自由度可以用来检验“这些点到底是不是在一个平面上”。那个thresh参数就是这道检验的门槛，代码里通常取 0.1 米量级，太大会把墙角、树叶这类非平面结构也当平面用，太小会导致大量点因为噪声被丢弃、约束不足。

残差是点到平面的有符号距离，雅可比则要一路链式传到 23 维状态上。对位置是法向量本身，对姿态是n^T · (-R·p^)，对外参旋转和平移各自还有一项。这串雅可比写错任何一项，表现都不是“精度差一点”，而是滤波器直接不收敛。

5. 排查：那些让 fast-lio 跑不出结果的坑

下面这几条都是现象明确、原因也明确，但从报错信息完全看不出来的问题。排查顺序建议按出现频率排，先查外参和时间，再查初始化，最后才怀疑算法。

5.1 外参与时间偏移：地图“炸开”的头号原因

现象：启动几秒内点云就糊成一团，或者地图沿某个方向被拉长成条状，速度越快越明显。

原因：雷达到 IMU 的旋转外参或平移外参填错，也可能是time_offset_lidar_to_imu的正负号反了。外参错一点，去畸变时每个点都被变换到错误的位置，残差的方向系统性偏斜，滤波器会把这些偏斜当成真实运动去拟合，越跑越偏。时间偏移相反时，静止状态下一切正常，一运动就开始飘——这个特征很好辨认。

解决：先把机器人静止放置，跑一遍纯建图看墙壁是否重影；再让机器人绕自身轴慢速旋转，观察地图是否跟着旋转；最后做直线往返，看往返两条轨迹是否重合。三步都过了再上速度。外参不要靠肉眼估，宁可用标定工具重新跑一遍。

5.2 静止初始化不静止：重力方向错了，后面全错

现象：启动后前几秒还行，之后姿态慢慢倾斜，地图整体呈斜面；或者滤波器协方差在某些方向上异常大。

原因：程序开头会用前若干帧 IMU 数据估计初始零偏和重力方向，取平均。如果这几帧里机器人被碰了一下、或者放在有震动的台面上，平均出来的重力方向就带偏差。重力在状态里只占两维，但它是加速度积分的基准，方向偏一点，速度就会持续累加误差。

解决：确保启动时真静止，最好放在稳固的台面上。可以在初始化阶段加一个方差检查，如果这批 IMU 数据的标准差超过阈值就重新采样。另外核对一下加速度计的读数模长是否接近 9.81，明显偏离说明标定有问题。

5.3 ikd-tree 频繁重建：单帧耗时突然跳变

现象：跑了几分钟后，某一帧耗时从 20 毫秒突然跳到 100 毫秒以上，随后又恢复正常，周期性出现。

原因：地图点数增长后，删除比例很快触及重建阈值，后台重建线程和主线程抢 CPU；或者filter_size_map设得太小，地图降采样过狠，近邻点不够，平面拟合频繁失败，滤波器反复重试。也有可能是建图范围cube_side_length设得太小，机器人一走出盒子，旧点被大批删除，触发重建。

解决：先看日志里地图点数的增长曲线，如果呈锯齿状就说明重建过频，适当调大降采样体素或建图立方体边长。多核机器上确认重建线程没有和主线程绑在同一个核。单帧耗时跳变如果伴随精度下降，还要怀疑是不是近邻不足导致约束变少。

5.4 Eigen 对齐崩溃：编译过了，运行时挂

现象：程序在某个函数入口直接崩，调用栈里没有任何业务逻辑，或者报“总线错误”；有时换个编译选项就好了，过两天又复现。

原因：含 Eigen 固定尺寸成员的结构体没有加EIGEN_MAKE_ALIGNED_OPERATOR_NEW，或者用std::vector装Eigen::Vector4d这类需要对齐的类型却没有指定分配器。对齐是编译期决定的，编译器不会在这件事上帮你兜底。

解决：所有含 Eigen 固定尺寸成员的类都补上那个宏；容器统一用Eigen::aligned_allocator；如果用了 C++17，确认编译器对过对齐类型的支持没问题。这一条属于典型的血泪经验，不崩则已，一崩就极难定位。

5.5 长走廊漂移：几何退化不是 bug

现象：在长直走廊、隧道、大平面广场里跑，横向误差肉眼可见地增大，回到有结构的地方又慢慢收敛。

原因：点到面残差只能约束法线方向。走廊两侧墙面平行，法向量都指向同一个方向，沿走廊方向没有任何几何约束，那个方向上的状态实际上是不可观的。滤波器不会报错，它只是在那条方向上靠 IMU 硬撑，时间一长必然漂。

解决：这不是调参能解决的，属于场景本身的退化。可行的做法是降速、增加转弯激励、引入其他传感器在退化方向上补约束，或者在后端加回环。识别退化本身也有价值：可以对 H 矩阵做特征值分解，最小特征值对应的方向就是当前约束最弱的方向，用它来动态调整过程噪声。

6. 设计取舍：23 维全状态滤波、无特征前端、增量地图，代价记在谁头上

这三个设计选择单独看都不新鲜，放在一起才形成 FAST-LIO 的性格，而每一种选择都把代价转移到了别处。

最值得琢磨的是第一行。把外参放进状态，本质上是拿“可观性”换“免标定”。这在论文里只是一句话，在工程上却是个分水岭：如果机器人的运动足够丰富，外参会慢慢收敛到真值附近；如果场景里全是直线和匀速，外参那条方向上的协方差会一直涨，滤波器对这些维度的信任度越来越低，最后等于没估。理解这一点之后，调参的思路会变——不是去压协方差，而是去保证运动激励足够。

第二行的取舍更微妙。去掉特征提取，省掉的不只是几毫秒，更是“特征阈值”这一整类需要按场景重调的参数。代价是每帧要处理上万个原始点，近邻搜索的次数成倍增加——这也解释了为什么必须有 ikd-tree，两件事是绑定的，拆开任何一件都不成立。

验证这套设计是否真的在工作，我一般看三个量：迭代次数是否稳定在两三次、地图点数增长是否平滑、H 矩阵最小特征值是否在合理范围。这三个量里任何一个异常，都比看轨迹误差更早暴露问题。从那以后我每次跑一个新的数据集，都会先把这三个量打印出来看一遍，再谈精度——毕竟误差曲线好看但内部已经退化的滤波器，换个场景就会翻车。希望帮到你。

本文还有配套的精品资源，点击获取