基于ROS1平台，整个流程是while循环，循环体内的流程以VIO和LIO迭代交替循环执行（如图1所示），这也是error state iterated kalman filter (ESIKF)的实现所需要的逻辑。
1、数据同步（sync_packages函数），如图所示，mode将在VIO和LIO间轮流变换，sync_pages会根据当前的mode进行对应的逻辑处理。
在VIO模式下，将以图片帧作为处理的截至时间，对齐imu和点云数据进行处理；如果图片的帧率过快，从buffer里获取的图片比last_lio_update_time时间还要早，则对图片进行丢帧处理，由于lidar传感器的扫描特性，原始帧每一个点云的采样时间不同，需要对原始帧的点云集合按对齐时间进行逻辑划分，比当前图片帧还要晚的点云点数据将会分配到下一个点云数据集合里。获取对齐的imu数据和lidar数据将更新到测量组里（MeasureGroup）。数据对齐完成后切换到LIO mode进行下一步的处理。
LIO模式下的数据同步主要来获取图像数据，图像数据获取后将切换到VIO mode进行下一步的处理。
图1：FAST-LIVO2处理流程图
2、数据预处理（processImu函数）在LIO模式下，主要处理包括：(1)、以预积分的方式计算imu在每一个时间戳下的初始位姿以及计算状态误差状态雅可比矩阵和误差传播协方差矩阵_state.cov，代码实现可以参考引文[1]；(2)、通过imu预积分的结果（imu预积分计算在imu采样的时间戳序列下的对应的初始位姿）对lidar点云数据进行运动补偿去除畸变。运动补偿基于点云点时间戳和其对应的最近imu的时间戳的时间差和imu位姿及加速度角速度等数据按照运动方程进行时间对齐后的点云位姿估计，具体代码实现可以参考引文[2]。在VIO模式下，在基于LIO的点云和IMU紧耦合的ESKF优化的基础上，在对点云数据基于图像可视视角范围的过滤后在于图像的光度误差作为误差观测状态（又称为直接法）和前期IMU的姿态优化的基础上采用ESKF算法再次对位姿进行优化。
3、状态优化和建图，函数名为stateEstimationAndMapping，在步骤2中也有说明，其中LIO和VIO的迭代ESKF优化框架ESIKF（error state iterated kalman filter）在LIO和VIO依次都用到了ESKF滤波方法，其误差状态方程都是基于IMU的误差量（如基于李代数的旋转扰动量等19个维度），但误差观测方程不同，基于LIO的ESKF的误差观测基于点云点到匹配平面的距离残差，而VIO的ESKF的误差观测基于光度误差（直接法），具体后续将会在相关文章中详细介绍。
References
Leave a Reply