大家好呀,我是一个SLAM方向的在读博士,深知SLAM学习过程一路走来的坎坷,也十分感谢各位大佬的优质文章和源码。随着知识的越来越多,越来越细,我准备整理一个自己的激光SLAM学习笔记专栏,从0带大家快速上手激光SLAM,也方便想入门SLAM的同学和小白学习参考,相信看完会有一定的收获。如有不对的地方欢迎指出,欢迎各位大佬交流讨论,一起进步。博主创建了一个科研互助群Q:951026257,欢迎大家加入讨论。
Fast-lio2原理解析见链接
从零入门激光SLAM(二十一)——看不懂FAST-LIO?进来_fastlio 雷达 更改频率-CSDN博客
注释版代码完整版见
GitHub - huashu996/Fast-lio2-Supernote: Fast-lio2 code with note
本节对代码的整体框架进行解析,将代码与论文中的公式对应
一、代码文件构成
LOG:放日志文件
PCD:放点云地图PCD文件
Config:放系统配置文件,各种雷达的参数,与IMU外参等
Doc:支持文件,论文
Include:存放第三方库,ESKF和ikd-tree
Launch:代码启动文件
Msg:自定义消息文件
Rviz_cfg:rviz可视化的配置文件
Src:主要代码逻辑文件
代码解析主要看include和src文件夹下的文件
- Include
1.IKEF和Ikd-tree的库
2.一些数学计算的头文件 - Src
IMU_Processing.hpp:IMU预处理与点云去畸变
laserMapping.cpp:主函数,整体代码的逻辑
preprocess.cpp:一些简单函数和结构体的定义,例如回调函数,转换等二、代码与原理
2.1 IMU预测步
- 定义状态向量、输入噪声
MTK_BUILD_MANIFOLD(state_ikfom, ((vect3, pos)) ((SO3, rot)) ((SO3, offset_R_L_I)) ((vect3, offset_T_L_I)) ((vect3, vel)) ((vect3, bg)) ((vect3, ba)) ((S2, grav)) ); // 定义的输入状态 MTK_BUILD_MANIFOLD(input_ikfom, ((vect3, acc)) ((vect3, gyro)) ); //定义的协方差噪声格式 MTK_BUILD_MANIFOLD(process_noise_ikfom, ((vect3, ng)) ((vect3, na)) ((vect3, nbg)) ((vect3, nba)) );
- IMU运动方程
/* 计算角速度 omega。 计算惯性系下的加速度 a_inertial。 将速度、角速度和加速度填充到结果矩阵 res 中,并返回该矩阵。 */ Eigen::Matrix<double, 24, 1> get_f(state_ikfom &s, const input_ikfom &in) { Eigen::Matrix<double, 24, 1> res = Eigen::Matrix<double, 24, 1>::Zero(); vect3 omega; //从陀螺仪测量值中减去陀螺仪偏差,得到实际的角速度 omega in.gyro.boxminus(omega, s.bg); //计算惯性系加速度 vect3 a_inertial = s.rot * (in.acc-s.ba); for(int i = 0; i < 3; i++ ){ res(i) = s.vel[i]; res(i + 3) = omega[i]; res(i + 12) = a_inertial[i] + s.grav[i]; } return res; } //状态转移矩阵Fx Eigen::Matrix<double, 24, 23> df_dx(state_ikfom &s, const input_ikfom &in) { Eigen::Matrix<double, 24, 23> cov = Eigen::Matrix<double, 24, 23>::Zero(); //设置速度与加速度偏差之间的关系 cov.template block<3, 3>(0, 12) = Eigen::Matrix3d::Identity(); //计算修正后的加速度和角速度 vect3 acc_; in.acc.boxminus(acc_, s.ba); vect3 omega; in.gyro.boxminus(omega, s.bg); //设置加速度对角速度和重力偏差的关系 cov.template block<3, 3>(12, 3) = -s.rot.toRotationMatrix()*MTK::hat(acc_); cov.template block<3, 3>(12, 18) = -s.rot.toRotationMatrix(); //设置重力对其他状态变量的关系 Eigen::Matrix<state_ikfom::scalar, 2, 1> vec = Eigen::Matrix<state_ikfom::scalar, 2, 1>::Zero(); Eigen::Matrix<state_ikfom::scalar, 3, 2> grav_matrix; s.S2_Mx(grav_matrix, vec, 21); cov.template block<3, 2>(12, 21) = grav_matrix; //设置角速度偏差对角速度的关系 cov.template block<3, 3>(3, 15) = -Eigen::Matrix3d::Identity(); return cov; } //计算系统状态对噪声变量的雅可比矩阵。雅可比矩阵是偏导数矩阵Fw Eigen::Matrix<double, 24, 12> df_dw(state_ikfom &s, const input_ikfom &in) { Eigen::Matrix<double, 24, 12> cov = Eigen::Matrix<double, 24, 12>::Zero(); //设置加速度对加速度噪声的关系 cov.template block<3, 3>(12, 3) = -s.rot.toRotationMatrix(); //设置角速度对角速度噪声的关系 cov.template block<3, 3>(3, 0) = -Eigen::Matrix3d::Identity(); //设置陀螺仪偏差噪声 cov.template block<3, 3>(15, 6) = Eigen::Matrix3d::Identity(); //设置加速度计偏差噪声 cov.template block<3, 3>(18, 9) = Eigen::Matrix3d::Identity(); return cov; }
- 向前传播
详情请见...
从零入门激光SLAM(二十二)——Fast-lio2代码详解(零) 整体代码框架解析 - 古月居 (guyuehome.com)