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

feat: Enhance lidar processing with detailed comments and synchronization checks

main
邱棚 2025-03-25 14:47:54 +08:00
parent db492f0498
commit e2689e5c7c
2 changed files with 251 additions and 234 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

366
src/laserMapping.cpp
View File

@ -297,40 +297,43 @@ void standard_pcl_cbk(const sensor_msgs::PointCloud2::ConstPtr &msg)
sig_buffer.notify_all(); sig_buffer.notify_all();
} }
double timediff_lidar_wrt_imu = 0.0; double timediff_lidar_wrt_imu = 0.0; // 激光雷达与IMU的时间差
bool timediff_set_flg = false; bool timediff_set_flg = false; // 时间差设置标志
void livox_pcl_cbk(const livox_ros_driver::CustomMsg::ConstPtr &msg) void livox_pcl_cbk(const livox_ros_driver::CustomMsg::ConstPtr &msg)
{ {
mtx_buffer.lock(); mtx_buffer.lock(); // 锁定互斥量，保护共享资源
double preprocess_start_time = omp_get_wtime(); double preprocess_start_time = omp_get_wtime(); // 获取当前时间，用于计算预处理时间
scan_count ++; scan_count ++; // 增加扫描计数
if (msg->header.stamp.toSec() < last_timestamp_lidar) if (msg->header.stamp.toSec() < last_timestamp_lidar) // 检查激光雷达时间戳是否小于最后一个激光雷达时间戳
{ {
ROS_ERROR("lidar loop back, clear buffer"); ROS_ERROR("lidar loop back, clear buffer"); // 打印错误信息
lidar_buffer.clear(); lidar_buffer.clear(); // 清空激光雷达缓冲区
} }
last_timestamp_lidar = msg->header.stamp.toSec(); last_timestamp_lidar = msg->header.stamp.toSec(); // 更新最后一个激光雷达时间戳
// 检查IMU和激光雷达是否同步
if (!time_sync_en && abs(last_timestamp_imu - last_timestamp_lidar) > 10.0 && !imu_buffer.empty() && !lidar_buffer.empty() ) if (!time_sync_en && abs(last_timestamp_imu - last_timestamp_lidar) > 10.0 && !imu_buffer.empty() && !lidar_buffer.empty() )
{ {
// 打印不同步信息
printf("IMU and LiDAR not Synced, IMU time: %lf, lidar header time: %lf \n",last_timestamp_imu, last_timestamp_lidar); printf("IMU and LiDAR not Synced, IMU time: %lf, lidar header time: %lf \n",last_timestamp_imu, last_timestamp_lidar);
} }
// 如果启用了时间同步且时间差未设置，并且IMU缓冲区不为空
if (time_sync_en && !timediff_set_flg && abs(last_timestamp_lidar - last_timestamp_imu) > 1 && !imu_buffer.empty()) if (time_sync_en && !timediff_set_flg && abs(last_timestamp_lidar - last_timestamp_imu) > 1 && !imu_buffer.empty())
{ {
timediff_set_flg = true; timediff_set_flg = true; // 设置时间差标志
timediff_lidar_wrt_imu = last_timestamp_lidar + 0.1 - last_timestamp_imu; timediff_lidar_wrt_imu = last_timestamp_lidar + 0.1 - last_timestamp_imu; // 计算激光雷达与IMU的时间差
printf("Self sync IMU and LiDAR, time diff is %.10lf \n", timediff_lidar_wrt_imu); printf("Self sync IMU and LiDAR, time diff is %.10lf \n", timediff_lidar_wrt_imu); // 打印时间差
} }
PointCloudXYZI::Ptr ptr(new PointCloudXYZI()); PointCloudXYZI::Ptr ptr(new PointCloudXYZI()); // 创建新的点云指针
p_pre->process(msg, ptr); p_pre->process(msg, ptr); // 处理激光雷达消息，生成点云
lidar_buffer.push_back(ptr); lidar_buffer.push_back(ptr); // 将点云添加到激光雷达缓冲区
time_buffer.push_back(last_timestamp_lidar); time_buffer.push_back(last_timestamp_lidar); // 将时间戳添加到时间缓冲区
s_plot11[scan_count] = omp_get_wtime() - preprocess_start_time; s_plot11[scan_count] = omp_get_wtime() - preprocess_start_time; // 计算并记录预处理时间
mtx_buffer.unlock(); mtx_buffer.unlock(); // 解锁互斥量
sig_buffer.notify_all(); sig_buffer.notify_all(); // 通知所有等待的线程
} }
void imu_cbk(const sensor_msgs::Imu::ConstPtr &msg_in) void imu_cbk(const sensor_msgs::Imu::ConstPtr &msg_in)
@ -363,64 +366,71 @@ void imu_cbk(const sensor_msgs::Imu::ConstPtr &msg_in)
sig_buffer.notify_all(); sig_buffer.notify_all();
} }
double lidar_mean_scantime = 0.0; double lidar_mean_scantime = 0.0; // 激光雷达平均扫描时间
int scan_num = 0; int scan_num = 0; // 扫描次数
bool sync_packages(MeasureGroup &meas) bool sync_packages(MeasureGroup &meas)
{ {
// 如果激光雷达缓冲区或IMU缓冲区为空，返回false
if (lidar_buffer.empty() || imu_buffer.empty()) { if (lidar_buffer.empty() || imu_buffer.empty()) {
return false; return false;
} }
/*** push a lidar scan ***/ // 推送一个激光雷达扫描
if(!lidar_pushed) if(!lidar_pushed)
{ {
meas.lidar = lidar_buffer.front(); meas.lidar = lidar_buffer.front(); // 获取激光雷达缓冲区的第一个元素
meas.lidar_beg_time = time_buffer.front(); meas.lidar_beg_time = time_buffer.front(); // 获取时间缓冲区的第一个元素
// 如果激光雷达点云的点数小于等于1，设置激光雷达结束时间为开始时间加上平均扫描时间
if (meas.lidar->points.size() <= 1) // time too little if (meas.lidar->points.size() <= 1) // time too little
{ {
lidar_end_time = meas.lidar_beg_time + lidar_mean_scantime; lidar_end_time = meas.lidar_beg_time + lidar_mean_scantime;
ROS_WARN("Too few input point cloud!\n"); ROS_WARN("Too few input point cloud!\n");
} }
// 如果最后一个点的曲率小于平均扫描时间的一半，设置激光雷达结束时间为开始时间加上平均扫描时间
else if (meas.lidar->points.back().curvature / double(1000) < 0.5 * lidar_mean_scantime) else if (meas.lidar->points.back().curvature / double(1000) < 0.5 * lidar_mean_scantime)
{ {
lidar_end_time = meas.lidar_beg_time + lidar_mean_scantime; lidar_end_time = meas.lidar_beg_time + lidar_mean_scantime;
} }
// 否则，更新扫描次数和激光雷达结束时间，并计算新的平均扫描时间
else else
{ {
scan_num ++; scan_num ++;
lidar_end_time = meas.lidar_beg_time + meas.lidar->points.back().curvature / double(1000); lidar_end_time = meas.lidar_beg_time + meas.lidar->points.back().curvature / double(1000);
lidar_mean_scantime += (meas.lidar->points.back().curvature / double(1000) - lidar_mean_scantime) / scan_num; lidar_mean_scantime += (meas.lidar->points.back().curvature / double(1000) - lidar_mean_scantime) / scan_num;
} }
// 如果激光雷达类型为MARSIM，设置激光雷达结束时间为开始时间
if(lidar_type == MARSIM) if(lidar_type == MARSIM)
lidar_end_time = meas.lidar_beg_time; lidar_end_time = meas.lidar_beg_time; // 设置测量的激光雷达结束时间
meas.lidar_end_time = lidar_end_time; meas.lidar_end_time = lidar_end_time; // 设置激光雷达已推送标志
lidar_pushed = true; lidar_pushed = true;
} }
// 如果最后一个IMU时间戳小于激光雷达结束时间，返回false
if (last_timestamp_imu < lidar_end_time) if (last_timestamp_imu < lidar_end_time)
{ {
return false; return false;
} }
/*** push imu data, and pop from imu buffer ***/ /*** push imu data, and pop from imu buffer ***/
double imu_time = imu_buffer.front()->header.stamp.toSec(); // 推送IMU数据，并从IMU缓冲区中弹出
meas.imu.clear(); double imu_time = imu_buffer.front()->header.stamp.toSec(); // 获取IMU缓冲区的第一个元素的时间戳
meas.imu.clear(); // 清空测量的IMU数据
while ((!imu_buffer.empty()) && (imu_time < lidar_end_time)) while ((!imu_buffer.empty()) && (imu_time < lidar_end_time))
{ {
imu_time = imu_buffer.front()->header.stamp.toSec(); imu_time = imu_buffer.front()->header.stamp.toSec(); // 更新IMU时间
if(imu_time > lidar_end_time) break; if(imu_time > lidar_end_time) break; // 如果IMU时间大于激光雷达结束时间，跳出循环
meas.imu.push_back(imu_buffer.front()); meas.imu.push_back(imu_buffer.front()); // 将IMU数据添加到测量的IMU数据中
imu_buffer.pop_front(); imu_buffer.pop_front(); // 从IMU缓冲区中弹出第一个元素
} }
lidar_buffer.pop_front(); lidar_buffer.pop_front(); // 从激光雷达缓冲区中弹出第一个元素
time_buffer.pop_front(); time_buffer.pop_front(); // 从时间缓冲区中弹出第一个元素
lidar_pushed = false; lidar_pushed = false; // 重置激光雷达已推送标志
return true; return true; // 返回true，表示同步成功
} }
int process_increments = 0; int process_increments = 0;
@ -755,187 +765,189 @@ void h_share_model(state_ikfom &s, esekfom::dyn_share_datastruct<double> &ekfom_
int main(int argc, char** argv) int main(int argc, char** argv)
{ {
ros::init(argc, argv, "laserMapping"); ros::init(argc, argv, "laserMapping"); // 初始化ROS节点，节点名为"laserMapping"
ros::NodeHandle nh; ros::NodeHandle nh; // 创建节点句柄
nh.param<bool>("publish/path_en",path_en, true); nh.param<bool>("publish/path_en", path_en, true); // 从参数服务器获取path_en参数，默认为true
nh.param<bool>("publish/scan_publish_en",scan_pub_en, true); nh.param<bool>("publish/scan_publish_en", scan_pub_en, true); // 从参数服务器获取scan_pub_en参数，默认为true
nh.param<bool>("publish/dense_publish_en",dense_pub_en, true); nh.param<bool>("publish/dense_publish_en", dense_pub_en, true); // 从参数服务器获取dense_pub_en参数，默认为true
nh.param<bool>("publish/scan_bodyframe_pub_en",scan_body_pub_en, true); nh.param<bool>("publish/scan_bodyframe_pub_en", scan_body_pub_en, true); // 从参数服务器获取scan_body_pub_en参数，默认为true
nh.param<int>("max_iteration",NUM_MAX_ITERATIONS,4); nh.param<int>("max_iteration", NUM_MAX_ITERATIONS, 4); // 从参数服务器获取最大迭代次数，默认为4
nh.param<string>("map_file_path",map_file_path,""); nh.param<string>("map_file_path", map_file_path, ""); // 从参数服务器获取地图文件路径，默认为空字符串
nh.param<string>("common/lid_topic",lid_topic,"/livox/lidar"); nh.param<string>("common/lid_topic", lid_topic, "/livox/lidar"); // 从参数服务器获取激光雷达话题名，默认为"/livox/lidar"
nh.param<string>("common/imu_topic", imu_topic,"/livox/imu"); nh.param<string>("common/imu_topic", imu_topic, "/livox/imu"); // 从参数服务器获取IMU话题名，默认为"/livox/imu"
nh.param<bool>("common/time_sync_en", time_sync_en, false); nh.param<bool>("common/time_sync_en", time_sync_en, false); // 从参数服务器获取时间同步使能标志，默认为false
nh.param<double>("common/time_offset_lidar_to_imu", time_diff_lidar_to_imu, 0.0); nh.param<double>("common/time_offset_lidar_to_imu", time_diff_lidar_to_imu, 0.0); // 从参数服务器获取激光雷达与IMU的时间偏移，默认为0.0
nh.param<double>("filter_size_corner",filter_size_corner_min,0.5); nh.param<double>("filter_size_corner", filter_size_corner_min, 0.5); // 从参数服务器获取角点滤波器尺寸，默认为0.5
nh.param<double>("filter_size_surf",filter_size_surf_min,0.5); nh.param<double>("filter_size_surf", filter_size_surf_min, 0.5); // 从参数服务器获取平面点滤波器尺寸，默认为0.5
nh.param<double>("filter_size_map",filter_size_map_min,0.5); nh.param<double>("filter_size_map", filter_size_map_min, 0.5); // 从参数服务器获取地图滤波器尺寸，默认为0.5
nh.param<double>("cube_side_length",cube_len,200); nh.param<double>("cube_side_length", cube_len, 200); // 从参数服务器获取立方体边长，默认为200
nh.param<float>("mapping/det_range",DET_RANGE,300.f); nh.param<float>("mapping/det_range", DET_RANGE, 300.f); // 从参数服务器获取检测范围，默认为300.0f
nh.param<double>("mapping/fov_degree",fov_deg,180); nh.param<double>("mapping/fov_degree", fov_deg, 180); // 从参数服务器获取视场角，默认为180
nh.param<double>("mapping/gyr_cov",gyr_cov,0.1); nh.param<double>("mapping/gyr_cov", gyr_cov, 0.1); // 从参数服务器获取陀螺仪协方差，默认为0.1
nh.param<double>("mapping/acc_cov",acc_cov,0.1); nh.param<double>("mapping/acc_cov", acc_cov, 0.1); // 从参数服务器获取加速度计协方差，默认为0.1
nh.param<double>("mapping/b_gyr_cov",b_gyr_cov,0.0001); nh.param<double>("mapping/b_gyr_cov", b_gyr_cov, 0.0001); // 从参数服务器获取陀螺仪偏置协方差，默认为0.0001
nh.param<double>("mapping/b_acc_cov",b_acc_cov,0.0001); nh.param<double>("mapping/b_acc_cov", b_acc_cov, 0.0001); // 从参数服务器获取加速度计偏置协方差，默认为0.0001
nh.param<double>("preprocess/blind", p_pre->blind, 0.01); nh.param<double>("preprocess/blind", p_pre->blind, 0.01); // 从参数服务器获取盲区距离，默认为0.01
nh.param<int>("preprocess/lidar_type", lidar_type, AVIA); nh.param<int>("preprocess/lidar_type", lidar_type, AVIA); // 从参数服务器获取激光雷达类型，默认为AVIA
nh.param<int>("preprocess/scan_line", p_pre->N_SCANS, 16); nh.param<int>("preprocess/scan_line", p_pre->N_SCANS, 16); // 从参数服务器获取扫描线数，默认为16
nh.param<int>("preprocess/timestamp_unit", p_pre->time_unit, US); nh.param<int>("preprocess/timestamp_unit", p_pre->time_unit, US); // 从参数服务器获取时间戳单位，默认为微秒
nh.param<int>("preprocess/scan_rate", p_pre->SCAN_RATE, 10); nh.param<int>("preprocess/scan_rate", p_pre->SCAN_RATE, 10); // 从参数服务器获取扫描频率，默认为10
nh.param<int>("point_filter_num", p_pre->point_filter_num, 2); nh.param<int>("point_filter_num", p_pre->point_filter_num, 2); // 从参数服务器获取点过滤数量，默认为2
nh.param<bool>("feature_extract_enable", p_pre->feature_enabled, false); nh.param<bool>("feature_extract_enable", p_pre->feature_enabled, false); // 从参数服务器获取特征提取使能标志，默认为false
nh.param<bool>("runtime_pos_log_enable", runtime_pos_log, 0); nh.param<bool>("runtime_pos_log_enable", runtime_pos_log, 0); // 从参数服务器获取运行时位置日志使能标志，默认为false
nh.param<bool>("mapping/extrinsic_est_en", extrinsic_est_en, true); nh.param<bool>("mapping/extrinsic_est_en", extrinsic_est_en, true); // 从参数服务器获取外参估计使能标志，默认为true
nh.param<bool>("pcd_save/pcd_save_en", pcd_save_en, false); nh.param<bool>("pcd_save/pcd_save_en", pcd_save_en, false); // 从参数服务器获取PCD保存使能标志，默认为false
nh.param<int>("pcd_save/interval", pcd_save_interval, -1); nh.param<int>("pcd_save/interval", pcd_save_interval, -1); // 从参数服务器获取PCD保存间隔，默认为-1
nh.param<vector<double>>("mapping/extrinsic_T", extrinT, vector<double>()); nh.param<vector<double>>("mapping/extrinsic_T", extrinT, vector<double>()); // 从参数服务器获取外参平移向量
nh.param<vector<double>>("mapping/extrinsic_R", extrinR, vector<double>()); nh.param<vector<double>>("mapping/extrinsic_R", extrinR, vector<double>()); // 从参数服务器获取外参旋转矩阵
p_pre->lidar_type = lidar_type;
cout<<"p_pre->lidar_type "<<p_pre->lidar_type<<endl;
path.header.stamp = ros::Time::now(); p_pre->lidar_type = lidar_type; // 设置预处理对象的激光雷达类型
path.header.frame_id ="camera_init"; cout<<"p_pre->lidar_type "<<p_pre->lidar_type<<endl; // 打印激光雷达类型
path.header.stamp = ros::Time::now(); // 设置路径消息的时间戳
path.header.frame_id ="camera_init"; // 设置路径消息的坐标系
/*** variables definition ***/ /*** variables definition ***/
int effect_feat_num = 0, frame_num = 0; int effect_feat_num = 0, frame_num = 0;
double deltaT, deltaR, aver_time_consu = 0, aver_time_icp = 0, aver_time_match = 0, aver_time_incre = 0, aver_time_solve = 0, aver_time_const_H_time = 0; double deltaT, deltaR, aver_time_consu = 0, aver_time_icp = 0, aver_time_match = 0, aver_time_incre = 0, aver_time_solve = 0, aver_time_const_H_time = 0;
bool flg_EKF_converged, EKF_stop_flg = 0; bool flg_EKF_converged, EKF_stop_flg = 0;
FOV_DEG = (fov_deg + 10.0) > 179.9 ? 179.9 : (fov_deg + 10.0); FOV_DEG = (fov_deg + 10.0) > 179.9 ? 179.9 : (fov_deg + 10.0); // 计算视场角
HALF_FOV_COS = cos((FOV_DEG) * 0.5 * PI_M / 180.0); HALF_FOV_COS = cos((FOV_DEG) * 0.5 * PI_M / 180.0); // 计算视场角的一半的余弦值
_featsArray.reset(new PointCloudXYZI()); _featsArray.reset(new PointCloudXYZI()); // 重置特征点云数组
memset(point_selected_surf, true, sizeof(point_selected_surf)); memset(point_selected_surf, true, sizeof(point_selected_surf)); // 初始化点选择数组
memset(res_last, -1000.0f, sizeof(res_last)); memset(res_last, -1000.0f, sizeof(res_last)); // 初始化残差数组
downSizeFilterSurf.setLeafSize(filter_size_surf_min, filter_size_surf_min, filter_size_surf_min); downSizeFilterSurf.setLeafSize(filter_size_surf_min, filter_size_surf_min, filter_size_surf_min); // 设置平面点滤波器尺寸
downSizeFilterMap.setLeafSize(filter_size_map_min, filter_size_map_min, filter_size_map_min); downSizeFilterMap.setLeafSize(filter_size_map_min, filter_size_map_min, filter_size_map_min); // 设置地图滤波器尺寸
memset(point_selected_surf, true, sizeof(point_selected_surf)); memset(point_selected_surf, true, sizeof(point_selected_surf)); // 初始化点选择数组
memset(res_last, -1000.0f, sizeof(res_last)); memset(res_last, -1000.0f, sizeof(res_last)); // 初始化残差数组
Lidar_T_wrt_IMU<<VEC_FROM_ARRAY(extrinT); Lidar_T_wrt_IMU<<VEC_FROM_ARRAY(extrinT); // 设置激光雷达相对于IMU的平移向量
Lidar_R_wrt_IMU<<MAT_FROM_ARRAY(extrinR); Lidar_R_wrt_IMU<<MAT_FROM_ARRAY(extrinR); // 设置激光雷达相对于IMU的旋转矩阵
p_imu->set_extrinsic(Lidar_T_wrt_IMU, Lidar_R_wrt_IMU); p_imu->set_extrinsic(Lidar_T_wrt_IMU, Lidar_R_wrt_IMU); // 设置IMU处理对象的外参
p_imu->set_gyr_cov(V3D(gyr_cov, gyr_cov, gyr_cov)); p_imu->set_gyr_cov(V3D(gyr_cov, gyr_cov, gyr_cov)); // 设置IMU处理对象的陀螺仪协方差
p_imu->set_acc_cov(V3D(acc_cov, acc_cov, acc_cov)); p_imu->set_acc_cov(V3D(acc_cov, acc_cov, acc_cov)); // 设置IMU处理对象的加速度计协方差
p_imu->set_gyr_bias_cov(V3D(b_gyr_cov, b_gyr_cov, b_gyr_cov)); p_imu->set_gyr_bias_cov(V3D(b_gyr_cov, b_gyr_cov, b_gyr_cov)); // 设置IMU处理对象的陀螺仪偏置协方差
p_imu->set_acc_bias_cov(V3D(b_acc_cov, b_acc_cov, b_acc_cov)); p_imu->set_acc_bias_cov(V3D(b_acc_cov, b_acc_cov, b_acc_cov)); // 设置IMU处理对象的加速度计偏置协方差
p_imu->lidar_type = lidar_type; p_imu->lidar_type = lidar_type; // 设置IMU处理对象的激光雷达类型
double epsi[23] = {0.001}; double epsi[23] = {0.001}; // 初始化误差数组
fill(epsi, epsi+23, 0.001); fill(epsi, epsi+23, 0.001); // 填充误差数组
kf.init_dyn_share(get_f, df_dx, df_dw, h_share_model, NUM_MAX_ITERATIONS, epsi); kf.init_dyn_share(get_f, df_dx, df_dw, h_share_model, NUM_MAX_ITERATIONS, epsi); // 初始化EKF
/*** debug record ***/ /*** 调试记录***/
FILE *fp; FILE *fp;
string pos_log_dir = root_dir + "/Log/pos_log.txt"; string pos_log_dir = root_dir + "/Log/pos_log.txt"; // 位置日志文件路径
fp = fopen(pos_log_dir.c_str(),"w"); fp = fopen(pos_log_dir.c_str(),"w"); // 打开位置日志文件
ofstream fout_pre, fout_out, fout_dbg; ofstream fout_pre, fout_out, fout_dbg;
fout_pre.open(DEBUG_FILE_DIR("mat_pre.txt"),ios::out); fout_pre.open(DEBUG_FILE_DIR("mat_pre.txt"),ios::out); // 打开预处理矩阵文件
fout_out.open(DEBUG_FILE_DIR("mat_out.txt"),ios::out); fout_out.open(DEBUG_FILE_DIR("mat_out.txt"),ios::out); // 打开输出矩阵文件
fout_dbg.open(DEBUG_FILE_DIR("dbg.txt"),ios::out); fout_dbg.open(DEBUG_FILE_DIR("dbg.txt"),ios::out); // 打开调试文件
if (fout_pre && fout_out) if (fout_pre && fout_out) // 如果文件打开成功，执行相应操作
cout << "~~~~"<<ROOT_DIR<<" file opened" << endl; cout << "~~~~"<<ROOT_DIR<<" file opened" << endl;
else else // 如果文件打开失败，执行相应操作
cout << "~~~~"<<ROOT_DIR<<" doesn't exist" << endl; cout << "~~~~"<<ROOT_DIR<<" doesn't exist" << endl;
/*** ROS subscribe initialization ***/ /*** ROS订阅初始化 ***/
ros::Subscriber sub_pcl = p_pre->lidar_type == AVIA ? \ ros::Subscriber sub_pcl = p_pre->lidar_type == AVIA ? \ // 根据激光雷达类型订阅相应的话题
nh.subscribe(lid_topic, 200000, livox_pcl_cbk) : \ nh.subscribe(lid_topic, 200000, livox_pcl_cbk) : \
nh.subscribe(lid_topic, 200000, standard_pcl_cbk); nh.subscribe(lid_topic, 200000, standard_pcl_cbk);
ros::Subscriber sub_imu = nh.subscribe(imu_topic, 200000, imu_cbk); ros::Subscriber sub_imu = nh.subscribe(imu_topic, 200000, imu_cbk); // 订阅IMU话题
ros::Publisher pubLaserCloudFull = nh.advertise<sensor_msgs::PointCloud2> ros::Publisher pubLaserCloudFull = nh.advertise<sensor_msgs::PointCloud2> // 发布完整点云
("/cloud_registered", 100000); ("/cloud_registered", 100000);
ros::Publisher pubLaserCloudFull_body = nh.advertise<sensor_msgs::PointCloud2> ros::Publisher pubLaserCloudFull_body = nh.advertise<sensor_msgs::PointCloud2> // 发布机体坐标系下的完整点云
("/cloud_registered_body", 100000); ("/cloud_registered_body", 100000);
ros::Publisher pubLaserCloudEffect = nh.advertise<sensor_msgs::PointCloud2> ros::Publisher pubLaserCloudEffect = nh.advertise<sensor_msgs::PointCloud2> // 发布有效点云
("/cloud_effected", 100000); ("/cloud_effected", 100000);
ros::Publisher pubLaserCloudMap = nh.advertise<sensor_msgs::PointCloud2> ros::Publisher pubLaserCloudMap = nh.advertise<sensor_msgs::PointCloud2> // 发布地图点云
("/Laser_map", 100000); ("/Laser_map", 100000);
ros::Publisher pubOdomAftMapped = nh.advertise<nav_msgs::Odometry> ros::Publisher pubOdomAftMapped = nh.advertise<nav_msgs::Odometry> // 发布映射后的里程计
("/Odometry", 100000); ("/Odometry", 100000);
ros::Publisher pubPath = nh.advertise<nav_msgs::Path> ros::Publisher pubPath = nh.advertise<nav_msgs::Path> // 发布路径
("/path", 100000); ("/path", 100000);
//------------------------------------------------------------------------------------------------------ //------------------------------------------------------------------------------------------------------
signal(SIGINT, SigHandle); signal(SIGINT, SigHandle); // 设置SIGINT信号处理函数，当接收到SIGINT信号（通
ros::Rate rate(5000); ros::Rate rate(5000); // 设置ROS循环频率为5000Hz
bool status = ros::ok(); bool status = ros::ok(); // 检查ROS节点是否正常运行
while (status) while (status) // 进入主循环
{ {
if (flg_exit) break; if (flg_exit) break; // 如果接收到退出信号，跳出循环
ros::spinOnce(); ros::spinOnce(); // 处理所有挂起的回调函数
if(sync_packages(Measures)) if(sync_packages(Measures)) // 如果同步数据包成功
{ {
if (flg_first_scan) if (flg_first_scan) // 如果是第一次扫描
{ {
first_lidar_time = Measures.lidar_beg_time; first_lidar_time = Measures.lidar_beg_time; // 设置第一次激光雷达时间
p_imu->first_lidar_time = first_lidar_time; p_imu->first_lidar_time = first_lidar_time; // 设置IMU处理对象的第一次激光雷达时间
flg_first_scan = false; flg_first_scan = false; // 设置第一次扫描标志为false
continue; continue;
} }
double t0,t1,t2,t3,t4,t5,match_start, solve_start, svd_time; double t0,t1,t2,t3,t4,t5,match_start, solve_start, svd_time; // 定义时间变量
match_time = 0; match_time = 0; // 匹配时间
kdtree_search_time = 0.0; kdtree_search_time = 0.0; // KD树搜索时间
solve_time = 0; solve_time = 0; // 解算时间
solve_const_H_time = 0; solve_const_H_time = 0; // 解算常数H时间
svd_time = 0; svd_time = 0; // SVD时间
t0 = omp_get_wtime(); t0 = omp_get_wtime(); // 获取当前时间
p_imu->Process(Measures, kf, feats_undistort); p_imu->Process(Measures, kf, feats_undistort); // IMU数据处理
state_point = kf.get_x(); state_point = kf.get_x(); // 获取状态点
pos_lid = state_point.pos + state_point.rot * state_point.offset_T_L_I; pos_lid = state_point.pos + state_point.rot * state_point.offset_T_L_I; // 计算激光雷达位置
if (feats_undistort->empty() || (feats_undistort == NULL)) if (feats_undistort->empty() || (feats_undistort == NULL)) // 如果特征点为空
{ {
ROS_WARN("No point, skip this scan!\n"); ROS_WARN("No point, skip this scan!\n");
continue; continue;
} }
// EKF初始化
flg_EKF_inited = (Measures.lidar_beg_time - first_lidar_time) < INIT_TIME ? \ flg_EKF_inited = (Measures.lidar_beg_time - first_lidar_time) < INIT_TIME ? \
false : true; false : true;
/*** Segment the map in lidar FOV ***/ /*** Segment the map in lidar FOV ***/
lasermap_fov_segment(); lasermap_fov_segment(); // 激光雷达地图视场分割
/*** downsample the feature points in a scan ***/ /*** downsample the feature points in a scan ***/
downSizeFilterSurf.setInputCloud(feats_undistort); downSizeFilterSurf.setInputCloud(feats_undistort); // 设置平面点滤波器输入点云
downSizeFilterSurf.filter(*feats_down_body); downSizeFilterSurf.filter(*feats_down_body); // 进行平面点滤波
t1 = omp_get_wtime(); t1 = omp_get_wtime(); // 获取当前时间
feats_down_size = feats_down_body->points.size(); feats_down_size = feats_down_body->points.size(); // 获取平面点数
/*** initialize the map kdtree ***/ /*** initialize the map kdtree ***/
if(ikdtree.Root_Node == nullptr) if(ikdtree.Root_Node == nullptr) // 如果KD树根节点为空
{ {
if(feats_down_size > 5) if(feats_down_size > 5) // 如果平面点数大于5
{ {
ikdtree.set_downsample_param(filter_size_map_min); ikdtree.set_downsample_param(filter_size_map_min); // 设置地图滤波器尺寸
feats_down_world->resize(feats_down_size); feats_down_world->resize(feats_down_size); // 重置平面点云大小
for(int i = 0; i < feats_down_size; i++) for(int i = 0; i < feats_down_size; i++) // 遍历平面点
{ {
pointBodyToWorld(&(feats_down_body->points[i]), &(feats_down_world->points[i])); pointBodyToWorld(&(feats_down_body->points[i]), &(feats_down_world->points[i])); // 机体坐标系转换到世界坐标系
} }
ikdtree.Build(feats_down_world->points); ikdtree.Build(feats_down_world->points); // 构建KD树
} }
continue; continue;
} }
int featsFromMapNum = ikdtree.validnum(); int featsFromMapNum = ikdtree.validnum(); // 有效地图点数
kdtree_size_st = ikdtree.size(); kdtree_size_st = ikdtree.size(); // KD树大小
// cout<<"[ mapping ]: In num: "<<feats_undistort->points.size()<<" downsamp "<<feats_down_size<<" Map num: "<<featsFromMapNum<<"effect num:"<<effct_feat_num<<endl; // cout<<"[ mapping ]: In num: "<<feats_undistort->points.size()<<" downsamp "<<feats_down_size<<" Map num: "<<featsFromMapNum<<"effect num:"<<effct_feat_num<<endl;
/*** ICP and iterated Kalman filter update ***/ /*** ICP and iterated Kalman filter update ***/
if (feats_down_size < 5) if (feats_down_size < 5) // 如果平面点数小于5
{ {
ROS_WARN("No point, skip this scan!\n"); ROS_WARN("No point, skip this scan!\n");
continue; continue;
} }
normvec->resize(feats_down_size); normvec->resize(feats_down_size); // 重置法向量
feats_down_world->resize(feats_down_size); feats_down_world->resize(feats_down_size); // 重置平面点云大小
V3D ext_euler = SO3ToEuler(state_point.offset_R_L_I); V3D ext_euler = SO3ToEuler(state_point.offset_R_L_I); // 外部欧拉角
fout_pre<<setw(20)<<Measures.lidar_beg_time - first_lidar_time<<" "<<euler_cur.transpose()<<" "<< state_point.pos.transpose()<<" "<<ext_euler.transpose() << " "<<state_point.offset_T_L_I.transpose()<< " " << state_point.vel.transpose() \ fout_pre<<setw(20)<<Measures.lidar_beg_time - first_lidar_time<<" "<<euler_cur.transpose()<<" "<< state_point.pos.transpose()<<" "<<ext_euler.transpose() << " "<<state_point.offset_T_L_I.transpose()<< " " << state_point.vel.transpose() \
<<" "<<state_point.bg.transpose()<<" "<<state_point.ba.transpose()<<" "<<state_point.grav<< endl; <<" "<<state_point.bg.transpose()<<" "<<state_point.ba.transpose()<<" "<<state_point.grav<< endl;
@ -947,39 +959,39 @@ int main(int argc, char** argv)
featsFromMap->points = ikdtree.PCL_Storage; featsFromMap->points = ikdtree.PCL_Storage;
} }
pointSearchInd_surf.resize(feats_down_size); pointSearchInd_surf.resize(feats_down_size); // 设置平面点搜索索引
Nearest_Points.resize(feats_down_size); Nearest_Points.resize(feats_down_size); // 设置最近点
int rematch_num = 0; int rematch_num = 0; // 重新匹配次数
bool nearest_search_en = true; // bool nearest_search_en = true; // 最近搜索使能标志
t2 = omp_get_wtime(); t2 = omp_get_wtime(); // 获取当前时间
/*** iterated state estimation ***/ /*** iterated state estimation ***/
double t_update_start = omp_get_wtime(); double t_update_start = omp_get_wtime(); // 获取当前时间
double solve_H_time = 0; double solve_H_time = 0; // 解算H时间
kf.update_iterated_dyn_share_modified(LASER_POINT_COV, solve_H_time); kf.update_iterated_dyn_share_modified(LASER_POINT_COV, solve_H_time); // 更新迭代动态共享
state_point = kf.get_x(); state_point = kf.get_x(); // 获取状态点
euler_cur = SO3ToEuler(state_point.rot); euler_cur = SO3ToEuler(state_point.rot); // 当前欧拉角
pos_lid = state_point.pos + state_point.rot * state_point.offset_T_L_I; pos_lid = state_point.pos + state_point.rot * state_point.offset_T_L_I; // 激光雷达位置
geoQuat.x = state_point.rot.coeffs()[0]; geoQuat.x = state_point.rot.coeffs()[0]; // 设置四元数x
geoQuat.y = state_point.rot.coeffs()[1]; geoQuat.y = state_point.rot.coeffs()[1]; // 设置四元数y
geoQuat.z = state_point.rot.coeffs()[2]; geoQuat.z = state_point.rot.coeffs()[2]; // 设置四元数z
geoQuat.w = state_point.rot.coeffs()[3]; geoQuat.w = state_point.rot.coeffs()[3]; // 设置四元数w
double t_update_end = omp_get_wtime(); double t_update_end = omp_get_wtime(); // 获取当前时间
/******* Publish odometry *******/ /******* Publish odometry *******/
publish_odometry(pubOdomAftMapped); publish_odometry(pubOdomAftMapped); // 发布映射后的里程计
/*** add the feature points to map kdtree ***/ /*** add the feature points to map kdtree ***/
t3 = omp_get_wtime(); t3 = omp_get_wtime(); // 获取当前时间
map_incremental(); map_incremental(); // 地图增量
t5 = omp_get_wtime(); t5 = omp_get_wtime(); // 获取当前时间
/******* Publish points *******/ /******* Publish points *******/
if (path_en) publish_path(pubPath); if (path_en) publish_path(pubPath); // 发布路径
if (scan_pub_en || pcd_save_en) publish_frame_world(pubLaserCloudFull); if (scan_pub_en || pcd_save_en) publish_frame_world(pubLaserCloudFull); // 发布完整点云
if (scan_pub_en && scan_body_pub_en) publish_frame_body(pubLaserCloudFull_body); if (scan_pub_en && scan_body_pub_en) publish_frame_body(pubLaserCloudFull_body); // 发布机体坐标系下的完整点云
// publish_effect_world(pubLaserCloudEffect); // publish_effect_world(pubLaserCloudEffect);
// publish_map(pubLaserCloudMap); // publish_map(pubLaserCloudMap);
@ -1021,7 +1033,7 @@ int main(int argc, char** argv)
/**************** save map ****************/ /**************** save map ****************/
/* 1. make sure you have enough memories /* 1. make sure you have enough memories
/* 2. pcd save will largely influence the real-time performences **/ /* 2. pcd save will largely influence the real-time performences **/
if (pcl_wait_save->size() > 0 && pcd_save_en) if (pcl_wait_save->size() > 0 && pcd_save_en) // 如果点云大小大于0且PCD保存使能标志为真
{ {
string file_name = string("scans.pcd"); string file_name = string("scans.pcd");
string all_points_dir(string(string(ROOT_DIR) + "PCD/") + file_name); string all_points_dir(string(string(ROOT_DIR) + "PCD/") + file_name);
@ -1033,7 +1045,7 @@ int main(int argc, char** argv)
fout_out.close(); fout_out.close();
fout_pre.close(); fout_pre.close();
if (runtime_pos_log) if (runtime_pos_log) // 如果运行时位置日志使能标志为真
{ {
vector<double> t, s_vec, s_vec2, s_vec3, s_vec4, s_vec5, s_vec6, s_vec7; vector<double> t, s_vec, s_vec2, s_vec3, s_vec4, s_vec5, s_vec6, s_vec7;
FILE *fp2; FILE *fp2;

117
src/preprocess.cpp
View File

@ -33,6 +33,7 @@ Preprocess::Preprocess()
Preprocess::~Preprocess() {} Preprocess::~Preprocess() {}
// 设置函数
void Preprocess::set(bool feat_en, int lid_type, double bld, int pfilt_num) void Preprocess::set(bool feat_en, int lid_type, double bld, int pfilt_num)
{ {
feature_enabled = feat_en; feature_enabled = feat_en;
@ -41,12 +42,14 @@ void Preprocess::set(bool feat_en, int lid_type, double bld, int pfilt_num)
point_filter_num = pfilt_num; point_filter_num = pfilt_num;
} }
// 处理 Livox 数据
void Preprocess::process(const livox_ros_driver::CustomMsg::ConstPtr &msg, PointCloudXYZI::Ptr &pcl_out) void Preprocess::process(const livox_ros_driver::CustomMsg::ConstPtr &msg, PointCloudXYZI::Ptr &pcl_out)
{ {
avia_handler(msg); avia_handler(msg);
*pcl_out = pl_surf; *pcl_out = pl_surf;
} }
// 处理其他传感器数据
void Preprocess::process(const sensor_msgs::PointCloud2::ConstPtr &msg, PointCloudXYZI::Ptr &pcl_out) void Preprocess::process(const sensor_msgs::PointCloud2::ConstPtr &msg, PointCloudXYZI::Ptr &pcl_out)
{ {
switch (time_unit) switch (time_unit)
@ -89,96 +92,98 @@ void Preprocess::process(const sensor_msgs::PointCloud2::ConstPtr &msg, PointClo
*pcl_out = pl_surf; *pcl_out = pl_surf;
} }
// 处理 Livox 数据
void Preprocess::avia_handler(const livox_ros_driver::CustomMsg::ConstPtr &msg) void Preprocess::avia_handler(const livox_ros_driver::CustomMsg::ConstPtr &msg)
{ {
pl_surf.clear(); pl_surf.clear(); // 清空 pl_surf 点云
pl_corn.clear(); pl_corn.clear(); // 清空 pl_corn 点云
pl_full.clear(); pl_full.clear(); // 清空 pl_full 点云
double t1 = omp_get_wtime(); double t1 = omp_get_wtime(); // 获取当前时间，用于计算处理时间
int plsize = msg->point_num; int plsize = msg->point_num; // 获取点云的点数
// cout<<"plsie: "<<plsize<<endl; // cout<<"plsie: "<<plsize<<endl;
pl_corn.reserve(plsize); pl_corn.reserve(plsize); // 预留 pl_corn 点云的存储空间
pl_surf.reserve(plsize); pl_surf.reserve(plsize); // 预留 pl_surf 点云的存储空间
pl_full.resize(plsize); pl_full.resize(plsize); // 调整 pl_full 点云的大小
for(int i=0; i<N_SCANS; i++) for(int i=0; i<N_SCANS; i++) // 遍历所有扫描线
{ {
pl_buff[i].clear(); pl_buff[i].clear(); // 清空每条扫描线的点云缓冲区
pl_buff[i].reserve(plsize); pl_buff[i].reserve(plsize); // 预留每条扫描线的点云缓冲区存储空间
} }
uint valid_num = 0; uint valid_num = 0; // 有效点计数器
if (feature_enabled) if (feature_enabled) // 如果启用了特征提取
{ {
for(uint i=1; i<plsize; i++) for(uint i=1; i<plsize; i++) // 遍历所有点
{ {
if((msg->points[i].line < N_SCANS) && ((msg->points[i].tag & 0x30) == 0x10 || (msg->points[i].tag & 0x30) == 0x00)) if((msg->points[i].line < N_SCANS) && ((msg->points[i].tag & 0x30) == 0x10 || (msg->points[i].tag & 0x30) == 0x00)) // 检查点的有效性
{ {
pl_full[i].x = msg->points[i].x; pl_full[i].x = msg->points[i].x; // 复制点的 x 坐标
pl_full[i].y = msg->points[i].y; pl_full[i].y = msg->points[i].y; // 复制点的 y 坐标
pl_full[i].z = msg->points[i].z; pl_full[i].z = msg->points[i].z; // 复制点的 z 坐标
pl_full[i].intensity = msg->points[i].reflectivity; pl_full[i].intensity = msg->points[i].reflectivity; // 复制点的反射强度
pl_full[i].curvature = msg->points[i].offset_time / float(1000000); //use curvature as time of each laser points pl_full[i].curvature = msg->points[i].offset_time / float(1000000); //use curvature as time of each laser points
// 将点的时间偏移存储在曲率字段中，单位为秒
bool is_new = false; bool is_new = false; // 标记是否为新点
if((abs(pl_full[i].x - pl_full[i-1].x) > 1e-7) if((abs(pl_full[i].x - pl_full[i-1].x) > 1e-7)
|| (abs(pl_full[i].y - pl_full[i-1].y) > 1e-7) || (abs(pl_full[i].y - pl_full[i-1].y) > 1e-7)
|| (abs(pl_full[i].z - pl_full[i-1].z) > 1e-7)) || (abs(pl_full[i].z - pl_full[i-1].z) > 1e-7)) // 检查点是否与前一个点不同
{ {
pl_buff[msg->points[i].line].push_back(pl_full[i]); pl_buff[msg->points[i].line].push_back(pl_full[i]); // 将点添加到对应扫描线的缓冲区
} }
} }
} }
static int count = 0; static int count = 0; // 处理计数器
static double time = 0.0; static double time = 0.0; // 累计处理时间
count ++; count ++; // 增加处理计数
double t0 = omp_get_wtime(); double t0 = omp_get_wtime(); // 获取当前时间，用于计算特征提取时间
for(int j=0; j<N_SCANS; j++) for(int j=0; j<N_SCANS; j++) // 遍历所有扫描线
{ {
if(pl_buff[j].size() <= 5) continue; if(pl_buff[j].size() <= 5) continue; // 如果扫描线的点数小于等于5，跳过
pcl::PointCloud<PointType> &pl = pl_buff[j]; pcl::PointCloud<PointType> &pl = pl_buff[j]; // 获取扫描线的点云
plsize = pl.size(); plsize = pl.size(); // 获取点云的点数
vector<orgtype> &types = typess[j]; vector<orgtype> &types = typess[j]; // 获取扫描线的类型数组
types.clear(); types.clear(); // 清空类型数组
types.resize(plsize); types.resize(plsize); // 调整类型数组的大小
plsize--; plsize--; // 减少点数
for(uint i=0; i<plsize; i++) for(uint i=0; i<plsize; i++) // 遍历点云中的点
{ {
types[i].range = sqrt(pl[i].x * pl[i].x + pl[i].y * pl[i].y); types[i].range = sqrt(pl[i].x * pl[i].x + pl[i].y * pl[i].y); // 计算点到原点的距离
vx = pl[i].x - pl[i + 1].x; vx = pl[i].x - pl[i + 1].x; // 计算相邻点的 x 坐标差
vy = pl[i].y - pl[i + 1].y; vy = pl[i].y - pl[i + 1].y; // 计算相邻点的 y 坐标差
vz = pl[i].z - pl[i + 1].z; vz = pl[i].z - pl[i + 1].z; // 计算相邻点的 z 坐标差
types[i].dista = sqrt(vx * vx + vy * vy + vz * vz); types[i].dista = sqrt(vx * vx + vy * vy + vz * vz); // 计算相邻点之间的距离
} }
types[plsize].range = sqrt(pl[plsize].x * pl[plsize].x + pl[plsize].y * pl[plsize].y); types[plsize].range = sqrt(pl[plsize].x * pl[plsize].x + pl[plsize].y * pl[plsize].y); // 计算最后一个点到原点的距离
give_feature(pl, types); give_feature(pl, types); // 提取特征
// pl_surf += pl; // pl_surf += pl;
} }
time += omp_get_wtime() - t0; time += omp_get_wtime() - t0; // 计算特征提取时间
printf("Feature extraction time: %lf \n", time / count); printf("Feature extraction time: %lf \n", time / count); // 打印平均特征提取时间
} }
else else // 如果未启用特征提取
{ {
for(uint i=1; i<plsize; i++) for(uint i=1; i<plsize; i++) // 遍历所有点
{ {
if((msg->points[i].line < N_SCANS) && ((msg->points[i].tag & 0x30) == 0x10 || (msg->points[i].tag & 0x30) == 0x00)) if((msg->points[i].line < N_SCANS) && ((msg->points[i].tag & 0x30) == 0x10 || (msg->points[i].tag & 0x30) == 0x00)) // 检查点的有效性
{ {
valid_num ++; valid_num ++; // 增加有效点计数
if (valid_num % point_filter_num == 0) if (valid_num % point_filter_num == 0) // 根据点过滤数量筛选点
{ {
pl_full[i].x = msg->points[i].x; pl_full[i].x = msg->points[i].x; // 复制点的 x 坐标
pl_full[i].y = msg->points[i].y; pl_full[i].y = msg->points[i].y; // 复制点的 y 坐标
pl_full[i].z = msg->points[i].z; pl_full[i].z = msg->points[i].z; // 复制点的 z 坐标
pl_full[i].intensity = msg->points[i].reflectivity; pl_full[i].intensity = msg->points[i].reflectivity; // 复制点的反射强度
pl_full[i].curvature = msg->points[i].offset_time / float(1000000); // use curvature as time of each laser points, curvature unit: ms pl_full[i].curvature = msg->points[i].offset_time / float(1000000); // 将点的时间偏移存储在曲率字段中，单位为秒
if(((abs(pl_full[i].x - pl_full[i-1].x) > 1e-7) if(((abs(pl_full[i].x - pl_full[i-1].x) > 1e-7)
|| (abs(pl_full[i].y - pl_full[i-1].y) > 1e-7) || (abs(pl_full[i].y - pl_full[i-1].y) > 1e-7)
|| (abs(pl_full[i].z - pl_full[i-1].z) > 1e-7)) || (abs(pl_full[i].z - pl_full[i-1].z) > 1e-7))
&& (pl_full[i].x * pl_full[i].x + pl_full[i].y * pl_full[i].y + pl_full[i].z * pl_full[i].z > (blind * blind))) && (pl_full[i].x * pl_full[i].x + pl_full[i].y * pl_full[i].y + pl_full[i].z * pl_full[i].z > (blind * blind))) // 检查点是否与前一个点不同且距离大于盲区
{ {
pl_surf.push_back(pl_full[i]); pl_surf.push_back(pl_full[i]); // 将点添加到 pl_surf 点云
} }
} }
} }