From ceff71c634b0ad11197c6e9c26e33deb19472af8 Mon Sep 17 00:00:00 2001 From: Ussama Naal Date: Mon, 14 Aug 2023 11:07:41 -0700 Subject: [PATCH] Quick prototype on how to reduce the vertical resolution for the point cloud thus far --- include/ouster_ros/os_ros.h | 2 +- src/os_ros.cpp | 10 +++++----- src/point_cloud_processor.h | 7 +++++-- 3 files changed, 11 insertions(+), 8 deletions(-) diff --git a/include/ouster_ros/os_ros.h b/include/ouster_ros/os_ros.h index 8868a9c9..738f24e8 100644 --- a/include/ouster_ros/os_ros.h +++ b/include/ouster_ros/os_ros.h @@ -143,7 +143,7 @@ void scan_to_cloud_f_destaggered(ouster_ros::Cloud& cloud, const ouster::PointsF& lut_offset, uint64_t scan_ts, const ouster::LidarScan& ls, const std::vector& pixel_shift_by_row, - int return_index); + int return_index, int skip_rings = 1); /** * Serialize a PCL point cloud to a ROS message diff --git a/src/os_ros.cpp b/src/os_ros.cpp index eea74fa7..6054df9f 100644 --- a/src/os_ros.cpp +++ b/src/os_ros.cpp @@ -203,7 +203,7 @@ void copy_scan_to_cloud_destaggered( const PointT& points, const ouster::img_t& range, const ouster::img_t& reflectivity, const ouster::img_t& near_ir, const ouster::img_t& signal, - const std::vector& pixel_shift_by_row) { + const std::vector& pixel_shift_by_row, int skip_rings) { auto timestamp = ls.timestamp(); const auto rg = range.data(); @@ -214,13 +214,13 @@ void copy_scan_to_cloud_destaggered( #ifdef __OUSTER_UTILIZE_OPENMP__ #pragma omp parallel for collapse(2) #endif - for (auto u = 0; u < ls.h; u++) { + for (auto u = 0; u < ls.h; u+=skip_rings) { for (auto v = 0; v < ls.w; v++) { const auto col_ts = timestamp[v]; const auto ts = col_ts > scan_ts ? col_ts - scan_ts : 0UL; const auto src_idx = u * ls.w + (v + ls.w - pixel_shift_by_row[u]) % ls.w; - const auto tgt_idx = u * ls.w + v; + const auto tgt_idx = (u / skip_rings) * ls.w + v; const auto xyz = points.row(src_idx); cloud.points[tgt_idx] = ouster_ros::Point{ {static_cast(xyz(0)), static_cast(xyz(1)), @@ -305,7 +305,7 @@ void scan_to_cloud_f_destaggered(ouster_ros::Cloud& cloud, const ouster::PointsF& lut_offset, uint64_t scan_ts, const ouster::LidarScan& ls, const std::vector& pixel_shift_by_row, - int return_index) { + int return_index, int skip_rings) { bool second = (return_index == 1); assert(cloud.width == static_cast(ls.w) && @@ -330,7 +330,7 @@ void scan_to_cloud_f_destaggered(ouster_ros::Cloud& cloud, copy_scan_to_cloud_destaggered(cloud, ls, scan_ts, points, range, reflectivity, near_ir, signal, - pixel_shift_by_row); + pixel_shift_by_row, skip_rings); } sensor_msgs::PointCloud2 cloud_to_cloud_msg(const Cloud& cloud, diff --git a/src/point_cloud_processor.h b/src/point_cloud_processor.h index 7990c31a..6bb039c7 100644 --- a/src/point_cloud_processor.h +++ b/src/point_cloud_processor.h @@ -19,6 +19,9 @@ namespace ouster_ros { class PointCloudProcessor { + private: + const int skip_rings = 4; // acceptable values = {1 (default), 2, 4, 8, 16} + public: using OutputType = std::vector>; using PostProcessingFn = std::function; @@ -30,7 +33,7 @@ class PointCloudProcessor { PostProcessingFn func) : frame(frame_id), pixel_shift_by_row(info.format.pixel_shift_by_row), - cloud{info.format.columns_per_frame, info.format.pixels_per_column}, + cloud{info.format.columns_per_frame, info.format.pixels_per_column / skip_rings}, pc_msgs(get_n_returns(info)), post_processing_fn(func) { for (size_t i = 0; i < pc_msgs.size(); ++i) @@ -64,7 +67,7 @@ class PointCloudProcessor { for (int i = 0; i < static_cast(pc_msgs.size()); ++i) { scan_to_cloud_f_destaggered(cloud, points, lut_direction, lut_offset, - scan_ts, lidar_scan, pixel_shift_by_row, i); + scan_ts, lidar_scan, pixel_shift_by_row, i, skip_rings); pcl_toROSMsg(cloud, *pc_msgs[i]); pc_msgs[i]->header.stamp = msg_ts; pc_msgs[i]->header.frame_id = frame;