Adding optional filter (#172)

* adding filter options to STVL

* Update src/spatio_temporal_voxel_layer.cpp

Co-authored-by: nickvaras <[email protected]>

* adding param description

* grammar

Co-authored-by: nickvaras <[email protected]>

Author: SteveMacenski

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

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:

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:

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:

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:

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<observation::MeasurementReading>::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;
 };

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<pcl::PCLPointCloud2> sor;
       sor.setInputCloud (cloud_pcl);
       sor.setFilterFieldName("z");
@@ -163,19 +162,21 @@ void MeasurementBuffer::BufferROSCloud(const sensor_msgs::PointCloud2& cloud)
                        (float)_voxel_size);
       sor.setMinimumPointsNumberPerVoxel(static_cast<unsigned int>(_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<pcl::PCLPointCloud2> pass_through_filter;
       pass_through_filter.setInputCloud(cloud_pcl);
       pass_through_filter.setKeepOrganized(false);
       pass_through_filter.setFilterFieldName("z");
       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)

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<ModelType>(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