From 61a194a0d7c7afaf9220eea1dcaa877702a1ef65 Mon Sep 17 00:00:00 2001 From: Aaron Hoy Date: Wed, 3 Feb 2016 17:01:18 -0800 Subject: [PATCH 1/2] Changing min_clearing_height to -infinity Similar to raising max_clearing_height to infinity. A noisy or otherwise slightly incorrect ground plane can cause some clearing rays to have endpoints with negative height values, but those rays are still valid for clearing. --- fetch_depth_layer/src/depth_layer.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/fetch_depth_layer/src/depth_layer.cpp b/fetch_depth_layer/src/depth_layer.cpp index d3a36780..3441880b 100644 --- a/fetch_depth_layer/src/depth_layer.cpp +++ b/fetch_depth_layer/src/depth_layer.cpp @@ -72,7 +72,7 @@ void FetchDepthLayer::onInitialize() // Observation range values for both marking and claering private_nh.param("min_obstacle_height", min_obstacle_height, 0.0); private_nh.param("max_obstacle_height", max_obstacle_height, 2.0); - private_nh.param("min_clearing_height", min_clearing_height, 0.0); + private_nh.param("min_clearing_height", min_clearing_height, -std::numeric_limits::infinity()); private_nh.param("max_clearing_height", max_clearing_height, std::numeric_limits::infinity()); marking_buf_ = boost::shared_ptr ( From aeb11154cebdf442e9f3c5951b873eb209e4cd56 Mon Sep 17 00:00:00 2001 From: Aaron Hoy Date: Thu, 4 Feb 2016 12:19:10 -0800 Subject: [PATCH 2/2] Adding ROS parameters to control usage of edge rays Adding per-edge parameters to control how many edge rays to skip Adding a master parameter to control usage of skipped rays for clearing --- .../include/fetch_depth_layer/depth_layer.h | 9 ++++++ fetch_depth_layer/src/depth_layer.cpp | 32 +++++++++++++++---- 2 files changed, 34 insertions(+), 7 deletions(-) diff --git a/fetch_depth_layer/include/fetch_depth_layer/depth_layer.h b/fetch_depth_layer/include/fetch_depth_layer/depth_layer.h index 394285ce..38476e30 100644 --- a/fetch_depth_layer/include/fetch_depth_layer/depth_layer.h +++ b/fetch_depth_layer/include/fetch_depth_layer/depth_layer.h @@ -89,6 +89,15 @@ class FetchDepthLayer : public VoxelLayer // should NANs be treated as +inf and used for clearing bool clear_nans_; + // skipping of potentially noisy rays near the edge of the image + int skip_rays_bottom_; + int skip_rays_top_; + int skip_rays_left_; + int skip_rays_right_; + + // should skipped edge rays be used for clearing? + bool clear_with_skipped_rays_; + // retrieves depth image from head_camera // used to fit ground plane to ros::Subscriber depth_image_sub_; diff --git a/fetch_depth_layer/src/depth_layer.cpp b/fetch_depth_layer/src/depth_layer.cpp index 3441880b..5df49237 100644 --- a/fetch_depth_layer/src/depth_layer.cpp +++ b/fetch_depth_layer/src/depth_layer.cpp @@ -69,12 +69,21 @@ void FetchDepthLayer::onInitialize() // Should NANs be used as clearing observations? private_nh.param("clear_nans", clear_nans_, false); - // Observation range values for both marking and claering + // Observation range values for both marking and clearing private_nh.param("min_obstacle_height", min_obstacle_height, 0.0); private_nh.param("max_obstacle_height", max_obstacle_height, 2.0); private_nh.param("min_clearing_height", min_clearing_height, -std::numeric_limits::infinity()); private_nh.param("max_clearing_height", max_clearing_height, std::numeric_limits::infinity()); + // Skipping of potentially noisy rays near the edge of the image + private_nh.param("skip_rays_bottom", skip_rays_bottom_, 20); + private_nh.param("skip_rays_top", skip_rays_top_, 20); + private_nh.param("skip_rays_left", skip_rays_left_, 20); + private_nh.param("skip_rays_right", skip_rays_right_, 20); + + // Should skipped edge rays be used for clearing? + private_nh.param("clear_with_skipped_rays", clear_with_skipped_rays_, false); + marking_buf_ = boost::shared_ptr ( new costmap_2d::ObservationBuffer(topic, observation_keep_time, expected_update_rate, min_obstacle_height, max_obstacle_height, @@ -264,9 +273,6 @@ void FetchDepthLayer::depthImageCallback( marking_points.header.stamp = msg->header.stamp; marking_points.header.frame_id = msg->header.frame_id; - // Points at edges of image can be very noisy, exclude them from marking (not from clearing!). - const int skip = 20; // TODO should be ROS param - // Put points in clearing/marking clouds for (size_t i = 0; i < points3d.rows; i++) { @@ -285,15 +291,27 @@ void FetchDepthLayer::depthImageCallback( !isnan(current_point.y) && !isnan(current_point.z)) { - // Mark all points for clearance. - clearing_points.points.push_back(current_point); + if (clear_with_skipped_rays_) + { + // If edge rays are to be used for clearing, go ahead and add them now. + clearing_points.points.push_back(current_point); + } // Do not consider boundary points for obstacles marking since they are very noisy. - if (i < skip || i >= points3d.rows - skip || j < skip || j >= points3d.cols - skip) + if (i < skip_rays_top_ || + i >= points3d.rows - skip_rays_bottom_ || + j < skip_rays_left_ || + j >= points3d.cols - skip_rays_right_) { continue; } + if (!clear_with_skipped_rays_) + { + // If edge rays are not to be used for clearing, only add them after the edge check. + clearing_points.points.push_back(current_point); + } + // Check if point is part of the ground plane if (fabs(ground_plane[0] * current_point.x + ground_plane[1] * current_point.y +