ikd-Tree：FAST-LIO2中的增量式地图管理结构

在激光雷达惯性里程计（LiDAR-Inertial Odometry）系统中，地图管理一直是一个棘手的问题。激光雷达以每秒数十万点的速度输出数据，系统需要将这些点组织起来，以便快速查找每个点在地图中的最近邻点，用于计算残差并更新状态估计。传统方案是用静态kd-tree：构建一次，查询多次，效果不错。但SLAM是一个持续更新的过程，机器人不断运动，新点需要插入，旧点需要移除。如果每次更新都重建整棵树，随着地图规模的增长，计算开销会变得难以承受。

香港大学MaRS实验室的Yixi Cai、Wei Xu和Fu Zhang在FAST-LIO2中提出了一种名为ikd-Tree（Incremental k-d Tree）的数据结构，专门应对这一挑战。这套数据结构支持增量式更新（插入和删除）、惰性删除、动态重平衡，并且可以在多线程环境下并行执行重建，不影响主线程的查询服务。代码已开源在GitHub仓库hku-mars/ikd-Tree。

从静态到动态

传统kd-tree是为静态数据集设计的。构建算法通常选择方差最大的维度作为划分轴，取中位数作为根节点，递归划分左右子树，最终形成一棵平衡二叉树。搜索复杂度为O(log N)，性能很好。

问题在于：SLAM的地图是动态的。机器人每前进一步，新的激光点需要加入地图，视野外的点需要移除。在静态kd-tree上做增量更新效率很低——直接插入新点会破坏平衡，搜索性能会随着树变得歪斜而下降；如果每次更新都重建整棵树，随着地图中点数增加，计算开销会线性增长，导致系统无法实时运行。

FAST-LIO2的作者正是被这个问题驱动，设计了一套“动”与“静”之间能够兼顾的方案。FAS