FAST-LIO2代码解析(五)
上一节我们将主函数部分while外面的部分给讲完了，下面我们将深入while来学习里面的知识。
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)的{
G
p
j
^Gp_j
Gpj}中的给定点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行）。
…详情请参照古月居
开放原子开发者工作坊旨在鼓励更多人参与开源活动，与志同道合的开发者们相互交流开发经验、分享开发心得、获取前沿技术趋势。工作坊有多种形式的开发者活动，如meetup、训练营等，主打技术交流，干货满满，真诚地邀请各位开发者共同参与！
更多推荐
1
0- 0
已为社区贡献56条内容
所有评论(0)