diff --git a/config/avia.yaml b/config/avia.yaml index 1b25e00..374bfba 100755 --- a/config/avia.yaml +++ b/config/avia.yaml @@ -34,7 +34,7 @@ mapping: match_s: 81 ivox_grid_resolution: 2.0 gravity: [0.0, 0.0, -9.810] # [0.0, 9.810, 0.0] # # [0.0, 0.0, -9.787561] # gvins # - gravity_init: [0.0, 0.0, -9.810] # preknown gravity for unstationary start in the initial IMU frame + gravity_init: [0.0, 0.0, -9.810] # preknown gravity in the initial IMU frame for unstationary start or in the initial LiDAR frame for using without IMU extrinsic_T: [ 0.04165, 0.02326, -0.0284 ] # avia # [0.011, 0.02329, -0.04412] # mid360 extrinsic_R: [ 1, 0, 0, 0, 1, 0, diff --git a/config/horizon.yaml b/config/horizon.yaml index 75041bd..81ebac2 100755 --- a/config/horizon.yaml +++ b/config/horizon.yaml @@ -34,6 +34,7 @@ mapping: match_s: 81 ivox_grid_resolution: 2.0 gravity: [0.0, 0.0, -9.810] # preknown gravity, use when imu_en is false or start from a non-stationary state + gravity_init: [0.0, 0.0, -9.810] # preknown gravity in the initial IMU frame for unstationary start or in the initial LiDAR frame for using without IMU extrinsic_T: [ 0.05512, 0.02226, -0.0297 ] extrinsic_R: [ 1, 0, 0, 0, 1, 0, diff --git a/config/ouster64.yaml b/config/ouster64.yaml index 08192d2..06edadf 100755 --- a/config/ouster64.yaml +++ b/config/ouster64.yaml @@ -34,6 +34,7 @@ mapping: match_s: 81 ivox_grid_resolution: 2.0 gravity: [0.0, 0.0, -9.810] # preknown gravity, use when imu_en is false or start from a non-stationary state + gravity_init: [0.0, 0.0, -9.810] # preknown gravity in the initial IMU frame for unstationary start or in the initial LiDAR frame for using without IMU extrinsic_T: [0.0, 0.0, 0.0] extrinsic_R: [ 1, 0, 0, 0, 1, 0, diff --git a/config/velody16.yaml b/config/velody16.yaml index a270e2e..825c8db 100755 --- a/config/velody16.yaml +++ b/config/velody16.yaml @@ -34,6 +34,7 @@ mapping: match_s: 81 ivox_grid_resolution: 2.0 gravity: [0.0, 0.0, -9.810] # [-0.30, 0.880, -9.76] # liosam [0.0, 9.810, 0.0] # # preknown gravity, use when imu_en is false or start from a non-stationary state + gravity_init: [0.0, 0.0, -9.810] # preknown gravity in the initial IMU frame for unstationary start or in the initial LiDAR frame for using without IMU extrinsic_T: [ 0, 0, 0.28] # ulhk # [-0.5, 1.4, 1.5] # utbm # extrinsic_R: [ 0, 1, 0, # -1, 0, 0, diff --git a/src/li_initialization.cpp b/src/li_initialization.cpp index 1152bec..74a417e 100755 --- a/src/li_initialization.cpp +++ b/src/li_initialization.cpp @@ -89,8 +89,11 @@ void standard_pcl_cbk(const sensor_msgs::PointCloud2::ConstPtr &msg) } else { - lidar_buffer.emplace_back(ptr); - time_buffer.emplace_back(msg->header.stamp.toSec()); + if (ptr->points.size() > 0) + { + lidar_buffer.emplace_back(ptr); + time_buffer.emplace_back(msg->header.stamp.toSec()); + } } } s_plot11[scan_count] = omp_get_wtime() - preprocess_start_time; @@ -165,8 +168,11 @@ void livox_pcl_cbk(const livox_ros_driver::CustomMsg::ConstPtr &msg) } else { - lidar_buffer.emplace_back(ptr); - time_buffer.emplace_back(msg->header.stamp.toSec()); + if (ptr->points.size() > 0) + { + lidar_buffer.emplace_back(ptr); + time_buffer.emplace_back(msg->header.stamp.toSec()); + } } } s_plot11[scan_count] = omp_get_wtime() - preprocess_start_time; diff --git a/src/parameters.cpp b/src/parameters.cpp index 067ff4c..02a5efe 100755 --- a/src/parameters.cpp +++ b/src/parameters.cpp @@ -38,7 +38,7 @@ double time_diff_lidar_to_imu = 0.0; double lidar_time_inte = 0.1, first_imu_time = 0.0; int cut_frame_num = 1, orig_odom_freq = 10; double online_refine_time = 20.0; //unit: s -bool cut_frame_init = true; +bool cut_frame_init = false; // true; MeasureGroup Measures; diff --git a/src/preprocess.cpp b/src/preprocess.cpp index f53ecb8..1850cdf 100755 --- a/src/preprocess.cpp +++ b/src/preprocess.cpp @@ -8,7 +8,7 @@ const bool time_list_cut_frame(PointType &x, PointType &y) { } Preprocess::Preprocess() - :lidar_type(AVIA), blind(0.01), point_filter_num(1) + :lidar_type(AVIA), blind(0.01), point_filter_num(1), det_range(1000) { inf_bound = 10; N_SCANS = 6; @@ -92,7 +92,6 @@ void Preprocess::process(const sensor_msgs::PointCloud2::ConstPtr &msg, PointClo *pcl_out = pl_surf; } - void Preprocess::process_cut_frame_livox(const livox_ros_driver::CustomMsg::ConstPtr &msg, deque &pcl_out, deque &time_lidar, const int required_frame_num, int scan_count) { @@ -117,7 +116,7 @@ void Preprocess::process_cut_frame_livox(const livox_ros_driver::CustomMsg::Cons pl_full[i].curvature = msg->points[i].offset_time / float(1000000); double dist = pl_full[i].x * pl_full[i].x + pl_full[i].y * pl_full[i].y + pl_full[i].z * pl_full[i].z; - if (dist < blind * blind) continue; + if (dist < blind * blind || dist > det_range * det_range) continue; 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) @@ -196,7 +195,7 @@ Preprocess::process_cut_frame_pcl2(const sensor_msgs::PointCloud2::ConstPtr &msg added_pt.curvature = pl_orig.points[i].time * 1000.0; //ms double dist = added_pt.x * added_pt.x + added_pt.y * added_pt.y + added_pt.z * added_pt.z; - if ( dist < blind * blind || isnan(added_pt.x) || isnan(added_pt.y) || isnan(added_pt.z)) + if ( dist < blind * blind || dist > det_range * det_range || isnan(added_pt.x) || isnan(added_pt.y) || isnan(added_pt.z)) continue; if (!given_offset_time) { @@ -245,7 +244,7 @@ Preprocess::process_cut_frame_pcl2(const sensor_msgs::PointCloud2::ConstPtr &msg added_pt.curvature = pl_orig.points[i].t / 1e6; //ns to ms double dist = added_pt.x * added_pt.x + added_pt.y * added_pt.y + added_pt.z * added_pt.z; - if ( dist < blind * blind || isnan(added_pt.x) || isnan(added_pt.y) || isnan(added_pt.z)) + if ( dist < blind * blind || dist > det_range * det_range || isnan(added_pt.x) || isnan(added_pt.y) || isnan(added_pt.z)) continue; if (i % point_filter_num == 0 && pl_orig.points[i].ring < N_SCANS) { @@ -269,7 +268,7 @@ Preprocess::process_cut_frame_pcl2(const sensor_msgs::PointCloud2::ConstPtr &msg added_pt.curvature = (pl_orig.points[i].timestamp - msg->header.stamp.toSec()) * 1000; //s to ms double dist = added_pt.x * added_pt.x + added_pt.y * added_pt.y + added_pt.z * added_pt.z; - if ( dist < blind * blind || isnan(added_pt.x) || isnan(added_pt.y) || isnan(added_pt.z)) + if ( dist < blind * blind || dist > det_range * det_range || isnan(added_pt.x) || isnan(added_pt.y) || isnan(added_pt.z)) continue; if (i % point_filter_num == 0 && pl_orig.points[i].ring < N_SCANS) { @@ -344,13 +343,13 @@ void Preprocess::avia_handler(const livox_ros_driver::CustomMsg::ConstPtr &msg) 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].intensity = msg->points[i].reflectivity; // z; // pl_full[i].curvature = msg->points[i].offset_time / float(1000000); // use curvature as time of each laser points, curvature unit: ms - + double dist = pl_full[i].x * pl_full[i].x + pl_full[i].y * pl_full[i].y + pl_full[i].z * pl_full[i].z; + if (dist < blind * blind || dist > det_range * det_range) continue; 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))) + || (abs(pl_full[i].z - pl_full[i-1].z) > 1e-7))) { pl_surf.push_back(pl_full[i]); } @@ -381,7 +380,7 @@ void Preprocess::oust64_handler(const sensor_msgs::PointCloud2::ConstPtr &msg) double range = pl_orig.points[i].x * pl_orig.points[i].x + pl_orig.points[i].y * pl_orig.points[i].y + pl_orig.points[i].z * pl_orig.points[i].z; - if (range < (blind * blind)) continue; + if (range < (blind * blind) || range > det_range * det_range || isnan(pl_orig.points[i].x) || isnan(pl_orig.points[i].y) || isnan(pl_orig.points[i].z)) continue; Eigen::Vector3d pt_vec; PointType added_pt; @@ -428,17 +427,17 @@ void Preprocess::velodyne_handler(const sensor_msgs::PointCloud2::ConstPtr &msg) else { given_offset_time = false; - double yaw_first = atan2(pl_orig.points[0].y, pl_orig.points[0].x) * 57.29578; - double yaw_end = yaw_first; - int layer_first = pl_orig.points[0].ring; - for (uint i = plsize - 1; i > 0; i--) - { - if (pl_orig.points[i].ring == layer_first) - { - yaw_end = atan2(pl_orig.points[i].y, pl_orig.points[i].x) * 57.29578; - break; - } - } + // double yaw_first = atan2(pl_orig.points[0].y, pl_orig.points[0].x) * 57.29578; + // double yaw_end = yaw_first; + // int layer_first = pl_orig.points[0].ring; + // for (uint i = plsize - 1; i > 0; i--) + // { + // if (pl_orig.points[i].ring == layer_first) + // { + // yaw_end = atan2(pl_orig.points[i].y, pl_orig.points[i].x) * 57.29578; + // break; + // } + // } } for (int i = 0; i < plsize; i++) @@ -454,9 +453,14 @@ void Preprocess::velodyne_handler(const sensor_msgs::PointCloud2::ConstPtr &msg) added_pt.z = pl_orig.points[i].z; added_pt.intensity = pl_orig.points[i].intensity; added_pt.curvature = pl_orig.points[i].time * time_unit_scale; // curvature unit: ms // cout< (blind * blind)) + if(dist > (blind * blind) + && dist < (det_range * det_range)) { pl_surf.points.push_back(added_pt); } @@ -584,9 +592,10 @@ void Preprocess::hesai_handler(const sensor_msgs::PointCloud2::ConstPtr &msg) time_last[layer]=added_pt.curvature; } - if (i % point_filter_num == 0) + if (i % point_filter_num == 0 && !std::isnan(added_pt.x) && !std::isnan(added_pt.y) && !std::isnan(added_pt.z)) { - if(added_pt.x*added_pt.x+added_pt.y*added_pt.y+added_pt.z*added_pt.z > (blind * blind)) + if(added_pt.x*added_pt.x+added_pt.y*added_pt.y+added_pt.z*added_pt.z > (blind * blind) + &&added_pt.x*added_pt.x+added_pt.y*added_pt.y+added_pt.z*added_pt.z < (det_range * det_range)) { pl_surf.points.push_back(added_pt); } diff --git a/src/preprocess.h b/src/preprocess.h index ba1630b..1c031fd 100755 --- a/src/preprocess.h +++ b/src/preprocess.h @@ -121,7 +121,7 @@ class Preprocess vector typess[128]; //maximum 128 line lidar float time_unit_scale; int lidar_type, point_filter_num, N_SCANS, SCAN_RATE, time_unit; - double blind; + double blind, det_range; bool given_offset_time; ros::Publisher pub_full, pub_surf, pub_corn;