KDTree原理介绍
KDTree 之所以快，核心原因是通过空间划分将「暴力遍历」的线性时间复杂度，优化为「分层搜索」的对数时间复杂度，尤其在三维点云近邻搜索场景中优势显著。结合你提供的 FAST-LIO 代码（使用ikd-Tree这个增量式 KDTree 实现），我们可以从原理和工程实现两方面拆解速度优势：
一、 核心原理：空间二分 + 剪枝搜索
KDTree（K-Dimensional Tree）是一种高维空间数据的索引结构，核心思想是「分而治之」，把高维空间划分为多个子空间，搜索时只在目标子空间内查找，避免遍历所有点。
以三维激光点云（x/y/z 三维）为例：
构建阶段：递归划分空间
- 选择一个维度（如 x 轴）作为分割轴，找到该维度的中位数点作为分割节点；
- 以分割节点为界，将所有点划分为「左子空间（x <中位数）」和「右子空间（x> 中位数）」；
- 对左右子空间递归执行上述操作，直到子空间内的点数小于设定阈值（如 10 个点），形成叶子节点。
- 最终得到一棵二叉树，每个节点对应一个子空间，叶子节点存储少量点。
搜索阶段：剪枝减少无效遍历当要找某一点的最近邻时，过程如下：
- 快速定位目标子空间：从根节点出发，根据查询点的维度值（如 x 坐标），逐层向下比较，找到查询点所在的叶子节点；
- 初步找最近邻：计算该叶子节点内所有点与查询点的距离，记录当前最近邻；
- 剪枝回溯：向上回溯到父节点，判断「另一子空间是否可能存在更近的点」（通过比较「查询点到分割面的距离」和「当前最近邻距离」）：
- 如果不可能，直接跳过该子空间（剪枝）；
- 如果可能，进入该子空间重复搜索流程。
- 最终得到全局最近邻。
复杂度对比（关键！）
搜索方式 时间复杂度 适用场景 暴力遍历所有点 O(N) 点数极少（<1000） KDTree 搜索 O(logN) 点数多（>10000） 在 FAST-LIO 中，局部地图点数通常在1 万～10 万级别，KDTree 能将单次近邻搜索时间从毫秒级压缩到微秒级，这也是算法能实时运行的核心保障。
二、 工程优化：ikd-Tree 让增量更新更快
你代码中使用的ikd-Tree是 FAST-LIO 团队优化的增量式 KDTree，相比传统 KDTree 进一步提升了速度，适配激光 SLAM 的「动态增删点」场景：
增量式更新，而非重建传统 KDTree 是「静态」的，新增 / 删除点需要重建整棵树（时间复杂度 O(NlogN)）；
ikd-Tree支持增量增删：新增点时，直接插入到对应的子空间并调整树结构；删除点时，标记节点状态而非立即重构，仅在树失衡时局部调整。在你代码的map_incremental()函数中，调用ikdtree.Add_Points()就是增量插入，时间复杂度接近 O(logN)。结合局部地图的范围约束在
lasermap_fov_segment()函数中，FAST-LIO 会根据当前位姿维护一个局部立方体空间（参数cube_len控制大小），只保留立方体范围内的点，超出范围的点通过ikdtree.Delete_Point_Boxes()批量删除。这种「局部地图 + 增量 KDTree」的组合，既减少了 KDTree 中的总点数，又避免了全局地图的冗余计算，进一步提升速度。并行化搜索（可选）代码中使用了
#pragma omp parallel for指令，支持多线程并行搜索多个点的近邻（如h_share_model函数中对所有特征点的近邻搜索），充分利用多核 CPU 算力。
三、 为什么在 FAST-LIO 中快得「恰到好处」
FAST-LIO 是激光 - 惯性里程计，对实时性要求极高（通常需要 10Hz 以上输出）。如果不用 KDTree：
- 假设局部地图有 5 万个点，当前帧有 1 万个特征点，暴力搜索的总时间为 10000×50000=5×108 次距离计算，耗时秒级，完全无法实时；
- 用 KDTree 后，总时间为 10000×log2(50000)≈10000×16=1.6×105 次计算，耗时毫秒级，满足实时性要求。
简单来说：KDTree 把「大海捞针」变成了「按图索骥」，而 ikd-Tree 则让「更新地图」和「捞针」一样高效。