Add voxel_min_points param for voxel filter (ROS2) (#161)

* Add voxel_min_points param for voxel filter

* Update params in example

* Fix Wreorder compiler warning

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

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

@@ -30,6 +30,7 @@ global_costmap:
           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

@@ -30,6 +30,7 @@ global_costmap:
           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

@@ -30,6 +30,7 @@ global_costmap:
           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

@@ -98,6 +98,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,
@@ -138,7 +139,9 @@ 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, _voxel_filter;
+  int _voxel_min_points;
+  bool _clear_buffer_after_reading, _enabled;
   ModelType _model_type;
   std::shared_ptr<rclcpp_lifecycle::LifecycleNode> node_;
 };

src/measurement_buffer.cpp

@@ -57,8 +57,8 @@ MeasurementBuffer::MeasurementBuffer(
   const double & vFOVPadding, const double & hFOV,
   const double & decay_acceleration, const bool & marking,
   const bool & clearing, const double & voxel_size, const bool & voxel_filter,
-  const bool & enabled, const bool & clear_buffer_after_reading,
-  const ModelType & model_type,
+  const int & voxel_min_points, const bool & enabled,
+  const bool & clear_buffer_after_reading, const ModelType & model_type,
   std::shared_ptr<rclcpp_lifecycle::LifecycleNode> node)
 : _buffer(tf), _observation_keep_time(observation_keep_time),
   _expected_update_rate(expected_update_rate), _last_updated(node->now()),
@@ -69,7 +69,7 @@ MeasurementBuffer::MeasurementBuffer(
   _vertical_fov(vFOV), _vertical_fov_padding(vFOVPadding),
   _horizontal_fov(hFOV), _decay_acceleration(decay_acceleration),
   _voxel_size(voxel_size), _marking(marking), _clearing(clearing),
-  _voxel_filter(voxel_filter),
+  _voxel_filter(voxel_filter), _voxel_min_points(voxel_min_points),
   _clear_buffer_after_reading(clear_buffer_after_reading),
   _enabled(enabled), _model_type(model_type), node_(node)
 /*****************************************************************************/
@@ -154,6 +154,7 @@ void MeasurementBuffer::BufferROSCloud(
       sor.setDownsampleAllData(false);
       float v_s = static_cast<float>(_voxel_size);
       sor.setLeafSize(v_s, v_s, v_s);
+      sor.setMinimumPointsNumberPerVoxel(static_cast<unsigned int>(_voxel_min_points));
       sor.filter(*cloud_filtered);
     } else {
       pcl::PassThrough<pcl::PCLPointCloud2> pass_through_filter;

src/spatio_temporal_voxel_layer.cpp

@@ -154,6 +154,7 @@ void SpatioTemporalVoxelLayer::onInitialize(void)
     std::string topic, sensor_frame, data_type;
     bool inf_is_valid = false, clearing, marking;
     bool voxel_filter, clear_after_reading, enabled;
+    int voxel_min_points;
 
     declareParameter(source + "." + "topic", rclcpp::ParameterValue(std::string("")));
     declareParameter(source + "." + "sensor_frame", rclcpp::ParameterValue(std::string("")));
@@ -175,6 +176,7 @@ void SpatioTemporalVoxelLayer::onInitialize(void)
     declareParameter(source + "." + "horizontal_fov_angle", rclcpp::ParameterValue(1.04));
     declareParameter(source + "." + "decay_acceleration", rclcpp::ParameterValue(0.0));
     declareParameter(source + "." + "voxel_filter", rclcpp::ParameterValue(false));
+    declareParameter(source + "." + "voxel_min_points", rclcpp::ParameterValue(0));
     declareParameter(source + "." + "clear_after_reading", rclcpp::ParameterValue(false));
     declareParameter(source + "." + "enabled", rclcpp::ParameterValue(true));
     declareParameter(source + "." + "model_type", rclcpp::ParameterValue(0));
@@ -207,6 +209,8 @@ void SpatioTemporalVoxelLayer::onInitialize(void)
     node_->get_parameter(name_ + "." + source + "." + "decay_acceleration", decay_acceleration);
     // performs an approximate voxel filter over the data to reduce
     node_->get_parameter(name_ + "." + source + "." + "voxel_filter", voxel_filter);
+    // minimum points per voxel for voxel filter
+    node_->get_parameter(name_ + "." + source + "." + "voxel_min_points", voxel_min_points);
     // clears measurement buffer after reading values from it
     node_->get_parameter(name_ + "." + source + "." + "clear_after_reading", clear_after_reading);
     // Whether the frustum is enabled on startup. Can be toggled with service
@@ -228,7 +232,8 @@ void SpatioTemporalVoxelLayer::onInitialize(void)
       max_obstacle_height, 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, node_)));
+      voxel_filter, voxel_min_points, enabled, clear_after_reading, model_type,
+      node_)));
 
     // Add buffer to marking observation buffers
     if (marking) {