FAST-LIO2是一个基于紧耦合的激光雷达惯性里程计(LIO)系统,它结合了激光雷达(LiDAR)和惯性测量单元(IMU)的数据,通过迭代扩展卡尔曼滤波(ESKF)和增量k-d树(iKD-Tree)来实现高精度的定位和建图。下面是对FAST-LIO2代码的基本框架、关键类和函数、激光雷达数据处理、状态估计和优化方法以及代码逻辑的详细解析。 1....
nh.param<int>("point_filter_num", p_pre->point_filter_num, 2); // 采样间隔,即每隔point_filter_num个点取1个点 nh.param<bool>("feature_extract_enable", p_pre->feature_enabled, false); // 是否提取特征点(FAST_LIO2默认不进行特征点提取) nh.param<bool>("runtime_pos_log_enable", ru...
FAST-LIO2代码解析(六) 0. 简介 上一节我们将while内部的IKD-Tree部分全部讲完,下面将是最后一部分,关于后端优化更新的部分。 1. 迭代更新 最后一块主要做的就是,拿当前帧与IKD-Tree建立的地图算出的残差,然后去计算更新自己的位置,并将更新后的结果通过map_incremental函数插入到由ikd-Tree表示的映射中。 //...
1. 激光雷达 对于FAST-LIO2代码来说,主要的数据是通过preprocess与IMU_Processing拿到相应的传感器数据,并在主程序当中调用ESF与IKD-Tree完成优化。首先我们来看一下激光雷达部分。 1.1 preprocess.h 首先在最上面先定义了一系列enum的信息,通过enum选择不同的激光雷达类型,特征点特征等,并通过orgtype存放一些激光雷达...
参考Fast-Lio及误差卡尔曼滤波(ESKF)相关资料,本文对Fast-Lio2代码及论文中相关重难点部分进行了解析,详细关注论文到代码的实现过程,对于部分推导,若与引用资料基本一致,本文将给出引用位置,对于论文与代码的重难点及跳跃较大的地方本文将详细对照说明,论文与代码之间存在些许变量定义差异,行文风格上,本文将主要以代码...
FAST-LIO2代码解析(三) 0. 简介 在讲述完一,二后IMU和雷达的数据输入算是讲明白了,下面几节我们将结合主程序来介绍本文最关键的迭代卡尔曼滤波器和iKD-Tree的算法。 1. 主程序 一开始在laserMapping.cpp主程序中定义了一系列待使用的变量定义,包含了ESKF和IKF-Tree的定义...
对我们在FAST-LIO2代码解析(二)中提到的,IMU在ImuProcess中会有前馈的操作,也就是kf_state.predict(dt, Q, in);,这里作者也是看的一知半解的,后面如果可以,希望各位大佬们补充,基本含义是拿到更新,并使用公式2完成x的更新,并得到更新后的F。
FAST-LIO2代码解析(二) 0. 简介 在上文中讲述了激光雷达的数据获取以及特征点提取的操作,这一节我们将围绕着IMU_Processing这一节来进行介绍。 1. ImuProcess类定义 在ImuProcess.hpp中,一开始就是完成了对ImuProcess类的申明,里面我们可以看到在imu中最主要的还是角速度和加速度这两项特征,这与视觉SLAM的imu...
对于FAST-LIO2代码来说,主要的数据是通过preprocess与IMU_Processing拿到相应的传感器数据,并在主程序当中调用ESF与IKD-Tree完成优化。首先我们来看一下激光雷达部分。 1.1 preprocess.h 首先在最上面先定义了一系列enum的信息,通过enum选择不同的激光雷达类型,特征点特征等,并通过orgtype存放一些激光雷达点的一些其他属...