From eb9632058f5173d41a6001edbcbd6ca8792c0989 Mon Sep 17 00:00:00 2001 From: akiroz Date: Thu, 5 Mar 2020 11:23:09 +0800 Subject: [PATCH 1/4] Add voxel_min_points param for voxel filter --- README.md | 1 + include/spatio_temporal_voxel_layer/measurement_buffer.hpp | 2 ++ src/measurement_buffer.cpp | 7 ++++--- src/spatio_temporal_voxel_layer.cpp | 7 ++++++- 4 files changed, 13 insertions(+), 4 deletions(-) diff --git a/README.md b/README.md index dbdcb5b5..5b91d971 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: 10 #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/include/spatio_temporal_voxel_layer/measurement_buffer.hpp b/include/spatio_temporal_voxel_layer/measurement_buffer.hpp index 8aa268ab..af6b0671 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, @@ -139,6 +140,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; std::shared_ptr node_; }; diff --git a/src/measurement_buffer.cpp b/src/measurement_buffer.cpp index 84c22e5f..5200a1dd 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((unsigned int) _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) { From 55007fddaba88371ccb0c23be69f580b6fe7b04f Mon Sep 17 00:00:00 2001 From: akiroz Date: Thu, 5 Mar 2020 12:26:06 +0800 Subject: [PATCH 2/4] Update params in example --- example/constrained_indoor_environment_config.yaml | 1 + example/outdoor_environment_config.yaml | 1 + example/standard_indoor_environment_config.yaml | 1 + example/vlp16_config.yaml | 1 + 4 files changed, 4 insertions(+) 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 From d68d2807449dad3e8fe4e4c3eace39ab2b060599 Mon Sep 17 00:00:00 2001 From: akiroz Date: Thu, 5 Mar 2020 12:44:35 +0800 Subject: [PATCH 3/4] Fix Wreorder compiler warning --- include/spatio_temporal_voxel_layer/measurement_buffer.hpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/include/spatio_temporal_voxel_layer/measurement_buffer.hpp b/include/spatio_temporal_voxel_layer/measurement_buffer.hpp index af6b0671..eaf1d2a4 100644 --- a/include/spatio_temporal_voxel_layer/measurement_buffer.hpp +++ b/include/spatio_temporal_voxel_layer/measurement_buffer.hpp @@ -139,8 +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_; }; From 50b820fdba244111027b36d65e9081a8c3aee936 Mon Sep 17 00:00:00 2001 From: akiroz Date: Thu, 5 Mar 2020 12:59:54 +0800 Subject: [PATCH 4/4] Use static_cast for type-casting & update default value in README --- README.md | 2 +- src/measurement_buffer.cpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/README.md b/README.md index 5b91d971..b52a66a1 100644 --- a/README.md +++ b/README.md @@ -116,7 +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: 10 #default 0, minimum points per voxel for voxel filter + 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/src/measurement_buffer.cpp b/src/measurement_buffer.cpp index 5200a1dd..d1269e65 100644 --- a/src/measurement_buffer.cpp +++ b/src/measurement_buffer.cpp @@ -154,7 +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((unsigned int) _voxel_min_points); + sor.setMinimumPointsNumberPerVoxel(static_cast(_voxel_min_points)); sor.filter(*cloud_filtered); } else { pcl::PassThrough pass_through_filter;