Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

ROS-175: Quick prototype on how to reduce the vertical resolution for the point cloud #191

Draft
wants to merge 1 commit into
base: master
Choose a base branch
from
Draft

ROS-175: Quick prototype on how to reduce the vertical resolution for the point cloud #191

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
2 changes: 1 addition & 1 deletion include/ouster_ros/os_ros.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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<int>& pixel_shift_by_row,
int return_index);
int return_index, int skip_rings = 1);

/**
* Serialize a PCL point cloud to a ROS message
Expand Down
10 changes: 5 additions & 5 deletions src/os_ros.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -203,7 +203,7 @@ void copy_scan_to_cloud_destaggered(
const PointT& points, const ouster::img_t<RangeT>& range,
const ouster::img_t<ReflectivityT>& reflectivity,
const ouster::img_t<NearIrT>& near_ir, const ouster::img_t<SignalT>& signal,
const std::vector<int>& pixel_shift_by_row) {
const std::vector<int>& pixel_shift_by_row, int skip_rings) {
auto timestamp = ls.timestamp();

const auto rg = range.data();
Expand All @@ -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<float>(xyz(0)), static_cast<float>(xyz(1)),
Expand Down Expand Up @@ -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<int>& pixel_shift_by_row,
int return_index) {
int return_index, int skip_rings) {
bool second = (return_index == 1);

assert(cloud.width == static_cast<std::uint32_t>(ls.w) &&
Expand All @@ -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,
Expand Down
7 changes: 5 additions & 2 deletions src/point_cloud_processor.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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<std::shared_ptr<sensor_msgs::PointCloud2>>;
using PostProcessingFn = std::function<void(OutputType)>;
Expand All @@ -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)
Expand Down Expand Up @@ -64,7 +67,7 @@ class PointCloudProcessor {
for (int i = 0; i < static_cast<int>(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;
Expand Down