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 ae1a037c6..527d680de 100644 --- a/depth_image_proc/src/nodelets/point_cloud_xyzrgb.cpp +++ b/depth_image_proc/src/nodelets/point_cloud_xyzrgb.cpp @@ -79,6 +79,15 @@ class PointCloudXyzrgbNodelet : 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,7 +111,15 @@ 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) ); - + + // 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); @@ -116,7 +133,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)); } @@ -324,12 +341,14 @@ 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) { T depth = depth_row[u]; - + // Check for invalid measurements if (!DepthTraits::valid(depth)) { @@ -337,12 +356,28 @@ void PointCloudXyzrgbNodelet::convert(const sensor_msgs::ImageConstPtr& depth_ms } 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 color *iter_a = 255; *iter_r = rgb[red_offset]; 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..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_) ); @@ -385,6 +402,7 @@ namespace depth_image_proc { { T depth = depth_row[u]; + // Missing points denoted by NaNs if (!DepthTraits::valid(depth)) { @@ -392,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; + } + } } }