Add voxel_min_points param for voxel filter (#160)

* Add voxel_min_points param for voxel filter

* Update params in example

* Use static_cast for type-casting & update default value in README

Author: SteveMacenski

README.md

@@ -116,6 +116,7 @@ rgbd_obstacle_layer:
     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 
+    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
     data_type: PointCloud2

example/constrained_indoor_environment_config.yaml

@@ -27,6 +27,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
     voxel_filter: false          # default off, apply voxel filter to sensor, 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:
     data_type: PointCloud2

example/outdoor_environment_config.yaml

@@ -27,6 +27,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
     voxel_filter: false          # default off, apply voxel filter to sensor, 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:
     data_type: PointCloud2

example/standard_indoor_environment_config.yaml

@@ -27,6 +27,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
     voxel_filter: false          # default off, apply voxel filter to sensor, 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:
     data_type: PointCloud2

example/vlp16_config.yaml

@@ -27,6 +27,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
     voxel_filter: false          # default off, apply voxel filter to sensor, 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:
     enabled: true                #default true, can be toggled on/off with associated service call

include/spatio_temporal_voxel_layer/measurement_buffer.hpp

@@ -95,6 +95,7 @@ class MeasurementBuffer
                     const bool& clearing,                   \
                     const double& voxel_size,               \
                     const bool& voxel_filter,               \
+                    const int& voxel_min_points,            \
                     const bool& enabled,                    \
                     const bool& clear_buffer_after_reading, \
                     const ModelType& model_type);
@@ -135,6 +136,7 @@ class MeasurementBuffer
   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;
+  int _voxel_min_points;
   ModelType _model_type;
 };
 

src/measurement_buffer.cpp

@@ -62,6 +62,7 @@ MeasurementBuffer::MeasurementBuffer(const std::string& topic_name,          \
                                      const bool& clearing,                   \
                                      const double& voxel_size,               \
                                      const bool& voxel_filter,               \
+                                     const int& voxel_min_points,            \
                                      const bool& enabled,                    \
                                      const bool& clear_buffer_after_reading, \
                                      const ModelType& model_type) :
@@ -75,8 +76,8 @@ 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), _enabled(enabled),
-    _clear_buffer_after_reading(clear_buffer_after_reading),
+    _voxel_filter(voxel_filter), _voxel_min_points(voxel_min_points),
+    _enabled(enabled), _clear_buffer_after_reading(clear_buffer_after_reading),
     _model_type(model_type)
 {
 }
@@ -160,6 +161,7 @@ void MeasurementBuffer::BufferROSCloud(const sensor_msgs::PointCloud2& cloud)
       sor.setLeafSize ((float)_voxel_size,
                        (float)_voxel_size,
                        (float)_voxel_size);
+      sor.setMinimumPointsNumberPerVoxel(static_cast<unsigned int>(_voxel_min_points));
       sor.filter(*cloud_filtered);
     }
     else

src/spatio_temporal_voxel_layer.cpp

@@ -149,6 +149,7 @@ void SpatioTemporalVoxelLayer::onInitialize(void)
     double hFOV, decay_acceleration;
     std::string topic, sensor_frame, data_type;
     bool inf_is_valid, clearing, marking, voxel_filter, clear_after_reading, enabled;
+    int voxel_min_points;
 
     source_node.param("topic", topic, source);
     source_node.param("sensor_frame", sensor_frame, std::string(""));
@@ -174,6 +175,8 @@ void SpatioTemporalVoxelLayer::onInitialize(void)
     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);
+    // 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
     source_node.param("clear_after_reading", clear_after_reading, false);
     // Whether the frustum is enabled on startup. Can be toggled with service
@@ -209,7 +212,8 @@ 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, enabled, clear_after_reading, model_type)));
+        voxel_filter, voxel_min_points, enabled, clear_after_reading,     \
+        model_type)));
 
     // Add buffer to marking observation buffers
     if (marking == true)