FAST-LIO2代码解析

lcyangzhanduo

已于 2023-01-31 10:55:26 修改

阅读量3.9k

点赞数 8

分类专栏：笔记文章标签：算法

于 2023-01-19 17:10:02 首次发布

本文链接：https://blog.csdn.net/lcyangzhanduo/article/details/128729408

版权

笔记专栏收录该内容

8 篇文章

订阅专栏

该程序主要处理激光雷达点云和IMU数据，采用ESKF进行状态估计。在main函数中，配置参数，订阅雷达和IMU消息。点云经过降采样和预处理，IMU数据存储在缓冲区。主循环中，同步点云和IMU数据，进行运动畸变矫正。初始化阶段计算IMU均值和方差，更新状态估计器。之后，迭代更新KF，对点云进行矫正，并增量式更新地图。程序利用多核并行计算优化性能。

摘要生成于 C知道，由 DeepSeek-R1 满血版支持，前往体验 >

laserMapping.cpp 为整个程序的入口文件，相关处理见main函数解析。

process.cpp为激光点云处理类，进行的处理有：ros消息转换成plc点云、特征提取。

IMU_Processing.hpp 中有IMU相关处理（初始化、推算、kf_state预测）和点云矫正处理。

code中直接使用ikdtree的库、IKFoM_toolkit库（ESKF、流形处理）、pcl库进行处理，因此代码比较精简。

main函数

1. 加载各项参数，设置激光点云的kd-tree叶子大小，设置预积分的参数：外参、陀螺加计噪声方差、陀螺加计零偏方差.

2. 设置kf相关信息： kf.init_dyn_share（设置F矩阵（用于更新x）、Jacobi矩阵（用于更新P）、观测模型、迭代次数、收敛阈值）。

3.根据激光雷达类型，订阅雷达消息在livox_pcl_cbk或standard_pcl_cbk中处理。

standard_pcl_cbk中，将ros消息转换成PointCloudXYZI::Ptr，同时对点进行降采样，velodyne每隔4个点取一个点，同时要求点到雷达的距离大于盲区距离，存放到lidar_buffer中。

4.订阅IMU消息，在imu_cbk函数中处理：将IMU数据存放到imu_buffer中。

5. 主循环，详解见下。

主循环

sync_packages函数，将lidar_buffer中的最早数据及lidar点云最后一个点时间之前的所有imu数据，存放到Measures中。
如果是第一帧雷达数据，记录第一帧雷达时间，同时设置ImuProcess中的first_lidar_time，进入下一次循环；
将Measures、kf放入Imu::Process处理，更新kf、对点云进行运动畸变矫正。
- 首次进入，需要进行初始化。IMU_init 计算Measures中陀螺、加计的均值和方差：mean_acc、mean_gyr、cov_acc、cov_gyr。初始化grav=S2(- mean_acc / mean_acc.norm() * G_m_s2)、mean_gyr作为陀螺初始bias和IMU与lidar之间的安装角、杆臂。初始化P阵。P阵为24维：分别是 Pxyz、rot（SO3）、R_L_I（SO3）、T_L_I、Vxyz、Bgxyz、Baxyz、g（重力 S2）。
```
  esekfom::esekf<state_ikfom, 12, input_ikfom>::cov init_P = kf_state.get_P();  
  init_P.setIdentity();
  init_P(6,6) = init_P(7,7) = init_P(8,8) = 0.00001; // 安装角P
  init_P(9,9) = init_P(10,10) = init_P(11,11) = 0.00001; // 杆臂P
  init_P(15,15) = init_P(16,16) = init_P(17,17) = 0.0001; // 陀螺bias
  init_P(18,18) = init_P(19,19) = init_P(20,20) = 0.001; // 加计bias
  init_P(21,21) = init_P(22,22) = 0.00001; // 重力的前两维？？？
```
- 当IMU数量超过10个时，才认为初始化成功。对cov_acc进行固定重力模值矫正。cov_acc和cov_gyr使用main中传入的参数。若初始化成功，直接退出。
- 若已经初始化成功，UndistortPcl 对点云进行矫正。
  - 将上次保存的最后一帧IMU数据放入measure的imu中；
  - 对点云按照时间进行排序；
  - 从kf中获取X，并将其作为0时刻，连同上次计算的合加速度、角速度传入set_pose6d，存放到Pose6D的vector——IMUpose中；
  - 从last_lidar_end_time_开始，把所有measure中的IMU进行kf predict，同时计算合加速度、角速度（除bias），按照相对当前lidar pcl_beg_time的offset_time放入IMUpose中。注意：这里将相邻的加计、陀螺取均值后作为输入。
  - 按照最后两帧IMU的输入推算到pcl_end_time。
  - 根据lidar点的时间，找到IMUpose前后的帧，推算到lidar点后，计算相对pcl_end_time位姿的相对xyz，作为补偿后的坐标。
根据KF最新的IMU状态和杆臂，更新lidar中心点坐标，pos_lid。
距离第一帧lidar时间在0.1s内，认为flg_EKF_inited为false。
lasermap_fov_segment
- 如果Localmap没有初始化，根据lidar中心坐标计算locamap的范围，初始化成功，直接return。
- 检测当前lidar中心点距离边界的距离，当离边界小于一定阈值后，认为需要移动地图中心。不需移动，直接return。
- 按一定策略移动整个地图的对角点，并将移出的区域传入ikdtree，进行地图点的删除。
对矫正后的点云feats_undistort下采样，feats_down_body。
如果ikdtree.Root_Node == nullptr，降采样后点数大于5，设置ikdtree参数，将feats_down_body点转换到world下，build到ikdtree中。continue进入下一个循环。
如果feats_down_body的点数小于5，continue进入下一个循环。
根据feats_down_body点数，resize pointSearchInd_surf、Nearest_Points。
kf.update_iterated_dyn_share_modified 迭代量测更新KF
- 将x_propagated和P_propagated保存迭代前的x_和P_。
- 最大迭代次数内，进行for循环：此处需要学习一下迭代Kalman滤波的方法。
  - 将x_和dyn_share传入laserMapping中的 h_share_model函数（该函数是在kf初始时设置的）。h_share_model解析见下方。最终观测信息h和观测矩阵放入dyn_share中。
  - 如果没有返回符合条件的观测点，continue。
  - 将P_设为P_propagated（初始P_），再更新P_阵。
  - 如果观测维数小于state::DOF，计算K_、K_h、K_x；否则用另一种方法计算K_h和K_x。
  - 计算dx_，并更新到x_。
  - 如果dx_中有变量大于收敛数值，dyn_share.converge置为false。如果dyn_share.converge为true，t++。
  - 当t为0且当前迭代次数为maximum_iter - 2，将dyn_share.converge置为true。避免一直不更新点云的近邻点。
  - 当t > 1 || i == maximum_iter - 1时，基于最新的x_，遍历x_.SO3_state和x_.S2_state更新P_、L_。
重新获取kf的x，更新state_point、geoQuat（四元数）以及 lidar中心点坐标pos_lid。
发布odometry。
map_incremental 将feats_down_body中的点云用更新后的姿态重新计算world下的坐标，更新到iktree中。
- 如果点云中的点附近已经存在了点，计算该店距离cell中心点的距离，如果之前查找的最近点不在cell内，将其放入PointNoNeedDownsample，等待加入地图中。如果查找的附近点数量少于NUM_MATCH_POINTS（5）个，将该点放入PointToAdd中。如果查找的点数>=5个，该点是距离cell中心最近的点，将该点放入PointToAdd中；否则直接舍弃该点。
- 如果该点附近没有查找到点，直接将该点放入PointToAdd。
- ikdtree以降采样方式加入PointToAdd，正常方式加入PointNoNeedDownsample。
发布各种点用于rviz显示。
休眠0.2ms，进入下一循环。

h_share_model(state_ikfom &s, esekfom::dyn_share_datastruct<double> &ekfom_data)

开启并行计算，并行计算数量是在CMake中根据核心数量定义的。
```
        omp_set_num_threads(MP_PROC_NUM);
        #pragma omp parallel for
```
对每个feats_down_body点进行如下处理：
1. 根据状态s，计算新的world下坐标更新到point_world（feats_down_world中）。
2. 如果ekfom_data.converge，在ikdtree中搜索5个最近的点放入Nearest_Points[i]。如果点数小于5，point_selected_surf[i]设为false；否则，如果最远距离大于5m，设为false，小于等于5m，设为true。
3. 如果point_selected_surf[i]为false，继续下一个点，只能等下次x更新后，看看有没有可能用来进行观测了。
4. point_selected_surf[i]置为false。
5. 对搜索到的最近5个点进行esti_plane，计算平面法方程，并对每个点进行验证，超过阈值返回false。
6. 如果esti_plane成功，满足如下条件，才会将point_selected_surf[i]置为true。
```
float pd2 = pabcd(0) * point_world.x + pabcd(1) * point_world.y + pabcd(2) * point_world.z + pabcd(3);
float s = 1 - 0.9 * fabs(pd2) / sqrt(p_body.norm());

if (s > 0.9)
{
    point_selected_surf[i] = true;
    normvec->points[i].x = pabcd(0);
    normvec->points[i].y = pabcd(1);
    normvec->points[i].z = pabcd(2);
    normvec->points[i].intensity = pd2;
    res_last[i] = abs(pd2);
}
```
将point_selected_surf[i]为true对应的点和面法向量分别放入laserCloudOri->points和corr_normvect->points。
如果没有满足条件的点，ekfom_data.valid置为false，并return。
计算平均残差 res_mean_last，ekfom_data.h_x的维度为 effct_feat_num * 12。
对满足条件的点进行遍历：
1. Lidar body下坐标转换成skew矩阵point_be_crossmat。
2. 将Lidar body下的坐标转换成IMU坐标下point_this，之前所述的点云body坐标都是相对Lidar中心点的坐标。point_this转换成skew矩阵point_crossmat。
3. 根据point_be_crossmat、point_crossmat、平面法向量计算ekfom_data.h_x.block<1, 12>矩阵，如果不估计杆臂和安装角，则将ekfom_data.h_x.block<1, 12>的后6维置为0.
4. 该点的观测值设为ekfom_data.h(i) = -norm_p.intensity，点到平面距离的取反。