引言
本文结合代码对ikdtree进行介绍，ikdtree是fastlio中用来管理地图的数据结构。kdtree是一种常用的树形结构。ikdtree在kdtree的基础上增加了较为快速的添加删除及重构树的操作，以便于对地图的动态管理。以下会结合fastlio中ikd_Tree.h和ikd_Tree.cpp两个文件进行介绍。
基础
下面对基础的数据结构和算法进行介绍，是后续了解ikdtree的基础
队列（queue）
在ikd_Tree.h中，定义了ikdtree类，ikdtree类中，定义了MANUAL_Q结构，代码如下。
这是一个比较标准的队列应用，队列中存储的元素是Operation_Logger_Type，实际上是针对树的一些操作，这些操作如果因为多线程阻塞的原因没有被执行，就会保存到队列中，等多线程释放后，再对新的树进行对应的操作。
class MANUAL_Q{private:int head = 0, tail = 0, counter = 0;Operation_Logger_Type q[Q_LEN]; // 一个长度较大的数组，用来存放对ikdtree的操作bool is_empty;public:void pop() // 扔出一个元素，注意这里只是改变了指针的位置，没有清理元素本身{if (counter == 0) // 先判断元素个数return;head++;head %= Q_LEN; // 对长度取模，可以看做一个循环队列counter--;if (counter == 0)is_empty = true;return;}Operation_Logger_Type front() // 拿到当前队列的第一个元素{return q[head];}Operation_Logger_Type back() // 拿到当前队列最后一个元素{return q[tail];}void clear() // 清空队列，实际就是指向位置的初始化{head = 0;tail = 0;counter = 0;is_empty = true;return;}void push(Operation_Logger_Type op) // 队列尾部添加一个元素，也是循环添加{q[tail] = op;counter++;if (is_empty)is_empty = false;tail++;tail %= Q_LEN;}bool empty() // 判断是否为空{return is_empty;}int size() // 返回元素个数{return counter;}};
堆（heap）
代码中采用了max-heap。其中比较难理解的是插入和删除操作。具体可以参考
https://www.geeksforgeeks.org/insertion-and-deletion-in-heaps/
这个链接中的实现方式和ikdtree中实现方式略有不同，但整体思路是一致的。
以下是经过注释的代码
class MANUAL_HEAP{public:MANUAL_HEAP(int max_capacity = 100){cap = max_capacity;heap = new PointType_CMP[max_capacity]; // 最大容量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;return;}private:PointType_CMP *heap;void MoveDown(int heap_index) // 下沉过程{int l = heap_index * 2 + 1; // 找到左子节点的indexPointType_CMP tmp = heap[heap_index]; // 复制出一个当前值while (l < heap_size) // 没有循环超过数组大小，如果heap_size是0，那么直接退出while，放入第一个点{ // 如果heap_size是1，也是直接退出。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; // 然后继续循环}elsebreak;}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; // 直到循环完成}elsebreak;}heap[heap_index] = tmp;return;}int heap_size = 0;int cap = 0;};
kdtree
关于kdtree本身的知识，主要参考Wikipedia：https://en.wikipedia.org/wiki/K-d_tree
这里主要讲kdtree的构建、添加点、删除点和最近邻搜索
kdtree的构建
构建函数的伪代码如下。
主要步骤包括：1. 选择分割轴；2. 沿分割轴排序，并找到中值；3. 构造节点，同时用中值将所有元素分成两个部分，分别是左右子树，进行递归构建。
function kdtree (list of points pointList, int depth)
{// Select axis based on depth so that axis cycles through all valid values// 选择一个分割轴，可以按照最大方差选取，也可以按顺序直接分var int axis := depth mod k;// Sort point list and choose median as pivot element// 将所有点按分割轴排序，并找到其中的中值select median by axis from pointList;// Create node and construct subtree// 构造节点，然后对左右子树进行递归循环构建node.location := median;node.leftChild := kdtree(points in pointList before median, depth+1);node.rightChild := kdtree(points in pointList after median, depth+1);return node;
}
添加元素
添加元素伪代码：
function insert_kdtree (node, point, depth)
{// Base case: If the node is null, create a new node here// 处理递归结束情况，如果是空节点，就新建一个if (node is null){node := create new node;node.location := point;node.leftChild := null;node.rightChild := null;return node;}// Calculate current dimension (axis) based on depth// 选择分割轴var int axis := depth mod k;// Compare the new point with the current node's point along the current axis看看是在当前节点，沿着分割轴的左侧还是右侧if (point[axis] < node.location[axis]){// Recur on the left subtree// 如果是左侧就接着递归左侧insert_kdtree(node.leftChild, point, depth+1);}else{// Recur on the right subtree// 如果是右侧就接着递归右侧insert_kdtree(node.rightChild, point, depth+1);}
}
删除元素
删除kdtree中的元素的方法在Wikipedia中给了两个：
- 将剩下的点直接重建
- 找个点替代这个点，比如从左子树中找沿着当前轴最大的点，或从右子树中找沿着当前轴最小的点。
下面给出方法2的一个伪代码。
function delete_kdtree(node, point, depth)
{// Base case: If node is null, the point is not found// 处理空节点if (node is null)return null;// Calculate current dimension (axis) based on depth// 找到分割轴var int axis := depth mod k;// If the point to be deleted is found at the current node// 如果当前点就是要删除的点if (node.location == point){// Case 1: Node is a leaf node// 如果是叶节点，就直接删if (node.leftChild is null and node.rightChild is null)return null; // Simply remove the leaf node// Case 2: Node has a right child// 如果不是叶节点，从右子树中找一个最小值if (node.rightChild is not null){// Find the minimum point in the right subtree along the current axisvar PointType minPoint := find_min(node.rightChild, axis, depth + 1);// Replace current node with the minimum point foundnode.location := minPoint;// Recursively delete the minimum point from the right subtree// 同时还要递归删除刚刚找到的那个点node.rightChild := delete_kdtree(node.rightChild, minPoint, depth + 1);}// Case 3: Node only has a left child// 或者从左子树中找一个最大值，else if (node.leftChild is not null){// Find the maximum point in the left subtree along the current axisvar PointType maxPoint := find_max(node.leftChild, axis, depth + 1);// Replace current node with the maximum point foundnode.location := maxPoint;// Recursively delete the maximum point from the left subtree// 同时还要递归删除刚刚找到的那个点node.leftChild := delete_kdtree(node.leftChild, maxPoint, depth + 1);}return node;}// Recursively search for the point in the left or right subtree// 如果不是当前点，就接着递归寻找if (point[axis] < node.location[axis]){// Go to left subtreenode.leftChild := delete_kdtree(node.leftChild, point, depth + 1);}else{// Go to right subtreenode.rightChild := delete_kdtree(node.rightChild, point, depth + 1);}return node;
}function find_min(node, axis, depth)
{if (node is null)return null;// Calculate current axisvar int current_axis := depth mod k;// If current axis matches the target axisif (current_axis == axis){// The minimum is in the left subtreeif (node.leftChild is null)return node.location;return find_min(node.leftChild, axis, depth + 1);}// Otherwise, find the minimum across both subtrees// 递归遍历找最小值var PointType leftMin := find_min(node.leftChild, axis, depth + 1);var PointType rightMin := find_min(node.rightChild, axis, depth + 1);// Return the smallest among node, leftMin, and rightMinreturn min(node.location, leftMin, rightMin);
}function find_max(node, axis, depth)
{if (node is null)return null;// Calculate current axisvar int current_axis := depth mod k;// If current axis matches the target axisif (current_axis == axis){// The maximum is in the right subtreeif (node.rightChild is null)return node.location;return find_max(node.rightChild, axis, depth + 1);}// Otherwise, find the maximum across both subtreesvar PointType leftMax := find_max(node.leftChild, axis, depth + 1);var PointType rightMax := find_max(node.rightChild, axis, depth + 1);// Return the largest among node, leftMax, and rightMaxreturn max(node.location, leftMax, rightMax);
}最近邻搜索
Visit(root):d= distance(target, root) // 计算和当前点的距离if d < r: // 比最近的距离还近r= d // 记录这个距离closest= root // 记录这个点if root.left and root.x - target.x < r: // x在这里就是对应root的分割轴，Visit(root.left)if root.right and target.x - root.x < r:Visit(root.right)
多线程锁
pthread_mutex_init：初始化一个互斥锁。
pthread_mutex_lock：以阻塞方式加锁互斥锁。
pthread_mutex_trylock：以非阻塞方式尝试加锁互斥锁。
pthread_mutex_unlock：解锁互斥锁。
pthread_mutex_destroy：销毁一个互斥锁。
ikdtree
接口API
以下从Fastlio的laserMapping.cpp中，选出用到的ikdtree的api。从这里可以看出ikdtree的用法，进而可以推断出ikdtree本身实现所需要包含的功能。
ikdtree是进行点云地图管理的，由于点云数量庞大，并且fastlio仅维护当前位置附近的局部地图，因此，ikdtree需要能快速的完成插入点删除点的动作。
同时，为了完成点云匹配计算，需要ikdtree能完成最近的K个点的查询功能。具体如下：
- 定义：
KD_TREE<PointType> ikdtree;
- 初始化构造：
if(ikdtree.Root_Node == nullptr) // 如果没有根节点，说明没构造{if(feats_down_size > 5){ikdtree.set_downsample_param(filter_size_map_min);feats_down_world->resize(feats_down_size);for(int i = 0; i < feats_down_size; i++){pointBodyToWorld(&(feats_down_body->points[i]), &(feats_down_world->points[i]));}ikdtree.Build(feats_down_world->points); // 给它一串点，构造初始的树}continue;}int featsFromMapNum = ikdtree.validnum(); // 读取一些树的特性kdtree_size_st = ikdtree.size(); // 读取树中节点的个数
- 删除点：
随着车辆的移动，需要删除某些区域的点，这样不至于地图过大，只保留激光雷达附近的局部地图即可
if(cub_needrm.size() > 0) kdtree_delete_counter = ikdtree.Delete_Point_Boxes(cub_needrm);
- 添加点：
随着车辆移动，需要新加入点
add_point_size = ikdtree.Add_Points(PointToAdd, true);ikdtree.Add_Points(PointNoNeedDownsample, false);
- 查询top-K最近点
在进行点云匹配时，会选择最近的K个点，组成平面，进行点面匹配计算
ikdtree.Nearest_Search(point_world, NUM_MATCH_POINTS, points_near, pointSearchSqDis);
主要功能
构造
主要构造过程函数
树的构造过程主要是看BuildTree函数。在rebuild功能中，也会调用这个函数完成数的创建。
/*** 树的构造过程函数，大概分成4部分* 1. 递归结束条件* 2. 根据x,y,z三个轴上的分布情况，选定分割轴* 3. 创建当前点，并且对左右子树递归循环创建* 4. 更新当前节点属性参数
*/
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; // 分配一块新的node内存出来，给root所指向的地址InitTreeNode(*root);int mid = (l + r) >> 1; // 等价于 (l + r) / 2int div_axis = 0;int i;// Find the best division Axisfloat 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 axisfor (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;switch (div_axis) // 这是在进行排序并找到中位数的点{case 0: // nth_element 是c++标准库中的一个函数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]; // 将中位数的点记录到当前node中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)); //更新当前节点的一些属性return;
}
节点属性参数更新函数
/*** Update 函数在 KD-Tree 中的作用是更新每个节点的统计信息和元数据。* 具体来说，它会更新节点的大小（TreeSize）、删除标记（tree_deleted）、空间范围（node_range_x, node_range_y, node_range_z），* 以及父子节点之间的关系。这个函数在构建树时被调用，以确保每个节点的相关信息都是最新的，尤其是在递归创建子树之后。*/
template <typename PointType>
void KD_TREE<PointType>::Update(KD_TREE_NODE *root)
{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 Sizeif (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));float x_L = (root->node_range_x[1] - root->node_range_x[0]) * 0.5;float y_L = (root->node_range_y[1] - root->node_range_y[0]) * 0.5;float z_L = (root->node_range_z[1] - root->node_range_z[0]) * 0.5;root->radius_sq = x_L*x_L + y_L * y_L + z_L * z_L; // 大概计算一个半径的平方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;
}
在构造树的过程中，生成完每个节点的坐标点，以及所有递归子树后，会进行update更新，此时更新的属性都是默认值，都是“不需要删除树”之类的值。此时的树非常“干净”没有什么特别的属性（删除相关属性）。
添加功能
Add_Points函数
里面调用了Add_by_point 和 Delete_by_range函数
template <typename PointType>
int KD_TREE<PointType>::Add_Points(PointVector &PointToAdd, bool downsample_on)
{int NewPointSize = PointToAdd.size(); // 要添加的点的个数int tree_size = size();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 (int i = 0; i < PointToAdd.size(); i++) // 对每个要添加的点都进行循环{if (downsample_switch) // 判断是否进行降采样{ // 如果要进行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;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 (int 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];}}if (Rebuild_Ptr == nullptr || *Rebuild_Ptr != Root_Node) // 如果没有在rebuild{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++;}}else // 如果在rebuild{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 // 如果不进行下采样{if (Rebuild_Ptr == nullptr || *Rebuild_Ptr != Root_Node){Add_by_point(&Root_Node, PointToAdd[i], true, Root_Node->division_axis); // 就直接添加}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;
}
Add_by_point函数
/*** 这个函数是一个点一个点的添加，通过递归的方式完成遍历，找到点需要在的位置，然后添加* 添加完成后要不断update来更新信息
*/
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;}(*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); // 每个循环遍历退出之后，都进行一下标志位自下而上的更新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); // 重建，重建以当前节点为root的树，不是从头都重建if ((*root) != nullptr)(*root)->working_flag = false;return;
}
删除功能
Delete_Point_Boxes函数
/*** 对外部的删除接口，给入一串方框，要求把方框中的点都删除
*/
template <typename PointType>
int KD_TREE<PointType>::Delete_Point_Boxes(vector<BoxPointType> &BoxPoints)
{int tmp_counter = 0;for (int i = 0; i < BoxPoints.size(); i++) // 对方框进行循环{if (Rebuild_Ptr == nullptr || *Rebuild_Ptr != Root_Node) // 如果没有在进行rebuild，或者只是局部rebuild{tmp_counter += Delete_by_range(&Root_Node, BoxPoints[i], true, false); // 调用Delete_by_range函数进行删除，} // 由于此时没有rebuild 任务，所以allow rebuild= trueelse // 如果正在进行rebuild，说明树的结果会发生变化{Operation_Logger_Type operation;operation.boxpoint = BoxPoints[i];operation.op = DELETE_BOX;pthread_mutex_lock(&working_flag_mutex);tmp_counter += Delete_by_range(&Root_Node, BoxPoints[i], false, false); // 但是这里依然调用了Delete_by_range，由于// 此时有rebuild任务，所以allow rebuild = falseif (rebuild_flag) // 如果正在进行多线程rebuild{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;
}Delete_by_range函数
/*** 这个函数执行具体的delete操作，具体的执行过程就是反复递归，然后对节点的是否删除属性进行赋值，然后* 根据重建标准，判定是否重建，也就是这里是lazy 删除，不是当场就删除，而是要等待合适的时机，通过* 重建树来执行彻底的删除操作
*/
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;(*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;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]){ // 如果有相交就要删除,这个if不仅是相交，是包含，如果时包含，就直接把这个点下所有的点都删掉(*root)->tree_deleted = true; // 所以这里要删除整个树(*root)->point_deleted = true;(*root)->need_push_down_to_left = true; // 需要把这个删除状态传递给子节点(*root)->need_push_down_to_right = true; // 那么什么时候把这个信息向下pushdown呢？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;elsedelete_box_log.op = DELETE_BOX; // 构造一个操作，然后看看是不是需要根据rebuild状态进行操作缓存。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); // 前面是pushdown，根据父节点确定子节点，现在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;
}查找功能
Nearest_Search函数
这个是对外的接口函数，内部会调用Search函数
/*** Top-K个最近点的搜索*
*/
template <typename PointType>
void KD_TREE<PointType>::Nearest_Search(PointType point, int k_nearest, PointVector &Nearest_Points, vector<float> &Point_Distance, float max_dist)
{MANUAL_HEAP q(2 * k_nearest); // 首先构造一个堆，q.clear(); // 通常利用树来完成最近邻搜索都是配合一个堆来进行vector<float>().swap(Point_Distance);if (Rebuild_Ptr == nullptr || *Rebuild_Ptr != Root_Node) // 如果没有重建{Search(Root_Node, k_nearest, point, q, max_dist); // 就直接search}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); // 等待结束直接searchpthread_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;
}
/*** 该函数是具体的search过程
*/
template <typename PointType>
void KD_TREE<PointType>::Search(KD_TREE_NODE *root, int k_nearest, PointType point, MANUAL_HEAP &q, float max_dist)
{if (root == nullptr || root->tree_deleted)return;float cur_dist = calc_box_dist(root, point);float 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)); // 保证比如，已经删了点不要search等}else{pthread_mutex_lock(&(root->push_down_mutex_lock));pthread_mutex_unlock(&(root->push_down_mutex_lock));}}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) // 如果小，就说明是top-k，就加到堆里q.pop();PointType_CMP current_point{root->point, dist};q.push(current_point);}}int cur_search_counter;float dist_left_node = calc_box_dist(root->left_son_ptr, point); // 计算和左子节点的距离float dist_right_node = calc_box_dist(root->right_son_ptr, point); // 计算和有子节点if (q.size() < k_nearest || dist_left_node < q.top().dist && dist_right_node < q.top().dist) // 如果heap没满肯定要搜索{ // 如果左右距离都比较小，肯定也要接着搜索if (dist_left_node <= dist_right_node) // 如果左侧距离小，肯定就先找左侧{if (Rebuild_Ptr == nullptr || *Rebuild_Ptr != root->left_son_ptr) // 处理多线程rebuild情况{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 (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); // 同样处理多线程rebuild的情况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);search_mutex_counter -= 1;pthread_mutex_unlock(&search_flag_mutex);}}}}else // 如果heap找满了，并且查询点到左右都有点远，或者右一个优点远{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;
}
rebuild功能
/*** 重建树的线程执行函数
*/
template <typename PointType>
void KD_TREE<PointType>::multi_thread_rebuild()
{bool terminated = false;KD_TREE_NODE *father_ptr, **new_node_ptr;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); // 初始化,swap 是为了高效的清理内存数据// Lock Searchpthread_mutex_lock(&search_flag_mutex); // 这个while的设计是等待其他线程中的条件满足，再向下进行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 cachepthread_mutex_lock(&points_deleted_rebuild_mutex_lock);flatten(*Rebuild_Ptr, Rebuild_PCL_Storage, MULTI_THREAD_REC); // 保存所有有效点，同时记录要删除的点// Unlock deleted points cachepthread_mutex_unlock(&points_deleted_rebuild_mutex_lock);// Unlock Searchpthread_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) // 有效点数大于0{BuildTree(&new_root_node, 0, Rebuild_PCL_Storage.size() - 1, Rebuild_PCL_Storage); // 重新构造树// Rebuild has been done. Updates the blocked operations into the new treepthread_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;int valid_new = new_root_node->TreeSize - new_root_node->invalid_point_num;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");
}
