FAST-LIO2 code analysis (2)

0. 简介

The operation of data acquisition and feature point extraction of lidar is described above,This section we will be aroundIMU_ProcessingThis section will introduce.

1. ImuProcess类定义

在ImuProcess.hpp中,It's done right from the startImuProcess类的申明,Inside we can see inimuThe most important of these are the two characteristics of angular velocity and acceleration,It's with visionSLAM的imuFusion is very similar.

//Determine whether the time of the point is reversed
const bool time_list(PointType &x, PointType &y) {
     return (x.curvature < y.curvature); };

/// *************IMU Process and undistortion
class ImuProcess
{
    
public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW

  ImuProcess();
  ~ImuProcess();

  void Reset();
  void Reset(double start_timestamp, const sensor_msgs::ImuConstPtr &lastimu);
  void set_extrinsic(const V3D &transl, const M3D &rot);
  void set_extrinsic(const V3D &transl);
  void set_extrinsic(const MD(4, 4) & T);
  void set_gyr_cov(const V3D &scaler);
  void set_acc_cov(const V3D &scaler);
  void set_gyr_bias_cov(const V3D &b_g);
  void set_acc_bias_cov(const V3D &b_a);
  Eigen::Matrix<double, 12, 12> Q;
  void Process(const MeasureGroup &meas, esekfom::esekf<state_ikfom, 12, input_ikfom> &kf_state, PointCloudXYZI::Ptr pcl_un_);

  ofstream fout_imu;       // imuparameter output file
  V3D cov_acc;             //Acceleration measurement covariance
  V3D cov_gyr;             //Angular velocity measurement covariance
  V3D cov_acc_scale;       //Acceleration measurement covariance
  V3D cov_gyr_scale;       //Angular velocity measurement covariance
  V3D cov_bias_gyr;        //Angular velocity measurement covariance bias
  V3D cov_bias_acc;        //Acceleration measurement covariance bias
  double first_lidar_time; //The first point cloud time of the current frame

private:
  void IMU_init(const MeasureGroup &meas, esekfom::esekf<state_ikfom, 12, input_ikfom> &kf_state, int &N);
  void UndistortPcl(const MeasureGroup &meas, esekfom::esekf<state_ikfom, 12, input_ikfom> &kf_state, PointCloudXYZI &pcl_in_out);

  PointCloudXYZI::Ptr cur_pcl_un_;        //The point cloud of the current frame is not dedistorted
  sensor_msgs::ImuConstPtr last_imu_;     // 上一帧imu
  deque<sensor_msgs::ImuConstPtr> v_imu_; // imu队列
  vector<Pose6D> IMUpose;                 // imu位姿
  vector<M3D> v_rot_pcl_;                 //未使用
  M3D Lidar_R_wrt_IMU;                    // lidar到IMUof rotating external parameters
  V3D Lidar_T_wrt_IMU;                    // lidar到IMUThe location external parameter
  V3D mean_acc;                           //mean acceleration,用于计算方差
  V3D mean_gyr;                           //Average angular velocity,用于计算方差
  V3D angvel_last;                        //The angular velocity of the previous frame
  V3D acc_s_last;                         //The acceleration of the previous frame
  double start_timestamp_;                //开始时间戳
  double last_lidar_end_time_;            //Last frame end timestamp
  int init_iter_num = 1;                  //初始化迭代次数
  bool b_first_frame_ = true;             //Is it the first frame
  bool imu_need_init_ = true;             //是否需要初始化imu
};

2. Constructor and incoming function

The first is to call the initialized constructor and reset function.

ImuProcess::ImuProcess()
    : b_first_frame_(true), imu_need_init_(true), start_timestamp_(-1)
{
    
  init_iter_num = 1;                          //初始化迭代次数
  Q = process_noise_cov();                    //调用use-ikfom.hpp里面的process_noise_covComplete the initialization of the noise covariance
  cov_acc = V3D(0.1, 0.1, 0.1);               //Acceleration measurement covariance initialization
  cov_gyr = V3D(0.1, 0.1, 0.1);               //Angular velocity measurement covariance initialization
  cov_bias_gyr = V3D(0.0001, 0.0001, 0.0001); //Angular velocity measurement covariance bias initialization
  cov_bias_acc = V3D(0.0001, 0.0001, 0.0001); //Acceleration measurement covariance bias initialization
  mean_acc = V3D(0, 0, -1.0);
  mean_gyr = V3D(0, 0, 0);
  angvel_last = Zero3d;                    //The angular velocity of the previous frame is initialized
  Lidar_T_wrt_IMU = Zero3d;                // lidar到IMUThe positional extrinsic parameter is initialized
  Lidar_R_wrt_IMU = Eye3d;                 // lidar到IMUThe rotation extrinsic parameter is initialized
  last_imu_.reset(new sensor_msgs::Imu()); //上一帧imu初始化
}

ImuProcess::~ImuProcess() {
    }

//重置参数
void ImuProcess::Reset()
{
    
  // ROS_WARN("Reset ImuProcess");
  mean_acc = V3D(0, 0, -1.0);
  mean_gyr = V3D(0, 0, 0);
  angvel_last = Zero3d;
  imu_need_init_ = true;                   //是否需要初始化imu
  start_timestamp_ = -1;                   //开始时间戳
  init_iter_num = 1;                       //初始化迭代次数
  v_imu_.clear();                          // imu队列清空
  IMUpose.clear();                         // imuPose is empty
  last_imu_.reset(new sensor_msgs::Imu()); //上一帧imu初始化
  cur_pcl_un_.reset(new PointCloudXYZI()); //The point cloud of the current frame is initialized without dewarping
}

Then there is the incoming of external parameters and bias covariance

//Incoming external parameters,包含R,T
void ImuProcess::set_extrinsic(const MD(4, 4) & T)
{
    
  Lidar_T_wrt_IMU = T.block<3, 1>(0, 3);
  Lidar_R_wrt_IMU = T.block<3, 3>(0, 0);
}

//Incoming external parameters,包含T
void ImuProcess::set_extrinsic(const V3D &transl)
{
    
  Lidar_T_wrt_IMU = transl;
  Lidar_R_wrt_IMU.setIdentity();
}

// Incoming external parameters,包含R,T
void ImuProcess::set_extrinsic(const V3D &transl, const M3D &rot)
{
    
  Lidar_T_wrt_IMU = transl;
  Lidar_R_wrt_IMU = rot;
}

// Pass in the gyroscope angular velocity covariance
void ImuProcess::set_gyr_cov(const V3D &scaler)
{
    
  cov_gyr_scale = scaler;
}

// Pass in the accelerometer acceleration covariance
void ImuProcess::set_acc_cov(const V3D &scaler)
{
    
  cov_acc_scale = scaler;
}
// Pass in the gyroscope angular velocity covariance bias
void ImuProcess::set_gyr_bias_cov(const V3D &b_g)
{
    
  cov_bias_gyr = b_g;
}
// Pass in the accelerometer acceleration covariance bias
void ImuProcess::set_acc_bias_cov(const V3D &b_a)
{
    
  cov_bias_acc = b_a;
}

3. IMU初始化

这一部分主要是IMU的初始化,内部主要是对x_和P_完成了初始化,这里涉及到esikfomKnowledge inside the file,I won't talk about it here for now

void ImuProcess::IMU_init(const MeasureGroup &meas, esekfom::esekf<state_ikfom, 12, input_ikfom> &kf_state, int &N)
{
    
  /** 1. 初始化重力、陀螺偏差、acc和陀螺仪协方差 /** 2. 将加速度测量值标准化为单位重力**/
  //这里应该是静止初始化

  V3D cur_acc, cur_gyr;

  if (b_first_frame_) //判断是否为第一帧
  {
    
    Reset(); //重置参数
    N = 1;   //将迭代次数置1
    b_first_frame_ = false;
    const auto &imu_acc = meas.imu.front()->linear_acceleration; //从common_lib.h中拿到imuacceleration at the initial moment
    const auto &gyr_acc = meas.imu.front()->angular_velocity;    //从common_lib.h中拿到imuAngular velocity at the initial moment
    mean_acc << imu_acc.x, imu_acc.y, imu_acc.z;                 //Acceleration measurements are taken as the initialization mean
    mean_gyr << gyr_acc.x, gyr_acc.y, gyr_acc.z;                 //Angular velocity measurements are taken as the initialization mean
    first_lidar_time = meas.lidar_beg_time;                      //will be currentimu帧对应的lidartime as the initial time
  }
  //计算方差
  for (const auto &imu : meas.imu) //拿到所有的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;
    //Based on the current frame and the mean difference as an update of the mean
    mean_acc += (cur_acc - mean_acc) / N;
    mean_gyr += (cur_gyr - mean_gyr) / N;
    //.cwiseProduct()对应系数相乘
    //The mean changes after each iteration,最后的方差公式中减的应该是最后的均值
    // https://blog.csdn.net/weixin_44479136/article/details/90510374 方差迭代计算公式
    //按照博客推导出来的下面方差递推公式有两种
    //第一种是
    cov_acc = cov_acc * (N - 1.0) / N + (cur_acc - mean_acc).cwiseProduct(cur_acc - mean_acc) / (N - 1.0);
    cov_gyr = cov_gyr * (N - 1.0) / N + (cur_gyr - mean_gyr).cwiseProduct(cur_gyr - mean_gyr) / (N - 1.0);
    //第二种是
    // cov_acc = cov_acc * (N - 1.0) / N + (cur_acc - mean_acc).cwiseProduct(cur_acc - 上一次的mean_acc) / N;
    // cov_gyr = cov_gyr * (N - 1.0) / N + (cur_gyr - mean_gyr).cwiseProduct(cur_gyr - 上一次的mean_gyr) / N;
    // cout<<"acc norm: "<<cur_acc.norm()<<" "<<mean_acc.norm()<<endl;
    N++;
  }
  state_ikfom init_state = kf_state.get_x();                  //在esekfom.hpp获得x_的状态
  init_state.grav = S2(-mean_acc / mean_acc.norm() * G_m_s2); //从common_lib.hGravity in,And it is calculated with the unit gravity of the mean value of the acceleration measurementSO2The gravitational acceleration of the rotation matrix type

  // state_inout.rot = Eye3d; // Exp(mean_acc.cross(V3D(0, 0, -1 / scale_gravity)));
  init_state.bg = mean_gyr;                  //Angular velocity is measured as gyroscope bias
  init_state.offset_T_L_I = Lidar_T_wrt_IMU; //将lidar和imuThe external parameter displacement is passed in
  init_state.offset_R_L_I = Lidar_R_wrt_IMU; //将lidar和imuThe external parameter rotation is passed in
  kf_state.change_x(init_state);             //Pass in the initialization stateesekfom.hpp中的x_

  esekfom::esekf<state_ikfom, 12, input_ikfom>::cov init_P = kf_state.get_P(); //在esekfom.hpp获得P_的协方差矩阵
  init_P.setIdentity();                                                        //Set the covariance matrix to the identity matrix
  init_P(6, 6) = init_P(7, 7) = init_P(8, 8) = 0.00001;                        //Set the covariance of the position and rotation of the covariance matrix to 0.00001
  init_P(9, 9) = init_P(10, 10) = init_P(11, 11) = 0.00001;                    //Set the covariance of the covariance matrix of velocity and pose to 0.00001
  init_P(15, 15) = init_P(16, 16) = init_P(17, 17) = 0.0001;                   //Set the covariance of gravity and attitude of the covariance matrix to 0.0001
  init_P(18, 18) = init_P(19, 19) = init_P(20, 20) = 0.001;                    //Set the gyroscope bias of the covariance matrix and the covariance of the attitude as 0.001
  init_P(21, 21) = init_P(22, 22) = 0.00001;                                   //will be the covariance matrixlidar和imuThe covariance of extrinsic displacements is set to 0.00001
  kf_state.change_P(init_P);                                                   //Pass in the initialized covariance matrixesekfom.hpp中的P_
  last_imu_ = meas.imu.back();                                                 //将最后一帧的imu数据传入last_imu_中,暂时没用到
}

4. IMU更新

在UndistortPclNot only in the functionIMUforward information,There is also the issue of lidar de-distortion,This section we revolve aroundIMUThe forward propagation of ,The code is done in the form of iterationIMU数据的更新,并将acc和gyro的数据传入到ESKF中,The detailed formula will be discussed later.

…详情请参照古月居