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

0. 简介

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

1. 一些初始化步骤

1.1 初始化代码

// 初始化kd-tree，并设置删除因子、平衡因子和包围盒长度
template <typename PointType>
void KD_TREE<PointType>::InitializeKDTree(float delete_param, float balance_param, float box_length){
    // 调用设置函数，设置删除因子、平衡因子和包围盒长度
    Set_delete_criterion_param(delete_param);
    Set_balance_criterion_param(balance_param);
    set_downsample_param(box_length);
}

// 初始化节点的信息
template <typename PointType>
void KD_TREE<PointType>::InitTreeNode(KD_TREE_NODE * root){
    // 初始化节点的坐标和包围盒范围 
    root->point.x = 0.0f;
    root->point.y = 0.0f;
    root->point.z = 0.0f;       
    root->node_range_x[0] = 0.0f;
    root->node_range_x[1] = 0.0f;
    root->node_range_y[0] = 0.0f;
    root->node_range_y[1] = 0.0f;    
    root->node_range_z[0] = 0.0f;
    root->node_range_z[1] = 0.0f;   
    // 初始化节点的分割轴、父节点和左右子节点指针
    root->division_axis = 0;
    root->father_ptr = nullptr;
    root->left_son_ptr = nullptr;
    root->right_son_ptr = nullptr;
    // 初始化节点的大小、无效点数、下采样删除点数和删除标志
    root->TreeSize = 0;
    root->invalid_point_num = 0;
    root->down_del_num = 0;
    root->point_deleted = false;
    root->tree_deleted = false;
    // 初始化节点的推入标志、下采样删除点标志和工作标志，并创建互斥锁
    root->need_push_down_to_left = false;
    root->need_push_down_to_right = false;
    root->point_downsample_deleted = false;
    root->working_flag = false;
    pthread_mutex_init(&(root->push_down_mutex_lock),NULL);
}   

1.2 ikd-Tree尺寸

// 返回kd-tree的大小
template <typename PointType>
int KD_TREE<PointType>::size(){
    int s = 0;
    // 如果重建指针为空或者指向的节点不是根节点
    if (Rebuild_Ptr == nullptr || *Rebuild_Ptr != Root_Node){
        // 直接返回根节点的大小
        if (Root_Node != nullptr) {
            return Root_Node->TreeSize;
        } else {
            return 0;
        }
    } else {
        // 如果正在重建，获取重建时的节点数
        if (!pthread_mutex_trylock(&working_flag_mutex)){
            s = Root_Node->TreeSize;
            pthread_mutex_unlock(&working_flag_mutex);
            return s;
        } else {
            return Treesize_tmp;
        }
    }
}

1.3 包围盒范围

// 返回kd-tree的包围盒范围
template <typename PointType>
BoxPointType KD_TREE<PointType>::tree_range(){
    BoxPointType range;// 如果重建指针为空或者指向的节点不是根节点
    if (Rebuild_Ptr == nullptr || *Rebuild_Ptr != Root_Node){
        if (Root_Node != nullptr) {
            // 直接返回根节点的包围盒范围 
            range.vertex_min[0] = Root_Node->node_range_x[0];
            range.vertex_min[1] = Root_Node->node_range_y[0];
            range.vertex_min[2] = Root_Node->node_range_z[0];
            range.vertex_max[0] = Root_Node->node_range_x[1];
            range.vertex_max[1] = Root_Node->node_range_y[1];
            range.vertex_max[2] = Root_Node->node_range_z[1];
        } else {
            memset(&range, 0, sizeof(range));
        }
    } else {
         // 如果正在重建，获取重建时的包围盒范围 
        if (!pthread_mutex_trylock(&working_flag_mutex)){
            range.vertex_min[0] = Root_Node->node_range_x[0];
            range.vertex_min[1] = Root_Node->node_range_y[0];
            range.vertex_min[2] = Root_Node->node_range_z[0];
            range.vertex_max[0] = Root_Node->node_range_x[1];
            range.vertex_max[1] = Root_Node->node_range_y[1];
            range.vertex_max[2] = Root_Node->node_range_z[1];
            pthread_mutex_unlock(&working_flag_mutex);
        } else {
            memset(&range, 0, sizeof(range));
        }
    }
    return range;
}

1.4 线程创建

// 启动kd-tree的多线程 
template <typename PointType>
void KD_TREE<PointType>::start_thread(){
    pthread_mutex_init(&termination_flag_mutex_lock, NULL);// 终止标志互斥锁
    pthread_mutex_init(&rebuild_ptr_mutex_lock, NULL);// 重建指针互斥锁 
    pthread_mutex_init(&rebuild_logger_mutex_lock, NULL);// 重建日志互斥锁
    pthread_mutex_init(&points_deleted_rebuild_mutex_lock, NULL); // 删除点重建互斥锁
    pthread_mutex_init(&working_flag_mutex, NULL);// 工作标志互斥锁
    pthread_mutex_init(&search_flag_mutex, NULL);// 搜索标志互斥锁
    pthread_create(&rebuild_thread, NULL, multi_thread_ptr, (void*) this);// 创建重建线程
    printf("Multi thread started \n");// 打印启动消息
}

2. 构建增量K-D树

建增量K-D树与构建静态K-D树类似，只是为增量更新维护额外信息，整个算法如算法1所示：

给定一个点阵列V，首先按协方差最大的分割轴对点进行排序（第4-5行），然后中值点保存到新树节点T的点（第6-7行），中位数下方和上方的点分别传递给T的左和右子节点，用于递归构建（第9-10行），第11-12行中的LazyLabelInit和Pullup更新了增量更新所需的所有属性。

// 初始化构建ikd-Tree；该函数也用作彻底重建Tree结构
template <typename PointType>
void KD_TREE<PointType>::Build(PointVector point_cloud){
    // // 如果已有tree结构，彻底清空
    if (Root_Node != nullptr){
        delete_tree_nodes(&Root_Node);
    }
    // 如果输入点云为空，直接返回
    if (point_cloud.size() == 0) return;
    STATIC_ROOT_NODE = new KD_TREE_NODE; // // 创建内存依赖上的根节点STATIC_ROOT_NODE，并初始化为叶节点 
    InitTreeNode(STATIC_ROOT_NODE); 
    // 递归构建kd-Tree
    BuildTree(&STATIC_ROOT_NODE->left_son_ptr, 0, point_cloud.size()-1, point_cloud);
    // 更新STATIC_ROOT_NODE的统计信息
    Update(STATIC_ROOT_NODE);
    STATIC_ROOT_NODE->TreeSize = 0;
    Root_Node = STATIC_ROOT_NODE->left_son_ptr; //  // 将逻辑上的根节点指向STATIC_ROOT_NODE的左子节点
}

// 构建kd-tree，参数为根节点指针、数据点集合的左右边界和点集合对象；这里的双指针形参很重要，能够允许我们传入一个空指针。
template <typename PointType>
void KD_TREE<PointType>::BuildTree(KD_TREE_NODE ** root, int l, int r, PointVector & Storage){
    if (l>r) return;// 递归终止条件，区间为空
    *root = new KD_TREE_NODE; // 分配新的节点空间
    InitTreeNode(*root);// 初始化节点
    int mid = (l+r)>>1; // 取区间中点
    int div_axis = 0; // 分割轴，初始设为x轴
    int i;// 找到分割轴，即最大值减最小值之差最大的轴
    // Find the best division Axis (wgh 也即分布最分散的那个轴，或者说最大值减最小值之差最大的那个轴)
    float min_value[3] = {INFINITY, INFINITY, INFINITY};// 每个轴的最小值
    float max_value[3] = {-INFINITY, -INFINITY, -INFINITY};// 每个轴的最大值
    float dim_range[3] = {0,0,0};// 每个轴的取值范围
    for (i=l;i<=r;i++){
        min_value[0] = min(min_value[0], Storage[i].x);
        min_value[1] = min(min_value[1], Storage[i].y);
        min_value[2] = min(min_value[2], Storage[i].z);
        max_value[0] = max(max_value[0], Storage[i].x);
        max_value[1] = max(max_value[1], Storage[i].y);
        max_value[2] = max(max_value[2], Storage[i].z);
    }
    // 按照最长的轴作为分割轴 
    for (i=0;i<3;i++) dim_range[i] = max_value[i] - min_value[i];
    for (i=1;i<3;i++) if (dim_range[i] > dim_range[div_axis]) div_axis = i;// 更新节点的分割轴信息
    // 按照分割轴的值对点集合进行排序
    (*root)->division_axis = div_axis;
    //按照主轴方向排序，排序结果放在Storage变量中。
    switch (div_axis)
    {
    case 0:
        // wgh 用C++算法库的函数进行排序，只需确保在mid位置的数大于左侧，且小于右侧即可，不必严格完全排序。
        nth_element(begin(Storage)+l, begin(Storage)+mid, begin(Storage)+r+1, point_cmp_x);
        break;
    case 1:
        nth_element(begin(Storage)+l, begin(Storage)+mid, begin(Storage)+r+1, point_cmp_y);
        break;
    case 2:
        nth_element(begin(Storage)+l, begin(Storage)+mid, begin(Storage)+r+1, point_cmp_z);
        break;
    default:
        nth_element(begin(Storage)+l, begin(Storage)+mid, begin(Storage)+r+1, point_cmp_x);
        break;
    }  
    (*root)->point = Storage[mid]; // 更新节点的数据点信息
    KD_TREE_NODE * left_son = nullptr, * right_son = nullptr; 
    // 递归构建整个tree（自上而下）。
    BuildTree(&left_son, l, mid-1, Storage);
    BuildTree(&right_son, mid+1, r, Storage);  
    (*root)->left_son_ptr = left_son;// 更新节点的左子树指针
    (*root)->right_son_ptr = right_son;
    // 更新根节点信息。
    Update((*root));  
    return;
}

3. ikd-Tree的最近邻搜索

…详情请参照古月居