从具体例子读懂这条方法链假设机器人扫描到前方一面墙。它需要回答“按我现在猜的位置把激光点放进地图它们是不是贴在墙上”FAST-LIO2 每帧围绕这个问题修正位姿。输入是带时间信息的点云、IMU 测量与已有局部地图输出是机器人状态和增量地图没有“正确导航动作”的训练标签。一个点怎样变成定位约束激光测到的点p L p_LpL​先通过外参换到 IMU 坐标再通过机器人位姿换到地图坐标。把合并后的变换简写为p W R p L t p_WRp_LtpW​RpL​tR RR表示朝向t tt表示位置W WW表示地图坐标。真实实现里外参和位姿分别处理。在地图中找这个点附近的几个点拟合一小块平面写成n T p d 0 , r n T p W d , ∥ n ∥ 1. n^T pd0,\qquad rn^T p_Wd,\qquad \|n\|1.nTpd0,rnTpW​d,∥n∥1.n nn是垂直于平面的单位方向d dd是平面常数r rr是有正负号的距离单位为米。残差就是当前预测与几何要求的差。教学例子墙是x 5 x5x5m取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 r0.12r0.12m。滤波器会联合很多点和 IMU 先验修正位姿争取让这些距离整体更小。不能只看一个点就武断地把机器人沿 x 轴移动 12 cm朝向误差、外参误差也可能导致类似现象。改了位姿再查一次一帧点云和点时间IMU辅助去畸变用当前位姿把点放到地图坐标ikd-Tree查近邻并拟合局部平面计算点到平面距离迭代滤波综合几何与IMU先验收敛后输出状态增量插点、删远点、维护局部地图直接法是跳过专门的边缘/平面特征提取让采样后的点直接参与地图匹配它仍然需要去畸变、降采样、近邻查询和有效平面判断。ikd-Tree像一个可以边使用边增删的空间通讯录按位置快速找到附近地图点不负责替滤波器计算机器人动作。如果把同一墙上离散的点拟合成平面后发现残差小说明这部分观测与地图一致不代表所有运动方向都被充分约束。与 FAST-LIO 的区别在于匹配点的使用及地图维护方式它仍需要 IMU 与迭代估计。原文依据FAST-LIO2 的点到地图测量模型和增量地图章节。以上墙坐标是教学数值不是论文实验数据。一句话定位在 FAST-LIO 的紧耦合迭代滤波上做「直接法」去畸变、采样后的点直接配地图并用 ikd-Tree 增量维护地图更快更准、更适配多种雷达扫描模式。本页概念卡FAST-LIO2 · FAST-LIOikd-Tree · 点到平面残差IESKF · 运动去畸变 · 紧耦合1. 背景概念给新人直接法这里的含义不先抽边缘/平面特征把采样点直接拿去和地图比类比视觉直接法「不提点、用像素」的思路但是几何距离。ikd-Tree可增量插删的 k-d 树专为实时建图查询。点到平面点到局部平面距离作残差见概念卡。2. 论文在干什么问题特征提取对环境与扫描模式敏感小 FoV / 弱结构时特征不够地图结构更新贵。贡献两条主线① 直接点到地图② ikd-Tree 增量地图。设定多种公开序列 固态雷达实机高转速、杂乱室内等。3. 核心创新点取消手工特征提取原始点进匹配弱结构也能吃到「细微几何」。ikd-Tree点级插入、轴对齐箱删、动态再平衡支持树上降采样。沿用并强化FAST-LIO 的流形迭代卡尔曼 快速增益公式 IMU 去畸变。系统级结果文中报告相对多种 SOTA 更高精度、更低算力室外可到很高里程计频率量级上百 Hz 口径。4.1 状态与传播状态含 IMU 位姿、速度、零偏、重力、雷达到 IMU 外参等流形表示。每个 IMU 时刻前向传播新扫描到达时把点补偿到扫描结束时刻。对每个有效点在地图找近邻 → 拟平面 → 点到平面残差。隐式测量模型真值点应落在平面上噪声导致有符号距离残差。IESKF反复在当前线性化点算H HH、更新误差状态收敛后更新P PP。4.3 建图有效点插入 ikd-Tree超出局部大立方体的点删除控制地图规模。5. 整体框架MermaidIMU前向传播原始扫描点反向去畸变ikd-Tree 近邻拟平面迭代卡尔曼更新状态增量更新 ikd-Treeikd-TreeIESKFIMUikd-TreeIESKFIMU扫描结束时刻状态loop[迭代至收敛]Predict 多次近邻查询局部平面残差H 更新状态插入有效点 / 删远点6. 实验与结论多序列对比精度与耗时优于多种当时 LIOikd-Tree 相对常见动态近邻结构更均衡。小 FoV 固态雷达、高角速度场景表现突出。局限树结构在极端点密度下仍有开销 → Faster-LIO 用体素近似 kNN 再提速。7. 要点总结「直接法」 不提特征不是光度法。8. 开放问题外参在线估计在论文实验中的开启条件