From 14e7f5ea63fc310b5134e108ae9e156bb3a7be14 Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Wed, 5 Aug 2020 18:51:30 -0700 Subject: [PATCH] Adding optional filter (#172) * adding filter options to STVL * Update src/spatio_temporal_voxel_layer.cpp Co-authored-by: nickvaras <34077897+nickvaras@users.noreply.github.com> * adding param description * grammar Co-authored-by: nickvaras <34077897+nickvaras@users.noreply.github.com> --- README.md | 4 ++- ...constrained_indoor_environment_config.yaml | 2 +- example/outdoor_environment_config.yaml | 2 +- .../standard_indoor_environment_config.yaml | 2 +- example/vlp16_config.yaml | 2 +- .../measurement_buffer.hpp | 12 +++++++-- src/measurement_buffer.cpp | 15 ++++++----- src/spatio_temporal_voxel_layer.cpp | 27 +++++++++++++++---- 8 files changed, 47 insertions(+), 19 deletions(-) diff --git a/README.md b/README.md index 6799f4f8..5a109b86 100644 --- a/README.md +++ b/README.md @@ -99,6 +99,8 @@ Required dependencies ROS Kinetic, navigation, OpenVDB, TBB. An example fully-described configuration is shown below. +Note: We supply two PCL filters within STVL to massage the data to lower compute overhead. STVL has an approximate voxel filter to make the data more sparse if very dense. It also has a passthrough filter to limit processing data within the valid minimum to maximum height bounds. The voxel filter is recommended if it lowers CPU overhead, otherwise, passthrough filter. No filter is also available if you pre-process your data or are not interested in performance optimizations. + ``` rgbd_obstacle_layer: enabled: true @@ -130,7 +132,7 @@ rgbd_obstacle_layer: observation_persistence: 0.0 #default 0, use all measurements taken during now-value, 0=latest inf_is_valid: false #default false, for laser scans clear_after_reading: true #default false, clear the buffer after the layer gets readings from it - voxel_filter: true #default off, apply voxel filter to sensor, recommend on + filter: "voxel" #default passthrough, apply "voxel", "passthrough", or no filter to sensor data, recommended to have at one filter on voxel_min_points: 0 #default 0, minimum points per voxel for voxel filter rgbd1_clear: enabled: true #default true, can be toggled on/off with associated service call diff --git a/example/constrained_indoor_environment_config.yaml b/example/constrained_indoor_environment_config.yaml index dbc182f4..36102b29 100644 --- a/example/constrained_indoor_environment_config.yaml +++ b/example/constrained_indoor_environment_config.yaml @@ -26,7 +26,7 @@ rgbd_obstacle_layer: expected_update_rate: 0.0 # default 0, if not updating at this rate at least, remove from buffer observation_persistence: 0.0 # default 0, use all measurements taken during now-value, 0=latest inf_is_valid: false # default false, for laser scans - voxel_filter: false # default off, apply voxel filter to sensor, recommend on + filter: "passthrough" # default passthrough, apply "voxel", "passthrough", or no filter to sensor data, recommend on voxel_min_points: 0 # default 0, minimum points per voxel for voxel filter clear_after_reading: true # default false, clear the buffer after the layer gets readings from it rgbd1_clear: diff --git a/example/outdoor_environment_config.yaml b/example/outdoor_environment_config.yaml index 4f223634..6912ce65 100644 --- a/example/outdoor_environment_config.yaml +++ b/example/outdoor_environment_config.yaml @@ -26,7 +26,7 @@ rgbd_obstacle_layer: expected_update_rate: 0.0 # default 0, if not updating at this rate at least, remove from buffer observation_persistence: 0.0 # default 0, use all measurements taken during now-value, 0=latest inf_is_valid: false # default false, for laser scans - voxel_filter: false # default off, apply voxel filter to sensor, recommend on + filter: "passthrough" # default passthrough, apply "voxel", "passthrough", or no filter to sensor data, recommend on voxel_min_points: 0 # default 0, minimum points per voxel for voxel filter clear_after_reading: true # default false, clear the buffer after the layer gets readings from it rgbd1_clear: diff --git a/example/standard_indoor_environment_config.yaml b/example/standard_indoor_environment_config.yaml index b13649e1..ffc2731d 100644 --- a/example/standard_indoor_environment_config.yaml +++ b/example/standard_indoor_environment_config.yaml @@ -26,7 +26,7 @@ rgbd_obstacle_layer: expected_update_rate: 0.0 # default 0, if not updating at this rate at least, remove from buffer observation_persistence: 0.0 # default 0, use all measurements taken during now-value, 0=latest inf_is_valid: false # default false, for laser scans - voxel_filter: false # default off, apply voxel filter to sensor, recommend on + filter: "passthrough" # default passthrough, apply "voxel", "passthrough", or no filter to sensor data, recommend on voxel_min_points: 0 # default 0, minimum points per voxel for voxel filter clear_after_reading: true # default false, clear the buffer after the layer gets readings from it rgbd1_clear: diff --git a/example/vlp16_config.yaml b/example/vlp16_config.yaml index f88e3f22..72f31e77 100644 --- a/example/vlp16_config.yaml +++ b/example/vlp16_config.yaml @@ -26,7 +26,7 @@ rgbd_obstacle_layer: expected_update_rate: 0.0 # default 0, if not updating at this rate at least, remove from buffer observation_persistence: 0.0 # default 0, use all measurements taken during now-value, 0=latest inf_is_valid: false # default false, for laser scans - voxel_filter: false # default off, apply voxel filter to sensor, recommend on + filter: "voxel" # default passthrough, apply "voxel", "passthrough", or no filter to sensor data, recommend on voxel_min_points: 0 # default 0, minimum points per voxel for voxel filter clear_after_reading: true # default false, clear the buffer after the layer gets readings from it rgbd1_clear: diff --git a/include/spatio_temporal_voxel_layer/measurement_buffer.hpp b/include/spatio_temporal_voxel_layer/measurement_buffer.hpp index 4e83deee..433ec1a9 100644 --- a/include/spatio_temporal_voxel_layer/measurement_buffer.hpp +++ b/include/spatio_temporal_voxel_layer/measurement_buffer.hpp @@ -67,6 +67,13 @@ namespace buffer { +enum class Filters +{ + NONE = 0, + VOXEL = 1, + PASSTHROUGH = 2 +}; + // conveniences for line lengths typedef std::list::iterator readings_iter; typedef sensor_msgs::PointCloud2::Ptr point_cloud_ptr; @@ -94,7 +101,7 @@ class MeasurementBuffer const bool& marking, \ const bool& clearing, \ const double& voxel_size, \ - const bool& voxel_filter, \ + const Filters& filter, \ const int& voxel_min_points, \ const bool& enabled, \ const bool& clear_buffer_after_reading, \ @@ -135,7 +142,8 @@ class MeasurementBuffer double _min_obstacle_height, _max_obstacle_height, _obstacle_range, _tf_tolerance; double _min_z, _max_z, _vertical_fov, _vertical_fov_padding, _horizontal_fov; double _decay_acceleration, _voxel_size; - bool _marking, _clearing, _voxel_filter, _clear_buffer_after_reading, _enabled; + bool _marking, _clearing, _clear_buffer_after_reading, _enabled; + Filters _filter; int _voxel_min_points; ModelType _model_type; }; diff --git a/src/measurement_buffer.cpp b/src/measurement_buffer.cpp index 77f6b938..b083a154 100644 --- a/src/measurement_buffer.cpp +++ b/src/measurement_buffer.cpp @@ -61,7 +61,7 @@ MeasurementBuffer::MeasurementBuffer(const std::string& topic_name, \ const bool& marking, \ const bool& clearing, \ const double& voxel_size, \ - const bool& voxel_filter, \ + const Filters& filter, \ const int& voxel_min_points, \ const bool& enabled, \ const bool& clear_buffer_after_reading, \ @@ -76,7 +76,7 @@ MeasurementBuffer::MeasurementBuffer(const std::string& topic_name, \ _vertical_fov(vFOV), _vertical_fov_padding(vFOVPadding), _horizontal_fov(hFOV), _decay_acceleration(decay_acceleration), _marking(marking), _clearing(clearing), _voxel_size(voxel_size), - _voxel_filter(voxel_filter), _voxel_min_points(voxel_min_points), + _filter(filter), _voxel_min_points(voxel_min_points), _enabled(enabled), _clear_buffer_after_reading(clear_buffer_after_reading), _model_type(model_type) { @@ -147,12 +147,11 @@ void MeasurementBuffer::BufferROSCloud(const sensor_msgs::PointCloud2& cloud) pcl::PCLPointCloud2::Ptr cloud_pcl (new pcl::PCLPointCloud2 ()); pcl::PCLPointCloud2::Ptr cloud_filtered (new pcl::PCLPointCloud2 ()); - pcl_conversions::toPCL(*cld_global, *cloud_pcl); - // remove points that are below or above our height restrictions, and // in the same time, remove NaNs and if user wants to use it, combine with a - if ( _voxel_filter ) + if (_filter == Filters::VOXEL) { + pcl_conversions::toPCL(*cld_global, *cloud_pcl); pcl::VoxelGrid sor; sor.setInputCloud (cloud_pcl); sor.setFilterFieldName("z"); @@ -163,9 +162,11 @@ void MeasurementBuffer::BufferROSCloud(const sensor_msgs::PointCloud2& cloud) (float)_voxel_size); sor.setMinimumPointsNumberPerVoxel(static_cast(_voxel_min_points)); sor.filter(*cloud_filtered); + pcl_conversions::fromPCL(*cloud_filtered, *cld_global); } - else + else if (_filter == Filters::PASSTHROUGH) { + pcl_conversions::toPCL(*cld_global, *cloud_pcl); pcl::PassThrough pass_through_filter; pass_through_filter.setInputCloud(cloud_pcl); pass_through_filter.setKeepOrganized(false); @@ -173,9 +174,9 @@ void MeasurementBuffer::BufferROSCloud(const sensor_msgs::PointCloud2& cloud) pass_through_filter.setFilterLimits( \ _min_obstacle_height,_max_obstacle_height); pass_through_filter.filter(*cloud_filtered); + pcl_conversions::fromPCL(*cloud_filtered, *cld_global); } - pcl_conversions::fromPCL(*cloud_filtered, *cld_global); _observation_list.front()._cloud = cld_global; } catch (tf::TransformException& ex) diff --git a/src/spatio_temporal_voxel_layer.cpp b/src/spatio_temporal_voxel_layer.cpp index 72d36ca7..a8f99a85 100644 --- a/src/spatio_temporal_voxel_layer.cpp +++ b/src/spatio_temporal_voxel_layer.cpp @@ -147,9 +147,10 @@ void SpatioTemporalVoxelLayer::onInitialize(void) double observation_keep_time, expected_update_rate, min_obstacle_height; double max_obstacle_height, min_z, max_z, vFOV, vFOVPadding; double hFOV, decay_acceleration; - std::string topic, sensor_frame, data_type; - bool inf_is_valid, clearing, marking, voxel_filter, clear_after_reading, enabled; + std::string topic, sensor_frame, data_type, filter_str; + bool inf_is_valid, clearing, marking, clear_after_reading, enabled; int voxel_min_points; + buffer::Filters filter; source_node.param("topic", topic, source); source_node.param("sensor_frame", sensor_frame, std::string("")); @@ -173,8 +174,8 @@ void SpatioTemporalVoxelLayer::onInitialize(void) source_node.param("horizontal_fov_angle", hFOV, 1.04); // acceleration scales the model's decay in presence of readings source_node.param("decay_acceleration", decay_acceleration, 0.); - // performs an approximate voxel filter over the data to reduce - source_node.param("voxel_filter", voxel_filter, false); + // Apply a PCL filter (Approximate VoxeGrid or PassThrough) or skip + source_node.param("filter", filter_str, std::string("passthrough")); // minimum points per voxel for voxel filter source_node.param("voxel_min_points", voxel_min_points, 0); // clears measurement buffer after reading values from it @@ -186,6 +187,22 @@ void SpatioTemporalVoxelLayer::onInitialize(void) source_node.param("model_type", model_type_int, 0); ModelType model_type = static_cast(model_type_int); + if (filter_str == "passthrough") + { + ROS_INFO("Passthough filter activated."); + filter = buffer::Filters::PASSTHROUGH; + } + else if (filter_str == "voxel") + { + ROS_INFO("Voxel filter activated."); + filter = buffer::Filters::VOXEL; + } + else + { + ROS_INFO("No filters activated."); + filter = buffer::Filters::NONE; + } + if (!sensor_frame.empty()) { sensor_frame = tf::resolve(tf_prefix, sensor_frame); @@ -212,7 +229,7 @@ void SpatioTemporalVoxelLayer::onInitialize(void) obstacle_range, *tf_, _global_frame, sensor_frame, \ transform_tolerance, min_z, max_z, vFOV, vFOVPadding, hFOV, \ decay_acceleration, marking, clearing, _voxel_size, \ - voxel_filter, voxel_min_points, enabled, clear_after_reading, \ + filter, voxel_min_points, enabled, clear_after_reading, \ model_type))); // Add buffer to marking observation buffers