FastLIO代码阅读之一:IMU数据处理

FastLIO代码阅读之一:IMU数据处理

文章目录

  • FastLIO代码阅读之一:IMU数据处理
    • 1 对IMU数据的前向传播
    • 2 后向传播
    • 3 IMU数据处理函数

SLAM小白,刚开始学习,请各位大佬抱着批判的眼光看待博客。有错误还请重喷,随时修改~

这两部分在代码中主要对应到IMU_Processing.hpp中的UndistortPcl函数。

void ImuProcess::UndistortPcl(const MeasureGroup &meas, esekfom::esekf &kf_state, PointCloudXYZI &pcl_out);

1 对IMU数据的前向传播

这个函数主要实现了论文中的前向传播和反向传播。

  • 前向传播:用于获得一次LiDAR_Scan中的IMU位姿和误差,从接收到IMU输入开始,用:

    • x ^ i + 1 = x ^ i ⊞ ( Δ t f ( x ^ i , u i , 0 ) ) , x 0 = x ‾ k − 1     ( 4 ) \hat{x}_{i+1}=\hat{x}_i\boxplus(\Delta tf(\hat{x}_i, u_i, 0)), x_0=\overline{x}_{k-1} \space\space\space(4) x^i+1=x^i(Δtf(x^i,ui,0)),x0=xk1   (4)传播状态;

    • x ~ i + 1 = x i + 1 ⊟ x ^ i + 1         = ( x i ⊞ ( Δ t f ( x i , u i , w i ) ) ⊟ ( x ^ i ⊞ ( Δ t f ( x ^ i , u i , 0 ) ) )     ( 5 ) \begin{array}{lcl} \widetilde{x}_{i+1}=x_{i+1}\boxminus \hat{x}_{i+1}\\ \space\space\space\space\space\space\space=(x_i\boxplus(\Delta tf(x_i,u_i,w_i))\boxminus(\hat{x}_i\boxplus(\Delta tf(\hat{x}_i,u_i,0)))\space\space\space \end{array} (5) x i+1=xi+1x^i+1       =(xi(Δtf(xi,ui,wi))(x^i(Δtf(x^i,ui,0)))   (5)传播协方差;

​ 一直到这一次扫描结束,会得到一个传播结果: x ^ k , P ^ k \hat{x}_k, \hat{P}_k x^k,P^k a ^ \hat{a} a^表示IKEF中的先验信息)。 P ^ k = x k ⊟ x ^ k \hat{P}_k=x_k\boxminus\hat{x}_k P^k=xkx^k.

入参为

  • meas:测量数据,主要包括imu和lidar两部分。
    meas的数据结构
struct MeasureGroup     // Lidar data and imu dates for the curent process
{
    MeasureGroup()
    {
        lidar_beg_time = 0.0;
        // typedef pcl::PointCloud PointCloudXYZI;
        // 这里本质上是新生成一个PointCloud对象指针并调用boost::shared_ptr::swap和当前的雷达互换内容
        this->lidar.reset(new PointCloudXYZI());	
    };
    double lidar_beg_time;
    double lidar_end_time;
    // 这里的lidar是一个点云的指针,也可以理解为是一个数组的起始位置。
    PointCloudXYZI::Ptr lidar;
    // imu是一个双向队列,相比于vector,不需要整块空间且可以从头部和尾部执行插入删除。
    deque<sensor_msgs::Imu::ConstPtr> imu;
};
  • kf_state
  • pcl_out:未加处理的点云信息
void ImuProcess::UndistortPcl(const MeasureGroup &meas, esekfom::esekf<state_ikfom, 12, input_ikfom> &kf_state, PointCloudXYZI &pcl_out)
{
  /*** add the imu of the last frame-tail to the of current frame-head ***/
  auto v_imu = meas.imu;	// imu数据,队列
  v_imu.push_front(last_imu_);	// 将上一次的imu数据加入队列
  const double &imu_beg_time = v_imu.front()->header.stamp.toSec();
  const double &imu_end_time = v_imu.back()->header.stamp.toSec();    
  const double &pcl_beg_time = meas.lidar_beg_time;
  const double &pcl_end_time = meas.lidar_end_time;
  
  /*** sort point clouds by offset time ***/
  pcl_out = *(meas.lidar);	// 取值,拿到lidar数据
  sort(pcl_out.points.begin(), pcl_out.points.end(), time_list);	// 对雷达数据按照时间先后进行排序
  // cout<<"[ IMU Process ]: Process lidar from "<

  /*** Initialize IMU pose ***/
  state_ikfom imu_state = kf_state.get_x();		// 获取先验imu姿态 \hat{x}_k
  IMUpose.clear();
  IMUpose.push_back(set_pose6d(0.0, acc_s_last, angvel_last, imu_state.vel, imu_state.pos, imu_state.rot.toRotationMatrix()));	// 存储IMU信息[时间间隔、上一帧加速度、角速度、速度、位置、旋转矩阵],论文中提到初始值为上一时刻的最优值。

  /*** forward propagation at each imu point ***/
  V3D angvel_avr, acc_avr, acc_imu, vel_imu, pos_imu;
  M3D R_imu;	// imu旋转矩阵

  double dt = 0;

  input_ikfom in;

  // 用iterator的方式迭代处理imu信息。
  for (auto it_imu = v_imu.begin(); it_imu < (v_imu.end() - 1); it_imu++)
  {
    auto &&head = *(it_imu);	// *(it_imu)其实就是拿到存放imu的deque,deque中存放的是shared_ptr,shared_ptr的类型是IMU信息的空间地址,所以这里的&&head拿到的就是imu数据的地址。
    auto &&tail = *(it_imu + 1); // 下一个imu测量数据
    
    // TODO:这里的last_lidar_end_time_是上一次scan结束的雷达还是上一次雷达特征点时间?
    if (tail->header.stamp.toSec() < last_lidar_end_time_)    continue; 	// 判断时间先后顺序。
    
    // 取两帧速度的中值
    angvel_avr<<0.5 * (head->angular_velocity.x + tail->angular_velocity.x),
                0.5 * (head->angular_velocity.y + tail->angular_velocity.y),
                0.5 * (head->angular_velocity.z + tail->angular_velocity.z);
    acc_avr   <<0.5 * (head->linear_acceleration.x + tail->linear_acceleration.x),
                0.5 * (head->linear_acceleration.y + tail->linear_acceleration.y),
                0.5 * (head->linear_acceleration.z + tail->linear_acceleration.z);

    // fout_imu << setw(10) << head->header.stamp.toSec() - first_lidar_time << " " << angvel_avr.transpose() << " " << acc_avr.transpose() << endl;
    // TODO: 平均加速度*重力加速度/加速度均值的标准差含义?
    acc_avr  = acc_avr * G_m_s2 / mean_acc.norm(); // - state_inout.ba;

    // 计算时间间隔
    if(head->header.stamp.toSec() < last_lidar_end_time_)
    {
      dt = tail->header.stamp.toSec() - last_lidar_end_time_;
      // dt = tail->header.stamp.toSec() - pcl_beg_time;
    }
    else
    {
      dt = tail->header.stamp.toSec() - head->header.stamp.toSec();
    }
    
    // 原始测量值的中值
    in.acc = acc_avr;
    in.gyro = angvel_avr;
    Q.block<3, 3>(0, 0).diagonal() = cov_gyr;
    Q.block<3, 3>(3, 3).diagonal() = cov_acc;
    Q.block<3, 3>(6, 6).diagonal() = cov_bias_gyr;
    Q.block<3, 3>(9, 9).diagonal() = cov_bias_acc;
    kf_state.predict(dt, Q, in);

    /* save the poses at each IMU measurements */
    imu_state = kf_state.get_x();
    // imu_state这里的流形相关代码暂时看不下去,太绕了,一层套一层的宏定义==,后续再来看吧,先过代码。 
    angvel_last = angvel_avr - imu_state.bg;	// 计算得到的角速度与预测的角速度的差。

    // TODO:确认acc_s_last中x,y,z的顺序
    acc_s_last  = imu_state.rot * (acc_avr - imu_state.ba); // 计算得到的加速度与预测的加速度的差,并转换到预测imu坐标系下。
    // 三个方向的加速度都加上重力,重力加速度矩阵为[9.809, 0, 0],这里感觉维度顺序对不上,还要仔细看看。
    for(int i=0; i<3; i++)
    {
      acc_s_last[i] += imu_state.grav[i];    
    }
    double &&offs_t = tail->header.stamp.toSec() - pcl_beg_time; // 后一个IMU时刻距离此次雷达开始的时间间隔
    IMUpose.push_back(set_pose6d(offs_t, acc_s_last, angvel_last, imu_state.vel, imu_state.pos, imu_state.rot.toRotationMatrix()));   
  }

  /*** calculated the pos and attitude prediction at the frame-end ***/
  double note = pcl_end_time > imu_end_time ? 1.0 : -1.0;
  dt = note * (pcl_end_time - imu_end_time);

  kf_state.predict(dt, Q, in);    // IMU前向传播,用中值作为更新状态
  
  /*save the poses at each IMU measurements*/
  imu_state = kf_state.get_x();  	// 获取到更新后的IMU姿态
  last_imu_ = meas.imu.back();		// 上一帧imu数据为IMU数列的最后一个对象
  last_lidar_end_time_ = pcl_end_time;	// 设置上一帧雷达结束时间

2 后向传播

  • 反向传播:用于对齐雷达测量时坐标系与扫描结束时坐标系。

    • Lidar数据的相对误差( ρ j ≤ t k \rho_j\le t_k ρjtk),会加剧里程计的误差。因此需要修正位姿映射到k时刻。对于两次IMU测量期间的所有LiDAR特征点,都用左侧的IMU测量值作为反向传播输入。
    • x ˘ j − 1 = x ˘ j ⊞ ( − Δ t f ( x ˘ j , u j , 0 ) ) 式 1 \breve{x}_{j-1}=\breve{x}_j\boxplus(-\Delta tf(\breve{x}_j,u_j,0)) 式1 x˘j1=x˘j(Δtf(x˘j,uj,0))1. j-1 时刻的最优估计由 j 时刻的最优估计减去 j-1 时刻的运动量。

    FastLIO代码阅读之一:IMU数据处理_第1张图片

    反向传播中的运动方程:

    • 上一时刻IMU的位置,为这一时刻IMU位置减去位移。
    • 上一时刻IMU的速度,为这一时刻IMU速度减去速度(线性加速度+重力加速度)变化量
    • 上一时刻IMU的旋转矩阵为这一时刻旋转矩阵乘以旋转矩阵(角速度*时间)

    反向传播会生成一个相对位姿 I k T ˘ I j ^{I_k}\breve{T}_{I_j} IkT˘Ij

    通过 L k P f j = I T L − 1 I k T ˘ I j I T L L j P f j     ( 10 ) ^{L_k}P_{f_j}={^IT^{-1}_{L}}{^{I_k}\breve{T}_{I_j}}{^IT_L}{^{L_j}P_{f_j}}\space\space\space(10) LkPfj=ITL1IkT˘IjITLLjPfj   (10)将特征点从该点采样时的雷达坐标系映射到一次Scan扫描结束时的雷达坐标系。

  // 后向传播
  /*** undistort each lidar point (backward propagation) ***/
  if (pcl_out.points.begin() == pcl_out.points.end()) return;	// 如果存放点云的vector为空(无点)则退出
  auto it_pcl = pcl_out.points.end() - 1;  // 拿到最后一个雷达点云数据
    
  // 倒序遍历IMU数据
  for (auto it_kp = IMUpose.end() - 1; it_kp != IMUpose.begin(); it_kp--)
  {
    // 取相邻两帧IMU数据作为head和tail,这里是要做运动补偿,将数据从雷达采样时间映射到IMU帧时间
    auto head = it_kp - 1;	// 当前雷达点的左侧IMU帧
    auto tail = it_kp;		// 当前雷达点的右侧IMU帧
	
    R_imu<<MAT_FROM_ARRAY(head->rot);	// 将左侧IMU数据的旋转矩阵赋值给R_imu
    // cout<<"head imu acc: "<
    vel_imu<<VEC_FROM_ARRAY(head->vel);	// 左侧IMU的速度
    pos_imu<<VEC_FROM_ARRAY(head->pos);	// 左侧IMU的位置
    acc_imu<<VEC_FROM_ARRAY(tail->acc);	// 右侧IMU的加速度
    angvel_avr<<VEC_FROM_ARRAY(tail->gyr);	// 右侧IMU的角速度
  
	
    // 在两帧IMU之间去畸变(按照两帧IMU之间的雷达采样点倒序处理,和外层的倒序IMU嵌套得到对一次Scan的运动补偿),要求时间要大于左侧IMU时间。(offset_time,IMU采样时间)
    for(; it_pcl->curvature / double(1000) > head->offset_time; it_pcl --)
    {
      dt = it_pcl->curvature / double(1000) - head->offset_time;	// 左侧IMU采样时间到雷达采样时间的时间间隔(论文中有提到对两帧IMU之间的所有雷达点,都用左侧IMU的测量值作为反向传播的输入值)

      /* Transform to the 'end' frame, using only the rotation
       * Note: Compensation direction is INVERSE of Frame's moving direction   
       * So if we want to compensate a point at timestamp-i to the frame-e
       * P_compensate = R_imu_e ^ T * (R_i * P_i + T_ei) where T_ei is represented in global frame */
      M3D R_i(R_imu * Exp(angvel_avr, dt));	// R_i代表在左侧IMU基础上做了一个dt的旋转量的旋转矩阵
      
      V3D P_i(it_pcl->x, it_pcl->y, it_pcl->z);	// 点在雷达坐标系下的位置
      V3D T_ei(pos_imu + vel_imu * dt + 0.5 * acc_imu * dt * dt - imu_state.pos);   // 平移量为上一帧imu位置+变化量 - 当前imu的位置
	
      // 这里对应着论文中的公式(10)和式1,四元数的共轭代表着同轴,旋转角相反。
      /*TODO:确认imu_status到底是什么?
      1 从lidar系映射到imu系 P_compensate = offset_R_L_I * P_i + offset_T_L_I
      2 展现这个点在dt时间内运动 P_compensate = R_i * P_compensate + T_ei
      3 反向运动,与上一时刻相比,旋转角度、位移量一致,方向相反。 P_compensate = rot.conjugate() * P_compensate - offset_T_L_I
      4 将数据转回Lidar坐标系 offset_R_L_I.conjugate() * p_compensate
      */
      V3D P_compensate = imu_state.offset_R_L_I.conjugate() * (imu_state.rot.conjugate() * (R_i * (imu_state.offset_R_L_I * P_i + imu_state.offset_T_L_I) + T_ei) - imu_state.offset_T_L_I);// not accurate!
      
      // save Undistorted points and their rotation
      it_pcl->x = P_compensate(0);
      it_pcl->y = P_compensate(1);
      it_pcl->z = P_compensate(2);

      if (it_pcl == pcl_out.points.begin()) break;
    }
  }
}

实现李代数指数映射部分

template<typename T, typename Ts>
Eigen::Matrix<T, 3, 3> Exp(const Eigen::Matrix<T, 3, 1> &ang_vel, const Ts &dt)
{
    T ang_vel_norm = ang_vel.norm();    // 三轴角速度的二范数,就是角速度的标量,表示旋转的有多快。
    Eigen::Matrix<T, 3, 3> Eye3 = Eigen::Matrix<T, 3, 3>::Identity(); // 得到三维单位阵

    if (ang_vel_norm > 0.0000001)       // 确实是在旋转的。
    {
        Eigen::Matrix<T, 3, 1> r_axis = ang_vel / ang_vel_norm; // 等价于normalized()得到一个模长为1的向量,这里是旋转轴的方向向量。
        Eigen::Matrix<T, 3, 3> K;   // 斜对称矩阵K

        K << SKEW_SYM_MATRX(r_axis);

        T r_ang = ang_vel_norm * dt;    // 旋转角

        /// Roderigous Tranformation. 推导过程:
        return Eye3 + std::sin(r_ang) * K + (1.0 - std::cos(r_ang)) * K * K;    // 罗德里格斯公式,将轴角转为旋转矩阵
        
    }
    else
    {
        return Eye3;
    }
}

这里的罗德里格斯公式和14讲里面的有区别(推导过程可以看后面的链接)

  • 这里的: R = I + s i n θ ⋅ K + ( 1 − c o s θ ) K 2 R=I+sin\theta\cdot K+(1-cos\theta)K^2 R=I+sinθK+(1cosθ)K2.罗德里格斯公式
  • 14讲的: R = c o s θ I + ( 1 − c o s θ ) n n T + s i n θ n R=cos\theta I + (1-cos\theta)nn^T+sin\theta n R=cosθI+(1cosθ)nnT+sinθn.罗德里格斯公式

3 IMU数据处理函数

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();

  if(meas.imu.empty()) {return;};		// 判断imu数据是否为空,为空退出
  ROS_ASSERT(meas.lidar != nullptr);	

  // IMU初始化
  if (imu_need_init_)
  {
    // The very first lidar frame
    IMU_init(meas, kf_state, init_iter_num);

    imu_need_init_ = true;
    
    last_imu_   = meas.imu.back();		// 最新imu为imu的最后一帧

    // 
    state_ikfom imu_state = kf_state.get_x();	// 拿到imu状态数据
    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;
      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_);	// 前向传播imu状态,后向去畸变

  t2 = omp_get_wtime();
  t3 = omp_get_wtime();
  
  // cout<<"[ IMU Process ]: Time: "<
}

参考链接

FastLIO官方github

一位师兄的FastLIO2代码笔记

你可能感兴趣的:(FastLio,自动驾驶,c++)