0. 简介
增量KD树,我们知道这类算法可以更加高效并且有意义地完成数据结构的动态空间划分. ikd-Tree可以主动监视树结构并重新平衡树结构，从而在后期实现高效的最近点搜索，ikd-Tree经过了精心设计，支持多线程并行计算，以最大限度地提高整体效率.mars实验室已经将这个代码公开了,而且有很多人对代码进行了总结与阐述.这里我们主要来看一下KennyWGH ikd-Tree,并对作者的一些疑问进行解释.
1. ikd-Tree的插入与删除
增量更新指的是增量操作，随后进行动态重新平衡。增量操作包括向k-D树插入、删除和重新插入新点，具体而言，插入操作将一个新点（即节点）附加到k-d树，在删除操作中，我们使用了延迟删除策略，也就是说，不会立即从树中删除点，而只会通过将属性deleted设置为true来标记为“deleted”（已删除）。如果已删除以T为根的子树上的所有节点，则T的属性treedeleted设置为true，因此，删除的属性和树删除的属性称为惰性标签。如果标记为“已删除”但未删除的点稍后插入到树中，则称为“重新插入”，只需将删除的属性设置回false即可有效地实现。否则，标记为“已删除”的点将在重建过程中从树中删除，我们的增量更新支持两种类型：点式更新和框式更新，逐点更新在树上插入、删除或重新插入单个点，而逐框更新在与数据坐标轴对齐的给定框中插入、删除或重新插入所有点，框式更新可能需要删除或重新插入以T为根的整个子树。在这种情况下，递归更新T的所有子节点的已删除和已删除的懒惰标签仍然是低效的。为了解决这个问题，我们使用了进一步的延迟策略来更新子节点的延迟标签。
1.1 插入节点
增量k-d树上的逐点更新以递归方式实现，类似于k-d树，对于逐点插入，该算法从根节点递归向下搜索，并将新点在分割轴上的坐标与存储在树节点上的点进行比较，直到找到一个叶节点来附加新的树节点，对于点P的删除或重新插入，该算法会找到存储点P的树节点，并修改删除的属性。ikd-Tree插入新点的流程如下:
-
首先将整个空间体素化，并明确新点落入哪个体素（目标体素）；
-
然后向ikd-Tree查询目标体素内是否已经有点以及有哪些点（查询过程参考box-wise delete）；
-
如果已经有点了，将已有点和新点一起排序，找出离体素中心最近的那个点，然后做判断：如果最近的点是已有点，意味着新点无必要再插入了，结束处理；如果最近的点是新点，则把已有点全部标记删除，并插入新点；
-
如果体素内尚不存在点，则直接插入新点。
// KD_TREE 类的 Add_Points 函数,用于将一组点添加到 KD 树中
template <typename PointType>
int KD_TREE<PointType>::Add_Points(PointVector & PointToAdd, bool downsample_on)
{
// 初始化一些变量
// int NewPointSize = PointToAdd.size(); // wgh: unused variable.
// int tree_size = size(); // wgh: unused variable.
BoxPointType Box_of_Point;
PointType downsample_result, mid_point;
bool downsample_switch = downsample_on && DOWNSAMPLE_SWITCH;
float min_dist, tmp_dist;
int tmp_counter = 0;
// 遍历所有点，逐个插入。
for (std::size_t i=0; i<PointToAdd.size();i++){
if (downsample_switch){
// 获得插入点所在的Voxel，计算Voxel的几何中心点（将来只保留最接近中心点的point）
Box_of_Point.vertex_min[0] = floor(PointToAdd[i].x/downsample_size)*downsample_size;
Box_of_Point.vertex_max[0] = Box_of_Point.vertex_min[0]+downsample_size;
Box_of_Point.vertex_min[1] = floor(PointToAdd[i].y/downsample_size)*downsample_size;
Box_of_Point.vertex_max[1] = Box_of_Point.vertex_min[1]+downsample_size;
Box_of_Point.vertex_min[2] = floor(PointToAdd[i].z/downsample_size)*downsample_size;
Box_of_Point.vertex_max[2] = Box_of_Point.vertex_min[2]+downsample_size;
mid_point.x = Box_of_Point.vertex_min[0] + (Box_of_Point.vertex_max[0]-Box_of_Point.vertex_min[0])/2.0;
mid_point.y = Box_of_Point.vertex_min[1] + (Box_of_Point.vertex_max[1]-Box_of_Point.vertex_min[1])/2.0;
mid_point.z = Box_of_Point.vertex_min[2] + (Box_of_Point.vertex_max[2]-Box_of_Point.vertex_min[2])/2.0;
// 在当前 Voxel 中搜索最近的点
PointVector ().swap(Downsample_Storage);
Search_by_range(Root_Node, Box_of_Point, Downsample_Storage);
min_dist = calc_dist(PointToAdd[i],mid_point);
downsample_result = PointToAdd[i];
// 找到距离当前点最近的点
for (std::size_t index = 0; index < Downsample_Storage.size(); index++){
tmp_dist = calc_dist(Downsample_Storage[index], mid_point);
if (tmp_dist < min_dist){
min_dist = tmp_dist;
downsample_result = Downsample_Storage[index];//降采样
}
}
// 如果当前没有re-balancing任务，也即没有并行线程，则直接执行`BoxDelete`和`插入一个点`。
if (Rebuild_Ptr == nullptr || *Rebuild_Ptr != Root_Node){
if (Downsample_Storage.size() > 1 || same_point(PointToAdd[i], downsample_result)){
if (Downsample_Storage.size() > 0) Delete_by_range(&Root_Node, Box_of_Point, true, true);
Add_by_point(&Root_Node, downsample_result, true, Root_Node->division_axis);
tmp_counter ++;
}
// 如果有re-balancing任务在并行运行，在对当前tree执行`BoxDelete`和`插入一个点`之外，还需要把这些操作缓存到logger里。
} else {
if (Downsample_Storage.size() > 1 || same_point(PointToAdd[i], downsample_result)){
Operation_Logger_Type operation_delete, operation;
operation_delete.boxpoint = Box_of_Point;
operation_delete.op = DOWNSAMPLE_DELETE;
operation.point = downsample_result;
operation.op = ADD_POINT;
pthread_mutex_lock(&working_flag_mutex);
if (Downsample_Storage.size() > 0) Delete_by_range(&Root_Node, Box_of_Point, false , true);
Add_by_point(&Root_Node, downsample_result, false, Root_Node->division_axis);
tmp_counter ++;
if (rebuild_flag){
pthread_mutex_lock(&rebuild_logger_mutex_lock);
if (Downsample_Storage.size() > 0) Rebuild_Logger.push(operation_delete);
Rebuild_Logger.push(operation);
pthread_mutex_unlock(&rebuild_logger_mutex_lock);
}
pthread_mutex_unlock(&working_flag_mutex);
};
}
}
else {
//如果不需要降采样，且无并行re-balancing任务，直接插入点。
if (Rebuild_Ptr == nullptr || *Rebuild_Ptr != Root_Node){
Add_by_point(&Root_Node, PointToAdd[i], true, Root_Node->division_axis);
}
// 如果有并行re-balancing任务，还需额外把当前操作放入logger缓存。
else {
Operation_Logger_Type operation;
operation.point = PointToAdd[i];
operation.op = ADD_POINT;
pthread_mutex_lock(&working_flag_mutex);
Add_by_point(&Root_Node, PointToAdd[i], false, Root_Node->division_axis);
if (rebuild_flag){
pthread_mutex_lock(&rebuild_logger_mutex_lock);
Rebuild_Logger.push(operation);
pthread_mutex_unlock(&rebuild_logger_mutex_lock);
}
pthread_mutex_unlock(&working_flag_mutex);
}
}
}
return tmp_counter;
}
// 工具属性，单纯地往tree结构中插入新节点。
template <typename PointType>
void KD_TREE<PointType>::Add_by_point(KD_TREE_NODE ** root, PointType point, bool allow_rebuild, int father_axis)
{
// 如果已经到达叶子节点，直接插入。
if (*root == nullptr){
*root = new KD_TREE_NODE;
InitTreeNode(*root);
(*root)->point = point;
(*root)->division_axis = (father_axis + 1) % 3;
Update(*root);
return;
}
// 标记当前节点为“工作中”，并将操作记录到Logger中
(*root)->working_flag = true;
Operation_Logger_Type add_log;
// struct timespec Timeout; // wgh: unused variable.
add_log.op = ADD_POINT;
add_log.point = point;
Push_Down(*root);
// 递归插入左子树
if (((*root)->division_axis == 0 && point.x < (*root)->point.x) ||
((*root)->division_axis == 1 && point.y < (*root)->point.y) ||
((*root)->division_axis == 2 && point.z < (*root)->point.z) )
{
if ((Rebuild_Ptr == nullptr) || (*root)->left_son_ptr != *Rebuild_Ptr){
Add_by_point(&(*root)->left_son_ptr, point, allow_rebuild, (*root)->division_axis);
}
else {
// 如果当前有re-balancing任务在并行运行，则将操作记录到Logger中
pthread_mutex_lock(&working_flag_mutex);
Add_by_point(&(*root)->left_son_ptr, point, false,(*root)->division_axis);
if (rebuild_flag){
pthread_mutex_lock(&rebuild_logger_mutex_lock);
Rebuild_Logger.push(add_log);
pthread_mutex_unlock(&rebuild_logger_mutex_lock);
}
pthread_mutex_unlock(&working_flag_mutex);
}
}
//递归插入右子树。
else {
// 如果当前有re-balancing任务在并行运行，则将操作记录到Logger中
if ((Rebuild_Ptr == nullptr) || (*root)->right_son_ptr != *Rebuild_Ptr){
Add_by_point(&(*root)->right_son_ptr, point, allow_rebuild,(*root)->division_axis);
}
else {
pthread_mutex_lock(&working_flag_mutex);
Add_by_point(&(*root)->right_son_ptr, point, false,(*root)->division_axis);
if (rebuild_flag){
pthread_mutex_lock(&rebuild_logger_mutex_lock);
Rebuild_Logger.push(add_log);
pthread_mutex_unlock(&rebuild_logger_mutex_lock);
}
pthread_mutex_unlock(&working_flag_mutex);
}
}
// 更新当前节点信息（自下而上），并检查是否需要re-balancing。
Update(*root);
if (Rebuild_Ptr != nullptr &&
*Rebuild_Ptr == *root &&
(*root)->TreeSize < Multi_Thread_Rebuild_Point_Num) Rebuild_Ptr = nullptr;
bool need_rebuild = allow_rebuild & Criterion_Check((*root));
if (need_rebuild) Rebuild(root);
// 将当前节点的“工作中”标志位设为false
if ((*root) != nullptr) (*root)->working_flag = false;
return;
}1.2 插入框节点
框式插入是通过在增量k-d树中逐个插入新点来实现的，其他框式更新（框式删除和重新插入）是利用属性range中的范围信息实现的，该属性range形成一个框式CT，并在树节点上使用惰性标签。伪代码如算法2所示。
// 这段代码是ikd-tree中的添加点框函数
template <typename PointType>
void KD_TREE<PointType>::Add_Point_Boxes(vector<BoxPointType> & BoxPoints){
// 对于要添加的每个点框
for (std::size_t i=0;i < BoxPoints.size();i++){
// 如果当前没有re-balancing任务，也即没有并行线程，则直接执行
if (Rebuild_Ptr == nullptr || *Rebuild_Ptr != Root_Node){
Add_by_range(&Root_Node ,BoxPoints[i], true);
} else { // 如果有re-balancing任务在并行运行，在对当前tree执行Add_by_range之外，还需要把这些操作缓存到logger里
Operation_Logger_Type operation;
operation.boxpoint = BoxPoints[i];
operation.op = ADD_BOX;
pthread_mutex_lock(&working_flag_mutex);
Add_by_range(&Root_Node ,BoxPoints[i], false);
if (rebuild_flag){
pthread_mutex_lock(&rebuild_logger_mutex_lock);
Rebuild_Logger.push(operation);
pthread_mutex_unlock(&rebuild_logger_mutex_lock);
}
pthread_mutex_unlock(&working_flag_mutex);
}
}
return;
}
template <typename PointType>
void KD_TREE<PointType>::Add_by_range(KD_TREE_NODE ** root, BoxPointType boxpoint, bool allow_rebuild){
// 如果根节点为空，直接返回
if ((*root) == nullptr) return;
// 标记当前节点正在处理
(*root)->working_flag = true;
// 将当前节点的标记向下传递
Push_Down(*root);
// 如果范围框的 x 坐标的最大值小于当前节点的 x 坐标范围的最小值，或者范围框的 x 坐标的最小值大于当前节点的 x 坐标范围的最大值，则直接返回
if (boxpoint.vertex_max[0] <= (*root)->node_range_x[0] || boxpoint.vertex_min[0] > (*root)->node_range_x[1]) return;
// 如果范围框的 y 坐标的最大值小于当前节点的 y 坐标范围的最小值，或者范围框的 y 坐标的最小值大于当前节点的 y 坐标范围的最大值，则直接返回
if (boxpoint.vertex_max[1] <= (*root)->node_range_y[0] || boxpoint.vertex_min[1] > (*root)->node_range_y[1]) return;
// 如果范围框的 z 坐标的最大值小于当前节点的 z 坐标范围的最小值，或者范围框的 z 坐标的最小值大于当前节点的 z 坐标范围的最大值，则直接返回
if (boxpoint.vertex_max[2] <= (*root)->node_range_z[0] || boxpoint.vertex_min[2] > (*root)->node_range_z[1]) return;
// 如果范围框完全包含当前节点，则将当前节点标记为未删除，将其左右子节点标记为需要向下传递，并更新其无效点数
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]){
(*root)->tree_deleted = false || (*root)->tree_downsample_deleted;
(*root)->point_deleted = false || (*root)->point_downsample_deleted;
(*root)->need_push_down_to_left = true;
(*root)->need_push_down_to_right = true;
(*root)->invalid_point_num = (*root)->down_del_num;
return;
}
// 如果当前节点是范围框的一部分，则将当前节点标记为需要删除
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){
(*root)->point_deleted = (*root)->point_downsample_deleted;
}
// 创建操作日志，记录添加范围框的操作
Operation_Logger_Type add_box_log;
// struct timespec Timeout; // wgh: unused variable.
add_box_log.op = ADD_BOX;
add_box_log.boxpoint = boxpoint;
// 如果重建指针为空，或者当前节点的左子节点不是重建指针，则向左子树递归添加范围框
if ((Rebuild_Ptr == nullptr) || (*root)->left_son_ptr != *Rebuild_Ptr){
Add_by_range(&((*root)->left_son_ptr), boxpoint, allow_rebuild);
} else {
// 如果当前节点的左子节点是重建指针，需要加锁，避免多线程竞争
pthread_mutex_lock(&working_flag_mutex);
// 向左子树递归添加范围框，不允许重建
Add_by_range(&((*root)->left_son_ptr), boxpoint, false);
// 如果需要重建，将操作日志加入重建日志队列中
if (rebuild_flag){
pthread_mutex_lock(&rebuild_logger_mutex_lock);
Rebuild_Logger.push(add_box_log);
pthread_mutex_unlock(&rebuild_logger_mutex_lock);
}
pthread_mutex_unlock(&working_flag_mutex);
}
// 如果重建指针为空，或者当前节点的右子节点不是重建指针，则向右子树递归添加范围框
if ((Rebuild_Ptr == nullptr) || (*root)->right_son_ptr != *Rebuild_Ptr){
Add_by_range(&((*root)->right_son_ptr), boxpoint, allow_rebuild);
} else {
// 如果当前节点的右子节点是重建指针，需要加锁，避免多线程竞争
pthread_mutex_lock(&working_flag_mutex);
// 向右子树递归添加范围框，不允许重建
Add_by_range(&((*root)->right_son_ptr), boxpoint, false);
if (rebuild_flag){
// 如果需要重建，将操作日志加入重建日志队列中
pthread_mutex_lock(&rebuild_logger_mutex_lock);
Rebuild_Logger.push(add_box_log);
pthread_mutex_unlock(&rebuild_logger_mutex_lock);
}
pthread_mutex_unlock(&working_flag_mutex);
}
// 更新当前节点的信息
Update(*root);
// 如果重建指针不为空，且当前节点是重建指针，且当前节点的子节点数小于多线程重建的点数，则重建指针置为空
if (Rebuild_Ptr != nullptr && *Rebuild_Ptr == *root && (*root)->TreeSize < Multi_Thread_Rebuild_Point_Num) Rebuild_Ptr = nullptr;
bool need_rebuild = allow_rebuild & Criterion_Check((*root));
// 根据是否允许重建和当前节点的分裂准则判断是否需要重建
if (need_rebuild) Rebuild(root);
// 标记当前节点处理结束
if ((*root) != nullptr) (*root)->working_flag = false;
return;
}1.3 ikd-Tree删除节点
在ikd-Tree中，这个操作叫作 box-wise delete，顾名思义，输入一个立方体区域给ikd-Tree，ikd-Tree帮你删除这个区域内的所有点。
box-wise delete 结合了range信息和 lazy delete 策略，以下图为例：红/绿/蓝色的框代表不同节点的range范围，灰色框代表我们期望删除的区域。
做删除时，从根节点开始，在每个节点处首先判断节点自身是否在灰色区域内，若是则标记删除；然后，判断两个子节点的range是否与删除区域有交叉，若无则直接剪枝，若有则重复上述过程；直至搜索完所有的子节点，完成所有被删除节点的标记删除。
// 工具属性，从根节点开始向下递归搜索，删除（仅标记）所有被Box包含的节点。
template <typename PointType>
int KD_TREE<PointType>::Delete_by_range(KD_TREE_NODE ** root, BoxPointType boxpoint,
bool allow_rebuild, bool is_downsample)
{
if ((*root) == nullptr || (*root)->tree_deleted) return 0;// 如果当前节点为空或已被删除，则直接返回0
(*root)->working_flag = true;// 标记当前节点正在被处理
Push_Down(*root);// 向下更新当前节点的信息
int tmp_counter = 0;// 临时计数器，记录被删除的节点数
// 当且仅当两个空间有交叉时，才有继续的必要。
if (boxpoint.vertex_max[0] <= (*root)->node_range_x[0] || boxpoint.vertex_min[0] > (*root)->node_range_x[1]) return 0;
if (boxpoint.vertex_max[1] <= (*root)->node_range_y[0] || boxpoint.vertex_min[1] > (*root)->node_range_y[1]) return 0;
if (boxpoint.vertex_max[2] <= (*root)->node_range_z[0] || boxpoint.vertex_min[2] > (*root)->node_range_z[1]) return 0;
// 当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])
{
(*root)->tree_deleted = true;// 将该节点标记为已删除
(*root)->point_deleted = true;
(*root)->need_push_down_to_left = true;
(*root)->need_push_down_to_right = true;
tmp_counter = (*root)->TreeSize - (*root)->invalid_point_num;
(*root)->invalid_point_num = (*root)->TreeSize;
if (is_downsample){
(*root)->tree_downsample_deleted = true;
(*root)->point_downsample_deleted = true;
(*root)->down_del_num = (*root)->TreeSize;
}
return tmp_counter;
}
// 如果当前节点的point被Box包含，标记删除该point。
if (!(*root)->point_deleted &&
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)
{
(*root)->point_deleted = true;
tmp_counter += 1;
if (is_downsample) (*root)->point_downsample_deleted = true;
}
//
Operation_Logger_Type delete_box_log;
// struct timespec Timeout; // wgh: unused variable.
if (is_downsample) delete_box_log.op = DOWNSAMPLE_DELETE;
else delete_box_log.op = DELETE_BOX;
delete_box_log.boxpoint = boxpoint;
// 左子树递归删除
if ((Rebuild_Ptr == nullptr) || (*root)->left_son_ptr != *Rebuild_Ptr){
tmp_counter += Delete_by_range(&((*root)->left_son_ptr), boxpoint, allow_rebuild, is_downsample);
}
else {
pthread_mutex_lock(&working_flag_mutex);
tmp_counter += Delete_by_range(&((*root)->left_son_ptr), boxpoint, false, is_downsample);
if (rebuild_flag){
pthread_mutex_lock(&rebuild_logger_mutex_lock);
Rebuild_Logger.push(delete_box_log);
pthread_mutex_unlock(&rebuild_logger_mutex_lock);
}
pthread_mutex_unlock(&working_flag_mutex);
}
// 右子树递归删除
if ((Rebuild_Ptr == nullptr) || (*root)->right_son_ptr != *Rebuild_Ptr){
// 在重建节点的子树中，暂时不允许进行重建操作，所以设置allow_rebuild为false
tmp_counter += Delete_by_range(&((*root)->right_son_ptr), boxpoint, allow_rebuild, is_downsample);
}
else {
pthread_mutex_lock(&working_flag_mutex);
tmp_counter += Delete_by_range(&((*root)->right_son_ptr), boxpoint, false, is_downsample);
if (rebuild_flag){
pthread_mutex_lock(&rebuild_logger_mutex_lock);
Rebuild_Logger.push(delete_box_log);
pthread_mutex_unlock(&rebuild_logger_mutex_lock);
}
pthread_mutex_unlock(&working_flag_mutex);
}
Update(*root); // 更新当前节点信息（自下而上更新）
if (Rebuild_Ptr != nullptr &&
*Rebuild_Ptr == *root &&
(*root)->TreeSize < Multi_Thread_Rebuild_Point_Num) Rebuild_Ptr = nullptr;
// 检查是否需要re-balancing当前子树。
bool need_rebuild = allow_rebuild & Criterion_Check((*root));
if (need_rebuild) Rebuild(root);
if ((*root) != nullptr) (*root)->working_flag = false;
return tmp_counter;
}2. ikd-Tree的平衡性判据
实现动态再平衡的前提条件是，首先要知道哪些 (sub)tree 是不平衡的。因此，ikd-Tree 提出了一个准则来随时判断任何一个 (sub)tree 平衡与否。这个准则包含两个小准则，如下。
其中， 啊-balanced准则描述的是我们最大能够容忍一个subtree的左右两侧有多不平衡，否则就会被执行重构； a-deleted描述的是我们最多能够容忍一个subtree中有多大比例“被标记为删除、但是还没有事实删除的点“，否则就会被执行重构。
当且仅当两项准则都符合时，才会认为一个 (sub)tree 是平衡的。
// 判断当前节点是否需要进行重建
template <typename PointType>
bool KD_TREE<PointType>::Criterion_Check(KD_TREE_NODE * root){
if (root->TreeSize <= Minimal_Unbalanced_Tree_Size){ // 如果当前节点的大小小于等于Minimal_Unbalanced_Tree_Size，则不需要进行重建
return false;
}
float balance_evaluation = 0.0f;
float delete_evaluation = 0.0f;
KD_TREE_NODE * son_ptr = root->left_son_ptr;
if (son_ptr == nullptr) son_ptr = root->right_son_ptr;
delete_evaluation = float(root->invalid_point_num)/ root->TreeSize;// 计算无效点所占比例
balance_evaluation = float(son_ptr->TreeSize) / (root->TreeSize-1); // 计算左子树节点数占比
if (delete_evaluation > delete_criterion_param){//如果无效点比例超过delete_criterion_param，则需要进行重建
return true;
}
if (balance_evaluation > balance_criterion_param || balance_evaluation < 1-balance_criterion_param){
// 如果左子树节点数占比超过balance_criterion_param或低于1-balance_criterion_param，则需要进行重建
return true;
}
return false;
}3. ikd-Tree的再平衡算法
3.1 再平衡算法
当ikd-Tree通过平衡性判据发现某一个(sub)tree不平衡时，就会触发对该(sub)tree的重建，以实现再平衡。这里又分为两种情况，如果该(sub)tree规模很小，重建时间花销可以忽略，就在主线程中直接重建，重建过程如下图所示。
简要来说，重建时先把(sub)tree的所有节点打乱重排，在这一过程中会拿掉标记删除的点（Flatten）；然后，对剩下的点按中值点法执行常规的kdtree构建过程（Build）；构建完成后，把新的子树替换到原来的位置，完成。
// 重建KD-Tree
template <typename PointType>
void KD_TREE<PointType>::Rebuild(KD_TREE_NODE ** root){
KD_TREE_NODE * father_ptr;
// 如果当前节点的大小大于等于Multi_Thread_Rebuild_Point_Num，则使用多线程重建
if ((*root)->TreeSize >= Multi_Thread_Rebuild_Point_Num) {
if (!pthread_mutex_trylock(&rebuild_ptr_mutex_lock)){
// 如果当前节点是需要进行重建的节点中最大的一个，则更新重建指针
if (Rebuild_Ptr == nullptr || ((*root)->TreeSize > (*Rebuild_Ptr)->TreeSize)) {
Rebuild_Ptr = root;
}
pthread_mutex_unlock(&rebuild_ptr_mutex_lock);
}
} else {
father_ptr = (*root)->father_ptr;
// int size_rec = (*root)->TreeSize; // wgh: unused variable.
PCL_Storage.clear();// 将当前节点及其子节点的所有点按顺序存储在PCL_Storage中
flatten(*root, PCL_Storage, DELETE_POINTS_REC); // 删除当前节点及其子节点
delete_tree_nodes(root);
BuildTree(root, 0, PCL_Storage.size()-1, PCL_Storage); // 构建新的KD-Tree
if (*root != nullptr) (*root)->father_ptr = father_ptr; // 更新新的节点的父节点指针
if (*root == Root_Node) STATIC_ROOT_NODE->left_son_ptr = *root;// 如果当前节点是根节点，则更新STATIC_ROOT_NODE的左子树指针
}
return;
}
template <typename PointType>
// 该函数用于将整个KD树展平为一个一维向量，存储在PointVector中
void KD_TREE<PointType>::flatten(KD_TREE_NODE * root, PointVector &Storage, delete_point_storage_set storage_type){
if (root == nullptr) return; // 如果根节点为空，则直接返回
Push_Down(root);// 向下推树
if (!root->point_deleted) {// 如果根节点未被删除，则将其加入到Storage中
Storage.push_back(root->point);
}
// 递归处理左右子树
flatten(root->left_son_ptr, Storage, storage_type);
flatten(root->right_son_ptr, Storage, storage_type);
// 根据存储类型，将被删除的点存储在相应的容器
switch (storage_type)
{
case NOT_RECORD:
break;
case DELETE_POINTS_REC: // 如果storage_type为DELETE_POINTS_REC，则将被删除的点存储在KD_TREE类的Points_deleted容器中
if (root->point_deleted && !root->point_downsample_deleted) {
Points_deleted.push_back(root->point);
}
break;
case MULTI_THREAD_REC:// 如果storage_type为MULTI_THREAD_REC，则将被删除的点存储在KD_TREE类的Multithread_Points_deleted容器中
if (root->point_deleted && !root->point_downsample_deleted) {
Multithread_Points_deleted.push_back(root->point);
}
break;
default:
break;
}
return;
}3.2 多线程重建
如果需要重建的(sub)tree规模很大，重建时间花销不可忽略；如果在主线程中执行重建，就会导致ikd-Tree一直被占用，外部的SLAM/LIO算法无法访问ikd-Tree执行查询或增删操作 —— 导致算法阻塞。
针对这种情况，ikd-Tree 采取了一种双线程策略，如下图所示：耗时的重建过程在额外的线程中执行，主线程仍旧对外提供查询和增删服务，其中涉及到“正在重建的(sub)tree”的增删操作会被额外缓存到一个叫作Rebuild_Logger的容器中；待额外线程中完成了重建后，会从Rebuild_Logger把增删操作“补作业”到新的(sub)tree上，这样就确保了新的(sub)tree没有漏掉任何过程；最后，把新子树替换到相应位置，完成。
// 多线程重建
template <typename PointType>
void KD_TREE<PointType>::multi_thread_rebuild(){
bool terminated = false;
KD_TREE_NODE * father_ptr;
// KD_TREE_NODE ** new_node_ptr; // wgh: unused variable.
pthread_mutex_lock(&termination_flag_mutex_lock);
terminated = termination_flag;
pthread_mutex_unlock(&termination_flag_mutex_lock);
while (!terminated){
pthread_mutex_lock(&rebuild_ptr_mutex_lock);
pthread_mutex_lock(&working_flag_mutex);
// 判断是否有未执行的操作，如果有则打印错误信息
if (Rebuild_Ptr != nullptr ){
/* Traverse and copy */
if (!Rebuild_Logger.empty()){
printf("\n\n\n\n\n\n\n\n\n\n\n ERROR!!! \n\n\n\n\n\n\n\n\n");
}
// 设置重建标志
rebuild_flag = true;
// 获取原有根节点的信息
if (*Rebuild_Ptr == Root_Node) {
Treesize_tmp = Root_Node->TreeSize;
Validnum_tmp = Root_Node->TreeSize - Root_Node->invalid_point_num;
alpha_bal_tmp = Root_Node->alpha_bal;
alpha_del_tmp = Root_Node->alpha_del;
}
KD_TREE_NODE * old_root_node = (*Rebuild_Ptr); // 清空重建点集
father_ptr = (*Rebuild_Ptr)->father_ptr;
PointVector ().swap(Rebuild_PCL_Storage);
// Lock Search
// 上锁，等待Search线程结束
pthread_mutex_lock(&search_flag_mutex);
while (search_mutex_counter != 0){
pthread_mutex_unlock(&search_flag_mutex);
usleep(1);
pthread_mutex_lock(&search_flag_mutex);
}
search_mutex_counter = -1;
pthread_mutex_unlock(&search_flag_mutex);
// Lock deleted points cache
// 上锁，等待点删除线程结束
pthread_mutex_lock(&points_deleted_rebuild_mutex_lock);
flatten(*Rebuild_Ptr, Rebuild_PCL_Storage, MULTI_THREAD_REC);// 将当前节点及其子节点的所有点按顺序存储在Rebuild_PCL_Storage中
// Unlock deleted points cache
// 解锁
pthread_mutex_unlock(&points_deleted_rebuild_mutex_lock);
// Unlock Search
// 解锁，通知Search线程可以继续运行
pthread_mutex_lock(&search_flag_mutex);
search_mutex_counter = 0;
pthread_mutex_unlock(&search_flag_mutex);
pthread_mutex_unlock(&working_flag_mutex);
/* Rebuild and update missed operations*/
Operation_Logger_Type Operation;
KD_TREE_NODE * new_root_node = nullptr;
// 如果重建点集不为空，则进行重建操作，并将未执行的操作应用到新的树上
if (int(Rebuild_PCL_Storage.size()) > 0){ // 构建新的KD-Tree
BuildTree(&new_root_node, 0, Rebuild_PCL_Storage.size()-1, Rebuild_PCL_Storage);// Rebuild已经完成。将被阻塞的操作更新到新树中
// Rebuild has been done. Updates the blocked operations into the new tree
pthread_mutex_lock(&working_flag_mutex);
pthread_mutex_lock(&rebuild_logger_mutex_lock);
int tmp_counter = 0;
while (!Rebuild_Logger.empty()){
Operation = Rebuild_Logger.front();
max_queue_size = max(max_queue_size, Rebuild_Logger.size());
Rebuild_Logger.pop();
pthread_mutex_unlock(&rebuild_logger_mutex_lock);
pthread_mutex_unlock(&working_flag_mutex);
run_operation(&new_root_node, Operation);
tmp_counter ++;
if (tmp_counter % 10 == 0) usleep(1);
pthread_mutex_lock(&working_flag_mutex);
pthread_mutex_lock(&rebuild_logger_mutex_lock);
}
pthread_mutex_unlock(&rebuild_logger_mutex_lock);
}
/* Replace to original tree*/
// 将新的树替换原有树
// pthread_mutex_lock(&working_flag_mutex);
pthread_mutex_lock(&search_flag_mutex);
while (search_mutex_counter != 0){
pthread_mutex_unlock(&search_flag_mutex);
usleep(1);
pthread_mutex_lock(&search_flag_mutex);// 根据原节点的父节点指针，将原节点的左/右子树指针指向新节点
}
search_mutex_counter = -1;
pthread_mutex_unlock(&search_flag_mutex);
if (father_ptr->left_son_ptr == *Rebuild_Ptr) {
father_ptr->left_son_ptr = new_root_node;
} else if (father_ptr->right_son_ptr == *Rebuild_Ptr){
father_ptr->right_son_ptr = new_root_node;
} else {
throw "Error: Father ptr incompatible with current node\n";
}
if (new_root_node != nullptr) new_root_node->father_ptr = father_ptr;
(*Rebuild_Ptr) = new_root_node;
// int valid_old = old_root_node->TreeSize-old_root_node->invalid_point_num; // wgh: unused variable.
// int valid_new = new_root_node->TreeSize-new_root_node->invalid_point_num; // wgh: unused variable.
if (father_ptr == STATIC_ROOT_NODE) Root_Node = STATIC_ROOT_NODE->left_son_ptr;
KD_TREE_NODE * update_root = *Rebuild_Ptr;
// 更新从新的根节点到原有根节点路径上的节点
while (update_root != nullptr && update_root != Root_Node){
update_root = update_root->father_ptr;
if (update_root->working_flag) break;
if (update_root == update_root->father_ptr->left_son_ptr && update_root->father_ptr->need_push_down_to_left) break;
if (update_root == update_root->father_ptr->right_son_ptr && update_root->father_ptr->need_push_down_to_right) break;
Update(update_root);
}
pthread_mutex_lock(&search_flag_mutex);
search_mutex_counter = 0;
pthread_mutex_unlock(&search_flag_mutex);
Rebuild_Ptr = nullptr;
pthread_mutex_unlock(&working_flag_mutex);
// 解除重建标志
rebuild_flag = false;
/* Delete discarded tree nodes */
// 删除原有树中的废弃节点
delete_tree_nodes(&old_root_node);
} else {
pthread_mutex_unlock(&working_flag_mutex);
}
pthread_mutex_unlock(&rebuild_ptr_mutex_lock);
pthread_mutex_lock(&termination_flag_mutex_lock);
terminated = termination_flag;
pthread_mutex_unlock(&termination_flag_mutex_lock);
usleep(100);
}
printf("Rebuild thread terminated normally\n");
}
4. 参考链接
https://zhuanlan.zhihu.com/p/529926254
评论（0）
您还未登录，请登录后发表或查看评论