天天看點
傳回

深入了解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所示：

深入了解ikd-Tree從閱讀代碼開始(二)

給定一個點陣列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 是否與目标解空間有重疊，隻有有重疊的子樹才會被繼續遞歸搜尋，沒重疊的子樹将直接被剪枝掉，實作搜尋加速。

點選深入了解ikd-Tree從閱讀代碼開始(二) - 古月居可檢視全文

上一篇: 日本填海造島鬧上聯合國，中國訴求被無視後，用出了最無解的陽謀
下一篇: 道德教育很重要，卻要在點滴中進行