从零入门激光SLAM（二十二）——Fast-lio2代码详解（二） iKD-Tree构建与点云观测模型

大家好呀，我是一个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)