FAST-LIO2源码分析-状态预测
简介
状态预测使用IMU积分，然后对点云进行运动畸变去除，将点云变换到一帧结束时刻的坐标系下。
该代码在IMU_Processing.hpp文件中。
文中若有错误的地方，请指出。期待和大家一起学习进步。
入口函数
入口函数是ImuProcess类的Process。
输入为当前的测量数据，上一时刻的位姿状态。其中测量数据包含点云数据和IMU数据。
输出为当前时刻的位姿状态，以及去除运动畸变后的点云。
该函数的步骤为：
1、首先判断雷达和IMU数据是否存在。
2、判断是否进行了IMU初始化，如果没有进行初始化，则将该数据进行IMU初始化。初始化成功的判断条件是用于初始化的IMU数据量大于MAX_INI_COUNT（该值为10）。无论初始化成功与否，都退出函数。
3、对于初始化后的的数据，进行IMU积分（即状态预测），并使用积分后的状态进行点云运动畸变去除。
点云运动畸变去除是通过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;};ROS_ASSERT(meas.lidar != nullptr);if (imu_need_init_){// IMU初始化/// The very first lidar frameIMU_init(meas, kf_state, init_iter_num);imu_need_init_ = true;last_imu_ = meas.imu.back();state_ikfom imu_state = kf_state.get_x();// 如果用于初始化的IMU数据大于MAX_INI_COUNT，则判定为初始化成功。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;}// 状态预测（IMU积分），以及点云运动畸变处理UndistortPcl(meas, kf_state, *cur_pcl_un_);t2 = omp_get_wtime();t3 = omp_get_wtime();// cout<<"[ IMU Process ]: Time: "<<t3 - t1<<endl;
}IMU初始化
IMU初始化的函数为IMU_init。
输入为：
1、点云和IMU数据，初始化过程中只使用了IMU数据
输出为：
1、状态量（esekfom::esekf<state_ikfom, 12, input_ikfom>），其中保存了零偏和协方差
2、初始化过程中使用的IMU数据量
具体步骤见以下代码注释：
void ImuProcess::IMU_init(const MeasureGroup &meas, esekfom::esekf<state_ikfom, 12, input_ikfom> &kf_state, int &N)
{/** 1. initializing the gravity, gyro bias, acc and gyro covariance** 2. normalize the acceleration measurenments to unit gravity **/V3D cur_acc, cur_gyr;// 第一帧数据if (b_first_frame_){Reset(); // 重置一些状态量，清空一些数组N = 1; // IMU数据计数器b_first_frame_ = false; // 只进行一次重置工作const auto &imu_acc = meas.imu.front()->linear_acceleration;const auto &gyr_acc = meas.imu.front()->angular_velocity;// 将第一个加速度计数据保存在mean_acc中，用于后续计算均值和方差mean_acc << imu_acc.x, imu_acc.y, imu_acc.z; // 将第一个陀螺仪数据保存在mean_gyr中，用于后续计算均值和方差mean_gyr << gyr_acc.x, gyr_acc.y, gyr_acc.z;first_lidar_time = meas.lidar_beg_time;}// 计算均值和方差for (const auto &imu : meas.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;mean_acc += (cur_acc - mean_acc) / N;mean_gyr += (cur_gyr - mean_gyr) / N;cov_acc = cov_acc * (N - 1.0) / N + (cur_acc - mean_acc).cwiseProduct(cur_acc - mean_acc) * (N - 1.0) / (N * N);cov_gyr = cov_gyr * (N - 1.0) / N + (cur_gyr - mean_gyr).cwiseProduct(cur_gyr - mean_gyr) * (N - 1.0) / (N * N);// cout<<"acc norm: "<<cur_acc.norm()<<" "<<mean_acc.norm()<<endl;N ++;}state_ikfom init_state = kf_state.get_x();// 初始化重力，即用于初始化的数据为IMU静止时采集的。// 加速度计输出的是比力，比力方程为 f = a - g// imu初始化中默认为静止状态，因此a为零。则imu输出为 f = -g// 因此此处应该给IMU的平均值加上负号，则为重力的方向。// 该款IMU加速度计输出的单位为g，因此需要乘以G_const// 此时对重力的表示为S2，其中成员变量vec= [0, 0, -9.8]init_state.grav = S2(- mean_acc / mean_acc.norm() * G_m_s2);//state_inout.rot = Eye3d; // Exp(mean_acc.cross(V3D(0, 0, -1 / scale_gravity)));init_state.bg = mean_gyr; // 陀螺仪零偏// IMU坐标系到雷达坐标系的平移和旋转。init_state.offset_T_L_I = Lidar_T_wrt_IMU;init_state.offset_R_L_I = Lidar_R_wrt_IMU;kf_state.change_x(init_state); // 保存当前状态//初始化协方差矩阵esekfom::esekf<state_ikfom, 12, input_ikfom>::cov init_P = kf_state.get_P();init_P.setIdentity();init_P(6,6) = init_P(7,7) = init_P(8,8) = 0.00001;init_P(9,9) = init_P(10,10) = init_P(11,11) = 0.00001;init_P(15,15) = init_P(16,16) = init_P(17,17) = 0.0001;init_P(18,18) = init_P(19,19) = init_P(20,20) = 0.001;init_P(21,21) = init_P(22,22) = 0.00001; kf_state.change_P(init_P);last_imu_ = meas.imu.back();
}IMU加速度计输出为比力，比例方程为 f = a - g。IMU初始化默认为静止状态下进行的，因此此时a为零。则f = -g。因此此处应该给IMU的平均值加上负号，则为重力的方向。该款IMU加速度计输出的单位为g，因此需要乘以G_const，此时对重力的表示为S2，其中成员变量vec= [0, 0, -9.809]
其中进行IMU状态值重置：
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; // 需要重新初始化start_timestamp_ = -1; // 好像没有用到// 初始化中使用，为初始化过程中的IMU数据数量init_iter_num = 1; v_imu_.clear(); // 好像没有用到IMUpose.clear(); // 保存一帧点云对应的每个时刻的IMU数据// 保存上一帧点云中最后时刻的IMU数据last_imu_.reset(new sensor_msgs::Imu()); // 保存去除运动畸变后的点云cur_pcl_un_.reset(new PointCloudXYZI());
}状态预测及协方差更新（函数predict）
状态预测及点云去畸变是在函数UndistortPcl中。该函数首先对IMU进行积分。
关键语句为：
kf_state.predict(dt, Q, in); // 更新当前状态以及协方差矩阵predict使用IMU数据进行状态预测的。
其中：
1、预测状态：计算了状态的更新量，然后对上一时刻的状态进行更新。
2、更新协方差矩阵。
计算 f
在该函数中，首先计算了论文中公式（2）的 f 函数（有一点不同的地方是，代码中对于位置，没有考虑加速度的影响。还有一点就是代码中公式的排列顺序和论文中不一致，需要自己对应位置速度旋转等各自公式在代码中的实现）。
f函数的计算结果乘以时间dt，作为该时间段内的积分结果，加上个状态的初始值，即可得到结束时刻的状态量。
Eigen::Matrix<double, 24, 1> f(state_ikfom &s, const input_ikfom &in)
{Eigen::Matrix<double, 24, 1> res = Eigen::Matrix<double, 24, 1>::Zero();vect3 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;
}f 函数的计算结果是一个24为的向量，从代码中可以看到，只有位置（索引0，维度3），姿态（索引0，维度3）和速度（索引12，维度3）有对应的更新量，其余都是零。
误差状态对误差状态的导数
计算 fx
f_x的类型为：Eigen::Matrix<double, 24, 23>。
f_x函数中计算了 当前时刻的误差状态 对 上一时刻的误差状态 的 导数 的 一部分。
f_x函数计算的结果中为Fx中需要乘以dt的项，但是在该函数中没有乘以dt，在predict的最后乘以了dt。
Eigen::Matrix<double, 24, 23> f_x(state_ikfom &s, const input_ikfom &in)
{Eigen::Matrix<double, 24, 23> cov = Eigen::Matrix<double, 24, 23>::Zero();// 位置误差对速度误差(需要乘以dt)cov.template block<3, 3>(0, 12) = Eigen::Matrix3d::Identity();vect3 acc_;in.acc.boxminus(acc_, s.ba); // acc_ = in.acc - s.bavect3 omega;in.gyro.boxminus(omega, s.bg); // omega = in.gyro - s.bg// 速度误差对旋转误差(需要乘以dt)cov.template block<3, 3>(12, 3) = -s.rot.toRotationMatrix() * MTK::hat(acc_);// 速度误差对加速度计零偏误差(需要乘以dt)cov.template block<3, 3>(12, 18) = -s.rot.toRotationMatrix();// state_ikfom::scalar => doubleEigen::Matrix<state_ikfom::scalar, 2, 1> vec = Eigen::Matrix<state_ikfom::scalar, 2, 1>::Zero();Eigen::Matrix<state_ikfom::scalar, 3, 2> grav_matrix;/*grav.S2_Mx(grav_matrix, vec);grav_matrix = | 0 0 || 0 9.8 || 9.8 0 |*/s.S2_Mx(grav_matrix, vec, 21); cov.template block<3, 2>(12, 21) = grav_matrix;// 旋转误差对陀螺仪零偏误差的导数的一部分 即-A(omega*dt)// 此处取了近似值，即-Icov.template block<3, 3>(3, 15) = -Eigen::Matrix3d::Identity();return cov;
}f_x函数的计算结果：
计算f_x_final
相比于f_x， f_x_final少一行。f_x_final的类型为：Eigen::Matrix<double, 23, 23>。
f_x_final中的值是来源于f_x_。
(1) vect_state
对于向量状态空间vect_state（位置，速度，IMU和雷达的坐标系平移，以及陀螺仪零偏和加加速度计零偏），f_x_final和f_x是一样的。
(2) SO3_state
对于旋转SO3_state，IMU和雷达坐标系的旋转处理如下：
对于旋转误差，每一列乘以A(-w*dt)。在f_x_中，旋转误差只有对陀螺仪零偏处是一个3x3的单位矩阵，其他地方为零。所以最后结果也是对这个单位矩阵乘以A(-omeg*dt)。
A_matrix代码如下：（目前还没有弄明白该公式怎么来的）
template<typename Base>
Eigen::Matrix<typename Base::scalar, 3, 3> A_matrix(const Base & v){Eigen::Matrix<typename Base::scalar, 3, 3> res;double squaredNorm = v[0] * v[0] + v[1] * v[1] + v[2] * v[2];double norm = std::sqrt(squaredNorm);if(norm < MTK::tolerance<typename Base::scalar>()){res = Eigen::Matrix<typename Base::scalar, 3, 3>::Identity();}else{res = Eigen::Matrix<typename Base::scalar, 3, 3>::Identity() + (1 - std::cos(norm)) / squaredNorm * hat(v) + (1 - std::sin(norm) / norm) / squaredNorm * hat(v) * hat(v);}return res;
}（疑问：为什么是-w*dt ？？？论文中此处无负号）
对于IMU和雷达坐标系的旋转误差，这几行本来就是零，乘以的也是单位矩阵，最后结果仍然为零。
(3) S2_state
对于S2_state，此处是重力误差对各项误差的导数。
此处代码中公式比较复杂，但实际上f_x_中此处都为零，因此最终结果也为零。
同时论文中，此处只有对上一时刻的重力误差处是单位矩阵，其余为零。
因此，f_x_final最终的值如下：
计算F_x1
F_x1初始化为23x23的单位矩阵。
Eigen::Matrix<double, 23, 23> F_x1 = Eigen::Matrix<double, 23, 23>::Identity();(1) vect_state
在对vect_state的遍历中没有改变其值。
(2) SO3_state
旋转误差处直接赋值：Exp(-w*dt)。
offset_R_L_I 处直接赋值为单位矩阵。
(3) S2_state
重力的类型为S2，其定义如下：
typedef MTK::S2<double, 98090, 10000, 1> S2;在S2内部，定义了一个double变量length。
scalar length = scalar(den)/scalar(num);结合S2的定义，此处为，即重力的大小。
double length = double(98090) / double(10000); // 9.809S2在定义时最后一个模板参数（S2_typ）为1，在默认构造函数中会将重力初始化为：
length * vec3(std::sqrt(1), 0, 0);但是在IMU初始化中，会对重力向量重新进行赋值，即：
init_state.grav = S2(-mean_acc / mean_acc.norm() * G_const);此处调用的S2的传入一个向量的构造函数，具体如下：
S2(const vect_type &_vec) : vec(_vec) {vec.normalize();vec = vec * length;
}在对S2_state处理过程中，对F_x1进行了赋值，此处idx的值为21，即重力在F_x1中的其实索引。
F_x1.template block<2, 2>(idx, idx) = Nx * res.toRotationMatrix() * Mx;res的值是用f_中计算得到的，f_中重力部分为零。因此res = (w:1, x:0, y:0, z:0)。res.toRotationMatrix()为单位矩阵。
Nx为
Mx为
因此F_x1的21, 21索引出的值为：
论文中只是将重力当作常规三维向量进行推到，而代码中重力是作为S2变量，因此此处和论文中不一样。S2的具体原理还在学习中...
最终F_x1的值是两部分的和：
F_x1 += f_x_final * dt;
误差状态对IMU噪声的导数
计算f_w_
f_w_的维数为24*12，是在f_w函数中计算的。
Eigen::Matrix<double, 24, 12> LIOdometry::f_w(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;
}其结果为：
计算f_w_final
(1) vect_state
对于向量空间的状态，f_w_final和f_w_的值一样。
(2) SO3_state
对于SO3状态，只有旋转误差对陀螺仪零偏处有值，其余地方都是零。因此只有此处是乘以了A(-w*dt)
(3) S2_state
代码有点复杂，但是由于f_w_此处为零。因此最终该处的结果仍然为零。
最终计算完的f_w_final结果如下。该矩阵乘以dt即为论文中Fw的值。
更新协方差矩阵
协方差矩阵是使用Fx和Fw进行更新的，代码如下：
F_x1 += f_x_final * dt;
P_ = (F_x1) * P_ * (F_x1).transpose() + (dt * f_w_final) * Q * (dt * f_w_final).transpose();函数predict
void predict(double &dt, Eigen::Matrix<double, 12, 12> &Q, const input_ikfom& i_in)
{// typedef Matrix<scalar_type, m, 1> flatted_state;// m = 24;Eigen::Matrix<double, 24, 1> f_ = f(x_, i_in);// 函数f即为原文中公式2，f_乘以dt为增量。// 该函数的实现与公式2有些差异，如位置的增量没有考虑加速度的影响// typedef Matrix<scalar_type, m, n> cov_;// m = 24// n = 23// f_x计算了当前时刻误差状态对上一时刻误差状态的一部分// 即其中的项为Fx中需要乘以dt的项,但f_x函数中没有乘以dtEigen::Matrix<double, 24, 23> f_x_ = f_x(x_, i_in);// typedef Matrix<scalar_type, n, n> cov;Eigen::Matrix<double, 23, 23> f_x_final;// f_w 误差状态对IMU噪声的导数Eigen::Matrix<double, 24, 12> f_w_ = f_w(x_, i_in);Eigen::Matrix<double, 23, 12> f_w_final;state_ikfom x_before = x_;// *this += scale * vec; *this为x_, vec 为 f_, scale 为 dt。// 对于向量，是直接相加// 对于SO3，是四元数乘法x_.oplus(f_, dt); // F_x1 = Eigen::Matrix<double, 23, 23>::Identity();Eigen::Matrix<double, 23, 23> F_x1 = Eigen::Matrix<double, 23, 23>::Identity();for(auto it = x_.vect_state.begin(); it!=x_.vect_state.end(); ++it){/*((0, 0), 3) pos 位置((9, 9), 3) offset_T_L_I((12, 12), 3) vel 速度((15, 15), 3) bw 陀螺仪零偏((18, 18), 3) ba 加速度计零偏*/const int idx = it->first.first;const int dim = it->first.second;const int dof = it->second; // 3for(int i=0; i<23 /*n: 23*/; ++i){for(int j=0; j<dof; ++j) // vect3 : dof = 3{f_x_final(idx + j, i) = f_x_(dim + j, i);}}for(int i=0; i<12 /*process_noise_dof: 12*/; ++i){for(int j=0; j<dof; ++j){f_w_final(idx + j, i) = f_w_(dim+j, i);}}}Eigen::Matrix<double, 3, 3> res_temp_SO3;// Eigen::Matrix<double, 3, 1> seg_SO3MTK::vect<3, double> seg_SO3;for(auto it = x_.SO3_state.begin(); it!=x_.SO3_state.end(); ++it){/*(3, 3) rotation 旋转(6, 6) offset_R_L_I*/const int idx = it->first;const int dim = it->second;for(int i=0; i<3; ++i){// rotation: seg_SO3 = -1 * omega * dt// offset_R_L_I seg_SO3 = {0, 0, 0}^Tseg_SO3(i) = -1. * f_(dim + i) * dt;}MTK::SO3<double> res; // Eigen::Quaternion<double> // res的结果：// 对于 rotation seg_SO3旋转向量转四元数// 对于 offset_R_L_I (w:1, x:0, y:0, z:0)res.w() = MTK::exp<double, 3>(res.vec(), seg_SO3, double(0.5)); // angle-axis to quaternion// F_x1的结果：// 对于 rotation 为 Exp(-w*dt)// 对于 offset_R_L_I 为 IF_x1.template block<3, 3>(idx, idx) = res.toRotationMatrix();res_temp_SO3 = MTK::A_matrix(seg_SO3); // A(-omega*dt)for(int i=0; i<23; ++i){f_x_final. template block<3, 1>(idx, i) = res_temp_SO3 * (f_x_. template block<3, 1>(dim, i)); // f_x_final. template block<3, 1>(idx, i) = (f_x_. template block<3, 1>(dim, i)); // also work}for(int i=0; i<12; ++i){f_w_final. template block<3, 1>(idx, i) = res_temp_SO3 * (f_w_. template block<3, 1>(dim, i));// f_w_final. template block<3, 1>(idx, i) = (f_w_. template block<3, 1>(dim, i)); // also work}}Eigen::Matrix<double, 2, 3> res_temp_S2;Eigen::Matrix<double, 2, 2> res_temp_S2_;MTK::vect<3, double> seg_S2;for(auto it = x_.S2_state.begin(); it!=x_.S2_state.end(); ++it){/*(21, 21)*/int idx = it->first;int dim = it->second;for(int i=0; i<3; ++i){seg_S2(i) = f_(dim + i) * dt;}// seg_S2 = [0, 0, 0]^TMTK::vect<2, double> vec = MTK::vect<2, double>::Zero();MTK::SO3<double> res;res.w() = MTK::exp<double, 3>(res.vec(), seg_S2, double(0.5));// res = (w:1, x:0, y:0, z:0)Eigen::Matrix<double, 2, 3> Nx;Eigen::Matrix<double, 3, 2> Mx;/*Nx = | -9.809 0 0 || 0 9.809 0 |*/x_.S2_Nx_yy(Nx, idx);// x_before是x_.oplus(f_, dt)之前的值// 对于重力来说，x_before和x_的值是一样的/*Mx = | -9.809 0 || 0 9.809 || 0 0 |*/x_before.S2_Mx(Mx, vec, idx);/*Nx * res.toRotationMatrix() * Mx =| 9.809*9.809 0 || 0 9.809*9.809|*/F_x1.template block<2, 2>(idx, idx) = Nx * res.toRotationMatrix() * Mx;Eigen::Matrix<double, 3, 3> x_before_hat;x_before.S2_hat(x_before_hat, idx);res_temp_S2 = -Nx * res.toRotationMatrix() * x_before_hat * MTK::A_matrix(seg_S2).transpose();for(int i=0; i<23/*n: 23*/; ++i){f_x_final. template block<2, 1>(idx, i) = res_temp_S2 * (f_x_. template block<3, 1>(dim, i));}for(int i = 0; i < 12 /*process_noise_dof : 12 */; i++){f_w_final. template block<2, 1>(idx, i) = res_temp_S2 * (f_w_. template block<3, 1>(dim, i));}}F_x1 += f_x_final * dt;P_ = (F_x1) * P_ * (F_x1).transpose() + (dt * f_w_final) * Q * (dt * f_w_final).transpose();
}点云去畸变
上一步已经把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)); // 当前时刻IMU系到全局坐标系的旋转V3D P_i(it_pcl->x, it_pcl->y, it_pcl->z); // 当前点坐标// pos_imu + vel_imu * dt + 0.5 * acc_imu * dt * dt 为当前点位置// imu_state.pos 该帧结束时刻的位置// 当前点到结束时刻的位移V3D T_ei(pos_imu + vel_imu * dt + 0.5 * acc_imu * dt * dt - imu_state.pos);// imu_state.offset_R_L_I * P_i + imu_state.offset_T_L_I 将雷达坐标系下的点转到IMU坐标系// (R_i * (imu_state.offset_R_L_I * P_i + imu_state.offset_T_L_I) + T_ei) 即 R_i * POS_IMU + T_ei，将点变换到结束时刻的全局坐标系下，然后平移到该帧结束时刻的位置// imu_state.rot.conjugate() * (R_i * (imu_state.offset_R_L_I * P_i + imu_state.offset_T_L_I) + T_ei) 旋转到该帧结束时刻的姿态。imu_state.rot.conjugate()：将全局坐标系的点的姿态旋转到该帧结束时刻的姿态// 最后，在将IMU坐标系下的点转换到该帧结束时刻的坐标系下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!// 作者标记该公式为 not accurate。可能是如下原因：// 1、当前只有IMU进行积分，没有考虑噪声，因此计算的IMU结束时刻的位姿存在累计误差。// 2、没有考虑IMU积分中的圆锥效应、划桨效应等误差。在代码中，现将点云变换到IMU坐标系下，然后变换到一帧结束时刻对应的IMU坐标系中，然后再变换回点云坐标系中。
UndistortPcl函数
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 ***/// 本次测量中的IMU数据auto v_imu = meas.imu;// 将上一帧最后一个IMU数据放到队列头部，// 保证IMU数据时间范围完全包含点云数据。v_imu.push_front(last_imu_);// IMU的起始时刻，该时刻要早于点云的开始时刻，即imu_beg_time < pcl_beg_timeconst double &imu_beg_time = v_imu.front()->header.stamp.toSec();// IMU的结束时刻，该时刻要晚于点云的结束时刻，即imu_end_time > pcl_end_timeconst 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);// 点云按时间排序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();IMUpose.clear(); // 清空队列// 队列第一个是上一帧最后一个IMU的位置，以此来保证IMU的时间范围包含点云数据的时间范围IMUpose.push_back(set_pose6d(0.0, acc_s_last, angvel_last, imu_state.vel, imu_state.pos, imu_state.rot.toRotationMatrix()));/*** forward propagation at each imu point ***/V3D angvel_avr, acc_avr, acc_imu, vel_imu, pos_imu;M3D R_imu;double dt = 0;input_ikfom in; // 输入，即公式（2）中的ufor (auto it_imu = v_imu.begin(); it_imu < (v_imu.end() - 1); it_imu++){// 每次取相邻的两个时刻的数据auto &&head = *(it_imu);auto &&tail = *(it_imu + 1);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;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;// Eigen::Matrix<double, 12, 12> Q;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();angvel_last = angvel_avr - imu_state.bg;acc_s_last = imu_state.rot * (acc_avr - imu_state.ba);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;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_state = kf_state.get_x();last_imu_ = meas.imu.back();last_lidar_end_time_ = pcl_end_time;/*** undistort each lidar point (backward propagation) ***/if (pcl_out.points.begin() == pcl_out.points.end()) return;auto it_pcl = pcl_out.points.end() - 1;for (auto it_kp = IMUpose.end() - 1; it_kp != IMUpose.begin(); it_kp--){// 选取两个相邻时刻的IMU数据auto head = it_kp - 1;auto tail = it_kp;R_imu<<MAT_FROM_ARRAY(head->rot); // 起始时刻的旋转矩阵// cout<<"head imu acc: "<<acc_imu.transpose()<<endl;vel_imu<<VEC_FROM_ARRAY(head->vel); // 起始时刻的速度pos_imu<<VEC_FROM_ARRAY(head->pos); // 起始时刻的位置acc_imu<<VEC_FROM_ARRAY(tail->acc); // 结束时刻的加速度。包含重力和偏置，并旋转到全局坐标系angvel_avr<<VEC_FROM_ARRAY(tail->gyr); // 结束时刻陀螺仪输出// 处理相邻两个IMU时刻之间的点，在上面该点云已经按时间进行了排序。for(; it_pcl->curvature / double(1000) > head->offset_time; it_pcl --){// 当前点到起始时刻的IMU。dt = it_pcl->curvature / double(1000) - head->offset_time;/* 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)); // 当前时刻IMU系到全局坐标系的旋转V3D P_i(it_pcl->x, it_pcl->y, it_pcl->z); // 当前点坐标// pos_imu + vel_imu * dt + 0.5 * acc_imu * dt * dt 为当前点位置// imu_state.pos 该帧结束时刻的位置// 当前点到结束时刻的位移V3D T_ei(pos_imu + vel_imu * dt + 0.5 * acc_imu * dt * dt - imu_state.pos);// imu_state.offset_R_L_I * P_i + imu_state.offset_T_L_I 将雷达坐标系下的点转到IMU坐标系// (R_i * (imu_state.offset_R_L_I * P_i + imu_state.offset_T_L_I) + T_ei) 即 R_i * POS_IMU + T_ei，将点变换到结束时刻的全局坐标系下，然后平移到该帧结束时刻的位置// imu_state.rot.conjugate() * (R_i * (imu_state.offset_R_L_I * P_i + imu_state.offset_T_L_I) + T_ei) 旋转到该帧结束时刻的姿态。imu_state.rot.conjugate()：将全局坐标系的点的姿态旋转到该帧结束时刻的姿态// 最后，在将IMU坐标系下的点转换到该帧结束时刻的坐标系下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!// 作者标记该公式为 not accurate。可能是如下原因：// 1、当前只有IMU进行积分，没有考虑噪声，因此计算的IMU结束时刻的位姿存在累积误差。// 2、没有考虑IMU积分中的圆锥效应、划桨效应等误差。// 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;}}
}