fastlio2的主要创新点：
引入ikd_tree增加配准的效率，修改卡尔曼增益减少卡尔曼的增益求逆的时间。本文主要目的是记录一下增量kd树的理解。
增量kd树的数据结构：
和普通的kd树类似，都需要点，划分轴，左右子节点，不过增量kd树中为了支持盒操作，增加了range，以及是否删除该点以及该点下面的子树。
Struct TreeNode:
PointType point;
TreeNode* leftchild,rightchild;
int axis;
int treesize, invalidnum;
bool deleted, treedeleted;
CuboidVertices range;
end
增量kd树构建树算法：
这个算法主要是空树时构建kd树
//根据点云建立ikd树
void KD_TREE::Build(PointVector point_cloud){
if (Root_Node != nullptr){
delete_tree_nodes(&Root_Node);//如果有root需要删除以前的root
}
if (point_cloud.size() == 0) return;
STATIC_ROOT_NODE = new KD_TREE_NODE;
InitTreeNode(STATIC_ROOT_NODE);//重建root
BuildTree(&STATIC_ROOT_NODE->left_son_ptr, 0, point_cloud.size()-1, point_cloud);//建立kdtree
Update(STATIC_ROOT_NODE);//更新root的属性
STATIC_ROOT_NODE->TreeSize = 0;
Root_Node = STATIC_ROOT_NODE->left_son_ptr;
}
里面会调用BuildTree函数，这个函数建树的过程与构建有序二叉树的过程类似，取中间的节点作为节点值进行递归。
void KD_TREE::BuildTree(KD_TREE_NODE ** root, int l, int r, PointVector & Storage){//创建以root为根节点的kd树
if (l>r) return;
*root = new KD_TREE_NODE;//给**root分配内存
InitTreeNode(*root);
int mid = (l+r)>>1;
int div_axis = 0;
int i;
// Find the best division Axis
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);
}
// Select the longest dimension as division axis
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;//为什么没有第一维？因为默认为第一维
// Divide by the division axis and recursively build.
(*root)->division_axis = div_axis;
/*********************************************
* nth_element(first, nth, last, compare)
求[first, last]这个区间中第n大小的元素，如果参数加入了compare函数，就按compare函数的方式比较。
这一段就是找切割维度的中位数
******************************************************/
switch (div_axis)
{
case 0:
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;
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)); //更新node的属性
return;
}
Update函数的主要目的是为了更新该接点以及它子树的点的范围以及size，还有他的del，bal，用来判断树是否平衡，是否需要重建。这个函数再原论文中是pullup操作。
void KD_TREE::Update(KD_TREE_NODE * root){//更新ikd树,更新节点以及他的子树的范围，size，以及所有节点的del bal
KD_TREE_NODE * left_son_ptr = root->left_son_ptr;
KD_TREE_NODE * right_son_ptr = root->right_son_ptr;
float tmp_range_x[2] = {INFINITY, -INFINITY};
float tmp_range_y[2] = {INFINITY, -INFINITY};
float tmp_range_z[2] = {INFINITY, -INFINITY};
// Update Tree Size
if (left_son_ptr != nullptr && right_son_ptr != nullptr){
root->TreeSize = left_son_ptr->TreeSize + right_son_ptr->TreeSize + 1;//加一是因为自己的大小
root->invalid_point_num = left_son_ptr->invalid_point_num + right_son_ptr->invalid_point_num + (root->point_deleted? 1:0);
root->down_del_num = left_son_ptr->down_del_num + right_son_ptr->down_del_num + (root->point_downsample_deleted? 1:0);
root->tree_downsample_deleted = left_son_ptr->tree_downsample_deleted & right_son_ptr->tree_downsample_deleted & root->point_downsample_deleted;
root->tree_deleted = left_son_ptr->tree_deleted && right_son_ptr->tree_deleted && root->point_deleted;
if (root->tree_deleted || (!left_son_ptr->tree_deleted && !right_son_ptr->tree_deleted && !root->point_deleted)){
tmp_range_x[0] = min(min(left_son_ptr->node_range_x[0],right_son_ptr->node_range_x[0]),root->point.x);
tmp_range_x[1] = max(max(left_son_ptr->node_range_x[1],right_son_ptr->node_range_x[1]),root->point.x);
tmp_range_y[0] = min(min(left_son_ptr->node_range_y[0],right_son_ptr->node_range_y[0]),root->point.y);
tmp_range_y[1] = max(max(left_son_ptr->node_range_y[1],right_son_ptr->node_range_y[1]),root->point.y);
tmp_range_z[0] = min(min(left_son_ptr->node_range_z[0],right_son_ptr->node_range_z[0]),root->point.z);
tmp_range_z[1] = max(max(left_son_ptr->node_range_z[1],right_son_ptr->node_range_z[1]),root->point.z);
} else {
if (!left_son_ptr->tree_deleted){
tmp_range_x[0] = min(tmp_range_x[0], left_son_ptr->node_range_x[0]);
tmp_range_x[1] = max(tmp_range_x[1], left_son_ptr->node_range_x[1]);
tmp_range_y[0] = min(tmp_range_y[0], left_son_ptr->node_range_y[0]);
tmp_range_y[1] = max(tmp_range_y[1], left_son_ptr->node_range_y[1]);
tmp_range_z[0] = min(tmp_range_z[0], left_son_ptr->node_range_z[0]);
tmp_range_z[1] = max(tmp_range_z[1], left_son_ptr->node_range_z[1]);
}
if (!right_son_ptr->tree_deleted){
tmp_range_x[0] = min(tmp_range_x[0], right_son_ptr->node_range_x[0]);
tmp_range_x[1] = max(tmp_range_x[1], right_son_ptr->node_range_x[1]);
tmp_range_y[0] = min(tmp_range_y[0], right_son_ptr->node_range_y[0]);
tmp_range_y[1] = max(tmp_range_y[1], right_son_ptr->node_range_y[1]);
tmp_range_z[0] = min(tmp_range_z[0], right_son_ptr->node_range_z[0]);
tmp_range_z[1] = max(tmp_range_z[1], right_son_ptr->node_range_z[1]);
}
if (!root->point_deleted){
tmp_range_x[0] = min(tmp_range_x[0], root->point.x);
tmp_range_x[1] = max(tmp_range_x[1], root->point.x);
tmp_range_y[0] = min(tmp_range_y[0], root->point.y);
tmp_range_y[1] = max(tmp_range_y[1], root->point.y);
tmp_range_z[0] = min(tmp_range_z[0], root->point.z);
tmp_range_z[1] = max(tmp_range_z[1], root->point.z);
}
}
} else if (left_son_ptr != nullptr){
root->TreeSize = left_son_ptr->TreeSize + 1;
root->invalid_point_num = left_son_ptr->invalid_point_num + (root->point_deleted?1:0);
root->down_del_num = left_son_ptr->down_del_num + (root->point_downsample_deleted?1:0);
root->tree_downsample_deleted = left_son_ptr->tree_downsample_deleted & root->point_downsample_deleted;
root->tree_deleted = left_son_ptr->tree_deleted && root->point_deleted;
if (root->tree_deleted || (!left_son_ptr->tree_deleted && !root->point_deleted)){
tmp_range_x[0] = min(left_son_ptr->node_range_x[0],root->point.x);
tmp_range_x[1] = max(left_son_ptr->node_range_x[1],root->point.x);
tmp_range_y[0] = min(left_son_ptr->node_range_y[0],root->point.y);
tmp_range_y[1] = max(left_son_ptr->node_range_y[1],root->point.y);
tmp_range_z[0] = min(left_son_ptr->node_range_z[0],root->point.z);
tmp_range_z[1] = max(left_son_ptr->node_range_z[1],root->point.z);
} else {
if (!left_son_ptr->tree_deleted){
tmp_range_x[0] = min(tmp_range_x[0], left_son_ptr->node_range_x[0]);
tmp_range_x[1] = max(tmp_range_x[1], left_son_ptr->node_range_x[1]);
tmp_range_y[0] = min(tmp_range_y[0], left_son_ptr->node_range_y[0]);
tmp_range_y[1] = max(tmp_range_y[1], left_son_ptr->node_range_y[1]);
tmp_range_z[0] = min(tmp_range_z[0], left_son_ptr->node_range_z[0]);
tmp_range_z[1] = max(tmp_range_z[1], left_son_ptr->node_range_z[1]);
}
if (!root->point_deleted){
tmp_range_x[0] = min(tmp_range_x[0], root->point.x);
tmp_range_x[1] = max(tmp_range_x[1], root->point.x);
tmp_range_y[0] = min(tmp_range_y[0], root->point.y);
tmp_range_y[1] = max(tmp_range_y[1], root->point.y);
tmp_range_z[0] = min(tmp_range_z[0], root->point.z);
tmp_range_z[1] = max(tmp_range_z[1], root->point.z);
}
}
} else if (right_son_ptr != nullptr){
root->TreeSize = right_son_ptr->TreeSize + 1;
root->invalid_point_num = right_son_ptr->invalid_point_num + (root->point_deleted? 1:0);
root->down_del_num = right_son_ptr->down_del_num + (root->point_downsample_deleted? 1:0);
root->tree_downsample_deleted = right_son_ptr->tree_downsample_deleted & root->point_downsample_deleted;
root->tree_deleted = right_son_ptr->tree_deleted && root->point_deleted;
if (root->tree_deleted || (!right_son_ptr->tree_deleted && !root->point_deleted)){
tmp_range_x[0] = min(right_son_ptr->node_range_x[0],root->point.x);
tmp_range_x[1] = max(right_son_ptr->node_range_x[1],root->point.x);
tmp_range_y[0] = min(right_son_ptr->node_range_y[0],root->point.y);
tmp_range_y[1] = max(right_son_ptr->node_range_y[1],root->point.y);
tmp_range_z[0] = min(right_son_ptr->node_range_z[0],root->point.z);
tmp_range_z[1] = max(right_son_ptr->node_range_z[1],root->point.z);
} else {
if (!right_son_ptr->tree_deleted){
tmp_range_x[0] = min(tmp_range_x[0], right_son_ptr->node_range_x[0]);
tmp_range_x[1] = max(tmp_range_x[1], right_son_ptr->node_range_x[1]);
tmp_range_y[0] = min(tmp_range_y[0], right_son_ptr->node_range_y[0]);
tmp_range_y[1] = max(tmp_range_y[1], right_son_ptr->node_range_y[1]);
tmp_range_z[0] = min(tmp_range_z[0], right_son_ptr->node_range_z[0]);
tmp_range_z[1] = max(tmp_range_z[1], right_son_ptr->node_range_z[1]);
}
if (!root->point_deleted){
tmp_range_x[0] = min(tmp_range_x[0], root->point.x);
tmp_range_x[1] = max(tmp_range_x[1], root->point.x);
tmp_range_y[0] = min(tmp_range_y[0], root->point.y);
tmp_range_y[1] = max(tmp_range_y[1], root->point.y);
tmp_range_z[0] = min(tmp_range_z[0], root->point.z);
tmp_range_z[1] = max(tmp_range_z[1], root->point.z);
}
}
} else {
root->TreeSize = 1;
root->invalid_point_num = (root->point_deleted? 1:0);
root->down_del_num = (root->point_downsample_deleted? 1:0);
root->tree_downsample_deleted = root->point_downsample_deleted;
root->tree_deleted = root->point_deleted;
tmp_range_x[0] = root->point.x;
tmp_range_x[1] = root->point.x;
tmp_range_y[0] = root->point.y;
tmp_range_y[1] = root->point.y;
tmp_range_z[0] = root->point.z;
tmp_range_z[1] = root->point.z;
}
memcpy(root->node_range_x,tmp_range_x,sizeof(tmp_range_x));
memcpy(root->node_range_y,tmp_range_y,sizeof(tmp_range_y));
memcpy(root->node_range_z,tmp_range_z,sizeof(tmp_range_z));
if (left_son_ptr != nullptr) left_son_ptr -> father_ptr = root;
if (right_son_ptr != nullptr) right_son_ptr -> father_ptr = root;
if (root == Root_Node && root->TreeSize > 3){
KD_TREE_NODE * son_ptr = root->left_son_ptr;
if (son_ptr == nullptr) son_ptr = root->right_son_ptr;
float tmp_bal = float(son_ptr->TreeSize) / (root->TreeSize-1);
root->alpha_del = float(root->invalid_point_num)/ root->TreeSize;
root->alpha_bal = (tmp_bal>=0.5-EPSS)?tmp_bal:1-tmp_bal;
}
return;
}
上述伪代码如下：
盒操作：
盒更新：
盒更新的伪代码如下：
这个函数我的理解是可以便利的对一整个范围的点进行更新，对应代码中的函数是：
//在范围内添加点
void KD_TREE::Add_by_range(KD_TREE_NODE ** root, BoxPointType boxpoint, bool allow_rebuild){
if ((*root) == nullptr) return;
(*root)->working_flag = true;
Push_Down(*root);
//论文中的判断部分，判断增加的范围是否与root的range的包含关系
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;
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;
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;
}
盒删除：
盒删除的伪代码如下：
这段主要的含义是比较盒与点的范围，如果是包含或者不相关就不处理，如果是相交则对删除的地方做递归判断是否需要删除。最后判断是否需要重建。
//删除整个范围内的点并返回删除数量
int KD_TREE::Delete_by_range(KD_TREE_NODE ** root, BoxPointType boxpoint, bool allow_rebuild, bool is_downsample){
if ((*root) == nullptr || (*root)->tree_deleted) return 0;
(*root)->working_flag = true;
Push_Down(*root);
int tmp_counter = 0;//前面这些都是判断立方体和t的距离关系
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;
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;
}
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;
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){
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);//update以后判断是否需要重建
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 tmp_counter;
}
需要注意的是论文中有提到需要删除点的时候并不是马上删除而是改变tree_deleted以及point_deleted这两个变量。
点操作：
点插入：
插入点的伪代码如下：
对应的代码中的部分：
//论文中的insert操作函数，在root树上添加point
void KD_TREE::Add_by_point(KD_TREE_NODE ** root, PointType point, bool allow_rebuild, int father_axis){
if (*root == nullptr){//如果是空的root则进行直接添加，找一个空的位置添加point
*root = new KD_TREE_NODE;
InitTreeNode(*root);
(*root)->point = point;
(*root)->division_axis = (father_axis + 1) % 3;
Update(*root);
return;
}
(*root)->working_flag = true;
Operation_Logger_Type add_log;
struct timespec Timeout;
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 {
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 { //在右边和在左边一样
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);
}
}
Update(*root);//更新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;
}
需要注意的是这里和前面建树的坐标轴选择思路不一样，这里用的轮换，前面用的方差选轴。
点删除
点删除的函数和一般的二叉树的删除节点思路很像，根据对应的大小判断往左还是友遍历。
//删除一个点
void KD_TREE::Delete_by_point(KD_TREE_NODE ** root, PointType point, bool allow_rebuild){
if ((*root) == nullptr || (*root)->tree_deleted) return;
(*root)->working_flag = true;
Push_Down(*root);
if (same_point((*root)->point, point) && !(*root)->point_deleted) {
(*root)->point_deleted = true;
(*root)->invalid_point_num += 1;
if ((*root)->invalid_point_num == (*root)->TreeSize) (*root)->tree_deleted = true;
return;
}
Operation_Logger_Type delete_log;
struct timespec Timeout;
delete_log.op = DELETE_POINT;
delete_log.point = point;
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){
Delete_by_point(&(*root)->left_son_ptr, point, allow_rebuild);
} else {
pthread_mutex_lock(&working_flag_mutex);
Delete_by_point(&(*root)->left_son_ptr, point,false);
if (rebuild_flag){
pthread_mutex_lock(&rebuild_logger_mutex_lock);
Rebuild_Logger.push(delete_log);
pthread_mutex_unlock(&rebuild_logger_mutex_lock);
}
pthread_mutex_unlock(&working_flag_mutex);
}
} else {
if ((Rebuild_Ptr == nullptr) || (*root)->right_son_ptr != *Rebuild_Ptr){
Delete_by_point(&(*root)->right_son_ptr, point, allow_rebuild);
} else {
pthread_mutex_lock(&working_flag_mutex);
Delete_by_point(&(*root)->right_son_ptr, point, false);
if (rebuild_flag){
pthread_mutex_lock(&rebuild_logger_mutex_lock);
Rebuild_Logger.push(delete_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;
}
kd_tree平衡标准和重建
1.判断是否平衡主要取决于下面两个公式，s代表树的尺寸，如果阿尔法是在0.5-1之间的时候，则认为是平衡的。
2.这里I表示无效的点，当无效的点小于树的尺寸则认为是平衡。
重建
当判断某节点的子树下不平衡的时候，则进行重建。重建会根据图的大小判断是否需要采用多线程提速。多线程重建的伪代码如下：
几种KD树的比较
参考文献
[1] ikd-Tree: An Incremental K-D Tree for Robotic Applications.
[2] FAST-LIO2: Fast Direct LiDAR-inertial Odometry
382
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
