大家好呀，我是一个SLAM方向的在读博士，深知SLAM学习过程一路走来的坎坷，也十分感谢各位大佬的优质文章和源码。随着知识的越来越多，越来越细，我准备整理一个自己的激光SLAM学习笔记专栏，从0带大家快速上手激光SLAM，也方便想入门SLAM的同学和小白学习参考，相信看完会有一定的收获。如有不对的地方欢迎指出，欢迎各位大佬交流讨论，一起进步。博主创建了一个科研互助群Q：951026257，欢迎大家加入讨论。
Fast-lio2原理解析见链接
从零入门激光SLAM（二十一）——看不懂FAST-LIO？进来_fastlio 雷达 更改频率-CSDN博客
注释版代码完整版见
https://github.com/huashu996/Fast-lio2-Supernote
本代码解析以算法流程的逻辑解析代码，一些简单的函数忽略讲解。
一、初始化卡尔曼滤波
初始化了卡尔曼滤波器的动态共享数据结构。通过设置过程模型、状态转移矩阵雅可比、过程噪声雅可比、测量模型、最大迭代次数和收敛阈值，准备了卡尔曼滤波器所需的所有参数和模型，以便进行状态预测和更新
/*初始化卡尔曼滤波器 get_f：过程模型（Process Model） df_dx: 表示状态转移矩阵的雅可比矩阵 df_dw: 表示过程噪声矩阵的雅可比矩阵 h_share_model: 观测模型 NUM_MAX_ITERATIONS: 表示最大迭代次数 epsi：表示收敛阈值（Convergence Threshold） */
kf.init_dyn_share(get_f, df_dx, df_dw, h_share_model, NUM_MAX_ITERATIONS, epsi);
/*** ROS subscribe initialization ***/
//sub
ros::Subscriber sub_pcl = p_pre->lidar_type == AVIA ? \
nh.subscribe(lid_topic, 200000, livox_pcl_cbk) : \
nh.subscribe(lid_topic, 200000, standard_pcl_cbk);
ros::Subscriber sub_imu = nh.subscribe(imu_topic, 200000, imu_cbk);
//pub
ros::Publisher pubLaserCloudFull = nh.advertise<sensor_msgs::PointCloud2>
("/cloud_registered", 100000);
ros::Publisher pubLaserCloudFull_body = nh.advertise<sensor_msgs::PointCloud2>
("/cloud_registered_body", 100000);
ros::Publisher pubLaserCloudEffect = nh.advertise<sensor_msgs::PointCloud2>
("/cloud_effected", 100000);
ros::Publisher pubLaserCloudMap = nh.advertise<sensor_msgs::PointCloud2>
("/Laser_map", 100000);
ros::Publisher pubOdomAftMapped = nh.advertise<nav_msgs::Odometry>
("/Odometry", 100000);
ros::Publisher pubPath = nh.advertise<nav_msgs::Path>
("/path", 100000);
-
初始化卡尔曼滤波参数
//接收系统特定的模型及其差异 //作为特征矩阵的测量，其维数是变化的。 //通过一个函数(h_dyn_share_in)完成了测量(z)，估计测量(h)，偏微分矩阵(h_x, h_v)和噪声协方差(R)的同时计算。 void init_dyn_share(processModel f_in, processMatrix1 f_x_in, processMatrix2 f_w_in, measurementModel_dyn_share h_dyn_share_in, int maximum_iteration, scalar_type limit_vector[n]) { //预测模型 f = f_in; //状态转移矩阵 f_x = f_x_in; //过程噪声矩阵 f_w = f_w_in; //动态共享观测模型 h_dyn_share = h_dyn_share_in; //最大迭代次数 maximum_iter = maximum_iteration; for(int i=0; i<n; i++) { limit[i] = limit_vector[i]; } x_.build_S2_state(); x_.build_SO3_state(); x_.build_vect_state(); }二、IMU预测与前向传播
2.1 整体框架
当IMU数据与LIDAR数据同步后，初始化系统参数，进入p_imu->Process函数进行对IMU数据处理，获得当前的预测位姿state_point
signal(SIGINT, SigHandle); ros::Rate rate(5000); bool status = ros::ok(); while (status) { if (flg_exit) break; ros::spinOnce(); if(sync_packages(Measures)) //信息同步 { //如果是第一帧 if (flg_first_scan) { first_lidar_time = Measures.lidar_beg_time; p_imu->first_lidar_time = first_lidar_time; flg_first_scan = false; continue; } auto start = std::chrono::high_resolution_clock::now(); //初始化参数 double t0,t1,t2,t3,t4,t5,match_start, solve_start, svd_time; match_time = 0; kdtree_search_time = 0.0; solve_time = 0; solve_const_H_time = 0; svd_time = 0; t0 = omp_get_wtime(); // 1. 对IMU数据进行预处理，其中包含了点云畸变处理 前向传播 反向传播 p_imu->Process(Measures, kf, feats_undistort); // 2.获取kf预测的全局状态（imu） state_point = kf.get_x(); //imu先验位姿2.2 IMU处理流程
meas：包含IMU和LiDAR数据的测量组。
kf_state：滤波器的状态，类型为 esekfom::esekf<state_ikfom, 12,="" input_ikfom="">。
cur_pcl_un_：当前未去畸变的点云数据，类型为 PointCloudXYZI::Ptr。</state_ikfom,>
- 初始化时间：记录开始处理的时间。
- 检查IMU数据和LiDAR数据：确保IMU数据不为空，并且LiDAR数据指针有效。
- IMU初始化：如果需要初始化IMU，调用 IMU_init 进行初始化，并检查初始化迭代次数，超过最大计数后，调整加速度协方差并标记初始化完成。
- 点云去畸变：调用 UndistortPcl 函数对点云数据进行去畸变处理。
-
记录时间：记录去畸变处理后的时间，用于后续分析和调试。
void ImuProcess::Process(const MeasureGroup &meas, esekfom::esekf<state_ikfom, 12, input_ikfom> &kf_state, PointCloudXYZI::Ptr cur_pcl_un_) { //初始化时间测量： double t1,t2,t3; t1 = omp_get_wtime(); //检查IMU数据是否为空： if(meas.imu.empty()) {return;}; //检查LiDAR数据指针是否为NULL： ROS_ASSERT(meas.lidar != nullptr); //IMU初始化： if (imu_need_init_) { /// The very first lidar frame //调用 IMU_init 函数进行初始化 IMU_init(meas, kf_state, init_iter_num); imu_need_init_ = true; //保存最新的IMU数据 last_imu_ = meas.imu.back(); //获取滤波器状态 state_ikfom imu_state = kf_state.get_x(); //如果初始化迭代次数超过最大计数 if (init_iter_num > MAX_INI_COUNT) { //调整加速度协方差 cov_acc *= pow(G_m_s2 / mean_acc.norm(), 2); imu_need_init_ = false; //重置加速度和陀螺仪的协方差比例 cov_acc = cov_acc_scale; cov_gyr = cov_gyr_scale; //打印IMU初始化完成信息。 ROS_INFO("IMU Initial Done"); // ROS_INFO("IMU Initial Done: Gravity: %.4f %.4f %.4f %.4f; state.bias_g: %.4f %.4f %.4f; acc covarience: %.8f %.8f %.8f; gry covarience: %.8f %.8f %.8f",\ // imu_state.grav[0], imu_state.grav[1], imu_state.grav[2], mean_acc.norm(), cov_bias_gyr[0], cov_bias_gyr[1], cov_bias_gyr[2], cov_acc[0], cov_acc[1], cov_acc[2], cov_gyr[0], cov_gyr[1], cov_gyr[2]); fout_imu.open(DEBUG_FILE_DIR("imu.txt"),ios::out); } return; } //前向传播，点云去畸变 UndistortPcl(meas, kf_state, *cur_pcl_un_); //记录时间 t2 = omp_get_wtime(); t3 = omp_get_wtime(); // cout<<"[ IMU Process ]: Time: "<<t3 - t1<<endl; }
- IMU初始化
用于初始化IMU，包括计算重力、陀螺仪偏差、加速度和陀螺仪的协方差，并将加速度测量归一化为单位重力
meas：包含IMU和LiDAR数据的测量组。
kf_state：滤波器的状态，类型为 esekfom::esekf<state_ikfom, 12,="" input_ikfom="">。
N：用于记录初始化过程中IMU数据的计数。</state_ikfom,>
- 初始化重力、陀螺仪偏差、加速度和陀螺仪的协方差：初始化当前加速度和陀螺仪数据。
- 处理第一帧IMU数据：如果是第一帧数据，重置参数并初始化平均加速度和陀螺仪。
- 计算均值和协方差：通过遍历IMU数据，计算加速度和陀螺仪的均值和协方差。
- 初始化滤波器状态：根据计算的均值和协方差，初始化滤波器状态，包括重力、陀螺仪偏差和平移旋转偏差。
- 初始化滤波器协方差矩阵：设置滤波器协方差矩阵的初始值。
- 记录最新IMU数据：保存最后一个IMU数据以备后续使用。
-
void ImuProcess::IMU_init(const MeasureGroup &meas, esekfom::esekf<state_ikfom, 12, input_ikfom> &kf_state, int &N) { /** 1. initializing the gravity, gyro bias, acc and gyro covariance ** 2. normalize the acceleration measurenments to unit gravity **/ //初始化协方差 V3D cur_acc, cur_gyr; //处理第一帧IMU数据： if (b_first_frame_) { Reset(); // 重置初始化参数 N = 1; // 设置初始计数 b_first_frame_ = false; // 标记第一帧已经处理 const auto &imu_acc = meas.imu.front()->linear_acceleration; //提取线加速度 const auto &gyr_acc = meas.imu.front()->angular_velocity; //提取角速度 mean_acc << imu_acc.x, imu_acc.y, imu_acc.z; // 初始化平均加速度 mean_gyr << gyr_acc.x, gyr_acc.y, gyr_acc.z; // 初始化平均陀螺仪 first_lidar_time = meas.lidar_beg_time; // 记录第一帧LiDAR时间 } //计算加速度和陀螺仪的均值和协方差： for (const auto &imu : meas.imu) { const auto &imu_acc = imu->linear_acceleration; const auto &gyr_acc = imu->angular_velocity; cur_acc << imu_acc.x, imu_acc.y, imu_acc.z; cur_gyr << gyr_acc.x, gyr_acc.y, gyr_acc.z; mean_acc += (cur_acc - mean_acc) / N; // 更新平均加速度 mean_gyr += (cur_gyr - mean_gyr) / N; // 更新平均陀螺仪 // 更新加速度协方差 cov_acc = cov_acc * (N - 1.0) / N + (cur_acc - mean_acc).cwiseProduct(cur_acc - mean_acc) * (N - 1.0) / (N * N); // 更新陀螺仪协方差 cov_gyr = cov_gyr * (N - 1.0) / N + (cur_gyr - mean_gyr).cwiseProduct(cur_gyr - mean_gyr) * (N - 1.0) / (N * N); // cout<<"acc norm: "<<cur_acc.norm()<<" "<<mean_acc.norm()<<endl; N ++; } //初始化滤波器状态 state_ikfom init_state = kf_state.get_x(); init_state.grav = S2(- mean_acc / mean_acc.norm() * G_m_s2); //设置重力方向 //state_inout.rot = Eye3d; // Exp(mean_acc.cross(V3D(0, 0, -1 / scale_gravity))); init_state.bg = mean_gyr; // 设置陀螺仪偏差 init_state.offset_T_L_I = Lidar_T_wrt_IMU; // 设置LiDAR相对于IMU的平移偏差 init_state.offset_R_L_I = Lidar_R_wrt_IMU; // 设置LiDAR相对于IMU的旋转偏差 kf_state.change_x(init_state); // 更新滤波器状态 //初始化滤波器协方差矩阵 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; // 设置位置协方差 init_P(9,9) = init_P(10,10) = init_P(11,11) = 0.00001; // 设置速度协方差 init_P(15,15) = init_P(16,16) = init_P(17,17) = 0.0001; // 设置陀螺仪偏差协方差 init_P(18,18) = init_P(19,19) = init_P(20,20) = 0.001; // 设置加速度偏差协方差 init_P(21,21) = init_P(22,22) = 0.00001; // 设置其他状态协方差 kf_state.change_P(init_P); // 更新滤波器协方差矩阵 //记录最新的IMU数据： last_imu_ = meas.imu.back(); }
- 点云畸变
用于对点云数据进行去畸变处理
meas：包含IMU和LiDAR数据的测量组。
kf_state：滤波器的状态，类型为 esekfom::esekf<state_ikfom, 12,="" input_ikfom="">。
pcl_out：输出去畸变后的点云数据，类型为 PointCloudXYZI。</state_ikfom,>
- 添加上一帧IMU数据：确保IMU数据连续性。
- 排序点云数据：按时间偏移排序点云。
- 初始化IMU位姿：获取滤波器状态并初始化IMU位姿列表。
- 前向传播每个IMU数据点：遍历IMU数据点，更新滤波器状态并保存IMU位姿。
- 计算帧末的位姿预测：根据点云结束时间预测帧末位姿。
-
对点云进行去畸变：遍历点云数据，使用IMU数据对点云进行去畸变处理。
详情请见...
从零入门激光SLAM（二十二）——Fast-lio2代码详解（一） IMU预测与点云去除畸变 - 古月居 (guyuehome.com)
662
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
