Thomas sonar filter

image-filtering/config/image_filtering_params.yaml

@@ -6,7 +6,7 @@
     output_encoding: "mono8"
 
     filter_params:
-      filter_type: "otsu"
+      filter_type: "temporal_noise"
       flip:
         flip_code: 1
       unsharpening:
@@ -40,3 +40,11 @@
         threshold: 20. # in percent
         maxval: 255.
         invert: true
+      temporal_noise:
+        median_kernel_size: 3
+        power_law_weight: 4.0 # Weight for the gamma function
+        erotion_size: 2
+        dilation_size: 4
+        canny_high: 90 # upper weight for canny edge detection
+        canny_low: 30 # lower weight  ---------- || ----------
+        edge_protection_radius: 2 # Radius for protecting edges

image-filtering/include/image_filtering/lib/filters/all_filters.hpp

@@ -11,6 +11,7 @@
 #include "image_filtering/lib/filters/otsu.hpp"
 #include "image_filtering/lib/filters/overlap.hpp"
 #include "image_filtering/lib/filters/sharpening.hpp"
+#include "image_filtering/lib/filters/temporal_noise.hpp"
 #include "image_filtering/lib/filters/unsharpening.hpp"
 #include "image_filtering/lib/filters/white_balancing.hpp"
 

image-filtering/include/image_filtering/lib/filters/temporal_noise.hpp

@@ -0,0 +1,86 @@
+#ifndef IMAGE_FILTERING__LIB__FILTERS__TEMPORAL_NOISE_HPP_
+#define IMAGE_FILTERING__LIB__FILTERS__TEMPORAL_NOISE_HPP_
+
+#include "abstract_filter_class.hpp"
+#include "image_filtering/lib/utilities.hpp"
+
+/////////////////////////////
+// Sonar noise
+/////////////////////////////
+namespace vortex::image_filtering {
+struct TemporalNoiseParams {
+    int median_kernel_size;
+    double power_law_weight;
+
+    int canny_low;
+    int canny_high;
+    int edge_protection_radius;
+
+    int erotion_size;
+    int dilation_size;
+};
+
+class TemporalNoise : public Filter {
+   public:
+    explicit TemporalNoise(TemporalNoiseParams params)
+        : filter_params_(params) {}
+    void apply_filter(const cv::Mat& original,
+                      cv::Mat& filtered) const override;
+
+   private:
+    TemporalNoiseParams filter_params_;
+    mutable cv::Mat previous_;
+    mutable bool has_prev_{false};
+};
+
+inline void TemporalNoise::apply_filter(const cv::Mat& original,
+                                        cv::Mat& filtered) const {
+    const double power_law_weight = filter_params_.power_law_weight;
+    const int erosion_size = filter_params_.erotion_size;
+    const int dilation_size = filter_params_.dilation_size;
+    const int protect_radius = filter_params_.edge_protection_radius;
+    const int canny_low = filter_params_.canny_low;
+    const int canny_high = filter_params_.canny_high;
+    const int median_kernel_size = filter_params_.median_kernel_size;
+
+    cv::Mat temp;
+    original.copyTo(temp);
+
+    apply_median(temp, temp, median_kernel_size);
+
+    apply_auto_gamma(temp, power_law_weight);
+
+    if (!has_prev_) {
+        temp.copyTo(previous_);
+        temp.copyTo(filtered);
+        has_prev_ = true;
+    } else {
+        cv::addWeighted(temp, 0.5, previous_, 0.5, 0.0, filtered);
+        temp.copyTo(previous_);
+    }
+
+    cv::Mat edges;
+    cv::Canny(filtered, edges, canny_low, canny_high);
+
+    // Thicken mask a bit so we protect the whole line, not just 1px edge.
+    if (protect_radius > 0) {
+        cv::Mat k = cv::getStructuringElement(
+            cv::MORPH_ELLIPSE,
+            cv::Size(2 * protect_radius + 1, 2 * protect_radius + 1));
+        cv::dilate(edges, edges, k);
+    }
+
+    // Invert mask: where NOT edges => safe to morph aggressively.
+    cv::Mat not_edges;
+    cv::bitwise_not(edges, not_edges);
+
+    // Morphing only outside the protected areas
+    cv::Mat morphed = filtered.clone();
+
+    apply_erosion(morphed, morphed, erosion_size);
+    apply_dilation(morphed, morphed, dilation_size);
+
+    morphed.copyTo(filtered, not_edges);
+}
+}  // namespace vortex::image_filtering
+#endif  // IMAGE_FILTERING__LIB__FILTERS__TEMPORAL_NOISE_HPP_

image-filtering/include/image_filtering/lib/typedef.hpp

@@ -22,6 +22,7 @@ enum class FilterType {
     Overlap,
     MedianBinary,
     Binary,
+    TemporalNoise,
 
     Unknown
 };
@@ -39,6 +40,7 @@ static constexpr std::pair<std::string_view, FilterType> kFilterMap[] = {
     {"overlap", FilterType::Overlap},
     {"median_binary", FilterType::MedianBinary},
     {"binary", FilterType::Binary},
+    {"temporal_noise", FilterType::TemporalNoise},
 
     {"unknown", FilterType::Unknown}};
 

image-filtering/src/ros/image_filtering_ros.cpp

@@ -144,6 +144,26 @@ void ImageFilteringNode::set_filter_params() {
             filter_ptr_ = std::make_unique<BinaryThreshold>(params);
             break;
         }
+        case FilterType::TemporalNoise: {
+            TemporalNoiseParams params;
+            params.median_kernel_size = declare_and_get<int>(
+                "filter_params.temporal_noise.median_kernel_size");
+            params.power_law_weight = declare_and_get<double>(
+                "filter_params.temporal_noise.power_law_weight");
+            params.erotion_size = declare_and_get<int>(
+                "filter_params.temporal_noise.erotion_size");
+            params.dilation_size = declare_and_get<int>(
+                "filter_params.temporal_noise.dilation_size");
+            params.canny_high =
+                declare_and_get<int>("filter_params.temporal_noise.canny_high");
+            params.canny_low =
+                declare_and_get<int>("filter_params.temporal_noise.canny_low");
+            params.edge_protection_radius = declare_and_get<int>(
+                "filter_params.temporal_noise.edge_protection_radius");
+
+            filter_ptr_ = std::make_unique<TemporalNoise>(params);
+            break;
+        }
 
         default:;
             if (filter_type == FilterType::Unknown) {