From 9b8100cd30c2514c17febfaec9da9b025a0f2f12 Mon Sep 17 00:00:00 2001 From: Arshad Date: Fri, 24 Jun 2022 12:48:10 +0100 Subject: [PATCH 1/3] added a min and max distance threshold for pointcloud --- depth_image_proc/src/nodelets/point_cloud_xyzrgb.cpp | 11 ++++++++--- .../src/nodelets/point_cloud_xyzrgb_radial.cpp | 1 + 2 files changed, 9 insertions(+), 3 deletions(-) diff --git a/depth_image_proc/src/nodelets/point_cloud_xyzrgb.cpp b/depth_image_proc/src/nodelets/point_cloud_xyzrgb.cpp index ae1a037c6..aab041738 100644 --- a/depth_image_proc/src/nodelets/point_cloud_xyzrgb.cpp +++ b/depth_image_proc/src/nodelets/point_cloud_xyzrgb.cpp @@ -79,6 +79,9 @@ class PointCloudXyzrgbNodelet : public nodelet::Nodelet image_geometry::PinholeCameraModel model_; + // variables + double max_range_ = -1; + virtual void onInit(); void connectCb(); @@ -102,6 +105,8 @@ void PointCloudXyzrgbNodelet::onInit() ros::NodeHandle depth_nh(nh, "depth_registered"); rgb_it_ .reset( new image_transport::ImageTransport(*rgb_nh_) ); depth_it_.reset( new image_transport::ImageTransport(depth_nh) ); + + private_nh.param("max_range", max_range_, std::numeric_limits::infinity()); // Read parameters int queue_size; @@ -116,7 +121,7 @@ void PointCloudXyzrgbNodelet::onInit() exact_sync_->registerCallback(boost::bind(&PointCloudXyzrgbNodelet::imageCb, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3)); } else - { + { sync_.reset( new Synchronizer(SyncPolicy(queue_size), sub_depth_, sub_rgb_, sub_info_) ); sync_->registerCallback(boost::bind(&PointCloudXyzrgbNodelet::imageCb, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3)); } @@ -329,9 +334,9 @@ void PointCloudXyzrgbNodelet::convert(const sensor_msgs::ImageConstPtr& depth_ms for (int u = 0; u < int(cloud_msg->width); ++u, rgb += color_step, ++iter_x, ++iter_y, ++iter_z, ++iter_a, ++iter_r, ++iter_g, ++iter_b) { T depth = depth_row[u]; - + // Check for invalid measurements - if (!DepthTraits::valid(depth)) + if (!DepthTraits::valid(depth) || DepthTraits::toMeters(depth) > max_range_ ) { *iter_x = *iter_y = *iter_z = bad_point; } diff --git a/depth_image_proc/src/nodelets/point_cloud_xyzrgb_radial.cpp b/depth_image_proc/src/nodelets/point_cloud_xyzrgb_radial.cpp index ab7be5a14..b1a8a4da8 100644 --- a/depth_image_proc/src/nodelets/point_cloud_xyzrgb_radial.cpp +++ b/depth_image_proc/src/nodelets/point_cloud_xyzrgb_radial.cpp @@ -385,6 +385,7 @@ namespace depth_image_proc { { T depth = depth_row[u]; + // Missing points denoted by NaNs if (!DepthTraits::valid(depth)) { From 53e44a6f9b2c1049287e2255ad8f657df51a72c2 Mon Sep 17 00:00:00 2001 From: Arshad Date: Sat, 30 Jul 2022 19:13:54 +0100 Subject: [PATCH 2/3] pointcloud_xyzrbg takes max/min x,y,z points for cropping the pointcloud --- .../src/nodelets/point_cloud_xyzrgb.cpp | 42 +++++++++++++++---- 1 file changed, 35 insertions(+), 7 deletions(-) diff --git a/depth_image_proc/src/nodelets/point_cloud_xyzrgb.cpp b/depth_image_proc/src/nodelets/point_cloud_xyzrgb.cpp index aab041738..d8e242c1a 100644 --- a/depth_image_proc/src/nodelets/point_cloud_xyzrgb.cpp +++ b/depth_image_proc/src/nodelets/point_cloud_xyzrgb.cpp @@ -80,7 +80,13 @@ class PointCloudXyzrgbNodelet : public nodelet::Nodelet image_geometry::PinholeCameraModel model_; // variables - double max_range_ = -1; + double max_x ; + double max_y ; + double max_z ; + double min_x ; + double min_y ; + double min_z ; + virtual void onInit(); @@ -106,7 +112,12 @@ void PointCloudXyzrgbNodelet::onInit() rgb_it_ .reset( new image_transport::ImageTransport(*rgb_nh_) ); depth_it_.reset( new image_transport::ImageTransport(depth_nh) ); - private_nh.param("max_range", max_range_, std::numeric_limits::infinity()); + private_nh.param("max_x", max_x, std::numeric_limits::infinity()); + private_nh.param("max_y", max_y, std::numeric_limits::infinity()); + private_nh.param("max_z", max_z, std::numeric_limits::infinity()); + private_nh.param("min_x", min_x, -1*std::numeric_limits::infinity()); + private_nh.param("min_y", min_y, -1*std::numeric_limits::infinity()); + private_nh.param("min_z", min_z, -1*std::numeric_limits::infinity()); // Read parameters int queue_size; @@ -329,6 +340,8 @@ void PointCloudXyzrgbNodelet::convert(const sensor_msgs::ImageConstPtr& depth_ms sensor_msgs::PointCloud2Iterator iter_b(*cloud_msg, "b"); sensor_msgs::PointCloud2Iterator iter_a(*cloud_msg, "a"); + + for (int v = 0; v < int(cloud_msg->height); ++v, depth_row += row_step, rgb += rgb_skip) { for (int u = 0; u < int(cloud_msg->width); ++u, rgb += color_step, ++iter_x, ++iter_y, ++iter_z, ++iter_a, ++iter_r, ++iter_g, ++iter_b) @@ -336,18 +349,33 @@ void PointCloudXyzrgbNodelet::convert(const sensor_msgs::ImageConstPtr& depth_ms T depth = depth_row[u]; // Check for invalid measurements - if (!DepthTraits::valid(depth) || DepthTraits::toMeters(depth) > max_range_ ) + if (!DepthTraits::valid(depth)) { *iter_x = *iter_y = *iter_z = bad_point; } else { - // Fill in XYZ - *iter_x = (u - center_x) * depth * constant_x; - *iter_y = (v - center_y) * depth * constant_y; - *iter_z = DepthTraits::toMeters(depth); + if ( + ( (u - center_x) * depth * constant_x ) < max_x && + ( (v - center_y) * depth * constant_y ) < max_y && + ( DepthTraits::toMeters(depth) ) < max_z && + ( (u - center_x) * depth * constant_x ) > min_x && + ( (v - center_y) * depth * constant_y ) > min_y && + ( DepthTraits::toMeters(depth) ) > min_z + ) + { + // Fill in XYZ + *iter_x = (u - center_x) * depth * constant_x; + *iter_y = (v - center_y) * depth * constant_y; + *iter_z = DepthTraits::toMeters(depth); + } + else + { + *iter_x = *iter_y = *iter_z = bad_point; + } } + // Fill in color *iter_a = 255; *iter_r = rgb[red_offset]; From 0a648fef157a94d6e55e0035922d3eaf670fc83b Mon Sep 17 00:00:00 2001 From: Arshad Date: Tue, 6 Dec 2022 15:55:33 +0000 Subject: [PATCH 3/3] added xyz crop test on all pointcloud classes --- .../src/nodelets/point_cloud_xyz_radial.cpp | 42 ++++++++++++++++--- .../src/nodelets/point_cloud_xyzi.cpp | 39 +++++++++++++++-- .../src/nodelets/point_cloud_xyzi_radial.cpp | 40 ++++++++++++++++-- .../src/nodelets/point_cloud_xyzrgb.cpp | 6 ++- .../nodelets/point_cloud_xyzrgb_radial.cpp | 42 +++++++++++++++++-- 5 files changed, 150 insertions(+), 19 deletions(-) diff --git a/depth_image_proc/src/nodelets/point_cloud_xyz_radial.cpp b/depth_image_proc/src/nodelets/point_cloud_xyz_radial.cpp index b28d6ba98..7f9dc1b5b 100644 --- a/depth_image_proc/src/nodelets/point_cloud_xyz_radial.cpp +++ b/depth_image_proc/src/nodelets/point_cloud_xyz_radial.cpp @@ -61,7 +61,15 @@ namespace depth_image_proc { std::vector D_; boost::array K_; - + + // range crop + double max_x ; + double max_y ; + double max_z ; + double min_x ; + double min_y ; + double min_z ; + int width_; int height_; @@ -130,6 +138,14 @@ namespace depth_image_proc { // Read parameters private_nh.param("queue_size", queue_size_, 5); + // min/max ranges for crop + private_nh.param("max_x", max_x, std::numeric_limits::infinity()); + private_nh.param("max_y", max_y, std::numeric_limits::infinity()); + private_nh.param("max_z", max_z, std::numeric_limits::infinity()); + private_nh.param("min_x", min_x, -1*std::numeric_limits::infinity()); + private_nh.param("min_y", min_y, -1*std::numeric_limits::infinity()); + private_nh.param("min_z", min_z, -1*std::numeric_limits::infinity()); + // Monitor whether anyone is subscribed to the output ros::SubscriberStatusCallback connect_cb = boost::bind(&PointCloudXyzRadialNodelet::connectCb, this); @@ -221,10 +237,26 @@ namespace depth_image_proc { continue; } const cv::Vec3f &cvPoint = binned.at(u,v) * DepthTraits::toMeters(depth); - // Fill in XYZ - *iter_x = cvPoint(0); - *iter_y = cvPoint(1); - *iter_z = cvPoint(2); + // test if the point is in the XYZ range + if ( + ( cvPoint(0) ) < max_x && + ( cvPoint(1) ) < max_y && + ( cvPoint(2) ) < max_z && + ( cvPoint(0) ) > min_x && + ( cvPoint(1) ) > min_y && + ( cvPoint(2) ) > min_z + ) + { + // Fill in XYZ + *iter_x = cvPoint(0); + *iter_y = cvPoint(1); + *iter_z = cvPoint(2); + } + else + { + *iter_x = *iter_y = *iter_z = bad_point; + continue; + } } } } diff --git a/depth_image_proc/src/nodelets/point_cloud_xyzi.cpp b/depth_image_proc/src/nodelets/point_cloud_xyzi.cpp index 78a96df30..2594c9bf5 100644 --- a/depth_image_proc/src/nodelets/point_cloud_xyzi.cpp +++ b/depth_image_proc/src/nodelets/point_cloud_xyzi.cpp @@ -75,6 +75,14 @@ class PointCloudXyziNodelet : public nodelet::Nodelet image_geometry::PinholeCameraModel model_; + // range crop + double max_x ; + double max_y ; + double max_z ; + double min_x ; + double min_y ; + double min_z ; + virtual void onInit(); void connectCb(); @@ -102,6 +110,14 @@ void PointCloudXyziNodelet::onInit() int queue_size; private_nh.param("queue_size", queue_size, 5); + // min/max ranges for crop + private_nh.param("max_x", max_x, std::numeric_limits::infinity()); + private_nh.param("max_y", max_y, std::numeric_limits::infinity()); + private_nh.param("max_z", max_z, std::numeric_limits::infinity()); + private_nh.param("min_x", min_x, -1*std::numeric_limits::infinity()); + private_nh.param("min_y", min_y, -1*std::numeric_limits::infinity()); + private_nh.param("min_z", min_z, -1*std::numeric_limits::infinity()); + // Synchronize inputs. Topic subscriptions happen on demand in the connection callback. sync_.reset( new Synchronizer(SyncPolicy(queue_size), sub_depth_, sub_intensity_, sub_info_) ); sync_->registerCallback(boost::bind(&PointCloudXyziNodelet::imageCb, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3)); @@ -307,10 +323,25 @@ void PointCloudXyziNodelet::convert(const sensor_msgs::ImageConstPtr& depth_msg, } else { - // Fill in XYZ - *iter_x = (u - center_x) * depth * constant_x; - *iter_y = (v - center_y) * depth * constant_y; - *iter_z = DepthTraits::toMeters(depth); + // test if the point is in the XYZ range + if ( + ( (u - center_x) * depth * constant_x ) < max_x && + ( (v - center_y) * depth * constant_y ) < max_y && + ( DepthTraits::toMeters(depth) ) < max_z && + ( (u - center_x) * depth * constant_x ) > min_x && + ( (v - center_y) * depth * constant_y ) > min_y && + ( DepthTraits::toMeters(depth) ) > min_z + ) + { + // Fill in XYZ + *iter_x = (u - center_x) * depth * constant_x; + *iter_y = (v - center_y) * depth * constant_y; + *iter_z = DepthTraits::toMeters(depth); + } + else + { + *iter_x = *iter_y = *iter_z = bad_point; + } } // Fill in intensity diff --git a/depth_image_proc/src/nodelets/point_cloud_xyzi_radial.cpp b/depth_image_proc/src/nodelets/point_cloud_xyzi_radial.cpp index 47198b490..c07df9181 100644 --- a/depth_image_proc/src/nodelets/point_cloud_xyzi_radial.cpp +++ b/depth_image_proc/src/nodelets/point_cloud_xyzi_radial.cpp @@ -78,6 +78,14 @@ namespace depth_image_proc { int width_; int height_; + // range crop + double max_x ; + double max_y ; + double max_z ; + double min_x ; + double min_y ; + double min_z ; + cv::Mat transform_; virtual void onInit(); @@ -154,6 +162,14 @@ namespace depth_image_proc { // Read parameters private_nh.param("queue_size", queue_size_, 5); + // min/max ranges for crop + private_nh.param("max_x", max_x, std::numeric_limits::infinity()); + private_nh.param("max_y", max_y, std::numeric_limits::infinity()); + private_nh.param("max_z", max_z, std::numeric_limits::infinity()); + private_nh.param("min_x", min_x, -1*std::numeric_limits::infinity()); + private_nh.param("min_y", min_y, -1*std::numeric_limits::infinity()); + private_nh.param("min_z", min_z, -1*std::numeric_limits::infinity()); + // Synchronize inputs. Topic subscriptions happen on demand in the connection callback. sync_.reset( new Synchronizer(SyncPolicy(queue_size_), sub_depth_, sub_intensity_, sub_info_) ); sync_->registerCallback(boost::bind(&PointCloudXyziRadialNodelet::imageCb, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3)); @@ -286,10 +302,26 @@ namespace depth_image_proc { continue; } const cv::Vec3f &cvPoint = transform_.at(u,v) * DepthTraits::toMeters(depth); - // Fill in XYZ - *iter_x = cvPoint(0); - *iter_y = cvPoint(1); - *iter_z = cvPoint(2); + // test if the point is in the XYZ range + if ( + ( cvPoint(0) ) < max_x && + ( cvPoint(1) ) < max_y && + ( cvPoint(2) ) < max_z && + ( cvPoint(0) ) > min_x && + ( cvPoint(1) ) > min_y && + ( cvPoint(2) ) > min_z + ) + { + // Fill in XYZ + *iter_x = cvPoint(0); + *iter_y = cvPoint(1); + *iter_z = cvPoint(2); + } + else + { + *iter_x = *iter_y = *iter_z = bad_point; + continue; + } } } } diff --git a/depth_image_proc/src/nodelets/point_cloud_xyzrgb.cpp b/depth_image_proc/src/nodelets/point_cloud_xyzrgb.cpp index d8e242c1a..527d680de 100644 --- a/depth_image_proc/src/nodelets/point_cloud_xyzrgb.cpp +++ b/depth_image_proc/src/nodelets/point_cloud_xyzrgb.cpp @@ -79,7 +79,7 @@ class PointCloudXyzrgbNodelet : public nodelet::Nodelet image_geometry::PinholeCameraModel model_; - // variables + // range crop double max_x ; double max_y ; double max_z ; @@ -112,13 +112,14 @@ void PointCloudXyzrgbNodelet::onInit() rgb_it_ .reset( new image_transport::ImageTransport(*rgb_nh_) ); depth_it_.reset( new image_transport::ImageTransport(depth_nh) ); + // min/max ranges for crop private_nh.param("max_x", max_x, std::numeric_limits::infinity()); private_nh.param("max_y", max_y, std::numeric_limits::infinity()); private_nh.param("max_z", max_z, std::numeric_limits::infinity()); private_nh.param("min_x", min_x, -1*std::numeric_limits::infinity()); private_nh.param("min_y", min_y, -1*std::numeric_limits::infinity()); private_nh.param("min_z", min_z, -1*std::numeric_limits::infinity()); - + // Read parameters int queue_size; private_nh.param("queue_size", queue_size, 5); @@ -355,6 +356,7 @@ void PointCloudXyzrgbNodelet::convert(const sensor_msgs::ImageConstPtr& depth_ms } else { + // test if the point is in the XYZ range if ( ( (u - center_x) * depth * constant_x ) < max_x && ( (v - center_y) * depth * constant_y ) < max_y && diff --git a/depth_image_proc/src/nodelets/point_cloud_xyzrgb_radial.cpp b/depth_image_proc/src/nodelets/point_cloud_xyzrgb_radial.cpp index b1a8a4da8..a4257a6bd 100644 --- a/depth_image_proc/src/nodelets/point_cloud_xyzrgb_radial.cpp +++ b/depth_image_proc/src/nodelets/point_cloud_xyzrgb_radial.cpp @@ -83,6 +83,14 @@ namespace depth_image_proc { int width_; int height_; + // range crop + double max_x ; + double max_y ; + double max_z ; + double min_x ; + double min_y ; + double min_z ; + cv::Mat transform_; image_geometry::PinholeCameraModel model_; @@ -163,6 +171,15 @@ namespace depth_image_proc { bool use_exact_sync; private_nh.param("exact_sync", use_exact_sync, false); + // min/max ranges for crop + private_nh.param("max_x", max_x, std::numeric_limits::infinity()); + private_nh.param("max_y", max_y, std::numeric_limits::infinity()); + private_nh.param("max_z", max_z, std::numeric_limits::infinity()); + private_nh.param("min_x", min_x, -1*std::numeric_limits::infinity()); + private_nh.param("min_y", min_y, -1*std::numeric_limits::infinity()); + private_nh.param("min_z", min_z, -1*std::numeric_limits::infinity()); + + // Synchronize inputs. Topic subscriptions happen on demand in the connection callback. if(use_exact_sync) { exact_sync_.reset( new ExactSynchronizer(ExactSyncPolicy(queue_size_), sub_depth_, sub_rgb_, sub_info_) ); @@ -393,10 +410,27 @@ namespace depth_image_proc { continue; } const cv::Vec3f &cvPoint = transform_.at(u,v) * DepthTraits::toMeters(depth); - // Fill in XYZ - *iter_x = cvPoint(0); - *iter_y = cvPoint(1); - *iter_z = cvPoint(2); + // test if the point is in the XYZ range + if ( + ( cvPoint(0) ) < max_x && + ( cvPoint(1) ) < max_y && + ( cvPoint(2) ) < max_z && + ( cvPoint(0) ) > min_x && + ( cvPoint(1) ) > min_y && + ( cvPoint(2) ) > min_z + ) + { + // Fill in XYZ + *iter_x = cvPoint(0); + *iter_y = cvPoint(1); + *iter_z = cvPoint(2); + } + else + { + *iter_x = *iter_y = *iter_z = bad_point; + continue; + } + } } }