From 16118ea4a86e6794a9b3f7ec118c1dc2140e1307 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Dr=2E=20Denis=20=C5=A0togl?= Date: Thu, 25 Jul 2024 12:45:49 +0200 Subject: [PATCH 1/8] Initial version for distance moving window filter. --- include/laser_filters/speckle_filter.h | 72 +++++++++++++++++++++++++- 1 file changed, 71 insertions(+), 1 deletion(-) diff --git a/include/laser_filters/speckle_filter.h b/include/laser_filters/speckle_filter.h index 3a3ca0a1..d35c25bc 100644 --- a/include/laser_filters/speckle_filter.h +++ b/include/laser_filters/speckle_filter.h @@ -40,6 +40,9 @@ #ifndef SPECKLE_FILTER_H #define SPECKLE_FILTER_H +#include +#include + #include #include @@ -49,7 +52,8 @@ namespace laser_filters enum SpeckleFilterType //Enum to select the filtering method { Distance = 0, // Range based filtering (distance between consecutive points - RadiusOutlier = 1 // Euclidean filtering based on radius outlier search + RadiusOutlier = 1, // Euclidean filtering based on radius outlier search + DistanceMovingWindow = 2 // Range based filtering based on number of in- and outliers }; class WindowValidator @@ -58,6 +62,63 @@ class WindowValidator virtual bool checkWindowValid(const sensor_msgs::msg::LaserScan& scan, size_t idx, size_t window, double max_range_difference) = 0; }; +class DistanceMovingWindowValidator : public WindowValidator +{ + virtual bool checkWindowValid(const sensor_msgs::msg::LaserScan& scan, size_t idx, size_t window, double max_range_difference) + { + const size_t half_window = std::floor(window/2.0); + + const float& range = scan.ranges[idx]; + if (std::isnan(range)) { + return false; + } + + size_t nr_inliers = 0; + size_t nr_outliers = 0; + + auto check_in_out_lier = [&](const size_t neighbor_idx) + { + const float& neighbor_range = scan.ranges[neighbor_idx]; + if (std::isnan(neighbor_range) || fabs(neighbor_range - range) > max_range_difference) + { + nr_outliers++; + } + else + { + nr_inliers++; + } + return; + }; + + for (size_t neighbor_idx_nr = 1; neighbor_idx_nr <= half_window; ++neighbor_idx_nr) + { + // check points before index if possible side of the + if (idx >= neighbor_idx_nr) // out of bounds check + { + const size_t neighbor_idx = idx - neighbor_idx_nr; + check_in_out_lier(neighbor_idx); + } + + // check points after the index + const size_t neighbor_idx = idx + neighbor_idx_nr; + if (neighbor_idx < scan.ranges.size()) // Out of bound check + { + check_in_out_lier(neighbor_idx); + } + } + + // more aggressive removal if the number is the same + // ignore filter if both values are equal 0 + if (!(nr_outliers != 0 && nr_inliers != 0) && nr_outliers >= nr_inliers) + { + std::cout << "Filtered out: " << idx << " inliner: " << nr_inliers << "; outliers: " << nr_outliers << std::endl; + return false; + } + + return true; + } +}; + class DistanceWindowValidator : public WindowValidator { virtual bool checkWindowValid(const sensor_msgs::msg::LaserScan& scan, size_t idx, size_t window, double max_range_difference) @@ -215,6 +276,15 @@ class LaserScanSpeckleFilter : public filters::FilterBaseget_logger(), filter_window < 2, "SpackleFilter needs window of at least size 2 to work properly. Please update the parameter and restart the filter!"); + break; + default: break; } From 07ae1cfdc4d3c71d6d29c0cb0d75898aeb8f300e Mon Sep 17 00:00:00 2001 From: Yannic Bachmann Date: Mon, 5 Aug 2024 16:24:06 +0200 Subject: [PATCH 2/8] Added LaserScanMedianFilter --- include/laser_filters/median_spatial_filter.h | 122 ++++++++++++++++++ laser_filters_plugins.xml | 6 + src/laser_scan_filters.cpp | 2 + 3 files changed, 130 insertions(+) create mode 100644 include/laser_filters/median_spatial_filter.h diff --git a/include/laser_filters/median_spatial_filter.h b/include/laser_filters/median_spatial_filter.h new file mode 100644 index 00000000..15b1bc1b --- /dev/null +++ b/include/laser_filters/median_spatial_filter.h @@ -0,0 +1,122 @@ +/********************************************************************* +* Software License Agreement (BSD License) +* +* Copyright (c) 2017, laser_filters authors +* All rights reserved. +* +* Redistribution and use in source and binary forms, with or without +* modification, are permitted provided that the following conditions +* are met: +* +* * Redistributions of source code must retain the above copyright +* notice, this list of conditions and the following disclaimer. +* * Redistributions in binary form must reproduce the above +* copyright notice, this list of conditions and the following +* disclaimer in the documentation and/or other materials provided +* with the distribution. +* * Neither the name of the Willow Garage nor the names of its +* contributors may be used to endorse or promote products derived +* from this software without specific prior written permission. +* +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +* POSSIBILITY OF SUCH DAMAGE. +*********************************************************************/ + +/* +\author Yannic Bachmann +*/ + +#ifndef LASER_SCAN_MEDIAN_SPATIAL_FILTER_H +#define LASER_SCAN_MEDIAN_SPATIAL_FILTER_H + +#include "filters/filter_base.hpp" +#include +#include +#include +#include + +namespace laser_filters +{ + +class LaserScanMedianSpatialFilter : public filters::FilterBase +{ +public: + int window_size_; + + bool configure() + { + // Default window size + window_size_ = 3; + getParam("window_size", window_size_); + + // Ensure window size is positive + if (window_size_ <= 0) + { + throw std::runtime_error("Window size must be positive"); + } + + // Ensure window size is odd + if (window_size_ % 2 == 0) + { + window_size_ += 1; + } + + return true; + } + + virtual ~LaserScanMedianSpatialFilter() {} + + bool update(const sensor_msgs::msg::LaserScan &input_scan, sensor_msgs::msg::LaserScan &filtered_scan) + { + filtered_scan = input_scan; + + int half_window = window_size_ / 2; + std::vector window; + + for (size_t i = 0; i < input_scan.ranges.size(); ++i) + { + window.clear(); + + // Collect points within the window + for (int j = -half_window; j <= half_window; ++j) + { + int index = i + j; + + if (index >= 0 && index < input_scan.ranges.size()) + { + if (!std::isnan(input_scan.ranges[index])) + { + window.push_back(input_scan.ranges[index]); + } + } + } + + if (!window.empty()) + { + // Calculate median + std::sort(window.begin(), window.end()); + filtered_scan.ranges[i] = window[window.size() / 2]; + } + else + { + filtered_scan.ranges[i] = std::numeric_limits::quiet_NaN(); + } + } + + return true; + } +}; + +} // namespace laser_filters + +#endif // LASER_SCAN_MEDIAN_SPATIAL_FILTER_H diff --git a/laser_filters_plugins.xml b/laser_filters_plugins.xml index 71f0be58..a437a48e 100644 --- a/laser_filters_plugins.xml +++ b/laser_filters_plugins.xml @@ -60,6 +60,12 @@ This is a filter that removes points on directions defined in a mask from a laser scan. + + + This is a spatial 1D median filter which filters sensor_msgs::msg::LaserScan messages. + + diff --git a/src/laser_scan_filters.cpp b/src/laser_scan_filters.cpp index 485d3c14..c1791cf6 100644 --- a/src/laser_scan_filters.cpp +++ b/src/laser_scan_filters.cpp @@ -28,6 +28,7 @@ */ #include "laser_filters/median_filter.h" +#include "laser_filters/median_spatial_filter.h" #include "laser_filters/array_filter.h" #include "laser_filters/intensity_filter.h" #include "laser_filters/range_filter.h" @@ -46,6 +47,7 @@ #include // NOLINT PLUGINLIB_EXPORT_CLASS(laser_filters::LaserMedianFilter, filters::FilterBase) +PLUGINLIB_EXPORT_CLASS(laser_filters::LaserScanMedianSpatialFilter, filters::FilterBase) PLUGINLIB_EXPORT_CLASS(laser_filters::LaserArrayFilter, filters::FilterBase) PLUGINLIB_EXPORT_CLASS(laser_filters::LaserScanIntensityFilter, filters::FilterBase) PLUGINLIB_EXPORT_CLASS(laser_filters::LaserScanRangeFilter, filters::FilterBase) From 35a00143c9338c475cbab2ed11dfec28f6659840 Mon Sep 17 00:00:00 2001 From: MAHA Maia Date: Tue, 6 Aug 2024 09:14:32 +0200 Subject: [PATCH 3/8] Revert "Initial version for distance moving window filter." This reverts commit 16118ea4a86e6794a9b3f7ec118c1dc2140e1307. --- include/laser_filters/speckle_filter.h | 72 +------------------------- 1 file changed, 1 insertion(+), 71 deletions(-) diff --git a/include/laser_filters/speckle_filter.h b/include/laser_filters/speckle_filter.h index d35c25bc..3a3ca0a1 100644 --- a/include/laser_filters/speckle_filter.h +++ b/include/laser_filters/speckle_filter.h @@ -40,9 +40,6 @@ #ifndef SPECKLE_FILTER_H #define SPECKLE_FILTER_H -#include -#include - #include #include @@ -52,8 +49,7 @@ namespace laser_filters enum SpeckleFilterType //Enum to select the filtering method { Distance = 0, // Range based filtering (distance between consecutive points - RadiusOutlier = 1, // Euclidean filtering based on radius outlier search - DistanceMovingWindow = 2 // Range based filtering based on number of in- and outliers + RadiusOutlier = 1 // Euclidean filtering based on radius outlier search }; class WindowValidator @@ -62,63 +58,6 @@ class WindowValidator virtual bool checkWindowValid(const sensor_msgs::msg::LaserScan& scan, size_t idx, size_t window, double max_range_difference) = 0; }; -class DistanceMovingWindowValidator : public WindowValidator -{ - virtual bool checkWindowValid(const sensor_msgs::msg::LaserScan& scan, size_t idx, size_t window, double max_range_difference) - { - const size_t half_window = std::floor(window/2.0); - - const float& range = scan.ranges[idx]; - if (std::isnan(range)) { - return false; - } - - size_t nr_inliers = 0; - size_t nr_outliers = 0; - - auto check_in_out_lier = [&](const size_t neighbor_idx) - { - const float& neighbor_range = scan.ranges[neighbor_idx]; - if (std::isnan(neighbor_range) || fabs(neighbor_range - range) > max_range_difference) - { - nr_outliers++; - } - else - { - nr_inliers++; - } - return; - }; - - for (size_t neighbor_idx_nr = 1; neighbor_idx_nr <= half_window; ++neighbor_idx_nr) - { - // check points before index if possible side of the - if (idx >= neighbor_idx_nr) // out of bounds check - { - const size_t neighbor_idx = idx - neighbor_idx_nr; - check_in_out_lier(neighbor_idx); - } - - // check points after the index - const size_t neighbor_idx = idx + neighbor_idx_nr; - if (neighbor_idx < scan.ranges.size()) // Out of bound check - { - check_in_out_lier(neighbor_idx); - } - } - - // more aggressive removal if the number is the same - // ignore filter if both values are equal 0 - if (!(nr_outliers != 0 && nr_inliers != 0) && nr_outliers >= nr_inliers) - { - std::cout << "Filtered out: " << idx << " inliner: " << nr_inliers << "; outliers: " << nr_outliers << std::endl; - return false; - } - - return true; - } -}; - class DistanceWindowValidator : public WindowValidator { virtual bool checkWindowValid(const sensor_msgs::msg::LaserScan& scan, size_t idx, size_t window, double max_range_difference) @@ -276,15 +215,6 @@ class LaserScanSpeckleFilter : public filters::FilterBaseget_logger(), filter_window < 2, "SpackleFilter needs window of at least size 2 to work properly. Please update the parameter and restart the filter!"); - break; - default: break; } From affc0eff86c20057987fcdd1cf03c173e8b6017d Mon Sep 17 00:00:00 2001 From: Yannic Bachmann Date: Tue, 13 Aug 2024 17:36:01 +0200 Subject: [PATCH 4/8] Use nan and infinity values for the median if they make up the majority of the window --- include/laser_filters/median_spatial_filter.h | 84 +++++++++++++------ 1 file changed, 59 insertions(+), 25 deletions(-) diff --git a/include/laser_filters/median_spatial_filter.h b/include/laser_filters/median_spatial_filter.h index 15b1bc1b..7a09bc0a 100644 --- a/include/laser_filters/median_spatial_filter.h +++ b/include/laser_filters/median_spatial_filter.h @@ -44,15 +44,19 @@ #include #include #include +#include +#include namespace laser_filters { class LaserScanMedianSpatialFilter : public filters::FilterBase { -public: +private: int window_size_; +public: + bool configure() { // Default window size @@ -81,36 +85,66 @@ class LaserScanMedianSpatialFilter : public filters::FilterBase window; + std::vector valid_values; + int nan_count = 0; + int neg_inf_count = 0; + int pos_inf_count = 0; - for (size_t i = 0; i < input_scan.ranges.size(); ++i) + for (size_t current_beam_index = 0; current_beam_index < input_scan.ranges.size(); ++current_beam_index) { - window.clear(); + valid_values.clear(); + nan_count = 0; + neg_inf_count = 0; + pos_inf_count = 0; - // Collect points within the window - for (int j = -half_window; j <= half_window; ++j) - { - int index = i + j; + // Collect points within the window + for (int window_offset = -half_window; window_offset <= half_window; ++window_offset) + { + int index = current_beam_index + window_offset; + + if (index >= 0 && index < input_scan.ranges.size()) + { + float value = input_scan.ranges[index]; + + if (std::isnan(value)) + { + nan_count++; + } + else if (value == -std::numeric_limits::infinity()) + { + neg_inf_count++; + } + else if (value == std::numeric_limits::infinity()) + { + pos_inf_count++; + } + else + { + valid_values.push_back(value); + } + } + } - if (index >= 0 && index < input_scan.ranges.size()) + // Determine which set is the largest + // In case of a tie, prioritize valid-values over nan-values over neg-inf-values over pos-inf-values + if (valid_values.size() >= nan_count && valid_values.size() >= neg_inf_count && valid_values.size() >= pos_inf_count) + { + // Sort the valid values and return the median + std::sort(valid_values.begin(), valid_values.end()); + filtered_scan.ranges[current_beam_index] = valid_values[valid_values.size() / 2]; + } + else if (nan_count >= valid_values.size() && nan_count >= neg_inf_count && nan_count >= pos_inf_count) + { + filtered_scan.ranges[current_beam_index] = std::numeric_limits::quiet_NaN(); + } + else if (neg_inf_count >= valid_values.size() && neg_inf_count >= nan_count && neg_inf_count >= pos_inf_count) + { + filtered_scan.ranges[current_beam_index] = -std::numeric_limits::infinity(); + } + else if (pos_inf_count >= valid_values.size() && pos_inf_count >= nan_count && pos_inf_count >= neg_inf_count) { - if (!std::isnan(input_scan.ranges[index])) - { - window.push_back(input_scan.ranges[index]); - } + filtered_scan.ranges[current_beam_index] = std::numeric_limits::infinity(); } - } - - if (!window.empty()) - { - // Calculate median - std::sort(window.begin(), window.end()); - filtered_scan.ranges[i] = window[window.size() / 2]; - } - else - { - filtered_scan.ranges[i] = std::numeric_limits::quiet_NaN(); - } } return true; From 210e1b16c2c36ef4e819b7fcfc9238aa75042c99 Mon Sep 17 00:00:00 2001 From: Yannic Bachmann Date: Tue, 13 Aug 2024 17:37:58 +0200 Subject: [PATCH 5/8] =?UTF-8?q?Don=C2=B4t=20declare+initialize=20window=5F?= =?UTF-8?q?size=20before=20getParam=20call?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- include/laser_filters/median_spatial_filter.h | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/include/laser_filters/median_spatial_filter.h b/include/laser_filters/median_spatial_filter.h index 7a09bc0a..5ea97532 100644 --- a/include/laser_filters/median_spatial_filter.h +++ b/include/laser_filters/median_spatial_filter.h @@ -59,14 +59,13 @@ class LaserScanMedianSpatialFilter : public filters::FilterBaseget_logger(), "Window size must be positive.\n"); + return false; } // Ensure window size is odd From c58a312a871eb46762569e76cb257a4d7506a7cc Mon Sep 17 00:00:00 2001 From: Yannic Bachmann Date: Tue, 13 Aug 2024 17:38:59 +0200 Subject: [PATCH 6/8] Expand description of filter in laser_filters_plugins.xml --- laser_filters_plugins.xml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/laser_filters_plugins.xml b/laser_filters_plugins.xml index a437a48e..421d819e 100644 --- a/laser_filters_plugins.xml +++ b/laser_filters_plugins.xml @@ -1,4 +1,4 @@ - +d @@ -63,7 +63,7 @@ - This is a spatial 1D median filter which filters sensor_msgs::msg::LaserScan messages. + This is a spatial 1D median filter which filters sensor_msgs::msg::LaserScan messages. It smoothes continuous obstacles/walls and removes isolated/scattered noise. Date: Tue, 13 Aug 2024 17:51:31 +0200 Subject: [PATCH 7/8] Fix typo --- laser_filters_plugins.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/laser_filters_plugins.xml b/laser_filters_plugins.xml index 421d819e..624beb6e 100644 --- a/laser_filters_plugins.xml +++ b/laser_filters_plugins.xml @@ -1,4 +1,4 @@ -d + From 044f45d2aab12952230e775749459ef9688f0bbe Mon Sep 17 00:00:00 2001 From: Yannic Bachmann Date: Fri, 23 Aug 2024 15:08:13 +0200 Subject: [PATCH 8/8] Added example launchfiles (xml+py) and example parameter yaml file. Added comment and warning when ensuring window_size_ is odd --- examples/median_spatial_filter.launch.py | 18 ++++++++++++++++++ examples/median_spatial_filter.launch.xml | 5 +++++ examples/median_spatial_filter_example.yaml | 17 +++++++++++++++++ include/laser_filters/median_spatial_filter.h | 2 ++ 4 files changed, 42 insertions(+) create mode 100644 examples/median_spatial_filter.launch.py create mode 100644 examples/median_spatial_filter.launch.xml create mode 100644 examples/median_spatial_filter_example.yaml diff --git a/examples/median_spatial_filter.launch.py b/examples/median_spatial_filter.launch.py new file mode 100644 index 00000000..ad00bcad --- /dev/null +++ b/examples/median_spatial_filter.launch.py @@ -0,0 +1,18 @@ +from launch import LaunchDescription +from launch.substitutions import PathJoinSubstitution +from launch_ros.actions import Node +from ament_index_python.packages import get_package_share_directory + + +def generate_launch_description(): + return LaunchDescription([ + Node( + package="laser_filters", + executable="scan_to_scan_filter_chain", + parameters=[ + PathJoinSubstitution([ + get_package_share_directory("laser_filters"), + "examples", "median_spatial_filter_example.yaml", + ])], + ) + ]) diff --git a/examples/median_spatial_filter.launch.xml b/examples/median_spatial_filter.launch.xml new file mode 100644 index 00000000..a7e1ce29 --- /dev/null +++ b/examples/median_spatial_filter.launch.xml @@ -0,0 +1,5 @@ + + + + + diff --git a/examples/median_spatial_filter_example.yaml b/examples/median_spatial_filter_example.yaml new file mode 100644 index 00000000..bbe95e05 --- /dev/null +++ b/examples/median_spatial_filter_example.yaml @@ -0,0 +1,17 @@ +scan_to_scan_filter_chain: + ros__parameters: + filter1: + name: median_spatial + type: laser_filters/LaserScanMedianSpatialFilter + params: + window_size: 31 + filter2: + name: median_filter + type: laser_filters/LaserArrayFilter + params: + range_filter_chain: + filter1: + name: median + type: filters/MultiChannelMedianFilterFloat + params: + number_of_observations: 3 \ No newline at end of file diff --git a/include/laser_filters/median_spatial_filter.h b/include/laser_filters/median_spatial_filter.h index 5ea97532..3d3aae72 100644 --- a/include/laser_filters/median_spatial_filter.h +++ b/include/laser_filters/median_spatial_filter.h @@ -69,8 +69,10 @@ class LaserScanMedianSpatialFilter : public filters::FilterBaseget_logger(), "Window size must be odd. Automatically setting window_size to %d instead of %d.\n", window_size_+1, window_size_); window_size_ += 1; }