diff --git a/CMakeLists.txt b/CMakeLists.txt index 42634e1..5da0b63 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,7 +1,10 @@ cmake_minimum_required(VERSION 3.0.2) project(sr_lio) -set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11 -O3") +set(CMAKE_BUILD_TYPE "Release") +set (CMAKE_CXX_STANDARD 14) +# set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11 -O3") + find_package(catkin REQUIRED COMPONENTS eigen_conversions diff --git a/config/nclt.yaml b/config/nclt.yaml index 1e5b060..55bb783 100644 --- a/config/nclt.yaml +++ b/config/nclt.yaml @@ -7,7 +7,7 @@ common: gravity_acc: [ 0.0, 0.0, 9.80416] lidar_parameter: - lidar_type: 2 # 1 for Livox serials LiDAR, 2 for Velodyne LiDAR, 3 for ouster LiDAR, + lidar_type: 2 # 1 for Livox serials LiDAR, 2 for Velodyne LiDAR, 3 for ouster LiDAR, 4 for robosense N_SCANS: 32 SCAN_RATE: 10 # only need to be set for velodyne, unit: Hz, time_unit: 2 # the unit of time/t field in the PointCloud2 rostopic: 0-second, 1-milisecond, 2-microsecond, 3-nanosecond. diff --git a/config/ouster.yaml b/config/ouster.yaml new file mode 100644 index 0000000..1b6a2b9 --- /dev/null +++ b/config/ouster.yaml @@ -0,0 +1,72 @@ +common: + lidar_topic: "/os_cloud_node/points" + imu_topic: "/os_cloud_node/imu" + point_filter_num: 6 + sweep_cut_num: 3 + time_sync_en: false # ONLY turn on when external time synchronization is really not possible + gravity_acc: [ 0.0, 0.0, 9.80416] + +lidar_parameter: + lidar_type: 3 # 1 for Livox serials LiDAR, 2 for Velodyne LiDAR, 3 for ouster LiDAR, 4 for robosense + N_SCANS: 32 + Horizon_SCAN: 1800 + SCAN_RATE: 10 # only need to be set for velodyne, unit: Hz, + time_unit: 3 # the unit of time/t field in the PointCloud2 rostopic: 0-second, 1-milisecond, 2-microsecond, 3-nanosecond. + blind: 2 + fov_degree: 180 + det_range: 100.0 + +imu_parameter: + acc_cov: 0.1 + gyr_cov: 0.1 + b_acc_cov: 0.0001 + b_gyr_cov: 0.0001 + time_diff_enable: false + +extrinsic_parameter: + extrinsic_enable: false # true: enable the online estimation of IMU-LiDAR extrinsic, + extrinsic_t: [ 0.0, 0.0, -0.0] + extrinsic_R: [ 1, 0, 0, + 0, 1, 0, + 0, 0, 1] + + +odometry_options: + voxel_size: 0.5 # The voxel size for the grid sampling of the new frame (before keypoints extraction) + sample_voxel_size: 1.5 # The size of a voxel for the selection of `keypoints` by grid sampling + max_distance: 2000.0 # The threshold of the distance to suppress voxels from the map + max_num_points_in_voxel: 20 # The maximum number of points per voxel of the map + method_system_init: STATIC_INIT # LIO system initialization [MOTION_INIT, STATIC_INIT] + init_num_frames: 20 + robust_registration: false + min_distance_points: 0.15 + distance_error_threshold: 100.0 # The motion of the sensor between two frames which is considered erroneous (stops the odometry) + motion_compensation: CONTINUOUS # The profile of the motion compensation (NONE, CONSTANT_VELOCITY, ITERATIVE, CONTINUOUS) + initialization: INIT_IMU # [INIT_CONSTANT_VELOCITY, INIT_IMU] + +icp_options: + size_voxel_map: 1.0 # The voxel size of in the voxel map + num_iters_icp: 15 # The number of iterations of the ICP + min_number_neighbors: 20 # The minimum number of neighbor points to define a valid neighborhood + voxel_neighborhood: 1 + max_number_neighbors: 20 + max_dist_to_plane_ct_icp: 0.3 + threshold_orientation_norm: 0.1 # Threshold on orientation changes (in degrees) for early termination of the ICP + threshold_translation_norm: 0.01 # Threshold on distance changes (in m) for early termination of the ICP + debug_print: false + point_to_plane_with_distortion: true + distance: CT_POINT_TO_PLANE # Options: [CT_POINT_TO_PLANE, POINT_TO_PLANE] + num_closest_neighbors: 1 + beta_location_consistency: 1.0 + beta_orientation_consistency: 1.0 + beta_constant_velocity: 1.0 # Trajectory Regularisation Weight to constrain the optimization + beta_small_velocity: 0.00 + solver: LIO # The type of SOLVER used in [LIO, LIDAR] + min_num_residuals: 200 # The minimum number of residuals for a valid ICP problem + max_num_residuals: 600 # The maximum number of residuals considered (if more keypoints exist, residuals are randomly sampled) + + loss_function: HUBER # Options: [CAUCHY, STANDARD, HUBER, TOLERANT, TRUNCATED] + ls_max_num_iters: 5 # The number of steps performed by ceres for each iteration of the ICP + ls_num_threads: 16 # The number of threads to build and solve the least square system + ls_sigma: 0.5 + ls_tolerant_min_threshold: 0.05 diff --git a/include/cloudMap.h b/include/cloudMap.h index 175bc23..5fd1810 100644 --- a/include/cloudMap.h +++ b/include/cloudMap.h @@ -18,8 +18,8 @@ struct point3D { Eigen::Vector3d raw_point; Eigen::Vector3d point; - double alpha_time = 0.0; - double relative_time = 0.0; + double alpha_time = 0.0; // reference to last point of current frame + double relative_time = 0.0; // feference to current frame double timestamp = 0.0; int index_frame = -1; diff --git a/include/cloudProcessing.h b/include/cloudProcessing.h index 7c37dce..086a8eb 100644 --- a/include/cloudProcessing.h +++ b/include/cloudProcessing.h @@ -19,7 +19,7 @@ #include "cloudMap.h" -enum LID_TYPE{AVIA = 1, VELO16 = 2, OUST64 = 3}; +enum LID_TYPE{AVIA = 1, VELO = 2, OUST = 3, ROBO = 4}; enum TIME_UNIT{SEC = 0, MS = 1, US = 2, NS = 3}; enum Feature{Nor, Poss_Plane, Real_Plane, Edge_Jump, Edge_Plane, Wire, ZeroPoint}; enum Surround{Prev, Next}; @@ -63,6 +63,38 @@ POINT_CLOUD_REGISTER_POINT_STRUCT(velodyne_ros::Point, (uint16_t, ring, ring) ) +namespace ouster_ros +{ + struct Point + { + PCL_ADD_POINT4D; + float intensity; + uint32_t t; + uint16_t reflectivity; + uint8_t ring; + // uint16_t noise; + uint16_t ambient; + uint32_t range; + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + } EIGEN_ALIGN16; +} +POINT_CLOUD_REGISTER_POINT_STRUCT(ouster_ros::Point, + (float, x, x)(float, y, y)(float, z, z)(float, intensity, intensity)(uint32_t, t, t)(uint16_t, reflectivity, reflectivity)(uint8_t, ring, ring)(uint16_t, ambient, ambient)(uint32_t, range, range)) + +namespace robosense_ros +{ + struct Point + { + PCL_ADD_POINT4D; + uint8_t intensity; + uint16_t ring = 0; + double timestamp = 0; + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + } EIGEN_ALIGN16; +} +POINT_CLOUD_REGISTER_POINT_STRUCT(robosense_ros::Point, + (float, x, x)(float, y, y)(float, z, z)(uint8_t, intensity, intensity)(uint16_t, ring, ring)(double, timestamp, timestamp)) + class cloudProcessing { private: @@ -92,9 +124,9 @@ class cloudProcessing // function void resetVector(); - void oust64Handler(const sensor_msgs::PointCloud2::ConstPtr &msg, std::vector> &v_cloud_out); + void ousterHandler(const sensor_msgs::PointCloud2::ConstPtr &msg, std::vector> &v_cloud_out, std::vector &v_dt_offset); void velodyneHandler(const sensor_msgs::PointCloud2::ConstPtr &msg, std::vector> &v_cloud_out, std::vector &v_dt_offset); - + void robosenseHandler(const sensor_msgs::PointCloud2::ConstPtr &msg, std::vector> &v_cloud_out, std::vector &v_dt_offset); public: diff --git a/include/lioOptimization.h b/include/lioOptimization.h index 37ded7e..ad0ef86 100644 --- a/include/lioOptimization.h +++ b/include/lioOptimization.h @@ -190,7 +190,7 @@ class lioOptimization{ std::queue> time_buffer; std::queue::Ptr>> feature_buffer; - std::queue>> lidar_buffer; + std::queue>> lidar_buffer; // 存储重构后的单帧数据 std::queue imu_buffer; std::vector all_cloud_frame; diff --git a/include/utility.h b/include/utility.h index 67eb6bb..0089a3a 100644 --- a/include/utility.h +++ b/include/utility.h @@ -38,6 +38,10 @@ bool time_list(point3D &point_1, point3D &point_2); bool time_list_velodyne(velodyne_ros::Point &point_1, velodyne_ros::Point &point_2); +bool time_list_ouster(ouster_ros::Point &point_1, ouster_ros::Point &point_2); + +bool time_list_robosense(robosense_ros::Point &point_1, robosense_ros::Point &point_2); + void point3DtoPCL(std::vector &v_point_temp, pcl::PointCloud::Ptr &p_cloud_temp); Eigen::Matrix3d mat33FromArray(std::vector &array); diff --git a/launch/lio_ouster.launch b/launch/lio_ouster.launch new file mode 100644 index 0000000..af38efc --- /dev/null +++ b/launch/lio_ouster.launch @@ -0,0 +1,16 @@ + + + + + + + + + + + + + + + + diff --git a/src/cloudProcessing.cpp b/src/cloudProcessing.cpp index da88745..ebe9763 100644 --- a/src/cloudProcessing.cpp +++ b/src/cloudProcessing.cpp @@ -97,16 +97,21 @@ void cloudProcessing::process(const sensor_msgs::PointCloud2::ConstPtr &msg, std { switch (lidar_type) { - case OUST64: - ROS_ERROR("Only Velodyne LiDAR interface is supported currently."); + case OUST: + ousterHandler(msg, v_cloud_out, v_dt_offset); + // ROS_ERROR("Only Velodyne LiDAR interface is supported currently."); break; - case VELO16: + case VELO: velodyneHandler(msg, v_cloud_out, v_dt_offset); break; + case ROBO: + robosenseHandler(msg, v_cloud_out, v_dt_offset); + break; + default: - ROS_ERROR("Only Velodyne LiDAR interface is supported currently."); + ROS_ERROR("Error Lidar Type, Please check."); break; } @@ -145,9 +150,282 @@ void cloudProcessing::process(const sensor_msgs::PointCloud2::ConstPtr &msg, std sweep_id++; } -void cloudProcessing::oust64Handler(const sensor_msgs::PointCloud2::ConstPtr &msg, std::vector> &v_cloud_out) +void cloudProcessing::ousterHandler(const sensor_msgs::PointCloud2::ConstPtr &msg, std::vector> &v_cloud_out, std::vector &v_dt_offset) +{ + resetVector(); + + pcl::PointCloud raw_cloud; + pcl::fromROSMsg(*msg, raw_cloud); + int size = raw_cloud.points.size(); + + double dt_last_point; + + if (size == 0) + { + v_cloud_out = v_cut_sweep; + v_dt_offset.resize(sweep_cut_num); + + for (int i = 0; i < sweep_cut_num; i++) + { + v_dt_offset[i] = (i + 1) * delta_cut_time; + } + + return; + } + + if (raw_cloud.points[size - 1].t > 0) + given_offset_time = true; + else + given_offset_time = false; + + if (given_offset_time) + { + sort(raw_cloud.points.begin(), raw_cloud.points.end(), time_list_ouster); + dt_last_point = raw_cloud.points.back().t * time_unit_scale; + delta_cut_time = dt_last_point / sweep_cut_num; + } + + double omega = 0.361 * SCAN_RATE; + + std::vector is_first; + is_first.resize(N_SCANS); + fill(is_first.begin(), is_first.end(), true); + + std::vector yaw_first_point; + yaw_first_point.resize(N_SCANS); + fill(yaw_first_point.begin(), yaw_first_point.end(), 0.0); + + std::vector v_point_full; + + for (int i = 0; i < size; i++) + { + point3D point_temp; + + point_temp.raw_point = Eigen::Vector3d(raw_cloud.points[i].x, raw_cloud.points[i].y, raw_cloud.points[i].z); + point_temp.point = point_temp.raw_point; + point_temp.relative_time = raw_cloud.points[i].t * time_unit_scale; + + if (!given_offset_time) + { + int layer = raw_cloud.points[i].ring; + double yaw_angle = atan2(point_temp.raw_point.y(), point_temp.raw_point.x()) * 57.2957; + + if (is_first[layer]) + { + yaw_first_point[layer] = yaw_angle; + is_first[layer] = false; + point_temp.relative_time = 0.0; + + v_point_full.push_back(point_temp); + + continue; + } + + if (yaw_angle <= yaw_first_point[layer]) + { + point_temp.relative_time = (yaw_first_point[layer] - yaw_angle) / omega; + } + else + { + point_temp.relative_time = (yaw_first_point[layer] - yaw_angle + 360.0) / omega; + } + + point_temp.timestamp = point_temp.relative_time / double(1000) + msg->header.stamp.toSec(); + v_point_full.push_back(point_temp); + } + + if (given_offset_time && i % point_filter_num == 0) + { + if (point_temp.raw_point.x() * point_temp.raw_point.x() + point_temp.raw_point.y() * point_temp.raw_point.y() + point_temp.raw_point.z() * point_temp.raw_point.z() > (blind * blind)) + { + point_temp.timestamp = point_temp.relative_time / double(1000) + msg->header.stamp.toSec(); + point_temp.alpha_time = point_temp.relative_time / dt_last_point; + + int id = int(point_temp.relative_time / delta_cut_time) + sweep_cut_num; + + id = id >= 2 * sweep_cut_num ? 2 * sweep_cut_num - 1 : id; + + v_cut_sweep[id].push_back(point_temp); + } + } + } + + if (!given_offset_time) + { + assert(v_point_full.size() == size); + + sort(v_point_full.begin(), v_point_full.end(), time_list); + dt_last_point = v_point_full.back().relative_time; + delta_cut_time = dt_last_point / sweep_cut_num; + + for (int i = 0; i < size; i++) + { + if (i % point_filter_num == 0) + { + point3D point_temp = v_point_full[i]; + point_temp.alpha_time = (point_temp.relative_time / dt_last_point); + + if (point_temp.alpha_time > 1) + point_temp.alpha_time = 1; + if (point_temp.alpha_time < 0) + point_temp.alpha_time = 0; + + int id = int(point_temp.relative_time / delta_cut_time) + sweep_cut_num; + + id = id >= 2 * sweep_cut_num ? 2 * sweep_cut_num - 1 : id; + + v_cut_sweep[id].push_back(point_temp); + } + } + } + + for (int i = 1; i <= sweep_cut_num - 1; i++) + v_dt_offset.push_back(delta_cut_time * i); + + v_dt_offset.push_back(dt_last_point); + + assert(v_dt_offset.size() == sweep_cut_num); + + v_cloud_out = v_cut_sweep; +} + +void cloudProcessing::robosenseHandler(const sensor_msgs::PointCloud2::ConstPtr &msg, std::vector> &v_cloud_out, std::vector &v_dt_offset) { + resetVector(); + + pcl::PointCloud raw_cloud; + pcl::fromROSMsg(*msg, raw_cloud); + int size = raw_cloud.points.size(); + + double dt_last_point; + + if (size == 0) + { + v_cloud_out = v_cut_sweep; + v_dt_offset.resize(sweep_cut_num); + + for (int i = 0; i < sweep_cut_num; i++) + { + v_dt_offset[i] = (i + 1) * delta_cut_time; + } + + return; + } + + if (raw_cloud.points[size - 1].timestamp > 0) + given_offset_time = true; + else + given_offset_time = false; + + if (given_offset_time) + { + sort(raw_cloud.points.begin(), raw_cloud.points.end(), time_list_robosense); + dt_last_point = raw_cloud.points.back().timestamp * time_unit_scale; + delta_cut_time = dt_last_point / sweep_cut_num; + } + + double omega = 0.361 * SCAN_RATE; + + std::vector is_first; + is_first.resize(N_SCANS); + fill(is_first.begin(), is_first.end(), true); + + std::vector yaw_first_point; + yaw_first_point.resize(N_SCANS); + fill(yaw_first_point.begin(), yaw_first_point.end(), 0.0); + + std::vector v_point_full; + + for (int i = 0; i < size; i++) + { + point3D point_temp; + + point_temp.raw_point = Eigen::Vector3d(raw_cloud.points[i].x, raw_cloud.points[i].y, raw_cloud.points[i].z); + point_temp.point = point_temp.raw_point; + point_temp.relative_time = raw_cloud.points[i].timestamp * time_unit_scale; + + if (!given_offset_time) + { + int layer = raw_cloud.points[i].ring; + double yaw_angle = atan2(point_temp.raw_point.y(), point_temp.raw_point.x()) * 57.2957; + + if (is_first[layer]) + { + yaw_first_point[layer] = yaw_angle; + is_first[layer] = false; + point_temp.relative_time = 0.0; + + v_point_full.push_back(point_temp); + + continue; + } + + if (yaw_angle <= yaw_first_point[layer]) + { + point_temp.relative_time = (yaw_first_point[layer] - yaw_angle) / omega; + } + else + { + point_temp.relative_time = (yaw_first_point[layer] - yaw_angle + 360.0) / omega; + } + + point_temp.timestamp = point_temp.relative_time / double(1000) + msg->header.stamp.toSec(); + v_point_full.push_back(point_temp); + } + + if (given_offset_time && i % point_filter_num == 0) + { + if (point_temp.raw_point.x() * point_temp.raw_point.x() + point_temp.raw_point.y() * point_temp.raw_point.y() + point_temp.raw_point.z() * point_temp.raw_point.z() > (blind * blind)) + { + point_temp.timestamp = point_temp.relative_time / double(1000) + msg->header.stamp.toSec(); + point_temp.alpha_time = point_temp.relative_time / dt_last_point; + + int id = int(point_temp.relative_time / delta_cut_time) + sweep_cut_num; + + id = id >= 2 * sweep_cut_num ? 2 * sweep_cut_num - 1 : id; + + v_cut_sweep[id].push_back(point_temp); + } + } + } + + if (!given_offset_time) + { + assert(v_point_full.size() == size); + + sort(v_point_full.begin(), v_point_full.end(), time_list); + dt_last_point = v_point_full.back().relative_time; + delta_cut_time = dt_last_point / sweep_cut_num; + + for (int i = 0; i < size; i++) + { + if (i % point_filter_num == 0) + { + point3D point_temp = v_point_full[i]; + point_temp.alpha_time = (point_temp.relative_time / dt_last_point); + + if (point_temp.alpha_time > 1) + point_temp.alpha_time = 1; + if (point_temp.alpha_time < 0) + point_temp.alpha_time = 0; + + int id = int(point_temp.relative_time / delta_cut_time) + sweep_cut_num; + + id = id >= 2 * sweep_cut_num ? 2 * sweep_cut_num - 1 : id; + + v_cut_sweep[id].push_back(point_temp); + } + } + } + + for (int i = 1; i <= sweep_cut_num - 1; i++) + v_dt_offset.push_back(delta_cut_time * i); + + v_dt_offset.push_back(dt_last_point); + + assert(v_dt_offset.size() == sweep_cut_num); + v_cloud_out = v_cut_sweep; } void cloudProcessing::velodyneHandler(const sensor_msgs::PointCloud2::ConstPtr &msg, std::vector> &v_cloud_out, std::vector &v_dt_offset) diff --git a/src/lioOptimization.cpp b/src/lioOptimization.cpp index bb0e519..f2546b9 100644 --- a/src/lioOptimization.cpp +++ b/src/lioOptimization.cpp @@ -371,23 +371,23 @@ void lioOptimization::standardCloudHandler(const sensor_msgs::PointCloud2::Const double sample_size = index_frame < options.init_num_frames ? options.init_voxel_size : options.voxel_size; assert(msg->header.stamp.toSec() > last_time_lidar); - - std::vector> v_cut_sweep; - std::vector v_dt_offset; - cloud_pro->process(msg, v_cut_sweep, v_dt_offset); + std::vector> v_cut_sweep; // 最多缓存两帧数据，分割为多个小块 + std::vector v_dt_offset; // 分块的时间戳 + + cloud_pro->process(msg, v_cut_sweep, v_dt_offset); // v_dt_offset数据等分的时间戳 for(int i = 1; i < sweep_cut_num + 1; i++) { - std::vector> v_buffer_temp; + std::vector> v_buffer_temp; // 降采样，重排数据，然后组成新一帧数据 for(int j = i; j < i + sweep_cut_num; j++) { std::vector frame(v_cut_sweep[j]); boost::mt19937_64 g; - std::shuffle(frame.begin(), frame.end(), g); + std::shuffle(frame.begin(), frame.end(), g); // 随机重排容器中的元素 subSampleFrame(frame, sample_size); @@ -399,7 +399,7 @@ void lioOptimization::standardCloudHandler(const sensor_msgs::PointCloud2::Const assert(v_buffer_temp.size() == sweep_cut_num); lidar_buffer.push(v_buffer_temp); - time_buffer.push(std::make_pair(msg->header.stamp.toSec(), v_dt_offset[i - 1] / (double)1000.0)); + time_buffer.push(std::make_pair(msg->header.stamp.toSec(), v_dt_offset[i - 1] / (double)1000.0)); // 用以存储新构建数据的时间戳 } assert(msg->header.stamp.toSec() > last_time_lidar); @@ -437,7 +437,7 @@ std::vector, std::vect return measurements; } - if (!(imu_buffer.front()->header.stamp.toSec() < time_buffer.front().first + time_buffer.front().second)) + if (!(imu_buffer.front()->header.stamp.toSec() < time_buffer.front().first + time_buffer.front().second)) // IMU大于重组建激光的时间戳 { time_buffer.pop(); @@ -450,7 +450,7 @@ std::vector, std::vect continue; } - double timestamp = time_buffer.front().first + time_buffer.front().second; + double timestamp = time_buffer.front().first + time_buffer.front().second; // 重组建激光的时间戳 double timestamp_begin = time_buffer.front().first; double timestamp_offset = time_buffer.front().second; time_buffer.pop(); @@ -471,7 +471,8 @@ std::vector, std::vect std::vector> v_cut_sweep = lidar_buffer.front(); - for(int i = 0; i < lidar_buffer.front().size(); i++) std::vector().swap(lidar_buffer.front()[i]); + for (int i = 0; i < lidar_buffer.front().size(); i++) // 移除lidar_buffer首个数据 + std::vector().swap(lidar_buffer.front()[i]); std::vector>().swap(lidar_buffer.front()); assert(lidar_buffer.front().size() == 0); lidar_buffer.pop(); @@ -980,7 +981,7 @@ void lioOptimization::stateEstimation(std::vector> &v_cut_s std::vector const_frame; - for(int i = 0; i < v_cut_sweep.size(); i++) + for (int i = 0; i < v_cut_sweep.size(); i++) // 组成一帧数据 const_frame.insert(const_frame.end(), v_cut_sweep[i].begin(), v_cut_sweep[i].end()); cloudFrame *p_frame = buildFrame(const_frame, imu_pro->current_state, timestamp_begin, timestamp_offset); @@ -1006,7 +1007,7 @@ void lioOptimization::stateEstimation(std::vector> &v_cut_s std::cout << "translation_end: " << p_frame->p_state->translation.x() << " " << p_frame->p_state->translation.y() << " " << p_frame->p_state->translation.z() << std::endl; imu_pro->last_state = imu_pro->current_state; - imu_pro->current_state = new state(imu_pro->last_state, false); + imu_pro->current_state = new state(imu_pro->last_state, false); // 重置状态量 publish_odometry(pub_odom,p_frame); publish_path(pub_path,p_frame); diff --git a/src/utility.cpp b/src/utility.cpp index 374343f..2f145ff 100644 --- a/src/utility.cpp +++ b/src/utility.cpp @@ -22,6 +22,15 @@ bool time_list_velodyne(velodyne_ros::Point &point_1, velodyne_ros::Point &point return (point_1.time < point_2.time); } +bool time_list_ouster(ouster_ros::Point &point_1, ouster_ros::Point &point_2) +{ + return (point_1.t < point_2.t); +} + +bool time_list_robosense(robosense_ros::Point &point_1, robosense_ros::Point &point_2) +{ + return (point_1.timestamp < point_2.timestamp); +} void point3DtoPCL(std::vector &v_point_temp, pcl::PointCloud::Ptr &p_cloud_temp) { for(int i = 0; i < v_point_temp.size(); i++)