diff --git a/image-filtering/config/image_filtering_params.yaml b/image-filtering/config/image_filtering_params.yaml index d13636a..6c03be1 100644 --- a/image-filtering/config/image_filtering_params.yaml +++ b/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" @@ -51,12 +51,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 hsv_val_high depending on brightness + hsv_sat_high: 255 + hsv_val_low: 50 # Adjust together with hsv_sat_low 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 diff --git a/image-filtering/include/image_filtering/lib/filters/binary_threshold.hpp b/image-filtering/include/image_filtering/lib/filters/binary_threshold.hpp index c33b28a..fd1fe0e 100644 --- a/image-filtering/include/image_filtering/lib/filters/binary_threshold.hpp +++ b/image-filtering/include/image_filtering/lib/filters/binary_threshold.hpp @@ -30,7 +30,9 @@ 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; // Params is in percent while cv::threshold + // takes in values from 0 to 255 const double maxval = this->filter_params_.maxval; const bool invert = this->filter_params_.invert; diff --git a/image-filtering/include/image_filtering/lib/filters/remove_grid.hpp b/image-filtering/include/image_filtering/lib/filters/remove_grid.hpp index 3bf1ba4..f574636 100644 --- a/image-filtering/include/image_filtering/lib/filters/remove_grid.hpp +++ b/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,19 +64,13 @@ 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 - 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(0, 2) += (center_dst.x - center_src.x); M.at(1, 2) += (center_dst.y - center_src.y); @@ -78,65 +78,62 @@ inline void RemoveGrid::apply_filter(const cv::Mat& original, 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 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); } diff --git a/image-filtering/src/ros/image_filtering_ros.cpp b/image-filtering/src/ros/image_filtering_ros.cpp index 704907e..546f159 100644 --- a/image-filtering/src/ros/image_filtering_ros.cpp +++ b/image-filtering/src/ros/image_filtering_ros.cpp @@ -238,14 +238,14 @@ void ImageFilteringNode::set_filter_params() { case FilterType::RemoveGrid: { RemoveGridParams params; - params.threshold_green = declare_and_get( - "filter_params.remove_grid.threshold_green"); + params.inpaint_radius = declare_and_get( + "filter_params.remove_grid.inpaint_radius"); params.threshold_binary = declare_and_get( "filter_params.remove_grid.threshold_binary"); - params.inpaint_radius = declare_and_get( - "filter_params.remove_grid.inpaint_radius"); + params.use_binary_threshold = declare_and_get( + "filter_params.remove_grid.use_binary_threshold"); params.rotation = declare_and_get("filter_params.remove_grid.rotation"); @@ -256,6 +256,19 @@ void ImageFilteringNode::set_filter_params() { params.height = declare_and_get("filter_params.remove_grid.height"); + params.hsv_hue_low = + declare_and_get("filter_params.remove_grid.hsv_hue_low"); + params.hsv_hue_high = + declare_and_get("filter_params.remove_grid.hsv_hue_high"); + params.hsv_sat_low = + declare_and_get("filter_params.remove_grid.hsv_sat_low"); + params.hsv_sat_high = + declare_and_get("filter_params.remove_grid.hsv_sat_high"); + params.hsv_val_low = + declare_and_get("filter_params.remove_grid.hsv_val_low"); + params.hsv_val_high = + declare_and_get("filter_params.remove_grid.hsv_val_high"); + filter_ptr_ = std::make_unique(params); break; }