FAST-LIO2代码解析(三)

目录

0. 简介

1. 主程序

2. 动态调整地图

3. 传感器信息获取

4. 时间戳对齐

5. 发布信息

---

0. 简介

在讲述完一，二后IMU和雷达的数据输入算是讲明白了，下面几节我们将结合主程序来介绍本文最关键的迭代卡尔曼滤波器和iKD-Tree的算法。

1. 主程序

一开始在laserMapping.cpp主程序中定义了一系列待使用的变量定义，包含了ESKF和IKF-Tree的定义

#define INIT_TIME (0.1)
#define LASER_POINT_COV (0.001)
#define MAXN (720000)
#define PUBFRAME_PERIOD (20)

/*** Time Log Variables ***/
// kdtree_incremental_time为kdtree建立时间，kdtree_search_time为kdtree搜索时间，kdtree_delete_time为kdtree删除时间;
double kdtree_incremental_time = 0.0, kdtree_search_time = 0.0, kdtree_delete_time = 0.0;
// T1为雷达初始时间戳，s_plot为整个流程耗时，s_plot2特征点数量,s_plot3为kdtree增量时间，s_plot4为kdtree搜索耗时，s_plot5为kdtree删除点数量
//，s_plot6为kdtree删除耗时，s_plot7为kdtree初始大小，s_plot8为kdtree结束大小,s_plot9为平均消耗时间，s_plot10为添加点数量，s_plot11为点云预处理的总时间
double T1[MAXN], s_plot[MAXN], s_plot2[MAXN], s_pl