diff --git a/src/laserMapping.cpp b/src/laserMapping.cpp index fc41408..bf41c75 100644 --- a/src/laserMapping.cpp +++ b/src/laserMapping.cpp @@ -297,40 +297,43 @@ void standard_pcl_cbk(const sensor_msgs::PointCloud2::ConstPtr &msg) sig_buffer.notify_all(); } -double timediff_lidar_wrt_imu = 0.0; -bool timediff_set_flg = false; +double timediff_lidar_wrt_imu = 0.0; // 激光雷达与IMU的时间差 +bool timediff_set_flg = false; // 时间差设置标志 void livox_pcl_cbk(const livox_ros_driver::CustomMsg::ConstPtr &msg) { - mtx_buffer.lock(); - double preprocess_start_time = omp_get_wtime(); - scan_count ++; - if (msg->header.stamp.toSec() < last_timestamp_lidar) + mtx_buffer.lock(); // 锁定互斥量，保护共享资源 + double preprocess_start_time = omp_get_wtime(); // 获取当前时间，用于计算预处理时间 + scan_count ++; // 增加扫描计数 + if (msg->header.stamp.toSec() < last_timestamp_lidar) // 检查激光雷达时间戳是否小于最后一个激光雷达时间戳 { - ROS_ERROR("lidar loop back, clear buffer"); - lidar_buffer.clear(); + ROS_ERROR("lidar loop back, clear buffer"); // 打印错误信息 + 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() ) { + // 打印不同步信息 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()) { - timediff_set_flg = true; - timediff_lidar_wrt_imu = last_timestamp_lidar + 0.1 - last_timestamp_imu; - printf("Self sync IMU and LiDAR, time diff is %.10lf \n", timediff_lidar_wrt_imu); + timediff_set_flg = true; // 设置时间差标志 + 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); // 打印时间差 } - PointCloudXYZI::Ptr ptr(new PointCloudXYZI()); - p_pre->process(msg, ptr); - lidar_buffer.push_back(ptr); - time_buffer.push_back(last_timestamp_lidar); + PointCloudXYZI::Ptr ptr(new PointCloudXYZI()); // 创建新的点云指针 + p_pre->process(msg, ptr); // 处理激光雷达消息，生成点云 + lidar_buffer.push_back(ptr); // 将点云添加到激光雷达缓冲区 + time_buffer.push_back(last_timestamp_lidar); // 将时间戳添加到时间缓冲区 - s_plot11[scan_count] = omp_get_wtime() - preprocess_start_time; - mtx_buffer.unlock(); - sig_buffer.notify_all(); + s_plot11[scan_count] = omp_get_wtime() - preprocess_start_time; // 计算并记录预处理时间 + mtx_buffer.unlock(); // 解锁互斥量 + sig_buffer.notify_all(); // 通知所有等待的线程 } 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(); } -double lidar_mean_scantime = 0.0; -int scan_num = 0; +double lidar_mean_scantime = 0.0; // 激光雷达平均扫描时间 +int scan_num = 0; // 扫描次数 bool sync_packages(MeasureGroup &meas) { + // 如果激光雷达缓冲区或IMU缓冲区为空，返回false if (lidar_buffer.empty() || imu_buffer.empty()) { return false; } - /*** push a lidar scan ***/ + // 推送一个激光雷达扫描 if(!lidar_pushed) { - meas.lidar = lidar_buffer.front(); - meas.lidar_beg_time = time_buffer.front(); + meas.lidar = lidar_buffer.front(); // 获取激光雷达缓冲区的第一个元素 + meas.lidar_beg_time = time_buffer.front(); // 获取时间缓冲区的第一个元素 + // 如果激光雷达点云的点数小于等于1，设置激光雷达结束时间为开始时间加上平均扫描时间 if (meas.lidar->points.size() <= 1) // time too little { lidar_end_time = meas.lidar_beg_time + lidar_mean_scantime; ROS_WARN("Too few input point cloud!\n"); } + // 如果最后一个点的曲率小于平均扫描时间的一半，设置激光雷达结束时间为开始时间加上平均扫描时间 else if (meas.lidar->points.back().curvature / double(1000) < 0.5 * lidar_mean_scantime) { lidar_end_time = meas.lidar_beg_time + lidar_mean_scantime; } + // 否则，更新扫描次数和激光雷达结束时间，并计算新的平均扫描时间 else { scan_num ++; 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; } + // 如果激光雷达类型为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; } + // 如果最后一个IMU时间戳小于激光雷达结束时间，返回false if (last_timestamp_imu < lidar_end_time) { return false; } /*** push imu data, and pop from imu buffer ***/ - double imu_time = imu_buffer.front()->header.stamp.toSec(); - meas.imu.clear(); + // 推送IMU数据，并从IMU缓冲区中弹出 + double imu_time = imu_buffer.front()->header.stamp.toSec(); // 获取IMU缓冲区的第一个元素的时间戳 + meas.imu.clear(); // 清空测量的IMU数据 while ((!imu_buffer.empty()) && (imu_time < lidar_end_time)) { - imu_time = imu_buffer.front()->header.stamp.toSec(); - if(imu_time > lidar_end_time) break; - meas.imu.push_back(imu_buffer.front()); - imu_buffer.pop_front(); + imu_time = imu_buffer.front()->header.stamp.toSec(); // 更新IMU时间 + if(imu_time > lidar_end_time) break; // 如果IMU时间大于激光雷达结束时间，跳出循环 + meas.imu.push_back(imu_buffer.front()); // 将IMU数据添加到测量的IMU数据中 + imu_buffer.pop_front(); // 从IMU缓冲区中弹出第一个元素 } - lidar_buffer.pop_front(); - time_buffer.pop_front(); - lidar_pushed = false; - return true; + lidar_buffer.pop_front(); // 从激光雷达缓冲区中弹出第一个元素 + time_buffer.pop_front(); // 从时间缓冲区中弹出第一个元素 + lidar_pushed = false; // 重置激光雷达已推送标志 + return true; // 返回true，表示同步成功 } int process_increments = 0; @@ -755,187 +765,189 @@ void h_share_model(state_ikfom &s, esekfom::dyn_share_datastruct &ekfom_ int main(int argc, char** argv) { - ros::init(argc, argv, "laserMapping"); - ros::NodeHandle nh; + ros::init(argc, argv, "laserMapping"); // 初始化ROS节点，节点名为"laserMapping" + ros::NodeHandle nh; // 创建节点句柄 - nh.param("publish/path_en",path_en, true); - nh.param("publish/scan_publish_en",scan_pub_en, true); - nh.param("publish/dense_publish_en",dense_pub_en, true); - nh.param("publish/scan_bodyframe_pub_en",scan_body_pub_en, true); - nh.param("max_iteration",NUM_MAX_ITERATIONS,4); - nh.param("map_file_path",map_file_path,""); - nh.param("common/lid_topic",lid_topic,"/livox/lidar"); - nh.param("common/imu_topic", imu_topic,"/livox/imu"); - nh.param("common/time_sync_en", time_sync_en, false); - nh.param("common/time_offset_lidar_to_imu", time_diff_lidar_to_imu, 0.0); - nh.param("filter_size_corner",filter_size_corner_min,0.5); - nh.param("filter_size_surf",filter_size_surf_min,0.5); - nh.param("filter_size_map",filter_size_map_min,0.5); - nh.param("cube_side_length",cube_len,200); - nh.param("mapping/det_range",DET_RANGE,300.f); - nh.param("mapping/fov_degree",fov_deg,180); - nh.param("mapping/gyr_cov",gyr_cov,0.1); - nh.param("mapping/acc_cov",acc_cov,0.1); - nh.param("mapping/b_gyr_cov",b_gyr_cov,0.0001); - nh.param("mapping/b_acc_cov",b_acc_cov,0.0001); - nh.param("preprocess/blind", p_pre->blind, 0.01); - nh.param("preprocess/lidar_type", lidar_type, AVIA); - nh.param("preprocess/scan_line", p_pre->N_SCANS, 16); - nh.param("preprocess/timestamp_unit", p_pre->time_unit, US); - nh.param("preprocess/scan_rate", p_pre->SCAN_RATE, 10); - nh.param("point_filter_num", p_pre->point_filter_num, 2); - nh.param("feature_extract_enable", p_pre->feature_enabled, false); - nh.param("runtime_pos_log_enable", runtime_pos_log, 0); - nh.param("mapping/extrinsic_est_en", extrinsic_est_en, true); - nh.param("pcd_save/pcd_save_en", pcd_save_en, false); - nh.param("pcd_save/interval", pcd_save_interval, -1); - nh.param>("mapping/extrinsic_T", extrinT, vector()); - nh.param>("mapping/extrinsic_R", extrinR, vector()); + nh.param("publish/path_en", path_en, true); // 从参数服务器获取path_en参数，默认为true + nh.param("publish/scan_publish_en", scan_pub_en, true); // 从参数服务器获取scan_pub_en参数，默认为true + nh.param("publish/dense_publish_en", dense_pub_en, true); // 从参数服务器获取dense_pub_en参数，默认为true + nh.param("publish/scan_bodyframe_pub_en", scan_body_pub_en, true); // 从参数服务器获取scan_body_pub_en参数，默认为true + nh.param("max_iteration", NUM_MAX_ITERATIONS, 4); // 从参数服务器获取最大迭代次数，默认为4 + nh.param("map_file_path", map_file_path, ""); // 从参数服务器获取地图文件路径，默认为空字符串 + nh.param("common/lid_topic", lid_topic, "/livox/lidar"); // 从参数服务器获取激光雷达话题名，默认为"/livox/lidar" + nh.param("common/imu_topic", imu_topic, "/livox/imu"); // 从参数服务器获取IMU话题名，默认为"/livox/imu" + nh.param("common/time_sync_en", time_sync_en, false); // 从参数服务器获取时间同步使能标志，默认为false + nh.param("common/time_offset_lidar_to_imu", time_diff_lidar_to_imu, 0.0); // 从参数服务器获取激光雷达与IMU的时间偏移，默认为0.0 + nh.param("filter_size_corner", filter_size_corner_min, 0.5); // 从参数服务器获取角点滤波器尺寸，默认为0.5 + nh.param("filter_size_surf", filter_size_surf_min, 0.5); // 从参数服务器获取平面点滤波器尺寸，默认为0.5 + nh.param("filter_size_map", filter_size_map_min, 0.5); // 从参数服务器获取地图滤波器尺寸，默认为0.5 + nh.param("cube_side_length", cube_len, 200); // 从参数服务器获取立方体边长，默认为200 + nh.param("mapping/det_range", DET_RANGE, 300.f); // 从参数服务器获取检测范围，默认为300.0f + nh.param("mapping/fov_degree", fov_deg, 180); // 从参数服务器获取视场角，默认为180 + nh.param("mapping/gyr_cov", gyr_cov, 0.1); // 从参数服务器获取陀螺仪协方差，默认为0.1 + nh.param("mapping/acc_cov", acc_cov, 0.1); // 从参数服务器获取加速度计协方差，默认为0.1 + nh.param("mapping/b_gyr_cov", b_gyr_cov, 0.0001); // 从参数服务器获取陀螺仪偏置协方差，默认为0.0001 + nh.param("mapping/b_acc_cov", b_acc_cov, 0.0001); // 从参数服务器获取加速度计偏置协方差，默认为0.0001 + nh.param("preprocess/blind", p_pre->blind, 0.01); // 从参数服务器获取盲区距离，默认为0.01 + nh.param("preprocess/lidar_type", lidar_type, AVIA); // 从参数服务器获取激光雷达类型，默认为AVIA + nh.param("preprocess/scan_line", p_pre->N_SCANS, 16); // 从参数服务器获取扫描线数，默认为16 + nh.param("preprocess/timestamp_unit", p_pre->time_unit, US); // 从参数服务器获取时间戳单位，默认为微秒 + nh.param("preprocess/scan_rate", p_pre->SCAN_RATE, 10); // 从参数服务器获取扫描频率，默认为10 + nh.param("point_filter_num", p_pre->point_filter_num, 2); // 从参数服务器获取点过滤数量，默认为2 + nh.param("feature_extract_enable", p_pre->feature_enabled, false); // 从参数服务器获取特征提取使能标志，默认为false + nh.param("runtime_pos_log_enable", runtime_pos_log, 0); // 从参数服务器获取运行时位置日志使能标志，默认为false + nh.param("mapping/extrinsic_est_en", extrinsic_est_en, true); // 从参数服务器获取外参估计使能标志，默认为true + nh.param("pcd_save/pcd_save_en", pcd_save_en, false); // 从参数服务器获取PCD保存使能标志，默认为false + nh.param("pcd_save/interval", pcd_save_interval, -1); // 从参数服务器获取PCD保存间隔，默认为-1 + nh.param>("mapping/extrinsic_T", extrinT, vector()); // 从参数服务器获取外参平移向量 + nh.param>("mapping/extrinsic_R", extrinR, vector()); // 从参数服务器获取外参旋转矩阵 - p_pre->lidar_type = lidar_type; - cout<<"p_pre->lidar_type "<lidar_type<lidar_type = lidar_type; // 设置预处理对象的激光雷达类型 + cout<<"p_pre->lidar_type "<lidar_type< 179.9 ? 179.9 : (fov_deg + 10.0); - HALF_FOV_COS = cos((FOV_DEG) * 0.5 * PI_M / 180.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); // 计算视场角的一半的余弦值 - _featsArray.reset(new PointCloudXYZI()); + _featsArray.reset(new PointCloudXYZI()); // 重置特征点云数组 - memset(point_selected_surf, true, sizeof(point_selected_surf)); - memset(res_last, -1000.0f, sizeof(res_last)); - 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); - memset(point_selected_surf, true, sizeof(point_selected_surf)); - memset(res_last, -1000.0f, sizeof(res_last)); + memset(point_selected_surf, true, sizeof(point_selected_surf)); // 初始化点选择数组 + memset(res_last, -1000.0f, sizeof(res_last)); // 初始化残差数组 + 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); // 设置地图滤波器尺寸 + memset(point_selected_surf, true, sizeof(point_selected_surf)); // 初始化点选择数组 + memset(res_last, -1000.0f, sizeof(res_last)); // 初始化残差数组 - Lidar_T_wrt_IMU<set_extrinsic(Lidar_T_wrt_IMU, Lidar_R_wrt_IMU); - p_imu->set_gyr_cov(V3D(gyr_cov, gyr_cov, gyr_cov)); - p_imu->set_acc_cov(V3D(acc_cov, acc_cov, acc_cov)); - p_imu->set_gyr_bias_cov(V3D(b_gyr_cov, b_gyr_cov, b_gyr_cov)); - p_imu->set_acc_bias_cov(V3D(b_acc_cov, b_acc_cov, b_acc_cov)); - p_imu->lidar_type = lidar_type; - double 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); + Lidar_T_wrt_IMU<set_extrinsic(Lidar_T_wrt_IMU, Lidar_R_wrt_IMU); // 设置IMU处理对象的外参 + 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)); // 设置IMU处理对象的加速度计协方差 + 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)); // 设置IMU处理对象的加速度计偏置协方差 + p_imu->lidar_type = lidar_type; // 设置IMU处理对象的激光雷达类型 + double 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); // 初始化EKF - /*** debug record ***/ + /*** 调试记录***/ FILE *fp; - string pos_log_dir = root_dir + "/Log/pos_log.txt"; - fp = fopen(pos_log_dir.c_str(),"w"); + string pos_log_dir = root_dir + "/Log/pos_log.txt"; // 位置日志文件路径 + fp = fopen(pos_log_dir.c_str(),"w"); // 打开位置日志文件 ofstream fout_pre, fout_out, fout_dbg; - fout_pre.open(DEBUG_FILE_DIR("mat_pre.txt"),ios::out); - fout_out.open(DEBUG_FILE_DIR("mat_out.txt"),ios::out); - fout_dbg.open(DEBUG_FILE_DIR("dbg.txt"),ios::out); - if (fout_pre && fout_out) + fout_pre.open(DEBUG_FILE_DIR("mat_pre.txt"),ios::out); // 打开预处理矩阵文件 + fout_out.open(DEBUG_FILE_DIR("mat_out.txt"),ios::out); // 打开输出矩阵文件 + fout_dbg.open(DEBUG_FILE_DIR("dbg.txt"),ios::out); // 打开调试文件 + if (fout_pre && fout_out) // 如果文件打开成功，执行相应操作 cout << "~~~~"<lidar_type == AVIA ? \ + /*** ROS订阅初始化 ***/ + ros::Subscriber sub_pcl = p_pre->lidar_type == AVIA ? \ // 根据激光雷达类型订阅相应的话题 nh.subscribe(lid_topic, 200000, livox_pcl_cbk) : \ nh.subscribe(lid_topic, 200000, standard_pcl_cbk); - ros::Subscriber sub_imu = nh.subscribe(imu_topic, 200000, imu_cbk); - ros::Publisher pubLaserCloudFull = nh.advertise + ros::Subscriber sub_imu = nh.subscribe(imu_topic, 200000, imu_cbk); // 订阅IMU话题 + ros::Publisher pubLaserCloudFull = nh.advertise // 发布完整点云 ("/cloud_registered", 100000); - ros::Publisher pubLaserCloudFull_body = nh.advertise + ros::Publisher pubLaserCloudFull_body = nh.advertise // 发布机体坐标系下的完整点云 ("/cloud_registered_body", 100000); - ros::Publisher pubLaserCloudEffect = nh.advertise + ros::Publisher pubLaserCloudEffect = nh.advertise // 发布有效点云 ("/cloud_effected", 100000); - ros::Publisher pubLaserCloudMap = nh.advertise + ros::Publisher pubLaserCloudMap = nh.advertise // 发布地图点云 ("/Laser_map", 100000); - ros::Publisher pubOdomAftMapped = nh.advertise + ros::Publisher pubOdomAftMapped = nh.advertise // 发布映射后的里程计 ("/Odometry", 100000); - ros::Publisher pubPath = nh.advertise + ros::Publisher pubPath = nh.advertise // 发布路径 ("/path", 100000); //------------------------------------------------------------------------------------------------------ - signal(SIGINT, SigHandle); - ros::Rate rate(5000); - bool status = ros::ok(); - while (status) + signal(SIGINT, SigHandle); // 设置SIGINT信号处理函数，当接收到SIGINT信号（通 + ros::Rate rate(5000); // 设置ROS循环频率为5000Hz + bool status = ros::ok(); // 检查ROS节点是否正常运行 + while (status) // 进入主循环 { - if (flg_exit) break; - ros::spinOnce(); - if(sync_packages(Measures)) + if (flg_exit) break; // 如果接收到退出信号，跳出循环 + ros::spinOnce(); // 处理所有挂起的回调函数 + if(sync_packages(Measures)) // 如果同步数据包成功 { - if (flg_first_scan) + if (flg_first_scan) // 如果是第一次扫描 { - first_lidar_time = Measures.lidar_beg_time; - p_imu->first_lidar_time = first_lidar_time; - flg_first_scan = false; + first_lidar_time = Measures.lidar_beg_time; // 设置第一次激光雷达时间 + p_imu->first_lidar_time = first_lidar_time; // 设置IMU处理对象的第一次激光雷达时间 + flg_first_scan = false; // 设置第一次扫描标志为false 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; - kdtree_search_time = 0.0; - solve_time = 0; - solve_const_H_time = 0; - svd_time = 0; - t0 = omp_get_wtime(); + match_time = 0; // 匹配时间 + kdtree_search_time = 0.0; // KD树搜索时间 + solve_time = 0; // 解算时间 + solve_const_H_time = 0; // 解算常数H时间 + svd_time = 0; // SVD时间 + t0 = omp_get_wtime(); // 获取当前时间 - p_imu->Process(Measures, kf, feats_undistort); - state_point = kf.get_x(); - pos_lid = state_point.pos + state_point.rot * state_point.offset_T_L_I; + p_imu->Process(Measures, kf, feats_undistort); // IMU数据处理 + state_point = kf.get_x(); // 获取状态点 + 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"); continue; } + // EKF初始化 flg_EKF_inited = (Measures.lidar_beg_time - first_lidar_time) < INIT_TIME ? \ false : true; /*** Segment the map in lidar FOV ***/ - lasermap_fov_segment(); + lasermap_fov_segment(); // 激光雷达地图视场分割 /*** downsample the feature points in a scan ***/ - downSizeFilterSurf.setInputCloud(feats_undistort); - downSizeFilterSurf.filter(*feats_down_body); - t1 = omp_get_wtime(); - feats_down_size = feats_down_body->points.size(); + downSizeFilterSurf.setInputCloud(feats_undistort); // 设置平面点滤波器输入点云 + downSizeFilterSurf.filter(*feats_down_body); // 进行平面点滤波 + t1 = omp_get_wtime(); // 获取当前时间 + feats_down_size = feats_down_body->points.size(); // 获取平面点数 /*** 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); - feats_down_world->resize(feats_down_size); - for(int i = 0; i < feats_down_size; i++) + ikdtree.set_downsample_param(filter_size_map_min); // 设置地图滤波器尺寸 + feats_down_world->resize(feats_down_size); // 重置平面点云大小 + 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; } - int featsFromMapNum = ikdtree.validnum(); - kdtree_size_st = ikdtree.size(); + int featsFromMapNum = ikdtree.validnum(); // 有效地图点数 + kdtree_size_st = ikdtree.size(); // KD树大小 // cout<<"[ mapping ]: In num: "<points.size()<<" downsamp "<resize(feats_down_size); - feats_down_world->resize(feats_down_size); + normvec->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<points = ikdtree.PCL_Storage; } - pointSearchInd_surf.resize(feats_down_size); - Nearest_Points.resize(feats_down_size); - int rematch_num = 0; - bool nearest_search_en = true; // + pointSearchInd_surf.resize(feats_down_size); // 设置平面点搜索索引 + Nearest_Points.resize(feats_down_size); // 设置最近点 + int rematch_num = 0; // 重新匹配次数 + bool nearest_search_en = true; // 最近搜索使能标志 - t2 = omp_get_wtime(); + t2 = omp_get_wtime(); // 获取当前时间 /*** iterated state estimation ***/ - double t_update_start = omp_get_wtime(); - double solve_H_time = 0; - kf.update_iterated_dyn_share_modified(LASER_POINT_COV, solve_H_time); - state_point = kf.get_x(); - euler_cur = SO3ToEuler(state_point.rot); - pos_lid = state_point.pos + state_point.rot * state_point.offset_T_L_I; - geoQuat.x = state_point.rot.coeffs()[0]; - geoQuat.y = state_point.rot.coeffs()[1]; - geoQuat.z = state_point.rot.coeffs()[2]; - geoQuat.w = state_point.rot.coeffs()[3]; + double t_update_start = omp_get_wtime(); // 获取当前时间 + double solve_H_time = 0; // 解算H时间 + kf.update_iterated_dyn_share_modified(LASER_POINT_COV, solve_H_time); // 更新迭代动态共享 + state_point = kf.get_x(); // 获取状态点 + euler_cur = SO3ToEuler(state_point.rot); // 当前欧拉角 + pos_lid = state_point.pos + state_point.rot * state_point.offset_T_L_I; // 激光雷达位置 + geoQuat.x = state_point.rot.coeffs()[0]; // 设置四元数x + geoQuat.y = state_point.rot.coeffs()[1]; // 设置四元数y + geoQuat.z = state_point.rot.coeffs()[2]; // 设置四元数z + 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(pubOdomAftMapped); + publish_odometry(pubOdomAftMapped); // 发布映射后的里程计 /*** add the feature points to map kdtree ***/ - t3 = omp_get_wtime(); - map_incremental(); - t5 = omp_get_wtime(); + t3 = omp_get_wtime(); // 获取当前时间 + map_incremental(); // 地图增量 + t5 = omp_get_wtime(); // 获取当前时间 /******* Publish points *******/ - if (path_en) publish_path(pubPath); - 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 (path_en) publish_path(pubPath); // 发布路径 + if (scan_pub_en || pcd_save_en) publish_frame_world(pubLaserCloudFull); // 发布完整点云 + if (scan_pub_en && scan_body_pub_en) publish_frame_body(pubLaserCloudFull_body); // 发布机体坐标系下的完整点云 // publish_effect_world(pubLaserCloudEffect); // publish_map(pubLaserCloudMap); @@ -1021,7 +1033,7 @@ int main(int argc, char** argv) /**************** save map ****************/ /* 1. make sure you have enough memories /* 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 all_points_dir(string(string(ROOT_DIR) + "PCD/") + file_name); @@ -1033,7 +1045,7 @@ int main(int argc, char** argv) fout_out.close(); fout_pre.close(); - if (runtime_pos_log) + if (runtime_pos_log) // 如果运行时位置日志使能标志为真 { vector t, s_vec, s_vec2, s_vec3, s_vec4, s_vec5, s_vec6, s_vec7; FILE *fp2; diff --git a/src/preprocess.cpp b/src/preprocess.cpp index 09a3959..9d0707b 100644 --- a/src/preprocess.cpp +++ b/src/preprocess.cpp @@ -33,6 +33,7 @@ Preprocess::Preprocess() Preprocess::~Preprocess() {} +// 设置函数 void Preprocess::set(bool feat_en, int lid_type, double bld, int pfilt_num) { 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; } +// 处理 Livox 数据 void Preprocess::process(const livox_ros_driver::CustomMsg::ConstPtr &msg, PointCloudXYZI::Ptr &pcl_out) { avia_handler(msg); *pcl_out = pl_surf; } +// 处理其他传感器数据 void Preprocess::process(const sensor_msgs::PointCloud2::ConstPtr &msg, PointCloudXYZI::Ptr &pcl_out) { switch (time_unit) @@ -89,96 +92,98 @@ void Preprocess::process(const sensor_msgs::PointCloud2::ConstPtr &msg, PointClo *pcl_out = pl_surf; } +// 处理 Livox 数据 void Preprocess::avia_handler(const livox_ros_driver::CustomMsg::ConstPtr &msg) { - pl_surf.clear(); - pl_corn.clear(); - pl_full.clear(); - double t1 = omp_get_wtime(); - int plsize = msg->point_num; + pl_surf.clear(); // 清空 pl_surf 点云 + pl_corn.clear(); // 清空 pl_corn 点云 + pl_full.clear(); // 清空 pl_full 点云 + double t1 = omp_get_wtime(); // 获取当前时间，用于计算处理时间 + int plsize = msg->point_num; // 获取点云的点数 // cout<<"plsie: "<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].y = msg->points[i].y; - pl_full[i].z = msg->points[i].z; - pl_full[i].intensity = msg->points[i].reflectivity; + pl_full[i].x = msg->points[i].x; // 复制点的 x 坐标 + pl_full[i].y = msg->points[i].y; // 复制点的 y 坐标 + pl_full[i].z = msg->points[i].z; // 复制点的 z 坐标 + 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 + // 将点的时间偏移存储在曲率字段中，单位为秒 - bool is_new = false; + bool is_new = false; // 标记是否为新点 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].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 double time = 0.0; - count ++; - double t0 = omp_get_wtime(); - for(int j=0; j &pl = pl_buff[j]; - plsize = pl.size(); - vector &types = typess[j]; - types.clear(); - types.resize(plsize); - plsize--; - for(uint i=0; i &pl = pl_buff[j]; // 获取扫描线的点云 + plsize = pl.size(); // 获取点云的点数 + vector &types = typess[j]; // 获取扫描线的类型数组 + types.clear(); // 清空类型数组 + types.resize(plsize); // 调整类型数组的大小 + plsize--; // 减少点数 + for(uint i=0; ipoints[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 ++; - if (valid_num % point_filter_num == 0) + valid_num ++; // 增加有效点计数 + if (valid_num % point_filter_num == 0) // 根据点过滤数量筛选点 { - pl_full[i].x = msg->points[i].x; - pl_full[i].y = msg->points[i].y; - pl_full[i].z = msg->points[i].z; - 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].x = msg->points[i].x; // 复制点的 x 坐标 + pl_full[i].y = msg->points[i].y; // 复制点的 y 坐标 + pl_full[i].z = msg->points[i].z; // 复制点的 z 坐标 + pl_full[i].intensity = msg->points[i].reflectivity; // 复制点的反射强度 + pl_full[i].curvature = msg->points[i].offset_time / float(1000000); // 将点的时间偏移存储在曲率字段中，单位为秒 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].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 点云 } } }