当前位置： 首页 > news >正文

FAST-LIO2代码解析(五)

news 来源：原创 2024/5/18 6:13:33

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行）。
在这里插入图片描述

…详情请参照古月居

相关文章：

  • 苦卷28天，阿里P8给我的Alibaba面试手册，终于成功踹开字节大门
  • Vue：v-on、v-bind、v-model、@click、：model用法以及区别（附代码实例）
  • 手写Sping IOC（基于Setter方法注入）
  • 一、SpringBoot前置（从0搭建Maven项目）
  • 宝藏教程：超详细一条龙教程！从零搭建React项目全家桶
  • 网络编程学习总结3
  • 欧姆龙CP1H如何进行PLC远程编程及数据采集
  • CSGO Bway电竞ETERNAL FIRE可以参加BLAST FALL，但MOUZ却错过了
  • 收获tips
  • Vue3+elementplus搭建通用管理系统实例十二：使用通用表格、表单实现对应功能
  • 抖音根据关键词取视频列表 API 返回值说明
  • 120页7万字XX云数据中心解决方案技术方案
  • 保险行业如何实现私域快速增长
  • 计算机网络的基础知识
  • 开发者测评：相比 Harbor，我选择 ACR 的三点原因
  • bootstrap创建登录注册页面
  • docker python 配置
  • Java IO学习笔记一
  • JavaScript 是如何工作的:WebRTC 和对等网络的机制！
  • MQ框架的比较
  • nginx 负载服务器优化
  • overflow: hidden IE7无效
  • Promise面试题，控制异步流程
  • Rancher如何对接Ceph-RBD块存储
  • react-native 安卓真机环境搭建
  • STAR法则
  • ucore操作系统实验笔记 - 重新理解中断
  • Vue源码解析（二）Vue的双向绑定讲解及实现
  • 对象引论
  • 目录与文件属性：编写ls
  • 七牛云 DV OV EV SSL 证书上线，限时折扣低至 6.75 折！
  • 学习笔记DL002:AI、机器学习、表示学习、深度学习，第一次大衰退
  • 用Node EJS写一个爬虫脚本每天定时给心爱的她发一封暖心邮件
  • 1.Ext JS 建立web开发工程
  • 测评：对于写作的人来说，Markdown是你最好的朋友 ...
  • 通过调用文摘列表API获取文摘
  • ​创新驱动，边缘计算领袖：亚马逊云科技海外服务器服务再进化
  • #{}和${}的区别是什么 -- java面试
  • $refs 、$nextTic、动态组件、name的使用
  • %3cscript放入php,跟bWAPP学WEB安全(PHP代码)--XSS跨站脚本攻击
  • （￥1011）－（一千零一拾一元整）输出
  • （原创）boost.property_tree解析xml的帮助类以及中文解析问题的解决
  • ***利用Ms05002溢出找“肉鸡
  • .Net MVC4 上传大文件，并保存表单
  • .net 打包工具_pyinstaller打包的exe太大？你需要站在巨人的肩膀上-VC++才是王道
  • .net 发送邮件
  • .NET/C# 项目如何优雅地设置条件编译符号？
  • .NetCore Flurl.Http 升级到4.0后 https 无法建立SSL连接
  • .net和php怎么连接,php和apache之间如何连接
  • .net企业级架构实战之7——Spring.net整合Asp.net mvc
  • .Net通用分页类(存储过程分页版,可以选择页码的显示样式,且有中英选择)
  • .NET学习全景图
  • .net之微信企业号开发(一) 所使用的环境与工具以及准备工作
  • ::前边啥也没有
  • @property括号内属性讲解