0. Introduction
Incremental KD tree, we know that this kind of algorithm can complete the dynamic spatial division of data structures more efficiently and meaningfully. ikd-Tree can actively monitor and rebalance the tree structure to achieve efficient near-point search at a later stage, ikd-Tree is carefully designed to support multi-threaded parallel computation to maximize overall efficiency. Here we will mainly take a look at KennyWGH ikd-Tree and explain some of the author's questions.
1. ikd-Tree的插入与删除
Incremental updates refer to incremental operations followed by dynamic rebalancing. Incremental operations include inserting, deleting, and reinserting new points into the K-D tree, specifically, the insertion operation attaches a new point (i.e., node) to the K-D tree, in which we use a delayed deletion strategy, that is, the point is not immediately removed from the tree, but only marked as "deleted" by setting the property deleted to true. If all nodes on the subtree with T as the root have been deleted, then T's property treedeleted is set to true, so the deleted property and the tree-deleted property are called lazy labels. If a point marked as "deleted" but not deleted is later inserted into the tree, it is called "reinserted" and can be effectively achieved by simply setting the deleted property back to false. Otherwise, points marked as "deleted" will be removed from the tree during the rebuild process, and our incremental updates support two types: point updates, which insert, delete, or reinsert individual points on the tree, and box updates, which insert, delete, or reinsert all points in a given box aligned with the data axes, and box updates that may require deletion or reinsertion of an entire subtree with T as the root. In this case, it is still inefficient to recursively update the deleted and deleted lazy tags of all children of T. To solve this problem, we use a further delay strategy to update the delay label of the child node.
1.1 Inserting a Node
Point-by-point updates on an incremental k-d tree are implemented recursively, similar to k-d trees, for point-by-point insertion, the algorithm searches recursively down from the root node and compares the coordinates of the new point on the split axis with the points stored on the tree node until it finds a leaf node to attach a new tree node, and for the deletion or reinsertion of point P, the algorithm finds the tree node where the point P is stored, and modifies the deleted attributes. The process of inserting a new vertex into the ikd-tree is as follows:
- First, the whole space is voxelized, and it is clear which voxel (target voxel) the new point falls into;
- Then, check the ikd-tree whether there are points in the target voxel and what points there are (refer to box-wise delete for the query process);
- If there is already a point, sort the existing point and the new point together, find the point closest to the voxel center, and then make a judgment: if the nearest point is an existing point, it means that the new point is not necessary to insert it again, and the processing is over; If the nearest point is a new point, all existing points are marked and deleted, and a new point is inserted.
- If a point does not yet exist within the voxel, insert a new point directly.
// KD_TREE 类的 Add_Points 函数,用于将一组点添加到 KD 树中
template <typename PointType>
int KD_TREE<PointType>::Add_Points(PointVector & PointToAdd, bool downsample_on)
{
// 初始化一些变量
// int NewPointSize = PointToAdd.size(); // wgh: unused variable.
// int tree_size = size(); // wgh: unused variable.
BoxPointType Box_of_Point;
PointType downsample_result, mid_point;
bool downsample_switch = downsample_on && DOWNSAMPLE_SWITCH;
float min_dist, tmp_dist;
int tmp_counter = 0;
// 遍历所有点，逐个插入。
for (std::size_t i=0; i<PointToAdd.size();i++){
if (downsample_switch){
// 获得插入点所在的Voxel，计算Voxel的几何中心点（将来只保留最接近中心点的point）
Box_of_Point.vertex_min[0] = floor(PointToAdd[i].x/downsample_size)*downsample_size;
Box_of_Point.vertex_max[0] = Box_of_Point.vertex_min[0]+downsample_size;
Box_of_Point.vertex_min[1] = floor(PointToAdd[i].y/downsample_size)*downsample_size;
Box_of_Point.vertex_max[1] = Box_of_Point.vertex_min[1]+downsample_size;
Box_of_Point.vertex_min[2] = floor(PointToAdd[i].z/downsample_size)*downsample_size;
Box_of_Point.vertex_max[2] = Box_of_Point.vertex_min[2]+downsample_size;
mid_point.x = Box_of_Point.vertex_min[0] + (Box_of_Point.vertex_max[0]-Box_of_Point.vertex_min[0])/2.0;
mid_point.y = Box_of_Point.vertex_min[1] + (Box_of_Point.vertex_max[1]-Box_of_Point.vertex_min[1])/2.0;
mid_point.z = Box_of_Point.vertex_min[2] + (Box_of_Point.vertex_max[2]-Box_of_Point.vertex_min[2])/2.0;
// 在当前 Voxel 中搜索最近的点
PointVector ().swap(Downsample_Storage);
Search_by_range(Root_Node, Box_of_Point, Downsample_Storage);
min_dist = calc_dist(PointToAdd[i],mid_point);
downsample_result = PointToAdd[i];
// 找到距离当前点最近的点
for (std::size_t index = 0; index < Downsample_Storage.size(); index++){
tmp_dist = calc_dist(Downsample_Storage[index], mid_point);
if (tmp_dist < min_dist){
min_dist = tmp_dist;
downsample_result = Downsample_Storage[index];//降采样
}
}
// 如果当前没有re-balancing任务，也即没有并行线程，则直接执行`BoxDelete`和`插入一个点`。
if (Rebuild_Ptr == nullptr || *Rebuild_Ptr != Root_Node){
if (Downsample_Storage.size() > 1 || same_point(PointToAdd[i], downsample_result)){
if (Downsample_Storage.size() > 0) Delete_by_range(&Root_Node, Box_of_Point, true, true);
Add_by_point(&Root_Node, downsample_result, true, Root_Node->division_axis);
tmp_counter ++;
}
// 如果有re-balancing任务在并行运行，在对当前tree执行`BoxDelete`和`插入一个点`之外，还需要把这些操作缓存到logger里。
} else {
if (Downsample_Storage.size() > 1 || same_point(PointToAdd[i], downsample_result)){
Operation_Logger_Type operation_delete, operation;
operation_delete.boxpoint = Box_of_Point;
operation_delete.op = DOWNSAMPLE_DELETE;
operation.point = downsample_result;
operation.op = ADD_POINT;
pthread_mutex_lock(&working_flag_mutex);
if (Downsample_Storage.size() > 0) Delete_by_range(&Root_Node, Box_of_Point, false , true);
Add_by_point(&Root_Node, downsample_result, false, Root_Node->division_axis);
tmp_counter ++;
if (rebuild_flag){
pthread_mutex_lock(&rebuild_logger_mutex_lock);
if (Downsample_Storage.size() > 0) Rebuild_Logger.push(operation_delete);
Rebuild_Logger.push(operation);
pthread_mutex_unlock(&rebuild_logger_mutex_lock);
}
pthread_mutex_unlock(&working_flag_mutex);
};
}
}
else {
//如果不需要降采样，且无并行re-balancing任务，直接插入点。
if (Rebuild_Ptr == nullptr || *Rebuild_Ptr != Root_Node){
Add_by_point(&Root_Node, PointToAdd[i], true, Root_Node->division_axis);
}
// 如果有并行re-balancing任务，还需额外把当前操作放入logger缓存。
else {
Operation_Logger_Type operation;
operation.point = PointToAdd[i];
operation.op = ADD_POINT;
pthread_mutex_lock(&working_flag_mutex);
Add_by_point(&Root_Node, PointToAdd[i], false, Root_Node->division_axis);
if (rebuild_flag){
pthread_mutex_lock(&rebuild_logger_mutex_lock);
Rebuild_Logger.push(operation);
pthread_mutex_unlock(&rebuild_logger_mutex_lock);
}
pthread_mutex_unlock(&working_flag_mutex);
}
}
}
return tmp_counter;
}
// 工具属性，单纯地往tree结构中插入新节点。
template <typename PointType>
void KD_TREE<PointType>::Add_by_point(KD_TREE_NODE ** root, PointType point, bool allow_rebuild, int father_axis)
{
// 如果已经到达叶子节点，直接插入。
if (*root == nullptr){
*root = new KD_TREE_NODE;
InitTreeNode(*root);
(*root)->point = point;
(*root)->division_axis = (father_axis + 1) % 3;
Update(*root);
return;
}
// 标记当前节点为“工作中”，并将操作记录到Logger中
(*root)->working_flag = true;
Operation_Logger_Type add_log;
// struct timespec Timeout; // wgh: unused variable.
add_log.op = ADD_POINT;
add_log.point = point;
Push_Down(*root);
// 递归插入左子树
if (((*root)->division_axis == 0 && point.x < (*root)->point.x) ||
((*root)->division_axis == 1 && point.y < (*root)->point.y) ||
((*root)->division_axis == 2 && point.z < (*root)->point.z) )
{
if ((Rebuild_Ptr == nullptr) || (*root)->left_son_ptr != *Rebuild_Ptr){
Add_by_point(&(*root)->left_son_ptr, point, allow_rebuild, (*root)->division_axis);
}
else {
// 如果当前有re-balancing任务在并行运行，则将操作记录到Logger中
pthread_mutex_lock(&working_flag_mutex);
Add_by_point(&(*root)->left_son_ptr, point, false,(*root)->division_axis);
if (rebuild_flag){
pthread_mutex_lock(&rebuild_logger_mutex_lock);
Rebuild_Logger.push(add_log);
pthread_mutex_unlock(&rebuild_logger_mutex_lock);
}
pthread_mutex_unlock(&working_flag_mutex);
}
}
//递归插入右子树。
else {
// 如果当前有re-balancing任务在并行运行，则将操作记录到Logger中
if ((Rebuild_Ptr == nullptr) || (*root)->right_son_ptr != *Rebuild_Ptr){
Add_by_point(&(*root)->right_son_ptr, point, allow_rebuild,(*root)->division_axis);
}
else {
pthread_mutex_lock(&working_flag_mutex);
Add_by_point(&(*root)->right_son_ptr, point, false,(*root)->division_axis);
if (rebuild_flag){
pthread_mutex_lock(&rebuild_logger_mutex_lock);
Rebuild_Logger.push(add_log);
pthread_mutex_unlock(&rebuild_logger_mutex_lock);
}
pthread_mutex_unlock(&working_flag_mutex);
}
}
// 更新当前节点信息（自下而上），并检查是否需要re-balancing。
Update(*root);
if (Rebuild_Ptr != nullptr &&
*Rebuild_Ptr == *root &&
(*root)->TreeSize < Multi_Thread_Rebuild_Point_Num) Rebuild_Ptr = nullptr;
bool need_rebuild = allow_rebuild & Criterion_Check((*root));
if (need_rebuild) Rebuild(root);
// 将当前节点的“工作中”标志位设为false
if ((*root) != nullptr) (*root)->working_flag = false;
return;
} 1.2 Insert Box Node
Box insertion is achieved by inserting new points one by one in the incremental k-d tree, and other box updates (box deletion and reinsertion) are implemented using range information in the property range, which forms a box CT, and uses lazy labels on the tree nodes. The pseudocode is shown in algorithm 2.
Click to understand IKD-TREE in depth, starting with reading the code (3) - Gu Yueju to view the full article