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

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

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

void ImuProcess::UndistortPcl(const MeasureGroup &meas, esekfom::esekf<state_ikfom, 12, input_ikfom> &kf_state, PointCloudXYZI &pcl_out);

1 对IMU数据的前向传播

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

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

  • ${\hat{x}}_{i+1}={\hat{x}}_i⊞(\Delta tf({\hat{x}}_i,u_i,0)),x_0={\overline{x}}_{k-1}(4)$传播状态；

  • $\begin{array}{l}{\tilde{x}}_{i+1}=x_{i+1}\boxminus {\hat{x}}_{i+1}\\ =(x_i⊞(\Delta tf(x_i,u_i,w_i))\boxminus ({\hat{x}}_i⊞(\Delta tf({\hat{x}}_i,u_i,0)))\end{array}(5)$传播协方差；

​ 一直到这一次扫描结束，会得到一个传播结果：${\hat{x}}_k,{\hat{P}}_k$（$\hat{a}$表示IKEF中的先验信息）。${\hat{P}}_k=x_k\boxminus {\hat{x}}_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<PointType> PointCloudXYZI;// 这里本质上是新生成一个PointCloud对象指针并调用boost::shared_ptr<T>::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 "<<pcl_beg_time<<" to "<<pcl_end_time<<", " \//          <<meas.imu.size()<<" imu msgs from "<<imu_beg_time<<" to "<<imu_end_time<<endl;/*** Initialize IMU pose ***/state_ikfom imu_state = kf_state.get_x();		// 获取先验imu姿态 \hat{x}_kIMUpose.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数据的相对误差（${\rho }_j\le t_k$），会加剧里程计的误差。因此需要修正位姿映射到k时刻。对于两次IMU测量期间的所有LiDAR特征点，都用左侧的IMU测量值作为反向传播输入。
  • ${\breve{x}}_{j-1}={\breve{x}}_j⊞(-\Delta tf({\breve{x}}_j,u_j,0))式1$. j-1 时刻的最优估计由 j 时刻的最优估计减去 j-1 时刻的运动量。

  

  反向传播中的运动方程：

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

  反向传播会生成一个相对位姿${}^{I_k}{\breve{T}}_{I_j}$。

  通过${}^{L_k}{P}_{f_j}={}^I{T}_L^{-1}{}^{I_k}{\breve{T}}_{I_j}{}^I{T}_L{}^{L_j}{P}_{f_j}(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: "<<acc_imu.transpose()<<endl;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_I2 展现这个点在dt时间内运动 P_compensate = R_i * P_compensate + T_ei3 反向运动，与上一时刻相比，旋转角度、位移量一致，方向相反。 P_compensate = rot.conjugate() * P_compensate - offset_T_L_I4 将数据转回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 rotationit_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;   // 斜对称矩阵KK << 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+sin\theta \cdot K+(1-cos\theta )K^2$.罗德里格斯公式
• 14讲的：$R=cos\theta I+(1-cos\theta )n{n}^T+sin\theta 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 frameIMU_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: "<<t3 - t1<<endl;
}

参考链接

FastLIO官方github

一位师兄的FastLIO2代码笔记