diff --git a/include/preprocess.h b/include/preprocess.h index 649ed838..9a4c9c36 100755 --- a/include/preprocess.h +++ b/include/preprocess.h @@ -70,13 +70,13 @@ struct EIGEN_ALIGN16 Point { PCL_ADD_POINT4D; float intensity; - std::uint32_t t; + float time; std::uint16_t ring; EIGEN_MAKE_ALIGNED_OPERATOR_NEW }; } // namespace velodyne_ros POINT_CLOUD_REGISTER_POINT_STRUCT(velodyne_ros::Point, - (float, x, x)(float, y, y)(float, z, z)(float, intensity, intensity)(std::uint32_t, t, t)(std::uint16_t, ring, ring)) + (float, x, x)(float, y, y)(float, z, z)(float, intensity, intensity)(float, time, time)(std::uint16_t, ring, ring)) /****************/ /*** Ouster ***/ @@ -146,7 +146,7 @@ class Preprocess PointCloudXYZI pl_full, pl_corn, pl_surf; PointCloudXYZI pl_buff[128]; // maximum 128 line lidar vector typess[128]; // maximum 128 line lidar - int lidar_type, point_filter_num, N_SCANS; + int lidar_type, point_filter_num, N_SCANS, SCAN_RATE; double blind, blind_sqr; bool feature_enabled, given_offset_time; diff --git a/src/LIVMapper.cpp b/src/LIVMapper.cpp index 427fe3eb..5eba7189 100755 --- a/src/LIVMapper.cpp +++ b/src/LIVMapper.cpp @@ -87,6 +87,7 @@ void LIVMapper::readParameters(ros::NodeHandle &nh) nh.param("preprocess/filter_size_surf", filter_size_surf_min, 0.5); nh.param("preprocess/lidar_type", p_pre->lidar_type, AVIA); nh.param("preprocess/scan_line", p_pre->N_SCANS, 6); + nh.param("preprocess/scan_rate", p_pre->SCAN_RATE, 10); nh.param("preprocess/point_filter_num", p_pre->point_filter_num, 3); nh.param("preprocess/feature_extract_enabled", p_pre->feature_enabled, false); diff --git a/src/preprocess.cpp b/src/preprocess.cpp index 322776b0..9a68d558 100755 --- a/src/preprocess.cpp +++ b/src/preprocess.cpp @@ -19,6 +19,7 @@ Preprocess::Preprocess() : feature_enabled(0), lidar_type(AVIA), blind(0.01), po { inf_bound = 10; N_SCANS = 6; + SCAN_RATE = 10; group_size = 8; disA = 0.01; disA = 0.1; // B? @@ -344,15 +345,16 @@ void Preprocess::velodyne_handler(const sensor_msgs::PointCloud2::ConstPtr &msg) pcl::PointCloud pl_orig; pcl::fromROSMsg(*msg, pl_orig); int plsize = pl_orig.points.size(); + if (plsize == 0) return; pl_surf.reserve(plsize); bool is_first[MAX_LINE_NUM]; double yaw_fp[MAX_LINE_NUM] = {0}; // yaw of first scan point - double omega_l = 3.61; // scan angular velocity + double omega_l = 0.361 * SCAN_RATE; // scan angular velocity float yaw_last[MAX_LINE_NUM] = {0.0}; // yaw of last scan point float time_last[MAX_LINE_NUM] = {0.0}; // last offset time - if (pl_orig.points[plsize - 1].t > 0) { given_offset_time = true; } + if (pl_orig.points[plsize - 1].time > 0) { given_offset_time = true; } else { given_offset_time = false; @@ -390,7 +392,7 @@ void Preprocess::velodyne_handler(const sensor_msgs::PointCloud2::ConstPtr &msg) added_pt.y = pl_orig.points[i].y; added_pt.z = pl_orig.points[i].z; added_pt.intensity = pl_orig.points[i].intensity; - added_pt.curvature = pl_orig.points[i].t / 1000.0; // units: ms + added_pt.curvature = pl_orig.points[i].time / 1000.0; // units: ms if (!given_offset_time) { @@ -453,7 +455,7 @@ void Preprocess::velodyne_handler(const sensor_msgs::PointCloud2::ConstPtr &msg) added_pt.y = pl_orig.points[i].y; added_pt.z = pl_orig.points[i].z; added_pt.intensity = pl_orig.points[i].intensity; - added_pt.curvature = pl_orig.points[i].t / 1000.0; + added_pt.curvature = pl_orig.points[i].time / 1000.0; if (!given_offset_time) { @@ -477,8 +479,6 @@ void Preprocess::velodyne_handler(const sensor_msgs::PointCloud2::ConstPtr &msg) if (added_pt.curvature < time_last[layer]) added_pt.curvature += 360.0 / omega_l; - // added_pt.curvature = pl_orig.points[i].t; - yaw_last[layer] = yaw_angle; time_last[layer] = added_pt.curvature; } @@ -491,8 +491,6 @@ void Preprocess::velodyne_handler(const sensor_msgs::PointCloud2::ConstPtr &msg) if (added_pt.x * added_pt.x + added_pt.y * added_pt.y + added_pt.z * added_pt.z > blind_sqr) { pl_surf.points.push_back(added_pt); - // printf("time mode: %d time: %d \n", given_offset_time, - // pl_orig.points[i].t); } } }