从具体例子读懂这条方法链

假设机器人扫描到前方一面墙。它需要回答：“按我现在猜的位置，把激光点放进地图，它们是不是贴在墙上？”FAST-LIO2 每帧围绕这个问题修正位姿。输入是带时间信息的点云、IMU 测量与已有局部地图；输出是机器人状态和增量地图，没有“正确导航动作”的训练标签。

一个点怎样变成定位约束

激光测到的点p L p_LpL​先通过外参换到 IMU 坐标，再通过机器人位姿换到地图坐标。把合并后的变换简写为p W = R p L + t p_W=Rp_L+tpW​=RpL​+t：R RR表示朝向，t tt表示位置，W WW表示地图坐标。真实实现里外参和位姿分别处理。

在地图中找这个点附近的几个点，拟合一小块平面，写成：

n T p + d = 0 , r = n T p W + d , ∥ n ∥ = 1. n^T p+d=0,\qquad r=n^T p_W+d,\qquad \|n\|=1.nTp+d=0,r=nTpW​+d,∥n∥=1.

n nn是垂直于平面的单位方向，d dd是平面常数；r rr是有正负号的距离，单位为米。残差就是当前预测与几何要求的差。

教学例子：墙是x = 5 x=5x=5m，取n = ( 1 , 0 , 0 ) n=(1,0,0)n=(1,0,0)、d = − 5 d=-5d=−5；当前估计把点放到了p W = ( 5.12 , 2 , 1 ) p_W=(5.12,2,1)pW​=(5.12,2,1)，于是r = 0.12 r=0.12r=0.12m。滤波器会联合很多点和 IMU 先验修正位姿，争取让这些距离整体更小。不能只看一个点，就武断地把机器人沿 x 轴移动 12 cm；朝向误差、外参误差也可能导致类似现象。

直接法是跳过专门的边缘/平面特征提取，让采样后的点直接参与地图匹配；它仍然需要去畸变、降采样、近邻查询和有效平面判断。ikd-Tree像一个可以边使用边增删的空间通讯录：按位置快速找到附近地图点，不负责替滤波器计算机器人动作。

如果把同一墙上离散的点拟合成平面后发现残差小，说明这部分观测与地图一致；不代表所有运动方向都被充分约束。与 FAST-LIO 的区别在于匹配点的使用及地图维护方式；它仍需要 IMU 与迭代估计。

原文依据：FAST-LIO2 的点到地图测量模型和增量地图章节。以上墙坐标是教学数值，不是论文实验数据。

一句话定位
在 FAST-LIO 的紧耦合迭代滤波上做「直接法」：去畸变、采样后的点直接配地图，并用 ikd-Tree 增量维护地图，更快更准、更适配多种雷达扫描模式。

本页概念卡

• FAST-LIO2 · FAST-LIO
• ikd-Tree · 点到平面残差
• IESKF · 运动去畸变 · 紧耦合

1. 背景概念（给新人）

• 直接法（这里的含义）：不先抽边缘/平面特征，把采样点直接拿去和地图比（类比视觉直接法「不提点、用像素」的思路，但是几何距离）。
• ikd-Tree：可增量插删的 k-d 树，专为实时建图查询。
• 点到平面：点到局部平面距离作残差（见概念卡）。

2. 论文在干什么

• 问题：特征提取对环境与扫描模式敏感；小 FoV / 弱结构时特征不够；地图结构更新贵。
• 贡献两条主线：① 直接点到地图；② ikd-Tree 增量地图。
• 设定：多种公开序列 + 固态雷达实机；高转速、杂乱室内等。

3. 核心创新点

1. 取消手工特征提取：原始点进匹配，弱结构也能吃到「细微几何」。
2. ikd-Tree：点级插入、轴对齐箱删、动态再平衡；支持树上降采样。
3. 沿用并强化FAST-LIO 的流形迭代卡尔曼 + 快速增益公式 + IMU 去畸变。
4. 系统级结果：文中报告相对多种 SOTA 更高精度、更低算力；室外可到很高里程计频率（量级上百 Hz 口径）。

4.1 状态与传播

• 状态含 IMU 位姿、速度、零偏、重力、雷达到 IMU 外参等（流形表示）。

• 每个 IMU 时刻前向传播；新扫描到达时，把点补偿到扫描结束时刻。

• 对每个有效点：在地图找近邻 → 拟平面 → 点到平面残差。

• 隐式测量模型：真值点应落在平面上；噪声导致有符号距离残差。

• IESKF：反复在当前线性化点算H HH、更新误差状态，收敛后更新P PP。

4.3 建图

• 有效点插入 ikd-Tree；超出局部大立方体的点删除，控制地图规模。

5. 整体框架（Mermaid）

6. 实验与结论

• 多序列对比：精度与耗时优于多种当时 LIO；ikd-Tree 相对常见动态近邻结构更均衡。
• 小 FoV 固态雷达、高角速度场景表现突出。
• 局限：树结构在极端点密度下仍有开销 → Faster-LIO 用体素近似 kNN 再提速。

7. 要点总结

• 「直接法」= 不提特征，不是光度法。

8. 开放问题

• 外参在线估计在论文实验中的开启条件？