laitimes
return

In-depth understanding of ikd-tree starts with reading the code (2)

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. Some initialization steps

1.1 Initialization Code 

// 初始化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 Bounding Box Scope 

// 返回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 Thread Creation 

// 启动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. Build an incremental K-D tree

Building an incremental K-D tree is similar to building a static K-D tree, except that additional information is maintained for incremental updates, as shown in Algorithm 1:

In-depth understanding of ikd-tree starts with reading the code (2)

Given a point array V, the points are first sorted by the partition axis with the greatest covariance (rows 4-5), then the median points are saved to the points of the new tree node T (rows 6-7), the points below and above the median are passed to the left and right child nodes of T respectively for recursive construction (rows 9-10), and the LazyLabelInit and Pullup in rows 11-12 update all the properties required for the incremental update.

// 初始化构建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的最近邻搜索

That is, at each node, in addition to calculating the distance between the node itself and the query point, it will also determine whether the range of the left and right subtrees overlaps with the target solution space, and only the overlapping subtrees will continue to be recursively searched, and the non-overlapping subtrees will be directly pruned off to achieve search acceleration.

Click to understand ikd-tree in depth, start by reading the code (2) - Gu Yueju to view the full article

Previous: Japan's reclamation of islands caused trouble with the United Nations, and after China's demands were ignored, the most insoluble conspiracy was used
Next: Moral education is important, but it should be carried out in small increments