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

敢敢のwings

分类：ROS

发布时间 2024.01.15阅读数 1133 评论数 0

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的最近邻搜索

ikd-Tree 中充分利用了数据结构部分提到的 range信息来进行剪枝加速，也即在每个节点处，除了计算节点本身与查询点的距离之外，也会分别判断左右两个子树的range 是否与目标解空间有重叠，只有有重叠的子树才会被继续递归搜索，没重叠的子树将直接被剪枝掉，实现搜索加速。

// 最近邻搜索的入口函数
template <typename PointType>
void KD_TREE<PointType>::Nearest_Search(
    PointType point, int k_nearest, PointVector& Nearest_Points, 
    vector<float> & Point_Distance, double max_dist)
{
    MANUAL_HEAP q(2*k_nearest); // // 预分配两倍空间的堆
    q.clear();// 清空堆 
    // 交换内存空间（实质上是交换指针地址），相当于清空了Point_Distance
    vector<float> ().swap(Point_Distance); 
    if (Rebuild_Ptr == nullptr || *Rebuild_Ptr != Root_Node){// 如果没有重建指针或者重建指针指向的树不是根节点 
        Search(Root_Node, k_nearest, point, q, max_dist); // 实质的搜索函数，递归调用。
    } else {           // 如果有并行任务，仅在允许读时再执行递归搜索。
        pthread_mutex_lock(&search_flag_mutex);// 如果有重建指针并且重建指针指向的树是根节点
        while (search_mutex_counter == -1)
        {
            pthread_mutex_unlock(&search_flag_mutex);
            usleep(1);
            pthread_mutex_lock(&search_flag_mutex);
        }
        search_mutex_counter += 1;
        pthread_mutex_unlock(&search_flag_mutex);  
        Search(Root_Node, k_nearest, point, q, max_dist); //实质的搜索函数，递归调用。
        pthread_mutex_lock(&search_flag_mutex);
        search_mutex_counter -= 1;
        pthread_mutex_unlock(&search_flag_mutex);      
    }
    int k_found = min(k_nearest,int(q.size())); // 实际找到的最近邻点个数
    PointVector ().swap(Nearest_Points);// 清空最近邻点向量
    vector<float> ().swap(Point_Distance);// 清空距离
    for (int i=0;i < k_found;i++){
        Nearest_Points.insert(Nearest_Points.begin(), q.top().point);
        Point_Distance.insert(Point_Distance.begin(), q.top().dist);
        q.pop();
    }
    return;
}
// 搜索指定范围内的点
template <typename PointType>
void KD_TREE<PointType>::Box_Search(const BoxPointType &Box_of_Point, PointVector &Storage)
{
    Storage.clear();// 清空存储向量
    Search_by_range(Root_Node, Box_of_Point, Storage);// 调用搜索函数
}
// 搜索指定半径内的点
template <typename PointType>
void KD_TREE<PointType>::Radius_Search(PointType point, const float radius, PointVector &Storage)
{
    Storage.clear();// 清空存储向量
    Search_by_radius(Root_Node, point, radius, Storage);// 调用搜索函数
}

对应函数算法

// 搜索k近邻点 
template <typename PointType>
void KD_TREE<PointType>::Search(KD_TREE_NODE * root, int k_nearest, PointType point, MANUAL_HEAP &q, double max_dist)
{
    // 如果整个subtree被标记为treedeleted，直接退出。
    if (root == nullptr || root->tree_deleted) return;   

    // // 比较节点张成的空间是否与point距离张成的球空间有交叉，如果无交叉，则不可能是ranged-kNN解，直接退出（剪枝加速）。
    double cur_dist = calc_box_dist(root, point);
    double max_dist_sqr = max_dist * max_dist;
    if (cur_dist > max_dist_sqr) return;    

    //如果当前节点需要更新状态信息，则先更新。
    int retval; 
    if (root->need_push_down_to_left || root->need_push_down_to_right) {
        retval = pthread_mutex_trylock(&(root->push_down_mutex_lock));
        if (retval == 0){
            Push_Down(root);
            pthread_mutex_unlock(&(root->push_down_mutex_lock));
        } else {
            pthread_mutex_lock(&(root->push_down_mutex_lock));
            pthread_mutex_unlock(&(root->push_down_mutex_lock));
        }
    }
    // 只要当前节点未被标记为删除，则计算当前节点到point的距离，如果在range之内，就放入结果缓存队列。
    if (!root->point_deleted){
        float dist = calc_dist(point, root->point);
        if (dist <= max_dist_sqr && (q.size() < k_nearest || dist < q.top().dist)){
            if (q.size() >= k_nearest) q.pop();
            PointType_CMP current_point{root->point, dist};                    
            q.push(current_point);            
        }
    }  

    // 继续向下递归搜索。「逻辑核心*」
    // int cur_search_counter; // wgh: unused variable.
    float dist_left_node = calc_box_dist(root->left_son_ptr, point);
    float dist_right_node = calc_box_dist(root->right_son_ptr, point);
    // 如果NN数量不足k个，且左枝或右枝可能存在NN。
    if (q.size()< k_nearest || (dist_left_node < q.top().dist && dist_right_node < q.top().dist)){
        // 优先搜索距离更小的分支
        if (dist_left_node <= dist_right_node) {
          // 如果无并行任务，直接递归搜索
          if (Rebuild_Ptr == nullptr || *Rebuild_Ptr != root->left_son_ptr){
              Search(root->left_son_ptr, k_nearest, point, q, max_dist);                       
          } 
          else {           // 如果有并行任务，仅在允许读时再执行递归搜索。
            // 加锁，确保当前只有一个线程在进行搜索
            pthread_mutex_lock(&search_flag_mutex);
            while (search_mutex_counter == -1)
            {
                pthread_mutex_unlock(&search_flag_mutex);
                usleep(1);
                pthread_mutex_lock(&search_flag_mutex);
            }
            // 将搜索计数器加1，表示当前有一个线程在进行搜索
            search_mutex_counter += 1;
            pthread_mutex_unlock(&search_flag_mutex);
            // 递归搜索左子树
            Search(root->left_son_ptr, k_nearest, point, q, max_dist);  
            pthread_mutex_lock(&search_flag_mutex);
            // 搜索完成，将搜索计数器减1 
            search_mutex_counter -= 1;
            pthread_mutex_unlock(&search_flag_mutex);
        }
        // bounds-overlap-ball搜索法（实现策略略有不同，效果一样）
        if (q.size() < k_nearest || dist_right_node < q.top().dist) {
            // 如果无并行任务，直接递归搜索
            if (Rebuild_Ptr == nullptr || *Rebuild_Ptr != root->right_son_ptr){
                Search(root->right_son_ptr, k_nearest, point, q, max_dist);                       
            }
            else {           // 如果有并行任务，仅在允许读时再执行递归搜索。
                // 加锁，确保当前只有一个线程在进行搜索
                pthread_mutex_lock(&search_flag_mutex);
                while (search_mutex_counter == -1)
                {
                    pthread_mutex_unlock(&search_flag_mutex);
                    usleep(1);
                    pthread_mutex_lock(&search_flag_mutex);
                }
                search_mutex_counter += 1;
                pthread_mutex_unlock(&search_flag_mutex);                    
                Search(root->right_son_ptr, k_nearest, point, q, max_dist);  
                pthread_mutex_lock(&search_flag_mutex);
                search_mutex_counter -= 1;
                pthread_mutex_unlock(&search_flag_mutex);
            }                
          }
        } else {
            // 如果无并行任务，直接递归搜索右子树
            if (Rebuild_Ptr == nullptr || *Rebuild_Ptr != root->right_son_ptr){
                Search(root->right_son_ptr, k_nearest, point, q, max_dist);                       
            } else {           // 如果有并行任务，仅在允许读时再执行递归搜索。
                pthread_mutex_lock(&search_flag_mutex);
                while (search_mutex_counter == -1)
                {
                    pthread_mutex_unlock(&search_flag_mutex);
                    usleep(1);
                    pthread_mutex_lock(&search_flag_mutex);
                }
                search_mutex_counter += 1;
                pthread_mutex_unlock(&search_flag_mutex);                   
                Search(root->right_son_ptr, k_nearest, point, q, max_dist);  
                pthread_mutex_lock(&search_flag_mutex);
                search_mutex_counter -= 1;
                pthread_mutex_unlock(&search_flag_mutex);
            }
            // 如果需要搜索左子树，则进行左子树的搜索
            if (q.size() < k_nearest || dist_left_node < q.top().dist) {
                // 如果无并行任务，直接递归搜索左子树
                if (Rebuild_Ptr == nullptr || *Rebuild_Ptr != root->left_son_ptr){
                    Search(root->left_son_ptr, k_nearest, point, q, max_dist);                       
                } else {
                    pthread_mutex_lock(&search_flag_mutex);
                    while (search_mutex_counter == -1)
                    {
                        pthread_mutex_unlock(&search_flag_mutex);
                        usleep(1);
                        pthread_mutex_lock(&search_flag_mutex);
                    }
                    search_mutex_counter += 1;
                    pthread_mutex_unlock(&search_flag_mutex);  
                    // 递归搜索左子树
                    Search(root->left_son_ptr, k_nearest, point, q, max_dist);  
                    pthread_mutex_lock(&search_flag_mutex);
                    // 搜索完成，将搜索计数器减1
                    search_mutex_counter -= 1;
                    pthread_mutex_unlock(&search_flag_mutex);
                }
            }
        }
    } 
    // 如果NN数量已有k个，当且仅当且左枝或右枝可能存在优于`当前最差解`的情况下，继续进行递归搜索。
    else {
        // 如果左子树可能存在优于当前最差解的情况，则继续递归搜索左子树
        if (dist_left_node < q.top().dist) {        
            // 如果无并行任务，直接递归搜索左子树
            if (Rebuild_Ptr == nullptr || *Rebuild_Ptr != root->left_son_ptr){
                Search(root->left_son_ptr, k_nearest, point, q, max_dist);                       
            } else {// 如果有并行任务，仅在允许读时再执行递归搜索。
                pthread_mutex_lock(&search_flag_mutex);
                while (search_mutex_counter == -1)
                {
                    pthread_mutex_unlock(&search_flag_mutex);// 加锁，确保当前只有一个线程在进行搜索
                    usleep(1);
                    pthread_mutex_lock(&search_flag_mutex);
                }
                search_mutex_counter += 1;
                pthread_mutex_unlock(&search_flag_mutex);  
                Search(root->left_son_ptr, k_nearest, point, q, max_dist);  
                pthread_mutex_lock(&search_flag_mutex);
                search_mutex_counter -= 1;
                pthread_mutex_unlock(&search_flag_mutex);
            }
        }
        if (dist_right_node < q.top().dist) {
            if (Rebuild_Ptr == nullptr || *Rebuild_Ptr != root->right_son_ptr){
                Search(root->right_son_ptr, k_nearest, point, q, max_dist);                       
            } else {
                pthread_mutex_lock(&search_flag_mutex);
                while (search_mutex_counter == -1)
                {
                    pthread_mutex_unlock(&search_flag_mutex);
                    usleep(1);
                    pthread_mutex_lock(&search_flag_mutex);
                }
                search_mutex_counter += 1;
                pthread_mutex_unlock(&search_flag_mutex);  
                Search(root->right_son_ptr, k_nearest, point, q, max_dist);
                pthread_mutex_lock(&search_flag_mutex);
                search_mutex_counter -= 1;
                pthread_mutex_unlock(&search_flag_mutex);
            }
        }
    }
    return;
}
// 工具属性，从根节点开始向下递归搜索，获得所有被Box包含的节点。
template <typename PointType>
void KD_TREE<PointType>::Search_by_range(KD_TREE_NODE *root, BoxPointType boxpoint, PointVector & Storage){
    // 如果当前节点为空，直接返回。
    if (root == nullptr) return;
    Push_Down(root); // 向下更新信息。

    // 当且仅当两个空间有交叉时，才有继续的必要。
    if (boxpoint.vertex_max[0] <= root->node_range_x[0] || boxpoint.vertex_min[0] > root->node_range_x[1]) return;
    if (boxpoint.vertex_max[1] <= root->node_range_y[0] || boxpoint.vertex_min[1] > root->node_range_y[1]) return;
    if (boxpoint.vertex_max[2] <= root->node_range_z[0] || boxpoint.vertex_min[2] > root->node_range_z[1]) return;

    // 当Box完全包含了节点所张成的空间时，把该节点subtree上的所有点返回。
    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])
    {
        flatten(root, Storage, NOT_RECORD);
        return;
    }

    // 如果当前节点的point被Box包含，记录该point。
    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)
    {
        if (!root->point_deleted) Storage.push_back(root->point);
    }

    // 递归搜索左子树。
    if ((Rebuild_Ptr == nullptr) || root->left_son_ptr != *Rebuild_Ptr){
        Search_by_range(root->left_son_ptr, boxpoint, Storage);
    } 
    else {
        // 如果当前节点是待重构节点的左子节点，需要加锁保证线程安全。
        pthread_mutex_lock(&search_flag_mutex);
        Search_by_range(root->left_son_ptr, boxpoint, Storage);
        pthread_mutex_unlock(&search_flag_mutex);
    }
    // 递归搜索右子树。
    if ((Rebuild_Ptr == nullptr) || root->right_son_ptr != *Rebuild_Ptr){
        Search_by_range(root->right_son_ptr, boxpoint, Storage);
    } 
    else {
        // 如果当前节点是待重构节点的右子节点，需要加锁保证线程安全。
        pthread_mutex_lock(&search_flag_mutex);
        Search_by_range(root->right_son_ptr, boxpoint, Storage);
        pthread_mutex_unlock(&search_flag_mutex);
    }

    return;
}

// KD_TREE 类的 Search_by_radius 函数
template <typename PointType>
void KD_TREE<PointType>::Search_by_radius(KD_TREE_NODE *root, PointType point, float radius, PointVector &Storage)
{ // 如果当前节点为空，则直接返回
    if (root == nullptr)
        return;
    // 将当前节点的子节点推入堆栈中
    Push_Down(root);
    // 计算当前节点的中心点
    PointType range_center;
    range_center.x = (root->node_range_x[0] + root->node_range_x[1]) * 0.5;
    range_center.y = (root->node_range_y[0] + root->node_range_y[1]) * 0.5;
    range_center.z = (root->node_range_z[0] + root->node_range_z[1]) * 0.5;
    // 计算当前节点与搜索点之间的距离
    float dist = sqrt(calc_dist(range_center, point));
    // 如果当前节点与搜索点的距离超过了搜索半径加上节点半径，则直接返回
    if (dist > radius + sqrt(root->radius_sq)) return;
    // 如果当前节点与搜索点的距离小于等于搜索半径减去节点半径，则将当前节点的所有子节点打平，将其中的点加入搜索结果向量中，并返回
    if (dist <= radius - sqrt(root->radius_sq)) 
    {
        flatten(root, Storage, NOT_RECORD);
        return;
    }
    // 如果当前节点没有被删除且当前节点的点与搜索点之间的距离小于等于搜索半径，则将当前节点的点加入搜索结果向量中
    if (!root->point_deleted && calc_dist(root->point, point) <= radius * radius){
        Storage.push_back(root->point);
    }
    // 递归搜索左子树
    if ((Rebuild_Ptr == nullptr) || root->left_son_ptr != *Rebuild_Ptr)
    {
        Search_by_radius(root->left_son_ptr, point, radius, Storage);
    }
    else
    {
        pthread_mutex_lock(&search_flag_mutex);
        Search_by_radius(root->left_son_ptr, point, radius, Storage);
        pthread_mutex_unlock(&search_flag_mutex);
    }
    // 递归搜索右子树
    if ((Rebuild_Ptr == nullptr) || root->right_son_ptr != *Rebuild_Ptr)
    {
        Search_by_radius(root->right_son_ptr, point, radius, Storage);
    }
    else
    {
        pthread_mutex_lock(&search_flag_mutex);
        Search_by_radius(root->right_son_ptr, point, radius, Storage);
        pthread_mutex_unlock(&search_flag_mutex);
    }    
    return;
}

4. 参考链接

https://zhuanlan.zhihu.com/p/529926254

https://blog.csdn.net/u013019296/article/details/126329046

https://arxiv.org/pdf/2102.10808.pdf

ikd-tree

原创文章作者：敢敢のwings。如若转载，请注明出处：古月居 https://www.guyuehome.com/45803

打赏 0

上一篇：经典文献阅读之--U-CE(用于语义分割的不确定性感知交叉熵)

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

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

敢敢のwings

0. 简介

1. 一些初始化步骤

1.1 初始化代码

1.2 ikd-Tree尺寸

1.3 包围盒范围

1.4 线程创建

2. 构建增量K-D树

3. ikd-Tree的最近邻搜索

4. 参考链接

为你推荐

IntelRealSense ZR300 For ROS相关 系列第一篇

DSO源码解读(三)：后端优化

开源自主导航小车MickX4（二）ROS底盘运动控制

ROSCon 2019 机器人操作系统开发者大会视频集&PPT

pcl点云库(1):迭代最近点算法(icp)算法实践及解析

【ROS-Gazebo】仿真插件编写教程（1）——概述

评论（0）

关于作者

敢敢のwings

361

2

805

17

经典文献阅读之--FAST-LIVO（Fast-LIO系列的激光视觉惯性里程计）

经典文献阅读之--NORLAB-ICP（重力约束ICP）

OpenCalib之Lidar_imu自动标定----自输入输入数据采集

相关推荐

ROS 2设计文章系列之二十——重映射名称

小白也能学会ros2的入门教程 内附知识图谱和思维导图

ROS操作系统（十四）launch启动文件的使用方法

8月7日直播已圆满结束，点此进入8月8日直播间报名

水一篇，手眼标定+jaka机械臂

ROS与机器人

热门泡泡

30积分 失眠，聊聊自己搞ROS的心得体会吧

ros学习路线

30积分 TF_REPEATED_DATA ignoring data错误

各位大佬，有什么ROS定位算法推荐吗

5积分 想买能用ROS2的开发套件。或者开发板

5积分 ros中启动gazebo时报错

TA的专栏

dlo系列

轨迹规划

大模型

Matlab-ros

深度学习

CUDA

FAST-LIO系列

规范学习

机器人仿真

传感器感知

ROS基础以及进阶

无人驾驶

从零到一的SLAM

机器人基础知识

经典论文阅读

C++模式

专栏导航

给作者打赏

忘记密码

修改头像

添加你感兴趣的标签

举报类型（必选）

举报详情（选填）

IntelRealSense ZR300 For ROS相关系列第一篇

小白也能学会ros2的入门教程内附知识图谱和思维导图

30积分失眠，聊聊自己搞ROS的心得体会吧

5积分想买能用ROS2的开发套件。或者开发板