0. 简介

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

1. 头文件介绍

// 定义EPSS为1e-6，表示误差范围 
#define EPSS 1e-6

// 定义最小不平衡树大小为10，即当某个子树的节点数小于10时，需要进行重平衡 
#define Minimal_Unbalanced_Tree_Size 10

// 定义需要重建子树的点数达到1500时，使用额外线程处理 
#define Multi_Thread_Rebuild_Point_Num 1500

// 定义DOWNSAMPLE_SWITCH为true，表示开启下采样功能 
#define DOWNSAMPLE_SWITCH true

// 定义强制重建百分比为0.2，即当某个子树中删除的节点数占总节点数的20%以上时，需要进行重建 
#define ForceRebuildPercentage 0.2

// 定义支持的最大队列长度为1000000 
#define Q_LEN 1000000

2. 结构体介绍

2.1 全局结构体

// 构造函数，用来初始化点的坐标
struct ikdTree_PointType
{
    float x,y,z;
    // 传递的是float类型，而不是类类型，因此不需要使用const&做形参。
    ikdTree_PointType (float px = 0.0f, float py = 0.0f, float pz = 0.0f){
        x = px;
        y = py;
        z = pz;
    }
};

// 定义box（一个box由两个边界点定义）。 
struct BoxPointType{ 
  float vertex_min[3]; // 定义最小顶点的坐标 
  float vertex_max[3]; // 定义最大顶点的坐标 
};

// 枚举ikdtree中的所有操作。
enum operation_set {
    ADD_POINT, 
    DELETE_POINT, 
    DELETE_BOX, 
    ADD_BOX, 
    DOWNSAMPLE_DELETE, 
    PUSH_DOWN
};

// 定义一个枚举类型，用于记录删除点的存储方式
enum delete_point_storage_set {
    NOT_RECORD, //没有记录
    DELETE_POINTS_REC, //删除点记录
    MULTI_THREAD_REC//多线程记录
};

// 定义一个手动实现的队列类
template <typename T>
class MANUAL_Q{
    private:
        int head = 0,tail = 0, counter = 0;// 定义队列的头、尾、大小
        T q[Q_LEN];// 定义一个数组作为队列
        bool is_empty;// 定义一个标志位，表示队列是否为空
    public:
        void pop();// 弹出队列的头元素
        T front();// 返回队列的头元素
        T back();// 返回队列的尾元素
        void clear(); // 清空队列
        void push(T op);// 在队列尾部插入元素
        bool empty();// 判断队列是否为空
        int size();// 返回队列大小
};

2.2 KD-TREE结构体

ikd树中树节点的属性如数据结构1所示

第2-4行是标准k-d树的常见属性，属性leftson和rightson分别是指向其左和右子节点的指针，点信息（例如点坐标、强度）存储在点中，由于一个点对应k-d树上的一个节点，我们将互换使用点和节点，分割轴记录在轴中。第5-7行是为增量更新设计的新属性。

using PointVector = vector<PointType, Eigen::aligned_allocator<PointType>>;// 定义一个使用Eigen内存分配器的点向量类型
using Ptr = shared_ptr<KD_TREE<PointType>>;// 定义一个智能指针类型，指向KD_TREE类型的对象,就是本class

// 定义KD树节点结构体
struct KD_TREE_NODE{
    PointType point;                            // 数据点
    uint8_t division_axis;                      // 分割轴
    int TreeSize = 1;                           // 总节点数
    int invalid_point_num = 0;                  // label为删除的点的数量
    int down_del_num = 0;                       // 下采样删除的点的数量
    bool point_deleted = false;                 // 标记节点是否被删除
    bool tree_deleted = false;                  // 整个tree标记为删除
    bool point_downsample_deleted = false;      // 标记节点是否被下采样删除
    bool tree_downsample_deleted = false;       // 标记整个树是否被下采样删除 
    bool need_push_down_to_left = false;        // 标记是否需要将操作向左子树下传
    bool need_push_down_to_right = false;       // 标记是否需要将操作向右子树下传 
    bool working_flag = false;                  // 标记节点是否正在被访问
    float radius_sq;                            // 节点范围的平方半径
    pthread_mutex_t push_down_mutex_lock;        // 用于多线程操作的互斥锁
    float node_range_x[2], node_range_y[2], node_range_z[2];    // tree对应的包络Box
    KD_TREE_NODE *left_son_ptr = nullptr;       // 左子树指针对应KD_TREE
    KD_TREE_NODE *right_son_ptr = nullptr;      // 右子树指针对应KD_TREE
    KD_TREE_NODE *father_ptr = nullptr;         // 父树
    // For paper data record
    float alpha_del;
    float alpha_bal;
};

// 定义操作日志结构体 
struct Operation_Logger_Type{
    PointType point;// 操作点 
    BoxPointType boxpoint;// 操作box 
    bool tree_deleted, tree_downsample_deleted;// 标记树是否被删除或下采样删除
    operation_set op;// 操作类型
};

2.3 MANUAL_HEAP堆栈

// 定义一个用于缓存近邻搜索结果的结构体
struct PointType_CMP{
    PointType point; // 搜索到的近邻点
    float dist = 0.0; // 搜索到的近邻点与查询点之间的距离
    PointType_CMP (PointType p = PointType(), float d = INFINITY){//自定义PointType_CMP点比较结构体的构造函数
        this->point = p;
        this->dist = d;
    };
    bool operator < (const PointType_CMP &a)const{// 重载小于运算符，用于排序
        if (fabs(dist - a.dist) < 1e-10) return point.x < a.point.x;
        else return dist < a.dist;
    }    
};

//在近邻搜索过程中，定义一个手动实现的小根堆类，用于缓存近邻搜索结果 
class MANUAL_HEAP{
  public:
      MANUAL_HEAP(int max_capacity = 100){
          cap = max_capacity;//容量
          heap = new PointType_CMP[max_capacity];//设置new一个数组
          heap_size = 0;//当前数量
      }

      ~MANUAL_HEAP(){ delete[] heap;}//析构函数

      void pop(){// 弹出堆顶元素
          if (heap_size == 0) return;
          heap[0] = heap[heap_size-1];
          heap_size--;
          MoveDown(0);// 维护堆的性质
          return;
      }

      PointType_CMP top(){ return heap[0];}// 返回堆顶元素

      void push(PointType_CMP point){// 向堆中插入元素
          if (heap_size >= cap) return;
          heap[heap_size] = point;
          FloatUp(heap_size);// 维护堆的性质
          heap_size++;
          return;
      }

      int size(){ return heap_size;}// 返回堆的大小

      void clear(){ heap_size = 0;}// 清空堆
  private:
      int heap_size = 0;// 堆的大小
      int cap = 0;      // 堆的容量
      PointType_CMP * heap;// 堆的数组
      // 维护堆的性质，将新插入的元素向下冒泡
      void MoveDown(int heap_index){
          int l = heap_index * 2 + 1;
          PointType_CMP tmp = heap[heap_index];
          while (l < heap_size){
              if (l + 1 < heap_size && heap[l] < heap[l+1]) l++;// 找到左右子节点中较小的一个
              if (tmp < heap[l]){// 如果堆顶元素比较小的子节点小，则交换它们，并继续向下移动
                  heap[heap_index] = heap[l];
                  heap_index = l;
                  l = heap_index * 2 + 1;
              } else break;
          }
          heap[heap_index] = tmp;
          return;
      }
      // 维护堆的性质，将新插入的元素向上冒泡
      void FloatUp(int heap_index){
          int ancestor = (heap_index-1)/2;
          PointType_CMP tmp = heap[heap_index];
          while (heap_index > 0){
              if (heap[ancestor] < tmp){// 如果新插入的元素比其父节点大，则交换它们，并继续向上冒泡
                  heap[heap_index] = heap[ancestor];
                  heap_index = ancestor;
                  ancestor = (heap_index-1)/2;
              } else break;
          }
          heap[heap_index] = tmp;
          return;
      }
};

3. ikd-Tree变量以及后续函数

  private:
    // 定义一些ikd-tree中用到的变量和函数
    bool termination_flag = false;// 用于线程终止标志
    bool rebuild_flag = false;// 用于重建标志
    pthread_t rebuild_thread;// 重建线程
    pthread_mutex_t termination_flag_mutex_lock, rebuild_ptr_mutex_lock, working_flag_mutex, search_flag_mutex;// 用于线程同步的互斥锁 
    pthread_mutex_t rebuild_logger_mutex_lock, points_deleted_rebuild_mutex_lock;
    // queue<Operation_Logger_Type> Rebuild_Logger;
    MANUAL_Q<Operation_Logger_Type> Rebuild_Logger;    // 重建日志队列 
    PointVector Rebuild_PCL_Storage; // 重建点云存储
    KD_TREE_NODE ** Rebuild_Ptr = nullptr;// 重建根节点指针
    int search_mutex_counter = 0;// 用于计数查询操作的互斥锁
    static void * multi_thread_ptr(void *arg);// 重建线程函数 
    void multi_thread_rebuild(); // 多线程重建函数
    void start_thread();// 启动重建线程
    void stop_thread(); // 停止重建线程
    void run_operation(KD_TREE_NODE ** root, Operation_Logger_Type operation); // 执行重建日志操作函数
    // KD Tree Functions and augmented variables
    int Treesize_tmp = 0, Validnum_tmp = 0;// 用于记录树的大小和有效节点数
    float alpha_bal_tmp = 0.5, alpha_del_tmp = 0.0;// 平衡因子和删除因子
    float delete_criterion_param = 0.5f;// 删除节点的条件参数
    float balance_criterion_param = 0.7f;// 平衡树的条件参数
    float downsample_size = 0.2f;// 下采样的比例
    bool Delete_Storage_Disabled = false; // 是否禁用删除点存储
    KD_TREE_NODE * STATIC_ROOT_NODE = nullptr; // 根节点的根，tree结构依存的基础，仅在build或彻底rebuild时改变之
    PointVector Points_deleted;// 被删除的点的集合
    PointVector Downsample_Storage;// 下采样点的集合
    PointVector Multithread_Points_deleted;// 多线程删除的点的集合
    void InitTreeNode(KD_TREE_NODE * root);// 初始化树节点 
    void Test_Lock_States(KD_TREE_NODE *root);// 测试节点的锁状态
    void BuildTree(KD_TREE_NODE ** root, int l, int r, PointVector & Storage);  // 构建kd-tree
    void Rebuild(KD_TREE_NODE ** root);// 重建kd-tree
    int Delete_by_range(KD_TREE_NODE ** root, BoxPointType boxpoint, bool allow_rebuild, bool is_downsample);// 根据范围删除节点
    void Delete_by_point(KD_TREE_NODE ** root, PointType point, bool allow_rebuild);// 根据点删除节点
    void Add_by_point(KD_TREE_NODE ** root, PointType point, bool allow_rebuild, int father_axis);// 根据点添加节点 
    void Add_by_range(KD_TREE_NODE ** root, BoxPointType boxpoint, bool allow_rebuild);// 根据范围添加节点
    void Search(KD_TREE_NODE * root, int k_nearest, PointType point, MANUAL_HEAP &q, double max_dist);//搜索最近点priority_queue<PointType_CMP>
    void Search_by_range(KD_TREE_NODE *root, BoxPointType boxpoint, PointVector &Storage);// 根据范围搜索点
    void Search_by_radius(KD_TREE_NODE *root, PointType point, float radius, PointVector &Storage);// 根据半径搜索点
    bool Criterion_Check(KD_TREE_NODE * root);// 检查平衡因子是否满足条件
    void Push_Down(KD_TREE_NODE * root);// 向下更新节点的信息
    void Update(KD_TREE_NODE * root); // 更新节点的信息 
    void delete_tree_nodes(KD_TREE_NODE ** root);// 删除kd-tree的所有节点
    void downsample(KD_TREE_NODE ** root);// 下采样kd-tree
    bool same_point(PointType a, PointType b);// 判断两个点是否相同 
    float calc_dist(PointType a, PointType b);// 计算两个点之间的距离
    float calc_box_dist(KD_TREE_NODE * node, PointType point);   // 计算点到包围盒的距离 
    static bool point_cmp_x(PointType a, PointType b);  // 按x轴比较两个点的大小
    static bool point_cmp_y(PointType a, PointType b);  // 按y轴比较两个点的大小
    static bool point_cmp_z(PointType a, PointType b); // 按z轴比较两个点的大小


  public:
    KD_TREE(float delete_param = 0.5, float balance_param = 0.6 , float box_length = 0.2);// 构造函数，设置删除因子、平衡因子和包围盒长度
    ~KD_TREE();// 析构函数，销毁kd-tree
    void Set_delete_criterion_param(float delete_param); // 设置删除因子
    void Set_balance_criterion_param(float balance_param); // 设置平衡因子 
    void set_downsample_param(float box_length);// 设置包围盒长度
    void InitializeKDTree(float delete_param = 0.5, float balance_param = 0.7, float box_length = 0.2);// 初始化kd-tree，并设置删除因子、平衡因子和包围盒长度 
    int size();// 返回kd-tree的大小
    int validnum();// 返回kd-tree的有效节点数
    void root_alpha(float &alpha_bal, float &alpha_del);// 获取根节点的平衡因子和删除因子
    void Build(PointVector point_cloud);// 用点云构建kd-tree 
    void Nearest_Search(PointType point, int k_nearest, PointVector &Nearest_Points, 
                        vector<float> & Point_Distance, double max_dist = INFINITY);// 根据点查找k个最近点
    void Box_Search(const BoxPointType &Box_of_Point, PointVector &Storage);// 根据包围盒查找点
    void Radius_Search(PointType point, const float radius, PointVector &Storage);// 根据半径查找点
    int Add_Points(PointVector & PointToAdd, bool downsample_on);// 添加点到kd-tree，并返回添加的点数
    void Add_Point_Boxes(vector<BoxPointType> & BoxPoints);// 添加点的包围盒到kd-tree
    void Delete_Points(PointVector & PointToDel);// 根据点删除节点
    int Delete_Point_Boxes(vector<BoxPointType> & BoxPoints);// 根据包围盒删除节点
    void flatten(KD_TREE_NODE * root, PointVector &Storage, delete_point_storage_set storage_type);// 将kd-tree展平为点集
    void acquire_removed_points(PointVector & removed_points);// 获取被删除的点的集合
    BoxPointType tree_range();// 获取kd-tree的包围盒 
    PointVector PCL_Storage;     // 点云存储 
    KD_TREE_NODE * Root_Node = nullptr;// 根节点指针
    int max_queue_size = 0;// 最大队列大小

4. 参考链接

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

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

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