大家好呀,我是一个SLAM方向的在读博士,深知SLAM学习过程一路走来的坎坷,也十分感谢各位大佬的优质文章和源码。随着知识的越来越多,越来越细,我准备整理一个自己的激光SLAM学习笔记专栏,从0带大家快速上手激光SLAM,也方便想入门SLAM的同学和小白学习参考,相信看完会有一定的收获。如有不对的地方欢迎指出,欢迎各位大佬交流讨论,一起进步。博主创建了一个科研互助群Q:951026257,欢迎大家加入讨论。
Fast-lio2原理解析见链接
从零入门激光SLAM(二十一)——看不懂FAST-LIO?进来_fastlio 雷达 更改频率-CSDN博客
注释版代码完整版见
https://github.com/huashu996/Fast-lio2-Supernote
本代码解析以算法流程的逻辑解析代码,一些简单的函数忽略讲解。
一、iKD-Tree构建
1.1 整体框架
点云预处理整体框架,点云下采样,调整地图区域、KD树构建、初始化搜索参数
//世界系下雷达坐标系的位置,用于调整局部地图
//世界系下雷达坐标系的位置,用于调整局部地图
pos_lid = state_point.pos + state_point.rot * state_point.offset_T_L_I;
if (feats_undistort->empty() || (feats_undistort == NULL))//没有点云跳过
{
ROS_WARN("No point, skip this scan!\n");
continue;
}
//判断是否初始化完成,需要满足第一次扫描的时间和第一个点云时间的差值大于INIT_TIME
flg_EKF_inited = (Measures.lidar_beg_time - first_lidar_time) < INIT_TIME ? \
false : true;
// Segment the map in lidar FOV
// 在拿到eskf前馈结果后,动态调整地图区域,防止地图过大而内存溢出
lasermap_fov_segment();
//点云数据的降采样、坐标转换以及使用 KD 树(K-Dimensional Tree)构建和管理点云数据
//下采样
downSizeFilterSurf.setInputCloud(feats_undistort);
//对点云数据进行滤波降采样,将结果存储在 feats_down_body
downSizeFilterSurf.filter(*feats_down_body); //滤波降采样后的点云数据
t1 = omp_get_wtime();
//获取降采样后的点云数据大小
feats_down_size = feats_down_body->points.size();
// initialize the map kdtree
//初始化 KD 树
if(ikdtree.Root_Node == nullptr) //检查 KD 树是否为空
{
//如果降采样后的点云数据大小大于 5
if(feats_down_size > 5)
{
//设置 KD 树的降采样参数
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]));//坐标系转换
}
//使用转换后的点云数据构建 KD 树
ikdtree.Build(feats_down_world->points); //构建ikd树
}
continue;
}
//获取 KD 树信息
// 获取ikd tree中的有效节点数,无效点就是被打了deleted标签的点
int featsFromMapNum = ikdtree.validnum();
// 获取Ikd tree中的节点数
kdtree_size_st = ikdtree.size();
// ICP and iterated Kalman filter update
if (feats_down_size < 5)
{
ROS_WARN("No point, skip this scan!\n");
continue;
}
//调整数据容器的大小
normvec->resize(feats_down_size);
feats_down_world->resize(feats_down_size);
if(0) // If you need to see map point, change to "if(1)"
{
PointVector ().swap(ikdtree.PCL_Storage);
ikdtree.flatten(ikdtree.Root_Node, ikdtree.PCL_Storage, NOT_RECORD);
featsFromMap->clear();
featsFromMap->points = ikdtree.PCL_Storage;
}
pointSearchInd_surf.resize(feats_down_size); //搜索索引
Nearest_Points.resize(feats_down_size); //将降采样处理后的点云用于搜索最近点
//初始化变量
int rematch_num = 0; //初始化重匹配计数器
bool nearest_search_en = true; //控制最近点搜索的启用状态
//记录时间戳
t2 = omp_get_wtime();
1.2 动态调整地图区域
根据激光雷达的位置,动态调整局部地图的边界,并记录需要移除的旧地图部分,以确保局部地图始终包含最新的激光雷达数据范围
void lasermap_fov_segment()
{
cub_needrm.clear();
//重置用于统计删除操作的计数器和时间
kdtree_delete_counter = 0;
kdtree_delete_time = 0.0;
//将某一点从机体坐标系转换到世界坐标系
pointBodyToWorld(XAxisPoint_body, XAxisPoint_world);
//激光雷达的位置
V3D pos_LiD = pos_lid;
//局部地图初始化
if (!Localmap_Initialized){
//以激光雷达位置为中心初始化一个立方体区域,并将其作为局部地图
for (int i = 0; i < 3; i++){
LocalMap_Points.vertex_min[i] = pos_LiD(i) - cube_len / 2.0;
LocalMap_Points.vertex_max[i] = pos_LiD(i) + cube_len / 2.0;
}
Localmap_Initialized = true;
return;
}
//计算距离并判断是否需要移动
float dist_to_map_edge[3][2];
bool need_move = false;
for (int i = 0; i < 3; i++){
dist_to_map_edge[i][0] = fabs(pos_LiD(i) - LocalMap_Points.vertex_min[i]);
dist_to_map_edge[i][1] = fabs(pos_LiD(i) - LocalMap_Points.vertex_max[i]);
if (dist_to_map_edge[i][0] <= MOV_THRESHOLD * DET_RANGE || dist_to_map_edge[i][1] <= MOV_THRESHOLD * DET_RANGE) need_move = true;
}
if (!need_move) return;
//更新局部地图边界并记录需要移除的部分
BoxPointType New_LocalMap_Points, tmp_boxpoints;
New_LocalMap_Points = LocalMap_Points;
//计算移动距离 mov_dist
float mov_dist = max((cube_len - 2.0 * MOV_THRESHOLD * DET_RANGE) * 0.5 * 0.9, double(DET_RANGE * (MOV_THRESHOLD -1)));
//更新局部地图边界
for (int i = 0; i < 3; i++){
tmp_boxpoints = LocalMap_Points;
if (dist_to_map_edge[i][0] <= MOV_THRESHOLD * DET_RANGE){
New_LocalMap_Points.vertex_max[i] -= mov_dist;
New_LocalMap_Points.vertex_min[i] -= mov_dist;
tmp_boxpoints.vertex_min[i] = LocalMap_Points.vertex_max[i] - mov_dist;
cub_needrm.push_back(tmp_boxpoints);
} else if (dist_to_map_edge[i][1] <= MOV_THRESHOLD * DET_RANGE){
New_LocalMap_Points.vertex_max[i] += mov_dist;
New_LocalMap_Points.vertex_min[i] += mov_dist;
tmp_boxpoints.vertex_max[i] = LocalMap_Points.vertex_min[i] + mov_dist;
cub_needrm.push_back(tmp_boxpoints);
}
}
//更新全局局部地图边界
LocalMap_Points = New_LocalMap_Points;
//清理和删除点云数据
points_cache_collect();
double delete_begin = omp_get_wtime();
if(cub_needrm.size() > 0) kdtree_delete_counter = ikdtree.Delete_Point_Boxes(cub_needrm);
kdtree_delete_time = omp_get_wtime() - delete_begin;
}
详情请见...
从零入门激光SLAM(二十二)——Fast-lio2代码详解(二) iKD-Tree构建与点云观测模型 - 古月居 (guyuehome.com)