从零入门激光SLAM（二十二）——Fast-lio2代码详解（零） 整体代码框架解析

大家好呀，我是一个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)