Fix/remove grid

image-filtering/config/image_filtering_params.yaml

@@ -1,6 +1,6 @@
 /**:
   ros__parameters:
-    sub_topic: "/cam/image_color"
+    sub_topic: "/realsense_d555/color/image_rect"
     pub_topic: "/filtered_image"
     input_encoding: "rgb8"
     output_encoding: "rgb8"
@@ -31,12 +31,18 @@
         erosion_size: 2
         dilation_size: 2
       remove_grid:
-        threshold_green: 0.5
-        threshold_binary: 30
-        inpaint_radius: 1.0
+        inpaint_radius: 1.0 # Increase when it get blurry close
+        threshold_binary: 90 # Adjust depending on how many black patches obstructs around the marker
+        use_binary_threshold: true
         rotation: 0
         height: 400
         width: 400
+        hsv_hue_low: 9 # This value is calibrated for yellow-green grids. Increase this value to ~40 if the grid is more orange
+        hsv_hue_high: 40 # Increase this value to ~80 if more orange
+        hsv_sat_low: 50 # Adjust together with value depending on brightness
+        hsv_sat_high: 255
+        hsv_val_low: 50 # Adjust together with saturation depending on brightness
+        hsv_val_high: 255
       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/binary_threshold.hpp

@@ -30,7 +30,7 @@ inline void BinaryThreshold::apply_filter(const cv::Mat& original,
                                           cv::Mat& filtered) const {
     CV_Assert(!original.empty());
 
-    const double thresh = this->filter_params_.threshold;
+    const double thresh = this->filter_params_.threshold * 255.0 / 100.0;
     const double maxval = this->filter_params_.maxval;
     const bool invert = this->filter_params_.invert;
 

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

@@ -12,12 +12,18 @@
 namespace vortex::image_filtering {
 
 struct RemoveGridParams {
-    double threshold_green;
-    int threshold_binary;
     double inpaint_radius;
+    int threshold_binary;
+    bool use_binary_threshold;
     int rotation;
     int height;
     int width;
+    int hsv_hue_low;
+    int hsv_hue_high;
+    int hsv_sat_low;
+    int hsv_sat_high;
+    int hsv_val_low;
+    int hsv_val_high;
 };
 
 class RemoveGrid : public Filter {
@@ -44,9 +50,9 @@ inline void RemoveGrid::apply_filter(const cv::Mat& original,
         return;
     }
 
-    // Rotate directly into cropped output
-    int crop_w = std::min(params_.width, original.cols);
-    int crop_h = std::min(params_.height, original.rows);
+    // Rotate/crop to ROI
+    const int crop_w = std::min(params_.width, original.cols);
+    const int crop_h = std::min(params_.height, original.rows);
 
     if (crop_w != params_.width || crop_h != params_.height) {
         spdlog::warn(
@@ -58,85 +64,76 @@ inline void RemoveGrid::apply_filter(const cv::Mat& original,
             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
+    const cv::Point2f center_src(original.cols * 0.5f, original.rows * 0.5f);
+    const cv::Point2f center_dst(crop_w * 0.5f, crop_h * 0.5f);
 
-    cv::Mat M = cv::getRotationMatrix2D(center_src, params_.rotation,
-                                        1.0);  // affine matrix
-    // Ensure type for at<double>
-    if (M.type() != CV_64F) {
+    cv::Mat M = cv::getRotationMatrix2D(center_src, params_.rotation, 1.0);
+    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);
+    // Detect yellow grid bars via HSV hue range (input is rgb8)
+    cv::Mat hsv;
+    cv::cvtColor(cropped, hsv, cv::COLOR_RGB2HSV);
+    cv::Mat grid_mask;
+    cv::inRange(hsv,
+                cv::Scalar(params_.hsv_hue_low, params_.hsv_sat_low,
+                           params_.hsv_val_low),
+                cv::Scalar(params_.hsv_hue_high, params_.hsv_sat_high,
+                           params_.hsv_val_high),
+                grid_mask);
+
+    // Dilate mask to fully cover grid bar edges
     static const cv::Mat kernel = cv::Mat::ones(3, 3, CV_8U);
     cv::Mat dilated;
     cv::dilate(grid_mask, dilated, kernel);
 
-    // prevent border leak
+    // 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;
     }
 
+    // Optionally apply binary threshold before inpainting
+    cv::Mat inpaint_src;
+    if (params_.use_binary_threshold) {
+        cv::Mat thresh_gray;
+        apply_fixed_threshold(cropped, thresh_gray, params_.threshold_binary,
+                              false);
+        cv::cvtColor(thresh_gray, inpaint_src, cv::COLOR_GRAY2BGR);
+    } else {
+        inpaint_src = cropped;
+    }
+
     // Inpaint grid
     cv::Mat inpainted;
-    cv::inpaint(cropped, dilated, inpainted, params_.inpaint_radius,
+    cv::inpaint(inpaint_src, 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)
+    // Warp inpainted ROI back into full-size image
     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::warpAffine(inpainted, 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 local_mask(inpainted.rows, inpainted.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);
 }

image-filtering/src/ros/image_filtering_ros.cpp

@@ -168,14 +168,14 @@ void ImageFilteringNode::set_filter_params() {
         case FilterType::RemoveGrid: {
             RemoveGridParams params;
 
-            params.threshold_green = declare_and_get<double>(
-                "filter_params.remove_grid.threshold_green");
+            params.inpaint_radius = declare_and_get<double>(
+                "filter_params.remove_grid.inpaint_radius");
 
-            params.threshold_binary = declare_and_get<double>(
+            params.threshold_binary = declare_and_get<int>(
                 "filter_params.remove_grid.threshold_binary");
 
-            params.inpaint_radius = declare_and_get<double>(
-                "filter_params.remove_grid.inpaint_radius");
+            params.use_binary_threshold = declare_and_get<bool>(
+                "filter_params.remove_grid.use_binary_threshold");
 
             params.rotation =
                 declare_and_get<int>("filter_params.remove_grid.rotation");
@@ -186,6 +186,19 @@ void ImageFilteringNode::set_filter_params() {
             params.height =
                 declare_and_get<int>("filter_params.remove_grid.height");
 
+            params.hsv_hue_low =
+                declare_and_get<int>("filter_params.remove_grid.hsv_hue_low");
+            params.hsv_hue_high =
+                declare_and_get<int>("filter_params.remove_grid.hsv_hue_high");
+            params.hsv_sat_low =
+                declare_and_get<int>("filter_params.remove_grid.hsv_sat_low");
+            params.hsv_sat_high =
+                declare_and_get<int>("filter_params.remove_grid.hsv_sat_high");
+            params.hsv_val_low =
+                declare_and_get<int>("filter_params.remove_grid.hsv_val_low");
+            params.hsv_val_high =
+                declare_and_get<int>("filter_params.remove_grid.hsv_val_high");
+
             filter_ptr_ = std::make_unique<RemoveGrid>(params);
             break;
         }