FastLIO代码阅读之一：IMU数据处理
FastLIO代码阅读之一：IMU数据处理
文章目录
- FastLIO代码阅读之一：IMU数据处理
-
- 1 对IMU数据的前向传播
- 2 后向传播
- 3 IMU数据处理函数
SLAM小白，刚开始学习，请各位大佬抱着批判的眼光看待博客。有错误还请重喷，随时修改~
这两部分在代码中主要对应到IMU_Processing.hpp中的UndistortPcl函数。
void ImuProcess::UndistortPcl(const MeasureGroup &meas, esekfom::esekf
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=xk−1 (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+1⊟x^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=xk⊟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 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 ρj≤tk），会加剧里程计的误差。因此需要修正位姿映射到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˘j−1=x˘j⊞(−Δtf(x˘j,uj,0))式1. j-1 时刻的最优估计由 j 时刻的最优估计减去 j-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=ITL−1IkT˘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+(1−cosθ)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+(1−cosθ)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代码笔记