Aruco grid filter

image-filtering/config/image_filtering_params.yaml

@@ -1,12 +1,12 @@
 /**:
   ros__parameters:
-    sub_topic: "/fls_publisher/display_mono"
+    sub_topic: "/cam/image_color"
     pub_topic: "/filtered_image"
-    input_encoding: "mono8"
-    output_encoding: "mono8"
+    input_encoding: "rgb8"
+    output_encoding: "rgb8"
 
     filter_params:
-      filter_type: "temporal_noise"
+      filter_type: "remove_grid"
       flip:
         flip_code: 1
       unsharpening:
@@ -30,6 +30,13 @@
         otsu_segmentation: true
         erosion_size: 2
         dilation_size: 2
+      remove_grid:
+        threshold_green: 0.5
+        threshold_binary: 30
+        inpaint_radius: 1.0
+        rotation: 0
+        height: 400
+        width: 400
       overlap:
         percentage_threshold: 20.0 # Percentage (0-100) to cap the pixel intensity difference
       median_binary: # finds the median of each n x n square around each pixel

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

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

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

@@ -0,0 +1,146 @@
+#ifndef IMAGE_FILTERING__LIB__FILTERS__REMOVE_GRID_HPP_
+#define IMAGE_FILTERING__LIB__FILTERS__REMOVE_GRID_HPP_
+
+#include <spdlog/spdlog.h>
+#include <algorithm>
+#include <opencv2/imgproc.hpp>
+#include <opencv2/photo.hpp>
+#include <vector>
+
+#include "abstract_filter_class.hpp"
+
+namespace vortex::image_filtering {
+
+struct RemoveGridParams {
+    double threshold_green;
+    int threshold_binary;
+    double inpaint_radius;
+    int rotation;
+    int height;
+    int width;
+};
+
+class RemoveGrid : public Filter {
+   public:
+    explicit RemoveGrid(RemoveGridParams params) : params_(params) {}
+
+    void apply_filter(const cv::Mat& original,
+                      cv::Mat& filtered) const override;
+
+   private:
+    RemoveGridParams params_;
+};
+
+inline void RemoveGrid::apply_filter(const cv::Mat& original,
+                                     cv::Mat& filtered) const {
+    if (original.empty()) {
+        spdlog::error("RemoveGrid: input image is empty");
+        return;
+    }
+
+    if (original.type() != CV_8UC3) {
+        spdlog::error("RemoveGrid: expected CV_8UC3 image");
+        original.copyTo(filtered);
+        return;
+    }
+
+    // Rotate directly into cropped output
+    int crop_w = std::min(params_.width, original.cols);
+    int crop_h = std::min(params_.height, original.rows);
+
+    if (crop_w != params_.width || crop_h != params_.height) {
+        spdlog::warn(
+            "RemoveGrid: requested crop size (width={}, height={}) does not "
+            "fit "
+            "within original image size (width={}, height={}); clamping to "
+            "(width={}, height={})",
+            params_.width, params_.height, original.cols, original.rows, crop_w,
+            crop_h);
+    }
+
+    const cv::Point2f center_src(
+        original.cols * 0.5f, original.rows * 0.5f);  // center of source image
+    const cv::Point2f center_dst(crop_w * 0.5f,
+                                 crop_h * 0.5f);  // center of destination image
+
+    cv::Mat M = cv::getRotationMatrix2D(center_src, params_.rotation,
+                                        1.0);  // affine matrix
+    // Ensure type for at<double>
+    if (M.type() != CV_64F) {
+        M.convertTo(M, CV_64F);
+    }
+
+    // Shift translation so original center maps to cropped center
+    M.at<double>(0, 2) += (center_dst.x - center_src.x);
+    M.at<double>(1, 2) += (center_dst.y - center_src.y);
+
+    cv::Mat cropped;
+    cv::warpAffine(original, cropped, M, cv::Size(crop_w, crop_h),
+                   cv::INTER_NEAREST, cv::BORDER_CONSTANT, cv::Scalar(0, 0, 0));
+
+    // Extract green grid mask
+    cv::Mat cropped_f;
+    cropped.convertTo(cropped_f, CV_32F, 1.0 / 255.0);
+
+    std::vector<cv::Mat> ch(3);  // make a vector for BGR
+    cv::split(cropped_f, ch);    // BGR
+
+    cv::Mat sum = ch[0] + ch[1] + ch[2] + 1e-6f;  // avoid division by zero
+    for (auto& c : ch)
+        c /= sum;  // normalized color values
+
+    // mask the green channel
+    cv::Mat grid_mask = (ch[1] > params_.threshold_green);
+    static const cv::Mat kernel = cv::Mat::ones(3, 3, CV_8U);
+    cv::Mat dilated;
+    cv::dilate(grid_mask, dilated, kernel);
+
+    // prevent border leak
+    dilated.row(0).setTo(0);
+    dilated.row(dilated.rows - 1).setTo(0);
+    dilated.col(0).setTo(0);
+    dilated.col(dilated.cols - 1).setTo(0);
+
+    if (cv::countNonZero(dilated) == 0) {
+        // If no grid detected, leave image unchanged
+        original.copyTo(filtered);
+        return;
+    }
+
+    // Inpaint grid
+    cv::Mat inpainted;
+    cv::inpaint(cropped, dilated, inpainted, params_.inpaint_radius,
+                cv::INPAINT_TELEA);
+
+    // Binary threshold (on cropped ROI)
+    cv::Mat thresh_gray;
+    apply_fixed_threshold(inpainted, thresh_gray, params_.threshold_binary,
+                          false);
+
+    cv::Mat thresh_bgr;
+    cv::cvtColor(thresh_gray, thresh_bgr, cv::COLOR_GRAY2BGR);
+
+    // Undo rotation & merge (using M)
+    cv::Mat invM;
+    cv::invertAffineTransform(M, invM);
+
+    // Warp ROI result back into full-size overlay
+    cv::Mat overlay_full;
+    cv::warpAffine(thresh_bgr, overlay_full, invM, original.size(),
+                   cv::INTER_NEAREST, cv::BORDER_CONSTANT, cv::Scalar(0, 0, 0));
+
+    // Warp a mask the same way (so black pixels are copied too)
+    cv::Mat local_mask(thresh_bgr.rows, thresh_bgr.cols, CV_8U,
+                       cv::Scalar(255));
+    cv::Mat mask_full;
+    cv::warpAffine(local_mask, mask_full, invM, original.size(),
+                   cv::INTER_NEAREST, cv::BORDER_CONSTANT, cv::Scalar(0));
+
+    // Merge into the original image
+    filtered = original.clone();
+    overlay_full.copyTo(filtered, mask_full);
+}
+
+}  // namespace vortex::image_filtering
+
+#endif  // IMAGE_FILTERING__LIB__FILTERS__REMOVE_GRID_HPP_

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

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

image-filtering/src/ros/image_filtering_ros.cpp

@@ -165,6 +165,31 @@ void ImageFilteringNode::set_filter_params() {
             break;
         }
 
+        case FilterType::RemoveGrid: {
+            RemoveGridParams params;
+
+            params.threshold_green = declare_and_get<double>(
+                "filter_params.remove_grid.threshold_green");
+
+            params.threshold_binary = declare_and_get<double>(
+                "filter_params.remove_grid.threshold_binary");
+
+            params.inpaint_radius = declare_and_get<double>(
+                "filter_params.remove_grid.inpaint_radius");
+
+            params.rotation =
+                declare_and_get<int>("filter_params.remove_grid.rotation");
+
+            params.width =
+                declare_and_get<int>("filter_params.remove_grid.width");
+
+            params.height =
+                declare_and_get<int>("filter_params.remove_grid.height");
+
+            filter_ptr_ = std::make_unique<RemoveGrid>(params);
+            break;
+        }
+
         default:;
             if (filter_type == FilterType::Unknown) {
                 spdlog::warn(fmt::format(fmt::fg(fmt::rgb(200, 180, 50)),