fast-lio代码学习
slam框架
首先放slam结构图
代码结构
fast-lio一共只有一个可执行文件laserMapping.cpp。其中有接受到点云和imu数据后会放入相应的buffer里面。然后算法的while循环会不断处理这些传感器数据。
首先每次接收到雷达数据时会对雷达点云提取特征，这里主要是平面点特征，在fast-lio2里面默认所有点都是平面点，但在fast -lio里面还是需要提取的。相应代码在preprocess.cpp和preprocess.h文件。
之后sync_packages函数不断将对应的雷达数据和imu数据打包为一个Measures。
imu的卡尔曼预测就是将这些measures前向传播，然后在后向传播，从而实现对每一个点云特征点去畸变，投影到$i+1$坐标系下。相应代码在imuProcess.hpp文件下。前向传播根据卡尔曼更新预测出当前时刻imu位姿，然后在后向传播计算每个特征点的位姿。所谓的后向传播就是类似与中值计算。
最后就是kf.update_iterated_dyn_share_modified()函数实现IESEKF。当前帧的特征点去ikd-tree中寻找最近的5个点，看是否能形成平面并计算残差，然后送入kf，作为观测优化24个变量。