引言
本文结合代码对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; // 找到左子节点的index
PointType_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; // 然后继续循环
}
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;
}
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 axis
var PointType minPoint := find_min(node.rightChild, axis, depth + 1);
// Replace current node with the minimum point found
node.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 axis
var PointType maxPoint := find_max(node.leftChild, axis, depth + 1);
// Replace current node with the maximum point found
node.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 subtree
node.leftChild := delete_kdtree(node.leftChild, point, depth + 1);
}
else
{
// Go to right subtree
node.rightChild := delete_kdtree(node.rightChild, point, depth + 1);
}
return node;
}
function find_min(node, axis, depth)
{
if (node is null)
return null;
// Calculate current axis
var int current_axis := depth mod k;
// If current axis matches the target axis
if (current_axis == axis)
{
// The minimum is in the left subtree
if (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 rightMin
return min(node.location, leftMin, rightMin);
}
function find_max(node, axis, depth)
{
if (node is null)
return null;
// Calculate current axis
var int current_axis := depth mod k;
// If current axis matches the target axis
if (current_axis == axis)
{
// The maximum is in the right subtree
if (node.rightChild is null)
return node.location;
return find_max(node.rightChild, axis, depth + 1);
}
// Otherwise, find the maximum across both subtrees
var 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 rightMax
return 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) / 2
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;
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 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));
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= true
else // 如果正在进行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 = false
if (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;
else
delete_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); // 等待结束直接search
pthread_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 Search
pthread_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 cache
pthread_mutex_lock(&points_deleted_rebuild_mutex_lock);
flatten(*Rebuild_Ptr, Rebuild_PCL_Storage, MULTI_THREAD_REC); // 保存所有有效点，同时记录要删除的点
// Unlock deleted points cache
pthread_mutex_unlock(&points_deleted_rebuild_mutex_lock);
// Unlock 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) // 有效点数大于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 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;
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");
}
扫一扫
1454
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
