0. 简介
上一节我们将主函数部分while外面的部分给讲完了,下面我们将深入while来学习里面的知识
1. 主函数while内部的eskf前馈
这部分是我们while内部前面的部分,内部的操作在前面几节中我们已经详细分析过了。
while (status)
{
// 如果有中断产生,则结束主循环
if (flg_exit)
break;
// ROS消息回调处理函数,放在ROS的主循环中
ros::spinOnce();
// 将激光雷达点云数据和IMU数据从缓存队列中取出,进行时间对齐,并保存到Measures中
if (sync_packages(Measures))
{
// 激光雷达第一次扫描
if (flg_reset)
{
ROS_WARN("reset when rosbag play back");
p_imu->Reset();
flg_reset = false;
Measures.imu.clear(); //判断状态,并把imu的数据清空
continue;
}
double t0, t1, t2, t3, t4, t5, match_start, solve_start, svd_time;
match_time = 0;
kdtree_search_time = 0.0;
solve_time = 0;
solve_const_H_time = 0;
svd_time = 0;
t0 = omp_get_wtime();
// 对IMU数据进行预处理,其中包含了点云畸变处理 前向传播 反向传播
p_imu->Process(Measures, kf, feats_undistort);
// 获取kf预测的全局状态(imu)
state_point = kf.get_x();
//世界系下雷达坐标系的位置
//下面式子的意义是W^p_L = W^p_I + W^R_I * I^t_L
pos_lid = state_point.pos + state_point.rot * state_point.offset_T_L_I;
if (feats_undistort->empty() || (feats_undistort == NULL)) //如果点云数据为空,则代表了激光雷达没有完成去畸变,此时还不能初始化成功
{
first_lidar_time = Measures.lidar_beg_time; //记录第一次扫描的时间
p_imu->first_lidar_time = first_lidar_time; //将第一帧的时间传给imu作为当前帧的第一个点云时间
// cout<<"FAST-LIO not ready"<<endl;
continue;
}
flg_EKF_inited = (Measures.lidar_beg_time - first_lidar_time) < INIT_TIME ? false : true; //判断是否初始化完成,需要满足第一次扫描的时间和第一个点云时间的差值大于INIT_TIME
/*** Segment the map in lidar FOV ***/
// 动态调整局部地图,在拿到eskf前馈结果后
lasermap_fov_segment();
/*** downsample the feature points in a scan ***/
downSizeFilterSurf.setInputCloud(feats_undistort); //获得去畸变后的点云数据
downSizeFilterSurf.filter(*feats_down_body); //滤波降采样后的点云数据
t1 = omp_get_wtime(); //记录时间
feats_down_size = feats_down_body->points.size(); //记录滤波后的点云数量
对我们在FAST-LIO2代码解析(二)中提到的,IMU在ImuProcess中会有前馈的操作,也就是kf_state.predict(dt, Q, in);
,这里作者也是看的一知半解的,后面如果可以,希望各位大佬们补充,基本含义是拿到更新,并使用公式2完成x的更新,并得到更新后的F。
因此在获取imu单帧后进行协方差矩阵的先验更新方程也被确定如FAST-LIO中的公式8。
// 迭代误差状态EKF传播
void predict(double &dt, processnoisecovariance &Q, const input &i_in) //这里的参数均从use-ikfom中传入
{
// f函数对应use-ikfom.hpp中的 get_f函数,对应fast_lio2论文公式(2)
flatted_state f_ = f(x_, i_in); // m*1
// 对应use-ikfom.hpp中的 df_dx函数
// 对应fast_lio2论文公式(7)
cov_ f_x_ = f_x(x_, i_in); // m*n
cov f_x_final; // n*n
// 对应use-ikfom.hpp中的 df_dw函数
// 对应fast_lio2论文公式(7)
Matrix<scalar_type, m, process_noise_dof> f_w_ = f_w(x_, i_in);
Matrix<scalar_type, n, process_noise_dof> f_w_final;
state x_before = x_; //保存x_的值,用于后面的更新
// 对应fast_lio2论文公式(2)
x_.oplus(f_, dt); //这个是公式2的整个函数,用于更新x的
F_x1 = cov::Identity(); //状态转移矩阵
// 更新f_x和f_w
for (std::vector<std::pair<std::pair<int, int>, int>>::iterator it = x_.vect_state.begin(); it != x_.vect_state.end(); it++)
{
int idx = (*it).first.first; //状态变量的索引
int dim = (*it).first.second; //状态变量的维数
int dof = (*it).second; //状态变量的自由度
for (int i = 0; i < n; i++)
{
for (int j = 0; j < dof; j++)
{
f_x_final(idx + j, i) = f_x_(dim + j, i); //更新f_x_final,形成n*n阵,用于更新
}
}
for (int i = 0; i < process_noise_dof; i++)
{
for (int j = 0; j < dof; j++)
{
f_w_final(idx + j, i) = f_w_(dim + j, i); //更新f_w_final,形成n*dof,用于更新
}
}
}
2. IKD-Tree树建立
在这部分主要完成了ID-Tree树的建立。ikd-Tree是一个二叉搜索树,ikd-Tree 中树节点的属性显示在”数据结构”中(Data Structure)。与许多仅在叶节点上存储点”桶”的k-d树的现有实现不同,我们的ikd-Tree将点存储在叶节点和内部节点上,以更好地支持动态点插入和树重新平衡。当使用单个 k-d 树 [41] 时,这种存储模式在 kNN 搜索中也显示出更有效,这就是我们的 ikd-Tree 的情况。由于点对应于 ikd-Tree 上的单个节点,我们将互换使用点和节点。
点信息(例如点坐标、强度)存储在 point 中。 属性 leftchild 和 rightchild 分别是指向其左右子节点的指针。用于分割空间的分割轴记录在 axis 中。根植于当前节点的(子)树的树节点数(包括有效和无效节点)保留在属性 treesize 中。当点从地图中删除时,节点不会立即从树中删除,而只是将布尔变量 deleted 设置为 true(有关详细信息,请参阅第Section V-C2 节)。 如果删除了以当前节点为根的整个(子)树,则将 treedeleted 设置为 true。 从(子)树中删除的点的数目汇总于属性 invalidnum 中。 属性 range 记录了(子)树上点的范围信息,被解释为包含所有点的受限制的轴对齐长方体。 受限制的长方体由其两个对角线顶点表示,每个顶点分别具有最小和最大坐标。
// 构建kd树
if (ikdtree.Root_Node == nullptr)
{
if (feats_down_size > 5)
{
// 设置ikd tree的降采样参数
ikdtree.set_downsample_param(filter_size_map_min);
feats_down_world->resize(feats_down_size); //将下采样得到的地图点大小于body系大小一致
for (int i = 0; i < feats_down_size; i++)
{
pointBodyToWorld(&(feats_down_body->points[i]), &(feats_down_world->points[i])); //将下采样得到的地图点转换为世界坐标系下的点云
}
// 组织ikd tree
ikdtree.Build(feats_down_world->points); //构建ikd树
}
continue;
}
// 获取ikd tree中的有效节点数,无效点就是被打了deleted标签的点
int featsFromMapNum = ikdtree.validnum();
// 获取Ikd tree中的节点数
kdtree_size_st = ikdtree.size();
然后我们详细来看一下构建 ikd-Tree,类似于在《Multidimensional binary search trees used for associative searching》 中构建静态 k-d 树。ikd-Tree 沿最长维度递归地分割中点处的空间,直到子空间中只有一个点。 Data Structure 中的属性在构建过程中被初始化,包括计算树的大小和(子)树的范围信息。
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);
//建立kdtree,这里直接从其左子树开始构建
BuildTree(&STATIC_ROOT_NODE->left_son_ptr, 0, point_cloud.size() - 1, point_cloud);
//更新root的属性
Update(STATIC_ROOT_NODE);
STATIC_ROOT_NODE->TreeSize = 0;
// 将左子树直接替换根节点
Root_Node = STATIC_ROOT_NODE->left_son_ptr;
}
3. ID-Tree点的增删
ikd-Tree 上的增量更新指的是增量操作,其次是第 V-D 节中详述的动态重新平衡。ikd-Tree支持两种类型的增量操作: 逐点操作和按框操作。 逐点操作在 k-d 树中插入、删除或重新插入单个点,而按框操作在给定的轴对齐长方体中插入、删除或重新插入所有点。在这两种情况下,点插入都进一步与树上降采样相集成,从而将地图保持在预定的分辨率。在本文中,我们仅解释 FAST-LIO2 的地图管理所需的逐点插入和按框删除。 下面我们来展示一下按点插入的部分
// 向树中插入新的点集
int KD_TREE::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);
// 在树中搜索最近邻点,且要求点都在Box中,将点存于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)
{
// 如果近邻点不止1个,或者当前点与原有的点很接近
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
{ // 需要重建树
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;
}
考虑到机器人应用,我们的 ikd-Tree 支持同时(simultaneous)点插入和地图缩减采样。 该算法在Algorithm 2 中有详细说明。对于来自状态估计模块(参见算法1)的{$^Gp_j$}中的给定点p和下采样分辨率l,该算法将空间均匀地划分成长度为l的立方体,然后找到包含点p的立方体CD(第2行)。该算法仅保留最靠近CD中心Pcenter的点(第3行),这是通过首先在k-d树上搜索CD中包含的所有点,并将它们与新点p一起存储在点数组V中(第4-5行)来实现的。最近点Pnearest是通过比较V中每个点到中心pcenter(第6行)的距离获得的。然后删除CD中现有的点(第7行),之后将最近的点pnearest插入到k-d树中(第8行)。箱式搜索的实现类似于SectionV-C2中介绍的箱式删除。
ikd-Tree上的点插入(线11-24)是递归地实现的。 该算法从根节点上搜索,直到发现空节点再追加一个新节点(第12-14行), 新叶节点的属性按照表一进行初始化。在每个非空节点上,将新点与存储在树节点上的点沿划分轴进行比较,以进一步递归(第15-20行)。
// 前序遍历的点级更新
// 在ik-d树上的点级更新以递归的方式实现,这类似于替罪羊k-d树。对于点级插入,
// 该算法递归地从根节点向下搜索,并将新点的除法轴上的坐标与存储在树节点上的点进行比较,
// 直到发现一个叶节点然后追加一个新的树节点。对于删除或重新插入一个点P,该算法会查找存
// 储该点P的树节点,并修改deleted属性。
void KD_TREE::Add_by_point(KD_TREE_NODE **root, PointType point, bool allow_rebuild, int father_axis)
{
// 如果当前节点为空的话,则新建节点
if (*root == nullptr)
{
// 新建节点并初始化
*root = new KD_TREE_NODE; // 注意到root是指针的指针,所以不需要返回地址即可实现父节点指向当前新节点
InitTreeNode(*root);
// 配置节点的值
(*root)->point = point;
// 标记该节点的划分轴
(*root)->division_axis = (father_axis + 1) % 3;
// 将新的节点信息汇总给父节点
Update(*root);
return;
}
// 当前节点不为空,说明还未到叶子节点,则递归查找
(*root)->working_flag = true;
// 创建Operation_Logger_Type用于记录新插入的点,重构树的时候需要用到
Operation_Logger_Type add_log;
struct timespec Timeout;
// 设置动作为ADD_POINT
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);
}
}
// 做一次pull-up, 更新root,判断是否需要进行重建
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;
}
如SectionV-C3中所介绍的,那些被访问节点的属性(例如,treesize、range)由最新信息(第21行)更新。检查并维护用新点更新的子树的平衡标准,以保持ikd-Tree的平衡属性(第22行),如SectionV-D所述。
4. 惰性标签按框删除
在删除操作中,我们使用惰性删除策略。也就是说,这些点不会立即从树中删除,而只是通过将属性deleted(参见数据结构,第6行)设置为true来标记为“已删除”。如果以节点T为根的子树上的所有节点都已被删除,则T的属性treedeleted将被设置为true。因此,属性deleted和treedeleted被称为懒惰标签。标有“已删除”的点将在重建过程中(参见SectionV-D)从树中移除。
箱式删除是利用属性范围中的范围信息和树节点上的惰性标签来实现。正如在SectionV-B中提到的,属性范围由外切的长方体CT表示。
伪代码如 Algorithm 3 所示。给定要从以T为根的(子)树中删除的点CO的长方体,该算法递归地向下搜索树,并将外切长方体CT与给定的长方体CO进行比较。如果CT和CO之间没有交集,则递归直接返回,而不更新树(第2行);如果外切长方体CT完全包含在给定的长方体CO中,则箱式删除将属性deleted和treedeleted设置为真(第5行),当(子)树上的所有点都被删除时,属性invalidnum等于treesize(第6行);对于CT相交但不包含在CO中的情况,如果当前点p包含在CO中,则首先将其从树中删除(第9行),之后该算法递归地查看子节点(第10-11行),而当前节点T的属性更新和余额维护在箱式删除操作之后被应用(第12-13行)。
// 当属性Pushdown 为true时,Pushdown 函数将被deleted, treedeleted,
// 和 pushdown的标签复制到它的子节点(但没有复制到它的的后代。
void KD_TREE::Push_Down(KD_TREE_NODE *root)
{
if (root == nullptr)
return;
// 实例化一个操作记录器
Operation_Logger_Type operation;
// 操作器设置为PUSH_DOWN状态
operation.op = PUSH_DOWN;
// 获取当前节点的删除标签,用于传递给子节点
operation.tree_deleted = root->tree_deleted;
// 同样,获取当前节点的降采样删除标签,用于传递给子节点
operation.tree_downsample_deleted = root->tree_downsample_deleted;
// 左子节点不为空,且该节点需要push down
if (root->need_push_down_to_left && root->left_son_ptr != nullptr)
{
if (Rebuild_Ptr == nullptr || *Rebuild_Ptr != root->left_son_ptr)
{
// 将当前节点的删除标签传递给左子节点
root->left_son_ptr->tree_downsample_deleted |= root->tree_downsample_deleted;
root->left_son_ptr->point_downsample_deleted |= root->tree_downsample_deleted;
root->left_son_ptr->tree_deleted = root->tree_deleted || root->left_son_ptr->tree_downsample_deleted;
root->left_son_ptr->point_deleted = root->left_son_ptr->tree_deleted || root->left_son_ptr->point_downsample_deleted;
if (root->tree_downsample_deleted)
root->left_son_ptr->down_del_num = root->left_son_ptr->TreeSize;
if (root->tree_deleted)
root->left_son_ptr->invalid_point_num = root->left_son_ptr->TreeSize;
else
root->left_son_ptr->invalid_point_num = root->left_son_ptr->down_del_num;
// 标记下左子节点的状态更新情况,设为true意味着左子节点更新完,它的属性也要传递给后代节点
root->left_son_ptr->need_push_down_to_left = true;
root->left_son_ptr->need_push_down_to_right = true;
// 当前节点属性已经Push down了,所以可以将其更新push down需求设为false
root->need_push_down_to_left = false;
}
else
{
pthread_mutex_lock(&working_flag_mutex);
root->left_son_ptr->tree_downsample_deleted |= root->tree_downsample_deleted;
root->left_son_ptr->point_downsample_deleted |= root->tree_downsample_deleted;
root->left_son_ptr->tree_deleted = root->tree_deleted || root->left_son_ptr->tree_downsample_deleted;
root->left_son_ptr->point_deleted = root->left_son_ptr->tree_deleted || root->left_son_ptr->point_downsample_deleted;
if (root->tree_downsample_deleted)
root->left_son_ptr->down_del_num = root->left_son_ptr->TreeSize;
if (root->tree_deleted)
root->left_son_ptr->invalid_point_num = root->left_son_ptr->TreeSize;
else
root->left_son_ptr->invalid_point_num = root->left_son_ptr->down_del_num;
root->left_son_ptr->need_push_down_to_left = true;
root->left_son_ptr->need_push_down_to_right = true;
if (rebuild_flag)
{
pthread_mutex_lock(&rebuild_logger_mutex_lock);
Rebuild_Logger.push(operation);
pthread_mutex_unlock(&rebuild_logger_mutex_lock);
}
root->need_push_down_to_left = false;
pthread_mutex_unlock(&working_flag_mutex);
}
}
// 对于右子节点也是相同的Push down操作,传递当前节点的属性下去
if (root->need_push_down_to_right && root->right_son_ptr != nullptr)
{
if (Rebuild_Ptr == nullptr || *Rebuild_Ptr != root->right_son_ptr)
{
root->right_son_ptr->tree_downsample_deleted |= root->tree_downsample_deleted;
root->right_son_ptr->point_downsample_deleted |= root->tree_downsample_deleted;
root->right_son_ptr->tree_deleted = root->tree_deleted || root->right_son_ptr->tree_downsample_deleted;
root->right_son_ptr->point_deleted = root->right_son_ptr->tree_deleted || root->right_son_ptr->point_downsample_deleted;
if (root->tree_downsample_deleted)
root->right_son_ptr->down_del_num = root->right_son_ptr->TreeSize;
if (root->tree_deleted)
root->right_son_ptr->invalid_point_num = root->right_son_ptr->TreeSize;
else
root->right_son_ptr->invalid_point_num = root->right_son_ptr->down_del_num;
root->right_son_ptr->need_push_down_to_left = true;
root->right_son_ptr->need_push_down_to_right = true;
root->need_push_down_to_right = false;
}
else
{
pthread_mutex_lock(&working_flag_mutex);
root->right_son_ptr->tree_downsample_deleted |= root->tree_downsample_deleted;
root->right_son_ptr->point_downsample_deleted |= root->tree_downsample_deleted;
root->right_son_ptr->tree_deleted = root->tree_deleted || root->right_son_ptr->tree_downsample_deleted;
root->right_son_ptr->point_deleted = root->right_son_ptr->tree_deleted || root->right_son_ptr->point_downsample_deleted;
if (root->tree_downsample_deleted)
root->right_son_ptr->down_del_num = root->right_son_ptr->TreeSize;
if (root->tree_deleted)
root->right_son_ptr->invalid_point_num = root->right_son_ptr->TreeSize;
else
root->right_son_ptr->invalid_point_num = root->right_son_ptr->down_del_num;
root->right_son_ptr->need_push_down_to_left = true;
root->right_son_ptr->need_push_down_to_right = true;
if (rebuild_flag)
{
pthread_mutex_lock(&rebuild_logger_mutex_lock);
Rebuild_Logger.push(operation);
pthread_mutex_unlock(&rebuild_logger_mutex_lock);
}
root->need_push_down_to_right = false;
pthread_mutex_unlock(&working_flag_mutex);
}
}
return;
}
5. 重新平衡
ikd-Tree在每次增量操作后会主动监控平衡属性,并通过仅重建相关的子树来动态地重新平衡自身。平衡准则由两个子准则组成:α-平衡准则和α-删除准则。假设ikd树的子树以T为根,当且仅当它满足以下条件时子树是α-平衡的。式中,$α_{bal} ∈(0.5, 1)$和$S(T)$是节点T的属性treesize。
式中,$α_{del} ∈(0, 1 )$, $I(T)$表示子树上无效节点的数量。以T为根的子树的$α-$删除准则是:
如果ikd-Tree的子树满足这两个标准,则该子树是平衡的。如果所有子树都是平衡的,则整个树是平衡的。违反任何一个标准都将触发重新构建过程来重新平衡该子树:α平衡标准保持着树的最大高度。很容易证明α-平衡树的最大高度是$log1/α_{bal}(n)=n$,其中n是树的大小;α-删除准则确保子树上的无效节点(即,标记为“已删除”)被移除以减小树的大小。降低k-d树的高度和大小允许将来高效的增量操作和查询。
假设在子树T上触发重建(见Fig 4),子树首先被展平成点存储数组V。在展平过程中,标记为“已删除”的树节点将被丢弃。然后同SectionV-B,用V中的所有点构建一个新的完全平衡的k-d树。在ikd-Tree上重新构建大型子树时,可能会出现相当大的延迟,并破坏FAST-LIO2的实时性能。为了保持较高的实时性,我们设计了一种双线程重建方法。我们提出的方法不是简单地在第二个线程中重建,而是通过操作记录器避免了两个线程中的信息丢失和内存冲突,从而始终保持knearest邻居搜索的完全准确性。
重建的方法参见 Algorithm 4。当违反平衡标准时,当子树的树大小小于预定值Nmax时,在主线程中重建子树;否则,在第二线程中重建子树。第二个线程上的重建算法如函数ParRebuild所示。将第二个线程中要重建的子树表示为t,将其根节点表示为T。第二个线程将锁定所有增量更新(即点插入和删除),但不锁定该子树上的查询(第12行)。然后,第二线程将子树t中包含的所有有效点复制到点数组V中(即,展平),同时保持原始子树不变,以用于重建过程中可能的查询(第13行)。展平后,原始子树被解锁,以便主线程进一步执行增量更新的请求(第14行)。这些请求将同时记录在一个名为operation logger的队列中。一旦第二个线程完成从点数组V构建新的平衡k-d树t‘(第15行),记录的更新请求将由函数IncrementalUpdates在t‘上再次执行(第16-18行)。
其中,并行重建开关被设置为false,因为它已经在第二个线程中。在所有未决请求被处理之后,原始子树t上的点信息与新子树t‘上的点信息完全相同,只是新子树在树结构中比原始子树更平衡。该算法从增量更新和查询中锁定节点T,并用新的节点T’替换它(第20-22行)。最后,该算法释放原始子树的内存(第23行)。
这种设计确保了在第二线程中的重建过程中,主线程中的映射过程仍然以里程计速率进行而没有任何中断,尽管由于暂时不平衡的k-d树结构而导致效率较低。我们应该注意,ockUpdates不会阻塞查询,查询可以在主线程中并行执行。相比之下,LockAll阻塞所有访问,包括查询,但它完成得非常快(即只有一条指令),允许在主线程中及时查询。函数LockUpdates和LockAll是通过互斥(mutex)实现的。
// Update函数的主要目的是为了更新该节点以及它子树的点的范围以及size,还有他的del,bal,
// 用来判断树是否平衡,是否需要重建。这个函数再原论文中是pull-up操作(树的属性由子向父传播)。
// Pullup函数将基于T的子树的信息汇总给T的以下属性:treesize(见数据结构1,第5行)保存子树上
// 所有节点数,invalidnum 存储子树上标记为“删除”的节点数,range(见数据结构1,第7行)汇总子
// 树上所有点坐标轴的范围,其中k为点维度。
void KD_TREE::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)
{
// 那当前树的节点数=左子树节点数+右子树节点数+1(当前节点本身)
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;
// 获取包围当前树的box
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]);
}
// 子树的box+root的点
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
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;
// 获取包围左子树+root的box
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
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;
// 获取包围右子树和root的box
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;
}
// 将树的区间范围拷贝给节点属性 node_range_x = [node_range_x_min, node_range_x_max]
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;
// 如果当前节点为整个树的根节点,且树的节点数大于3
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;
}
6. 最近邻搜索
尽管与那些众所周知的k-d树库[43]-[45]中的现有实现类似,但最近搜索算法在ikd树上被彻底优化。使用[41]中详述的“边界重叠球”测试,充分利用树节点上的范围信息来加速我们的最近邻搜索。维护优先级队列q以存储迄今为止遇到的k个最近邻点以及它们到目标点的距离。当从树的根节点向下递归搜索树时,首先计算从目标点到树节点的立方体CT的最小距离dmin。如果最小距离dmin不小于q中的最大距离,则不需要处理该节点及其后代节点。此外,在FAST-LIO2(和许多其他激光雷达里程计)中,只有当邻近点在目标点周围的给定阈值内时,才会被视为内点,并因此用于状态估计,这自然为knearest邻近点的范围搜索提供了最大搜索距离[43]。在任一情况下,范围搜索通过将dmin与最大距离进行比较来删减算法,从而减少回溯量以提高时间性能。需要注意的是,我们的ikd-Tree支持并行计算架构的多线程k近邻搜索。
// K近邻搜索
void KD_TREE::Nearest_Search(PointType point, int k_nearest, PointVector &Nearest_Points, vector<float> &Point_Distance, double max_dist)
{
// 初始化自定义的大顶堆,容量为2*k_nearest
MANUAL_HEAP q(2 * k_nearest);
q.clear();
// 清空Point_Distance
vector<float>().swap(Point_Distance);
// 开始搜索
if (Rebuild_Ptr == nullptr || *Rebuild_Ptr != Root_Node)
{
Search(Root_Node, 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_Node, k_nearest, point, q, max_dist);
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()));
// 清空Nearest_Points和Point_Distance
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;
}
7. 参考链接
https://blog.csdn.net/MWY123_/article/details/124110021
https://zhuanlan.zhihu.com/p/471876531
https://zhuanlan.zhihu.com/p/471876061
评论(0)
您还未登录,请登录后发表或查看评论