深入理解ikd-Tree从阅读代码开始(三)

0. 简介

增量KD树,我们知道这类算法可以更加高效并且有意义地完成数据结构的动态空间划分. ikd-Tree可以主动监视树结构并重新平衡树结构，从而在后期实现高效的最近点搜索，ikd-Tree经过了精心设计，支持多线程并行计算，以最大限度地提高整体效率.mars实验室已经将这个代码公开了,而且有很多人对代码进行了总结与阐述.这里我们主要来看一下KennyWGH ikd-Tree,并对作者的一些疑问进行解释.

1. ikd-Tree的插入与删除

增量更新指的是增量操作，随后进行动态重新平衡。增量操作包括向k-D树插入、删除和重新插入新点，具体而言，插入操作将一个新点（即节点）附加到k-d树，在删除操作中，我们使用了延迟删除策略，也就是说，不会立即从树中删除点，而只会通过将属性deleted设置为true来标记为“deleted”（已删除）。如果已删除以T为根的子树上的所有节点，则T的属性treedeleted设置为true，因此，删除的属性和树删除的属性称为惰性标签。如果标记为“已删除”但未删除的点稍后插入到树中，则称为“重新插入”，只需将删除的属性设置回false即可有效地实现。否则，标记为“已删除”的点将在重建过程中从树中删除，我们的增量更新支持两种类型：点式更新和框式更新，逐点更新在树上插入、删除或重新插入单个点，而逐框更新在与数据坐标轴对齐的给定框中插入、删除或重新插入所有点，框式更新可能需要删除或重新插入以T为根的整个子树。在这种情况下，递归更新T的所有子节点的已删除和已删除的懒惰标签仍然是低效的。为了解决这个问题，我们使用了进一步的延迟策略来更新子节点的延迟标签。

1.1 插入节点

增量k-d树上的逐点更新以递归方式实现，类似于k-d树，对于逐点插入，该算法从根节点递归向下搜索，并将新点在分割轴上的坐标与存储在树节点上的点进行比较，直到找到一个叶节点来附加新的树节点，对于点P的删除或重新插入，该算法会找到存储点P的树节点，并修改删除的属性。ikd-Tree插入新点的流程如下:

1. 首先将整个空间体素化，并明确新点落入哪个体素（目标体素）；

2. 然后向ikd-Tree查询目标体素内是否已经有点以及有哪些点（查询过程参考box-wise delete）；

3. 如果已经有点了，将已有点和新点一起排序，找出离体素中心最近的那个点，然后做判断：如果最近的点是已有点，意味着新点无必要再插入了，结束处理；如果最近的点是新点，则把已有点全部标记删除，并插入新点；

4. 如果体素内尚不存在点，则直接插入新点。

   
   

   

// 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 插入框节点

框式插入是通过在增量k-d树中逐个插入新点来实现的，其他框式更新（框式删除和重新插入）是利用属性range中的范围信息实现的，该属性range形成一个框式CT，并在树节点上使用惰性标签。伪代码如算法2所示。

// 这段代码是ikd-tree中的添加点框函数
template <typename PointType>
void KD_TREE<PointType>::Add_Point_Boxes(vector<BoxPointType> & BoxPoints){     
    // 对于要添加的每个点框
    for (std::size_t i=0;i < BoxPoints.size();i++){
        // 如果当前没有re-balancing任务，也即没有并行线程，则直接执行
        if (Rebuild_Ptr == nullptr || *Rebuild_Ptr != Root_Node){
            Add_by_range(&Root_Node ,BoxPoints[i], true);
        } else { // 如果有re-balancing任务在并行运行，在对当前tree执行Add_by_range之外，还需要把这些操作缓存到logger里
            Operation_Logger_Type operation;
            operation.boxpoint = BoxPoints[i];
            operation.op = ADD_BOX;
            pthread_mutex_lock(&working_flag_mutex);
            Add_by_range(&Root_Node ,BoxPoints[i], false);
            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;
}

template <typename PointType>
void KD_TREE<PointType>::Add_by_range(KD_TREE_NODE ** root, BoxPointType boxpoint, bool allow_rebuild){
    // 如果根节点为空，直接返回
    if ((*root) == nullptr) return;
    // 标记当前节点正在处理
    (*root)->working_flag = true;
    // 将当前节点的标记向下传递
    Push_Down(*root);       
    // 如果范围框的 x 坐标的最大值小于当前节点的 x 坐标范围的最小值，或者范围框的 x 坐标的最小值大于当前节点的 x 坐标范围的最大值，则直接返回
    if (boxpoint.vertex_max[0] <= (*root)->node_range_x[0] || boxpoint.vertex_min[0] > (*root)->node_range_x[1]) return;
     // 如果范围框的 y 坐标的最大值小于当前节点的 y 坐标范围的最小值，或者范围框的 y 坐标的最小值大于当前节点的 y 坐标范围的最大值，则直接返回
    if (boxpoint.vertex_max[1] <= (*root)->node_range_y[0] || boxpoint.vertex_min[1] > (*root)->node_range_y[1]) return;
    // 如果范围框的 z 坐标的最大值小于当前节点的 z 坐标范围的最小值，或者范围框的 z 坐标的最小值大于当前节点的 z 坐标范围的最大值，则直接返回
    if (boxpoint.vertex_max[2] <= (*root)->node_range_z[0] || boxpoint.vertex_min[2] > (*root)->node_range_z[1]) return;
    // 如果范围框完全包含当前节点，则将当前节点标记为未删除，将其左右子节点标记为需要向下传递，并更新其无效点数
    if (boxpoint.vertex_min[0] <= (*root)->node_range_x[0] && boxpoint.vertex_max[0] > (*root)->node_range_x[1] && boxpoint.vertex_min[1] <= (*root)->node_range_y[0] && boxpoint.vertex_max[1]> (*root)->node_range_y[1] && boxpoint.vertex_min[2] <= (*root)->node_range_z[0] && boxpoint.vertex_max[2] > (*root)->node_range_z[1]){
        (*root)->tree_deleted = false || (*root)->tree_downsample_deleted;
        (*root)->point_deleted = false || (*root)->point_downsample_deleted;
        (*root)->need_push_down_to_left = true;
        (*root)->need_push_down_to_right = true;
        (*root)->invalid_point_num = (*root)->down_del_num; 
        return;
    }
    // 如果当前节点是范围框的一部分，则将当前节点标记为需要删除
    if (boxpoint.vertex_min[0] <= (*root)->point.x && boxpoint.vertex_max[0] > (*root)->point.x && boxpoint.vertex_min[1] <= (*root)->point.y && boxpoint.vertex_max[1] > (*root)->point.y && boxpoint.vertex_min[2] <= (*root)->point.z && boxpoint.vertex_max[2] > (*root)->point.z){
        (*root)->point_deleted = (*root)->point_downsample_deleted;
    }
     // 创建操作日志，记录添加范围框的操作
    Operation_Logger_Type add_box_log;
    // struct timespec Timeout; // wgh: unused variable.
    add_box_log.op = ADD_BOX;
    add_box_log.boxpoint = boxpoint;
    // 如果重建指针为空，或者当前节点的左子节点不是重建指针，则向左子树递归添加范围框
    if ((Rebuild_Ptr == nullptr) || (*root)->left_son_ptr != *Rebuild_Ptr){
        Add_by_range(&((*root)->left_son_ptr), boxpoint, allow_rebuild);
    } else {
        // 如果当前节点的左子节点是重建指针，需要加锁，避免多线程竞争
        pthread_mutex_lock(&working_flag_mutex);
        // 向左子树递归添加范围框，不允许重建
        Add_by_range(&((*root)->left_son_ptr), boxpoint, false);
        // 如果需要重建，将操作日志加入重建日志队列中
        if (rebuild_flag){
            pthread_mutex_lock(&rebuild_logger_mutex_lock);
            Rebuild_Logger.push(add_box_log);
            pthread_mutex_unlock(&rebuild_logger_mutex_lock);                 
        }        
        pthread_mutex_unlock(&working_flag_mutex);
    }
     // 如果重建指针为空，或者当前节点的右子节点不是重建指针，则向右子树递归添加范围框
    if ((Rebuild_Ptr == nullptr) || (*root)->right_son_ptr != *Rebuild_Ptr){
        Add_by_range(&((*root)->right_son_ptr), boxpoint, allow_rebuild);
    } else {
        // 如果当前节点的右子节点是重建指针，需要加锁，避免多线程竞争
        pthread_mutex_lock(&working_flag_mutex);
        // 向右子树递归添加范围框，不允许重建
        Add_by_range(&((*root)->right_son_ptr), boxpoint, false);
        if (rebuild_flag){
            // 如果需要重建，将操作日志加入重建日志队列中
            pthread_mutex_lock(&rebuild_logger_mutex_lock);
            Rebuild_Logger.push(add_box_log);
            pthread_mutex_unlock(&rebuild_logger_mutex_lock);                 
        }
        pthread_mutex_unlock(&working_flag_mutex);
    }
    // 更新当前节点的信息
    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);
    // 标记当前节点处理结束
    if ((*root) != nullptr) (*root)->working_flag = false;   
    return;
}

…详情请参照古月居