12345qiupeng
/
Fast-Lio
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

feat: Improve lidar and IMU data processing with enhanced comments and initialization checks

main
邱棚 2025-03-25 15:29:42 +08:00
parent e2689e5c7c
commit 703bf5b7ad
2 changed files with 51 additions and 31 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

13
include/common_lib.h
View File

@ -52,17 +52,18 @@ M3F Eye3f(M3F::Identity());
V3D Zero3d(0, 0, 0);
V3F Zero3f(0, 0, 0);
// 当前处理的激光雷达数据和IMU数据
struct MeasureGroup // Lidar data and imu dates for the curent process
{
MeasureGroup()
{
lidar_beg_time = 0.0;
this->lidar.reset(new PointCloudXYZI());
lidar_beg_time = 0.0; // 初始化激光雷达开始时间为0.0
this->lidar.reset(new PointCloudXYZI()); // 重置激光雷达点云指针，分配新的点云对象
};
double lidar_beg_time;
double lidar_end_time;
PointCloudXYZI::Ptr lidar;
deque<sensor_msgs::Imu::ConstPtr> imu;
double lidar_beg_time; // 激光雷达扫描开始时间
double lidar_end_time; // 激光雷达扫描结束时间
PointCloudXYZI::Ptr lidar; // 激光雷达点云数据指针
deque<sensor_msgs::Imu::ConstPtr> imu; // IMU数据队列，存储IMU消息的常量指针
};
struct StatesGroup

69
src/IMU_Processing.hpp
View File

@ -213,34 +213,45 @@ void ImuProcess::IMU_init(const MeasureGroup &meas, esekfom::esekf<state_ikfom,
}
/**
* @brief 去畸变点云数据
*
* @param meas 当前处理的激光雷达和IMU数据
* @param kf_state 扩展卡尔曼滤波器状态
* @param 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 ***/
// 将上一帧的IMU数据添加到当前帧的IMU数据前面
auto v_imu = meas.imu;
v_imu.push_front(last_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 &imu_beg_time = v_imu.front()->header.stamp.toSec(); // 获取IMU数据的开始时间
const double &imu_end_time = v_imu.back()->header.stamp.toSec(); // 获取IMU数据的结束时间
double pcl_beg_time = meas.lidar_beg_time;
double pcl_end_time = meas.lidar_end_time;
double pcl_beg_time = meas.lidar_beg_time; // 获取激光雷达数据的开始时间
double pcl_end_time = meas.lidar_end_time; // 获取激光雷达数据的结束时间
if (lidar_type == MARSIM) {
pcl_beg_time = last_lidar_end_time_;
pcl_end_time = meas.lidar_beg_time;
pcl_beg_time = last_lidar_end_time_; // 如果激光雷达类型为MARSIM，设置激光雷达开始时间为上一帧的结束时间
pcl_end_time = meas.lidar_beg_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 ***/
// 初始化IMU位姿
state_ikfom imu_state = kf_state.get_x();
IMUpose.clear();
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 ***/
// 在每个IMU点进行前向传播
V3D angvel_avr, acc_avr, acc_imu, vel_imu, pos_imu;
M3D R_imu;
@ -252,7 +263,7 @@ void ImuProcess::UndistortPcl(const MeasureGroup &meas, esekfom::esekf<state_ikf
auto &&head = *(it_imu);
auto &&tail = *(it_imu + 1);
if (tail->header.stamp.toSec() < last_lidar_end_time_) continue;
if (tail->header.stamp.toSec() < last_lidar_end_time_) continue; // 如果IMU时间小于上一帧激光雷达结束时间，跳过
angvel_avr<<0.5 * (head->angular_velocity.x + tail->angular_velocity.x),
0.5 * (head->angular_velocity.y + tail->angular_velocity.y),
@ -263,16 +274,16 @@ void ImuProcess::UndistortPcl(const MeasureGroup &meas, esekfom::esekf<state_ikf
// 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;
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() - last_lidar_end_time_; // 计算时间差
// dt = tail->header.stamp.toSec() - pcl_beg_time;
}
else
{
dt = tail->header.stamp.toSec() - head->header.stamp.toSec();
dt = tail->header.stamp.toSec() - head->header.stamp.toSec(); // 计算时间差
}
in.acc = acc_avr;
@ -281,9 +292,10 @@ void ImuProcess::UndistortPcl(const MeasureGroup &meas, esekfom::esekf<state_ikf
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);
kf_state.predict(dt, Q, in); // 预测状态
/* save the poses at each IMU measurements */
// 保存每个IMU测量的位姿
imu_state = kf_state.get_x();
angvel_last = angvel_avr - imu_state.bg;
acc_s_last = imu_state.rot * (acc_avr - imu_state.ba);
@ -296,6 +308,7 @@ void ImuProcess::UndistortPcl(const MeasureGroup &meas, esekfom::esekf<state_ikf
}
/*** 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);
@ -305,6 +318,7 @@ void ImuProcess::UndistortPcl(const MeasureGroup &meas, esekfom::esekf<state_ikf
last_lidar_end_time_ = pcl_end_time;
/*** undistort each lidar point (backward propagation) ***/
// 去畸变每个激光雷达点（反向传播）
if (pcl_out.points.begin() == pcl_out.points.end()) return;
if(lidar_type != MARSIM){
@ -320,6 +334,7 @@ void ImuProcess::UndistortPcl(const MeasureGroup &meas, esekfom::esekf<state_ikf
acc_imu<<VEC_FROM_ARRAY(tail->acc);
angvel_avr<<VEC_FROM_ARRAY(tail->gyr);
// 去畸变每个激光雷达点（反向传播）
for(; it_pcl->curvature / double(1000) > head->offset_time; it_pcl --)
{
dt = it_pcl->curvature / double(1000) - head->offset_time;
@ -328,6 +343,7 @@ void ImuProcess::UndistortPcl(const MeasureGroup &meas, esekfom::esekf<state_ikf
* 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));
V3D P_i(it_pcl->x, it_pcl->y, it_pcl->z);
@ -335,6 +351,7 @@ void ImuProcess::UndistortPcl(const MeasureGroup &meas, esekfom::esekf<state_ikf
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);
@ -348,38 +365,40 @@ void ImuProcess::UndistortPcl(const MeasureGroup &meas, esekfom::esekf<state_ikf
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();
t1 = omp_get_wtime(); // 获取当前时间
if(meas.imu.empty()) {return;};
ROS_ASSERT(meas.lidar != nullptr);
if(meas.imu.empty()) {return;}; // 如果IMU数据为空，直接返回
ROS_ASSERT(meas.lidar != nullptr); // 断言激光雷达数据不为空
if (imu_need_init_)
{
// 如果需要初始化IMU
/// The very first lidar frame
IMU_init(meas, kf_state, init_iter_num);
IMU_init(meas, kf_state, init_iter_num); // 初始化IMU
imu_need_init_ = true;
imu_need_init_ = true; // 设置IMU需要初始化标志为真
last_imu_ = meas.imu.back();
last_imu_ = meas.imu.back(); // 获取最后一个IMU数据
state_ikfom imu_state = kf_state.get_x();
state_ikfom imu_state = kf_state.get_x(); // 获取当前状态
if (init_iter_num > MAX_INI_COUNT)
{
cov_acc *= pow(G_m_s2 / mean_acc.norm(), 2);
imu_need_init_ = false;
// 如果初始化迭代次数大于最大初始化次数
cov_acc *= pow(G_m_s2 / mean_acc.norm(), 2); // 调整加速度协方差
imu_need_init_ = false; // 设置IMU需要初始化标志为假
cov_acc = cov_acc_scale;
cov_gyr = cov_gyr_scale;
ROS_INFO("IMU Initial Done");
cov_acc = cov_acc_scale; // 设置加速度协方差
cov_gyr = cov_gyr_scale; // 设置陀螺仪协方差
ROS_INFO("IMU Initial Done"); // 打印IMU初始化完成信息
// 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);
fout_imu.open(DEBUG_FILE_DIR("imu.txt"),ios::out); // 打开IMU调试文件
}
return;
}
UndistortPcl(meas, kf_state, *cur_pcl_un_);
UndistortPcl(meas, kf_state, *cur_pcl_un_); // 去畸变点云
t2 = omp_get_wtime();
t3 = omp_get_wtime();