feat: Enhance lidar processing with detailed comments and synchronization checks
parent
db492f0498
commit
e2689e5c7c
|
|
@ -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<double> &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<bool>("publish/path_en",path_en, true);
|
||||
nh.param<bool>("publish/scan_publish_en",scan_pub_en, true);
|
||||
nh.param<bool>("publish/dense_publish_en",dense_pub_en, true);
|
||||
nh.param<bool>("publish/scan_bodyframe_pub_en",scan_body_pub_en, true);
|
||||
nh.param<int>("max_iteration",NUM_MAX_ITERATIONS,4);
|
||||
nh.param<string>("map_file_path",map_file_path,"");
|
||||
nh.param<string>("common/lid_topic",lid_topic,"/livox/lidar");
|
||||
nh.param<string>("common/imu_topic", imu_topic,"/livox/imu");
|
||||
nh.param<bool>("common/time_sync_en", time_sync_en, false);
|
||||
nh.param<double>("common/time_offset_lidar_to_imu", time_diff_lidar_to_imu, 0.0);
|
||||
nh.param<double>("filter_size_corner",filter_size_corner_min,0.5);
|
||||
nh.param<double>("filter_size_surf",filter_size_surf_min,0.5);
|
||||
nh.param<double>("filter_size_map",filter_size_map_min,0.5);
|
||||
nh.param<double>("cube_side_length",cube_len,200);
|
||||
nh.param<float>("mapping/det_range",DET_RANGE,300.f);
|
||||
nh.param<double>("mapping/fov_degree",fov_deg,180);
|
||||
nh.param<double>("mapping/gyr_cov",gyr_cov,0.1);
|
||||
nh.param<double>("mapping/acc_cov",acc_cov,0.1);
|
||||
nh.param<double>("mapping/b_gyr_cov",b_gyr_cov,0.0001);
|
||||
nh.param<double>("mapping/b_acc_cov",b_acc_cov,0.0001);
|
||||
nh.param<double>("preprocess/blind", p_pre->blind, 0.01);
|
||||
nh.param<int>("preprocess/lidar_type", lidar_type, AVIA);
|
||||
nh.param<int>("preprocess/scan_line", p_pre->N_SCANS, 16);
|
||||
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>("point_filter_num", p_pre->point_filter_num, 2);
|
||||
nh.param<bool>("feature_extract_enable", p_pre->feature_enabled, false);
|
||||
nh.param<bool>("runtime_pos_log_enable", runtime_pos_log, 0);
|
||||
nh.param<bool>("mapping/extrinsic_est_en", extrinsic_est_en, true);
|
||||
nh.param<bool>("pcd_save/pcd_save_en", pcd_save_en, false);
|
||||
nh.param<int>("pcd_save/interval", pcd_save_interval, -1);
|
||||
nh.param<vector<double>>("mapping/extrinsic_T", extrinT, vector<double>());
|
||||
nh.param<vector<double>>("mapping/extrinsic_R", extrinR, vector<double>());
|
||||
nh.param<bool>("publish/path_en", path_en, true); // 从参数服务器获取path_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); // 从参数服务器获取dense_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); // 从参数服务器获取最大迭代次数，默认为4
|
||||
nh.param<string>("map_file_path", map_file_path, ""); // 从参数服务器获取地图文件路径，默认为空字符串
|
||||
nh.param<string>("common/lid_topic", lid_topic, "/livox/lidar"); // 从参数服务器获取激光雷达话题名，默认为"/livox/lidar"
|
||||
nh.param<string>("common/imu_topic", imu_topic, "/livox/imu"); // 从参数服务器获取IMU话题名，默认为"/livox/imu"
|
||||
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); // 从参数服务器获取激光雷达与IMU的时间偏移，默认为0.0
|
||||
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); // 从参数服务器获取平面点滤波器尺寸，默认为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); // 从参数服务器获取立方体边长，默认为200
|
||||
nh.param<float>("mapping/det_range", DET_RANGE, 300.f); // 从参数服务器获取检测范围，默认为300.0f
|
||||
nh.param<double>("mapping/fov_degree", fov_deg, 180); // 从参数服务器获取视场角，默认为180
|
||||
nh.param<double>("mapping/gyr_cov", gyr_cov, 0.1); // 从参数服务器获取陀螺仪协方差，默认为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); // 从参数服务器获取陀螺仪偏置协方差，默认为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); // 从参数服务器获取盲区距离，默认为0.01
|
||||
nh.param<int>("preprocess/lidar_type", lidar_type, AVIA); // 从参数服务器获取激光雷达类型，默认为AVIA
|
||||
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/scan_rate", p_pre->SCAN_RATE, 10); // 从参数服务器获取扫描频率，默认为10
|
||||
nh.param<int>("point_filter_num", p_pre->point_filter_num, 2); // 从参数服务器获取点过滤数量，默认为2
|
||||
nh.param<bool>("feature_extract_enable", p_pre->feature_enabled, false); // 从参数服务器获取特征提取使能标志，默认为false
|
||||
nh.param<bool>("runtime_pos_log_enable", runtime_pos_log, 0); // 从参数服务器获取运行时位置日志使能标志，默认为false
|
||||
nh.param<bool>("mapping/extrinsic_est_en", extrinsic_est_en, true); // 从参数服务器获取外参估计使能标志，默认为true
|
||||
nh.param<bool>("pcd_save/pcd_save_en", pcd_save_en, false); // 从参数服务器获取PCD保存使能标志，默认为false
|
||||
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_R", extrinR, vector<double>()); // 从参数服务器获取外参旋转矩阵
|
||||
|
||||
p_pre->lidar_type = lidar_type;
|
||||
cout<<"p_pre->lidar_type "<<p_pre->lidar_type<<endl;
|
||||
|
||||
p_pre->lidar_type = lidar_type; // 设置预处理对象的激光雷达类型
|
||||
cout<<"p_pre->lidar_type "<<p_pre->lidar_type<<endl; // 打印激光雷达类型
|
||||
|
||||
path.header.stamp = ros::Time::now();
|
||||
path.header.frame_id ="camera_init";
|
||||
path.header.stamp = ros::Time::now(); // 设置路径消息的时间戳
|
||||
path.header.frame_id ="camera_init"; // 设置路径消息的坐标系
|
||||
|
||||
/*** variables definition ***/
|
||||
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;
|
||||
bool flg_EKF_converged, EKF_stop_flg = 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);
|
||||
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<<VEC_FROM_ARRAY(extrinT);
|
||||
Lidar_R_wrt_IMU<<MAT_FROM_ARRAY(extrinR);
|
||||
p_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<<VEC_FROM_ARRAY(extrinT); // 设置激光雷达相对于IMU的平移向量
|
||||
Lidar_R_wrt_IMU<<MAT_FROM_ARRAY(extrinR); // 设置激光雷达相对于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)); // 设置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 << "~~~~"<<ROOT_DIR<<" file opened" << endl;
|
||||
else
|
||||
else // 如果文件打开失败，执行相应操作
|
||||
cout << "~~~~"<<ROOT_DIR<<" doesn't exist" << endl;
|
||||
|
||||
/*** ROS subscribe initialization ***/
|
||||
ros::Subscriber sub_pcl = p_pre->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<sensor_msgs::PointCloud2>
|
||||
ros::Subscriber sub_imu = nh.subscribe(imu_topic, 200000, imu_cbk); // 订阅IMU话题
|
||||
ros::Publisher pubLaserCloudFull = nh.advertise<sensor_msgs::PointCloud2> // 发布完整点云
|
||||
("/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);
|
||||
ros::Publisher pubLaserCloudEffect = nh.advertise<sensor_msgs::PointCloud2>
|
||||
ros::Publisher pubLaserCloudEffect = nh.advertise<sensor_msgs::PointCloud2> // 发布有效点云
|
||||
("/cloud_effected", 100000);
|
||||
ros::Publisher pubLaserCloudMap = nh.advertise<sensor_msgs::PointCloud2>
|
||||
ros::Publisher pubLaserCloudMap = nh.advertise<sensor_msgs::PointCloud2> // 发布地图点云
|
||||
("/Laser_map", 100000);
|
||||
ros::Publisher pubOdomAftMapped = nh.advertise<nav_msgs::Odometry>
|
||||
ros::Publisher pubOdomAftMapped = nh.advertise<nav_msgs::Odometry> // 发布映射后的里程计
|
||||
("/Odometry", 100000);
|
||||
ros::Publisher pubPath = nh.advertise<nav_msgs::Path>
|
||||
ros::Publisher pubPath = nh.advertise<nav_msgs::Path> // 发布路径
|
||||
("/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: "<<feats_undistort->points.size()<<" downsamp "<<feats_down_size<<" Map num: "<<featsFromMapNum<<"effect num:"<<effct_feat_num<<endl;
|
||||
|
||||
/*** 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");
|
||||
continue;
|
||||
}
|
||||
|
||||
normvec->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<<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;
|
||||
|
||||
|
|
@ -947,39 +959,39 @@ int main(int argc, char** argv)
|
|||
featsFromMap->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<double> t, s_vec, s_vec2, s_vec3, s_vec4, s_vec5, s_vec6, s_vec7;
|
||||
FILE *fp2;
|
||||
|
|
|
|||
|
|
@ -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: "<<plsize<<endl;
|
||||
|
||||
pl_corn.reserve(plsize);
|
||||
pl_surf.reserve(plsize);
|
||||
pl_full.resize(plsize);
|
||||
pl_corn.reserve(plsize); // 预留 pl_corn 点云的存储空间
|
||||
pl_surf.reserve(plsize); // 预留 pl_surf 点云的存储空间
|
||||
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].reserve(plsize);
|
||||
pl_buff[i].clear(); // 清空每条扫描线的点云缓冲区
|
||||
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].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<N_SCANS; j++)
|
||||
static int count = 0; // 处理计数器
|
||||
static double time = 0.0; // 累计处理时间
|
||||
count ++; // 增加处理计数
|
||||
double t0 = omp_get_wtime(); // 获取当前时间，用于计算特征提取时间
|
||||
for(int j=0; j<N_SCANS; j++) // 遍历所有扫描线
|
||||
{
|
||||
if(pl_buff[j].size() <= 5) continue;
|
||||
pcl::PointCloud<PointType> &pl = pl_buff[j];
|
||||
plsize = pl.size();
|
||||
vector<orgtype> &types = typess[j];
|
||||
types.clear();
|
||||
types.resize(plsize);
|
||||
plsize--;
|
||||
for(uint i=0; i<plsize; i++)
|
||||
if(pl_buff[j].size() <= 5) continue; // 如果扫描线的点数小于等于5，跳过
|
||||
pcl::PointCloud<PointType> &pl = pl_buff[j]; // 获取扫描线的点云
|
||||
plsize = pl.size(); // 获取点云的点数
|
||||
vector<orgtype> &types = typess[j]; // 获取扫描线的类型数组
|
||||
types.clear(); // 清空类型数组
|
||||
types.resize(plsize); // 调整类型数组的大小
|
||||
plsize--; // 减少点数
|
||||
for(uint i=0; i<plsize; i++) // 遍历点云中的点
|
||||
{
|
||||
types[i].range = sqrt(pl[i].x * pl[i].x + pl[i].y * pl[i].y);
|
||||
vx = pl[i].x - pl[i + 1].x;
|
||||
vy = pl[i].y - pl[i + 1].y;
|
||||
vz = pl[i].z - pl[i + 1].z;
|
||||
types[i].dista = sqrt(vx * vx + vy * vy + vz * vz);
|
||||
types[i].range = sqrt(pl[i].x * pl[i].x + pl[i].y * pl[i].y); // 计算点到原点的距离
|
||||
vx = pl[i].x - pl[i + 1].x; // 计算相邻点的 x 坐标差
|
||||
vy = pl[i].y - pl[i + 1].y; // 计算相邻点的 y 坐标差
|
||||
vz = pl[i].z - pl[i + 1].z; // 计算相邻点的 z 坐标差
|
||||
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);
|
||||
give_feature(pl, types);
|
||||
types[plsize].range = sqrt(pl[plsize].x * pl[plsize].x + pl[plsize].y * pl[plsize].y); // 计算最后一个点到原点的距离
|
||||
give_feature(pl, types); // 提取特征
|
||||
// pl_surf += pl;
|
||||
}
|
||||
time += omp_get_wtime() - t0;
|
||||
printf("Feature extraction time: %lf \n", time / count);
|
||||
time += omp_get_wtime() - t0; // 计算特征提取时间
|
||||
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 ++;
|
||||
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 点云
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
|
|||
Loading…
Reference in New Issue