diff --git a/README.md b/README.md index dbdcb5b5..b52a66a1 100644 --- a/README.md +++ b/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 diff --git a/example/constrained_indoor_environment_config.yaml b/example/constrained_indoor_environment_config.yaml index 1bfd39dc..7af9f05e 100644 --- a/example/constrained_indoor_environment_config.yaml +++ b/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 diff --git a/example/outdoor_environment_config.yaml b/example/outdoor_environment_config.yaml index 9268b1b2..4fe2e3e4 100644 --- a/example/outdoor_environment_config.yaml +++ b/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 diff --git a/example/standard_indoor_environment_config.yaml b/example/standard_indoor_environment_config.yaml index 96731d9b..01455169 100644 --- a/example/standard_indoor_environment_config.yaml +++ b/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 diff --git a/example/vlp16_config.yaml b/example/vlp16_config.yaml index c053848e..fee6c606 100644 --- a/example/vlp16_config.yaml +++ b/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 diff --git a/include/spatio_temporal_voxel_layer/measurement_buffer.hpp b/include/spatio_temporal_voxel_layer/measurement_buffer.hpp index 8aa268ab..eaf1d2a4 100644 --- a/include/spatio_temporal_voxel_layer/measurement_buffer.hpp +++ b/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 node_; }; diff --git a/src/measurement_buffer.cpp b/src/measurement_buffer.cpp index 84c22e5f..d1269e65 100644 --- a/src/measurement_buffer.cpp +++ b/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 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(_voxel_size); sor.setLeafSize(v_s, v_s, v_s); + sor.setMinimumPointsNumberPerVoxel(static_cast(_voxel_min_points)); sor.filter(*cloud_filtered); } else { pcl::PassThrough pass_through_filter; diff --git a/src/spatio_temporal_voxel_layer.cpp b/src/spatio_temporal_voxel_layer.cpp index e2f1a473..a4dd6efc 100644 --- a/src/spatio_temporal_voxel_layer.cpp +++ b/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) {