diff --git a/image-filtering/config/image_filtering_params.yaml b/image-filtering/config/image_filtering_params.yaml index 581c2f4..6e0ac28 100644 --- a/image-filtering/config/image_filtering_params.yaml +++ b/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 diff --git a/image-filtering/include/image_filtering/lib/filters/all_filters.hpp b/image-filtering/include/image_filtering/lib/filters/all_filters.hpp index dc0e7ee..d3f57fc 100644 --- a/image-filtering/include/image_filtering/lib/filters/all_filters.hpp +++ b/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" diff --git a/image-filtering/include/image_filtering/lib/filters/remove_grid.hpp b/image-filtering/include/image_filtering/lib/filters/remove_grid.hpp new file mode 100644 index 0000000..3bf1ba4 --- /dev/null +++ b/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 +#include +#include +#include +#include + +#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 + 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); + + 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 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_ diff --git a/image-filtering/include/image_filtering/lib/typedef.hpp b/image-filtering/include/image_filtering/lib/typedef.hpp index ad143bc..195f38b 100644 --- a/image-filtering/include/image_filtering/lib/typedef.hpp +++ b/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 kFilterMap[] = { {"overlap", FilterType::Overlap}, {"median_binary", FilterType::MedianBinary}, {"binary", FilterType::Binary}, + {"remove_grid", FilterType::RemoveGrid}, {"temporal_noise", FilterType::TemporalNoise}, {"unknown", FilterType::Unknown}}; diff --git a/image-filtering/src/ros/image_filtering_ros.cpp b/image-filtering/src/ros/image_filtering_ros.cpp index 74cd865..9c91d27 100644 --- a/image-filtering/src/ros/image_filtering_ros.cpp +++ b/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( + "filter_params.remove_grid.threshold_green"); + + 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.rotation = + declare_and_get("filter_params.remove_grid.rotation"); + + params.width = + declare_and_get("filter_params.remove_grid.width"); + + params.height = + declare_and_get("filter_params.remove_grid.height"); + + filter_ptr_ = std::make_unique(params); + break; + } + default:; if (filter_type == FilterType::Unknown) { spdlog::warn(fmt::format(fmt::fg(fmt::rgb(200, 180, 50)),