From 703bf5b7ad7f01c694c2bab15c31fa81078aa820 Mon Sep 17 00:00:00 2001 From: 12345qiupeng Date: Tue, 25 Mar 2025 15:29:42 +0800 Subject: [PATCH] feat: Improve lidar and IMU data processing with enhanced comments and initialization checks --- include/common_lib.h | 13 ++++---- src/IMU_Processing.hpp | 69 +++++++++++++++++++++++++++--------------- 2 files changed, 51 insertions(+), 31 deletions(-) diff --git a/include/common_lib.h b/include/common_lib.h index 82a57cb..6e0241e 100644 --- a/include/common_lib.h +++ b/include/common_lib.h @@ -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 imu; + double lidar_beg_time; // 激光雷达扫描开始时间 + double lidar_end_time; // 激光雷达扫描结束时间 + PointCloudXYZI::Ptr lidar; // 激光雷达点云数据指针 + deque imu; // IMU数据队列，存储IMU消息的常量指针 }; struct StatesGroup diff --git a/src/IMU_Processing.hpp b/src/IMU_Processing.hpp index 344d07f..4805b1f 100644 --- a/src/IMU_Processing.hpp +++ b/src/IMU_Processing.hpp @@ -213,34 +213,45 @@ void ImuProcess::IMU_init(const MeasureGroup &meas, esekfom::esekf &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 "<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::esekfheader.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(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 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::esekfacc); angvel_avr<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::esekfx, it_pcl->y, it_pcl->z); @@ -335,6 +351,7 @@ void ImuProcess::UndistortPcl(const MeasureGroup &meas, esekfom::esekfx = 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 &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();