From 0063195f984f7e0ecacc63cc09977ae4c8cc1d05 Mon Sep 17 00:00:00 2001 From: jorgenfj <144696109+jorgenfj@users.noreply.github.com> Date: Sun, 26 May 2024 15:33:41 +0200 Subject: [PATCH 01/53] Initial commit --- LICENSE | 21 +++++++++++++++++++++ 1 file changed, 21 insertions(+) create mode 100644 LICENSE diff --git a/LICENSE b/LICENSE new file mode 100644 index 0000000..a1def54 --- /dev/null +++ b/LICENSE @@ -0,0 +1,21 @@ +MIT License + +Copyright (c) 2024 Vortex NTNU + +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: + +The above copyright notice and this permission notice shall be included in all +copies or substantial portions of the Software. + +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +SOFTWARE. From 11d9ab74ee51a005314814fd1cb6b87bae73f6c2 Mon Sep 17 00:00:00 2001 From: jorgenfj Date: Sun, 26 May 2024 16:07:05 +0200 Subject: [PATCH 02/53] initial commit --- README.md | 124 ++++++++++++++++ image-filtering/CMakeLists.txt | 78 ++++++++++ .../image_filters/image_filtering_ros.hpp | 111 +++++++++++++++ .../image_filters/image_processing.hpp | 110 ++++++++++++++ .../launch/image_filtering_launch.py | 16 +++ image-filtering/package.xml | 24 ++++ .../params/image_filtering_params.yaml | 23 +++ image-filtering/src/image_filtering_node.cpp | 13 ++ image-filtering/src/image_filtering_ros.cpp | 134 ++++++++++++++++++ image-filtering/src/image_processing.cpp | 104 ++++++++++++++ image-filtering/test/CMakeLists.txt | 21 +++ .../test/image_processing_test.cpp | 0 12 files changed, 758 insertions(+) create mode 100644 README.md create mode 100644 image-filtering/CMakeLists.txt create mode 100644 image-filtering/include/image_filters/image_filtering_ros.hpp create mode 100644 image-filtering/include/image_filters/image_processing.hpp create mode 100644 image-filtering/launch/image_filtering_launch.py create mode 100644 image-filtering/package.xml create mode 100644 image-filtering/params/image_filtering_params.yaml create mode 100644 image-filtering/src/image_filtering_node.cpp create mode 100644 image-filtering/src/image_filtering_ros.cpp create mode 100644 image-filtering/src/image_processing.cpp create mode 100644 image-filtering/test/CMakeLists.txt create mode 100644 image-filtering/test/image_processing_test.cpp diff --git a/README.md b/README.md new file mode 100644 index 0000000..4ec8fd0 --- /dev/null +++ b/README.md @@ -0,0 +1,124 @@ +# Image Filtering Node + +The `image_filtering_node` is a ROS 2 node developed in the `vortex::image_filters` namespace. It is designed to subscribe to image topics, apply various image filters using OpenCV, and publish the filtered images back to ROS. + +## Features + +- **Multiple Filters**: Supports sharpening, unsharpening, eroding, dilating, white balancing, and a custom "ebus" filter. +- **Dynamic Reconfiguration**: Allows for runtime changes to filter parameters and subscribed image topics via ROS 2 parameters. +- **Parameter-Driven Configuration**: Configures filter types and specific attributes through ROS 2 parameters. + +## Configuration + +Configure the node using ROS 2 parameters: + +- **`image_topic`**: Topic from which the node will subscribe to image messages. +- **`filter_params.filter_type`**: Specifies the filter to apply. Current options include: + - `nofilter` + - `sharpening` + - `unsharpening` + - `eroding` + - `dilating` + - `white_balancing` + - `ebus` +- **Other filter-specific parameters** such as `blur_size`, `size`, `contrast_percentage`, can be configured through parameter `filter_params`.{filter_name}.{param_name} + +Parameters can be set through a YAML file or dynamically adjusted at runtime. + +## Additional filters + +## Implementing New Filters + +To extend the functionality of the `image_filtering_node` by adding new filters, follow these steps to ensure compatibility and integration with the existing codebase: + +### Step 1: Define Filter Parameters + +Each filter should have its own set of parameters encapsulated in a structure. Define this structure within the `vortex::image_filters` namespace. + +```cpp +struct YourFilterParams { + // Add necessary parameters here + int example_param; +}; +``` + +### Step 2: Add to FilterParams Structure + +Integrate your new filter parameters structure into the existing `FilterParams` structure. This allows the `apply_filter` function to access the parameters specific to your filter. + +```cpp +struct FilterParams { + std::string filter_type; + UnsharpeningFilterParams unsharpening; + ErodingFilterParams eroding; + DilatingFilterParams dilating; + WhiteBalancingFilterParams white_balancing; + EbusFilterParams ebus; + YourFilterParams your_filter; // Add your filter params here +}; +``` + +### Step 3: Create the Filter Function + +Implement your filter function. This function should take the `cv::Mat` objects for the input and output images and a `const FilterParams&` which includes your specific filter parameters. Make sure to use your parameter structure within this function. + +```cpp +void your_filter_function(const cv::Mat &original, cv::Mat &filtered, const FilterParams& filter_params) { + // Access your filter-specific parameters like this: + int example_param = filter_params.your_filter.example_param; + + // Implement your filtering logic here +} +``` + +### Step 4: Register the Filter Function + +Add an entry to the `filter_functions` map for your new filter. This step is crucial as it links the filter name (as a string) to the corresponding filter function pointer. + +```cpp +std::map filter_functions = { + {"no_filter", no_filter}, + {"sharpening", sharpening_filter}, + {"unsharpening", unsharpening_filter}, + {"eroding", eroding_filter}, + {"dilating", dilating_filter}, + {"white_balancing", white_balance_filter}, + {"ebus", ebus_filter}, + {"your_filter", your_filter_function} // Add your filter here +}; +``` + +### Step 5: Declare and Assign Parameters + +Declare the new filter parameters in the ROS 2 node constructor and assign these parameters to the `FilterParams` structure within the `set_filter_params` function. + +#### In the Node Constructor + +In the constructor of your ROS 2 node, declare each of the new filter parameters using the `declare_parameter` function. This sets the default values and prepares the node to accept these parameters at runtime through command line or a YAML configuration file. + +```cpp +ImageFilteringNode::ImageFilteringNode() : Node("image_filtering_node") +{ + this->declare_parameter("filter_params.your_filter.example_param", "default_value"); + ... + // Other parameters declarations +} +``` + + +#### In the set_filter_params Function + +In the set_filter_params function, retrieve and assign the parameters to the corresponding fields in the FilterParams structure. Ensure to handle cases where the parameter might not be set or provided. + +```cpp + +void ImageFilteringNode::set_filter_params(){ + FilterParams params = filter_params_; // assuming filter_params_ is already defined in your class + + params.your_filter.example_param = this->get_parameter("filter_params.your_filter.example_param").as_string(); + ... + // Retrieve other parameters and handle cases where parameters might not be provided + filter_params_ = params; // Update the filter parameters structure + RCLCPP_INFO(this->get_logger(), "Filter parameters updated for your_filter."); +} +``` \ No newline at end of file diff --git a/image-filtering/CMakeLists.txt b/image-filtering/CMakeLists.txt new file mode 100644 index 0000000..cc6f6bd --- /dev/null +++ b/image-filtering/CMakeLists.txt @@ -0,0 +1,78 @@ +cmake_minimum_required(VERSION 3.8) +project(image_filtering) + +# === C++ standard === +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 20) +endif() + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(rclcpp_components REQUIRED) +find_package(OpenCV 4.5.4 REQUIRED) +find_package(sensor_msgs REQUIRED) +find_package(cv_bridge REQUIRED) + +include_directories(include) + +set(LIB_NAME "${PROJECT_NAME}_component") + +add_library(${LIB_NAME} SHARED + src/image_processing.cpp + src/image_filtering_ros.cpp +) + +target_link_libraries(${LIB_NAME} PUBLIC + ${OpenCV_LIBS} +) + +target_include_directories(${LIB_NAME} PUBLIC + $ + $ + ${OpenCV_INCLUDE_DIRS} +) + +ament_target_dependencies(${LIB_NAME} PUBLIC + rclcpp + rclcpp_components + cv_bridge + sensor_msgs +) + +rclcpp_components_register_node( + ${LIB_NAME} + PLUGIN "vortex::image_processing::ImageFilteringNode" + EXECUTABLE ${PROJECT_NAME}_node +) + +# Export the target for other packages to use +ament_export_targets(export_${LIB_NAME}) + +install(TARGETS ${LIB_NAME} + EXPORT export_${LIB_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin +) + +install( + DIRECTORY include/ + DESTINATION include +) + +install(DIRECTORY + launch + params + DESTINATION share/${PROJECT_NAME}/ +) + +if(BUILD_TESTING) + add_subdirectory(test) +endif() + +ament_package() diff --git a/image-filtering/include/image_filters/image_filtering_ros.hpp b/image-filtering/include/image_filters/image_filtering_ros.hpp new file mode 100644 index 0000000..d88a3a4 --- /dev/null +++ b/image-filtering/include/image_filters/image_filtering_ros.hpp @@ -0,0 +1,111 @@ +#ifndef IMAGE_FILTERING_ROS_HPP +#define IMAGE_FILTERING_ROS_HPP + +#include +#include +#include +#include +#include "image_processing.hpp" +#include + + + +namespace vortex::image_processing +{ +class ImageFilteringNode : public rclcpp::Node{ + + +public: + explicit ImageFilteringNode(const rclcpp::NodeOptions & options); + ~ImageFilteringNode(){}; + + +private: + /** + * @brief Subscribes to image topic + */ + rclcpp::Subscription::SharedPtr image_sub_; + /** + * @brief Publishes the filtered image + */ + rclcpp::Publisher::SharedPtr image_pub_; + + /** + * @brief Check and subscribe to image if not yet subscribed. Allows for dynaminc reconfiguration of image topic. + * If a new topic is set, the old subscription is cancelled and a new one is bound to the callback function. + * + */ + void check_and_subscribe_to_image_topic(); + + /** + * @brief Set the filter parameters for the FilterParams struct. + * + */ + void set_filter_params(); + + /** + * @brief Initialize the parameter handler and a parameter event callback. + * + */ + void initialize_parameter_handler(); + /** + * @brief Callback function for parameter events. + * Checks for parameter changes that matches the nodes' namespace and invokes the relevant initializer functions to update member variables. + * + * @param event The parameter event. + */ + void on_parameter_event(const rcl_interfaces::msg::ParameterEvent &event); + + /** + * @brief Manages parameter events for the node. + * + * This handle is used to set up a mechanism to listen for and react to changes in parameters. + * Parameters can be used to configure the node's operational behavior dynamically, + * allowing adjustments without altering the code. The `param_handler_` is responsible for + * registering callbacks that are triggered on parameter changes, providing a centralized + * management system within the node for such events. + */ + std::shared_ptr param_handler_; + + /** + * @brief Handle to the registration of the parameter event callback. + * + * Represents a token or reference to the specific callback registration made with + * the parameter event handler (`param_handler_`). This handle allows for management + * of the lifecycle of the callback, such as removing the callback if it's no longer needed. + * It ensures that the node can respond to parameter changes with the registered callback + * in an efficient and controlled manner. + */ + rclcpp::ParameterEventCallbackHandle::SharedPtr param_cb_handle_; + + /** + * @brief Callback function for image topic + * + * @param msg The image message + */ + void image_callback(const sensor_msgs::msg::Image::SharedPtr msg); + + /** + * @brief The image topic to subscribe to + * + */ + std::string image_topic_; + + /** + * @brief The filter parameters + * + */ + FilterParams filter_params_; + + /** + * @brief filter to apply + * + */ + std::string filter_; + +}; + +} // namespace vortex::image_processing + + +#endif // IMAGE_FILTERING_ROS_HPP \ No newline at end of file diff --git a/image-filtering/include/image_filters/image_processing.hpp b/image-filtering/include/image_filters/image_processing.hpp new file mode 100644 index 0000000..e87f395 --- /dev/null +++ b/image-filtering/include/image_filters/image_processing.hpp @@ -0,0 +1,110 @@ +#ifndef IMAGE_PROCESSING_HPP +#define IMAGE_PROCESSING_HPP + +#include +#include +#include +#include +#include +#include +#include + +namespace vortex::image_processing +{ + +struct FlipParams { + int flip_code; +}; +struct UnsharpeningParams { + int blur_size; +}; + +struct ErodingParams { + int size; +}; + +struct DilatingParams { + int size; +}; + +struct WhiteBalancingParams { + double contrast_percentage; +}; + +struct EbusParams { + int erosion_size; + int blur_size; + int mask_weight; +}; + +struct FilterParams { + FlipParams flip; + UnsharpeningParams unsharpening; + ErodingParams eroding; + DilatingParams dilating; + WhiteBalancingParams white_balancing; + EbusParams ebus; +}; + +typedef void (*FilterFunction)(const FilterParams&, const cv::Mat&, cv::Mat&); + +/** + * Reads the filter_type from the FilterParams struct + * and calls the appropriate filter function from the filter_functions map. + */ +void apply_filter(const std::string& filter, const FilterParams& params, const cv::Mat &original, cv::Mat &filtered); + +/** + * No filter, just copy the image + */ +void no_filter(const FilterParams& params, const cv::Mat &original, cv::Mat &modified); + +/** + * Flips the image + */ +void flip_filter(const FilterParams& params, const cv::Mat &original, cv::Mat &modified); + +/** + * Makes edges harder + */ +void sharpening_filter(const FilterParams& params, const cv::Mat &original, cv::Mat &modified); + +/** + * Makes edges harder in a smarter way + */ +void unsharpening_filter(const FilterParams& params, const cv::Mat &original, cv::Mat &modified); + +/** + * Expands the dark areas of the image + */ +void eroding_filter(const FilterParams& params, const cv::Mat &original, cv::Mat &modified); + +/** + * Expands the bright areas of the image + */ +void dilating_filter(const FilterParams& params, const cv::Mat &original, cv::Mat &modified); + +/** + * White Balancing Filter + */ +void white_balance_filter(const FilterParams& params, const cv::Mat &original, cv::Mat &filtered); + +/** + * A filter that worked well-ish in the mc-lab conditions easter 2023 + * Uses a combination of dilation and unsharpening + */ +void ebus_filter(const FilterParams& params, const cv::Mat &original, cv::Mat &filtered); + +const static std::map filter_functions ={ + {"no_filter", no_filter}, + {"flip", flip_filter}, + {"sharpening", sharpening_filter}, + {"unsharpening", unsharpening_filter}, + {"eroding", eroding_filter}, + {"dilating", dilating_filter}, + {"white_balancing", white_balance_filter}, + {"ebus", ebus_filter}}; + + +} // namespace image_processing +#endif // IMAGE_PROCESSING_HPP \ No newline at end of file diff --git a/image-filtering/launch/image_filtering_launch.py b/image-filtering/launch/image_filtering_launch.py new file mode 100644 index 0000000..74c4feb --- /dev/null +++ b/image-filtering/launch/image_filtering_launch.py @@ -0,0 +1,16 @@ +import os +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch_ros.actions import Node + +def generate_launch_description(): + image_filtering_node = Node( + package='image_filtering', + executable='image_filtering_node', + name='image_filtering_node', + parameters=[os.path.join(get_package_share_directory('image_filtering'),'params','image_filtering_params.yaml')], + output='screen', + ) + return LaunchDescription([ + image_filtering_node + ]) \ No newline at end of file diff --git a/image-filtering/package.xml b/image-filtering/package.xml new file mode 100644 index 0000000..99a87ad --- /dev/null +++ b/image-filtering/package.xml @@ -0,0 +1,24 @@ + + + + image_filtering + 0.0.1 + The image filtering package + Jorgen Fjermedal + MIT + + ament_cmake + + ament_lint_auto + ament_lint_common + + rclcpp_components + cv_bridge + sensor_msgs + image_transport + rclcpp + + + ament_cmake + + diff --git a/image-filtering/params/image_filtering_params.yaml b/image-filtering/params/image_filtering_params.yaml new file mode 100644 index 0000000..db69b5e --- /dev/null +++ b/image-filtering/params/image_filtering_params.yaml @@ -0,0 +1,23 @@ +image_filtering_node: + ros__parameters: + sub_topic: "/flir_camera/image_raw" + pub_topic: "/filtered_image" + filter_params: + filter_type: "ebus" + flip: + flip_code: 1 + unsharpening: + blur_size: 8 + erosion: + size: 1 + dilation: + size: 1 + white_balancing: + contrast_percentage: 0.8 + ebus: + erosion_size: 2 + blur_size: 30 + mask_weight: 5 + +# Filter params should reflect the FilterParams struct +# defined in /include/image_filters/image_processing.hpp \ No newline at end of file diff --git a/image-filtering/src/image_filtering_node.cpp b/image-filtering/src/image_filtering_node.cpp new file mode 100644 index 0000000..075e394 --- /dev/null +++ b/image-filtering/src/image_filtering_node.cpp @@ -0,0 +1,13 @@ +#include + +int main(int argc, char * argv[]) +{ + rclcpp::init(argc, argv); + + rclcpp::NodeOptions options; + auto node = std::make_shared(options); + + rclcpp::spin(node); + rclcpp::shutdown(); + return 0; +} \ No newline at end of file diff --git a/image-filtering/src/image_filtering_ros.cpp b/image-filtering/src/image_filtering_ros.cpp new file mode 100644 index 0000000..6b42ddb --- /dev/null +++ b/image-filtering/src/image_filtering_ros.cpp @@ -0,0 +1,134 @@ +#include +#include + +using std::placeholders::_1; + +namespace vortex::image_processing { + + +ImageFilteringNode::ImageFilteringNode(const rclcpp::NodeOptions & options) : Node("image_filtering_node", options) +{ + this->declare_parameter("sub_topic", "/flir_camera/image_raw"); + this->declare_parameter("pub_topic", "/filtered_image"); + this->declare_parameter("filter_params.filter_type", "ebus"); + this->declare_parameter("filter_params.flip.flip_code", 0); + this->declare_parameter("filter_params.unsharpening.blur_size", 8); + this->declare_parameter("filter_params.erosion.size", 1); + this->declare_parameter("filter_params.dilation.size", 1); + this->declare_parameter("filter_params.white_balancing.contrast_percentage", 0.8); + this->declare_parameter("filter_params.ebus.erosion_size", 2); + this->declare_parameter("filter_params.ebus.blur_size", 30); + this->declare_parameter("filter_params.ebus.mask_weight", 5); + + // Set up the QoS profile for the image subscriber + rmw_qos_profile_t qos_profile = rmw_qos_profile_sensor_data; + auto qos_sensor_data = rclcpp::QoS(rclcpp::QoSInitialization(qos_profile.history, 1), qos_profile); + + check_and_subscribe_to_image_topic(); + set_filter_params(); + + initialize_parameter_handler(); + + image_pub_ = this->create_publisher("/filtered_image", qos_sensor_data); +} + +void ImageFilteringNode::set_filter_params(){ + FilterParams params; + std::string filter = this->get_parameter("filter_params.filter_type").as_string(); + if(!filter_functions.contains(filter)){ + RCLCPP_ERROR_STREAM_THROTTLE(this->get_logger(), *this->get_clock(), 1000, "Invalid filter type: " << filter << " Setting to no_filter."); + filter_ = "no_filter"; + } else{ + filter_ = filter; + } + params.flip.flip_code = this->get_parameter("filter_params.flip.flip_code").as_int(); + params.unsharpening.blur_size = this->get_parameter("filter_params.unsharpening.blur_size").as_int(); + params.eroding.size = this->get_parameter("filter_params.erosion.size").as_int(); + params.dilating.size = this->get_parameter("filter_params.dilation.size").as_int(); + params.white_balancing.contrast_percentage = this->get_parameter("filter_params.white_balancing.contrast_percentage").as_double(); + params.ebus.erosion_size = this->get_parameter("filter_params.ebus.erosion_size").as_int(); + params.ebus.blur_size = this->get_parameter("filter_params.ebus.blur_size").as_int(); + params.ebus.mask_weight = this->get_parameter("filter_params.ebus.mask_weight").as_int(); + filter_params_ = params; + RCLCPP_INFO(this->get_logger(), "Filter parameters updated."); +} + +void ImageFilteringNode::check_and_subscribe_to_image_topic() { + std::string image_topic = this->get_parameter("sub_topic").as_string(); + if (image_topic_ != image_topic) { + image_sub_ = this->create_subscription( + image_topic, 10, std::bind(&ImageFilteringNode::image_callback, this, _1)); + image_topic_ = image_topic; + RCLCPP_INFO(this->get_logger(), "Subscribed to image topic: %s", image_topic.c_str()); + } +} + +void ImageFilteringNode::initialize_parameter_handler() { + param_handler_ = std::make_shared(this); + + // Register the parameter event callback with the correct signature + auto parameter_event_callback = + [this](const rcl_interfaces::msg::ParameterEvent & event) -> void { + this->on_parameter_event(event); + }; + + // Register the callback with the parameter event handler + param_cb_handle_ = param_handler_->add_parameter_event_callback(parameter_event_callback); +} + +void ImageFilteringNode::on_parameter_event(const rcl_interfaces::msg::ParameterEvent & event) { + // Get the fully qualified name of the current node + auto node_name = this->get_fully_qualified_name(); + + // Filter out events not related to this node + if (event.node != node_name) { + return; // Early return if the event is not from this node + } + RCLCPP_INFO(this->get_logger(), "Received parameter event"); + for (const auto& changed_parameter : event.changed_parameters) { + if (changed_parameter.name.find("sub_topic") == 0) check_and_subscribe_to_image_topic(); + if (changed_parameter.name.find("filter_params") == 0) set_filter_params(); + } +} + +void ImageFilteringNode::image_callback(const sensor_msgs::msg::Image::SharedPtr msg) { + RCLCPP_INFO_ONCE(this->get_logger(), "Received image message."); + cv_bridge::CvImagePtr cv_ptr; + + try + { + cv_ptr = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::BGR8); + + if (cv_ptr->image.empty()) { + RCLCPP_WARN_STREAM_THROTTLE(this->get_logger(), *this->get_clock(), 1000, "Empty image received, skipping processing."); + return; + } + + } + catch (cv_bridge::Exception &e) + { + RCLCPP_WARN_STREAM_THROTTLE(this->get_logger(), *this->get_clock(), 1000, "cv_bridge exception: " << e.what()); + return; + } + + cv::Mat input_image = cv_ptr->image; + cv::Mat filtered_image; + apply_filter(filter_, filter_params_, input_image, filtered_image); + +// auto message = cv_bridge::CvImage(msg->header, "bgr8", filtered_image).toImageMsg(); + +// image_pub_->publish(*message); + // Create a unique pointer for the message + // RCLCPP_INFO_STREAM_THROTTLE(this->get_logger(), *this->get_clock(), 1000, "Addr of image message: " << msg.get()); + auto message = std::make_unique(); + cv_bridge::CvImage(msg->header, "bgr8", filtered_image).toImageMsg(*message); + + // RCLCPP_INFO_STREAM_THROTTLE(this->get_logger(), *this->get_clock(), 1000, "Addr of image message filtering: " << message.get()); + + // Publish the message using a unique pointer + image_pub_->publish(std::move(message)); +} + +RCLCPP_COMPONENTS_REGISTER_NODE(vortex::image_processing::ImageFilteringNode) + +} // namespace vortex::image_processing diff --git a/image-filtering/src/image_processing.cpp b/image-filtering/src/image_processing.cpp new file mode 100644 index 0000000..19b9da3 --- /dev/null +++ b/image-filtering/src/image_processing.cpp @@ -0,0 +1,104 @@ +#include +#include + +namespace vortex::image_processing +{ + +void no_filter([[maybe_unused]] const FilterParams& params, const cv::Mat &original, cv::Mat &filtered) +{ + original.copyTo(filtered); +} + +void flip_filter([[maybe_unused]] const FilterParams& params, const cv::Mat &original, cv::Mat &filtered) +{ + int flip_code = params.flip.flip_code; // 0: x-axis, 1: y-axis, -1: both + cv::flip(original, filtered, flip_code); +} + +void sharpening_filter([[maybe_unused]] const FilterParams& params, const cv::Mat &original, cv::Mat &filtered) +{ + // Sharpen image + cv::Mat kernel = (cv::Mat_(3, 3) << 0, -1, 0, -1, 5, -1, 0, -1, 0); + cv::filter2D(original, filtered, -1, kernel); +} + + +void unsharpening_filter(const FilterParams& params, const cv::Mat &original, cv::Mat &filtered) +{ + int blur_size = params.unsharpening.blur_size; + // Create a blurred version of the image + cv::Mat blurred; + GaussianBlur(original, blurred, cv::Size(2 * blur_size + 1, 2 * blur_size + 1), 0); + + // Compute the unsharp mask + cv::Mat mask = original - blurred; + cv::Mat unsharp; + + addWeighted(original, 1, mask, 3, 0, filtered); +} + +void eroding_filter(const FilterParams& params, const cv::Mat &original, cv::Mat &filtered) +{ + int erosion_size = params.eroding.size; + // Create a structuring element for dilation and erosion + cv::Mat element = cv::getStructuringElement(cv::MORPH_RECT, cv::Size(2 * erosion_size + 1, 2 * erosion_size + 1), cv::Point(erosion_size, erosion_size)); + + // Apply erosion to the image + cv::erode(original, filtered, element); +} + +void dilating_filter(const FilterParams& params, const cv::Mat &original, cv::Mat &filtered) +{ + int dilation_size = params.dilating.size; + // Create a structuring element for dilation and erosion + cv::Mat element = cv::getStructuringElement(cv::MORPH_RECT, cv::Size(2 * dilation_size + 1, 2 * dilation_size + 1), cv::Point(dilation_size, dilation_size)); + + // Apply dilation to the image + cv::dilate(original, filtered, element); +} + +void white_balance_filter(const FilterParams& params, const cv::Mat &original, cv::Mat &filtered) +{ + double contrast_percentage = params.white_balancing.contrast_percentage; + cv::Ptr balance = cv::xphoto::createSimpleWB(); + balance->setP(contrast_percentage); + balance->balanceWhite(original, filtered); +} + +void ebus_filter(const FilterParams& params, const cv::Mat &original, cv::Mat &filtered) +{ + int blur_size = params.ebus.blur_size; + int mask_weight = params.ebus.mask_weight; + // Erode image to make blacks more black + cv::Mat eroded; + + int erosion_size = params.eroding.size; + // Create a structuring element for dilation and erosion + cv::Mat element = cv::getStructuringElement(cv::MORPH_RECT, cv::Size(2 * erosion_size + 1, 2 * erosion_size + 1), cv::Point(erosion_size, erosion_size)); + + // Apply erosion to the image + cv::erode(original, eroded, element); + + // Make an unsharp mask from original image + cv::Mat blurred; + GaussianBlur(original, blurred, cv::Size(2 * blur_size + 1, 2 * blur_size + 1), 0); + + // Compute the unsharp mask + cv::Mat mask = original - blurred; + cv::Mat unsharp; + + // Add mask to the eroded image instead of the original + // Higher weight of mask will create a sharper but more noisy image + addWeighted(eroded, 1, mask, mask_weight, 0, filtered); +} + +void apply_filter(const std::string& filter, const FilterParams& params, const cv::Mat &original, cv::Mat &filtered) +{ + if(filter_functions.contains(filter)){ + ((filter_functions.at(filter)))(params, original, filtered); + } else { + original.copyTo(filtered); // Default to no filter + } +} + +} // namespace vortex::image_processing \ No newline at end of file diff --git a/image-filtering/test/CMakeLists.txt b/image-filtering/test/CMakeLists.txt new file mode 100644 index 0000000..a866251 --- /dev/null +++ b/image-filtering/test/CMakeLists.txt @@ -0,0 +1,21 @@ +find_package(ament_cmake_gtest REQUIRED) +ament_add_gtest(${PROJECT_NAME}_test + image_processing_test.cpp + ../src/image_processing.cpp +) +target_link_libraries(${PROJECT_NAME}_test + ${OpenCV_LIBS} # Link OpenCV libraries + + + # Eigen3::Eigen # Link Eigen3 if your tests require it + # Add any other libraries your tests might require +) + +target_include_directories(${PROJECT_NAME}_test PUBLIC + $ + $ + ${OpenCV_INCLUDE_DIRS} # Ensure OpenCV include dirs are available to the test +) +ament_target_dependencies(${PROJECT_NAME}_test + cv_bridge +) diff --git a/image-filtering/test/image_processing_test.cpp b/image-filtering/test/image_processing_test.cpp new file mode 100644 index 0000000..e69de29 From 39e8379c281597aa3e03f67140c1d94041edff51 Mon Sep 17 00:00:00 2001 From: jorgenfj Date: Mon, 27 May 2024 11:05:38 +0200 Subject: [PATCH 03/53] added gitignore --- .gitignore | 52 ++++++++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 52 insertions(+) create mode 100644 .gitignore diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..4aff857 --- /dev/null +++ b/.gitignore @@ -0,0 +1,52 @@ +devel/ +logs/ +build/ +bin/ +lib/ +msg_gen/ +srv_gen/ +msg/*Action.msg +msg/*ActionFeedback.msg +msg/*ActionGoal.msg +msg/*ActionResult.msg +msg/*Feedback.msg +msg/*Goal.msg +msg/*Result.msg +msg/_*.py +build_isolated/ +devel_isolated/ +*/launch/__pycache__ + +# Generated by dynamic reconfigure +*.cfgc +/cfg/cpp/ +/cfg/*.py + +# Ignore generated docs +*.dox +*.wikidoc + +# eclipse stuff +.project +.cproject + +# qcreator stuff +CMakeLists.txt.user + +srv/_*.py +*.pcd +*.pyc +qtcreator-* +*.user + +/planning/cfg +/planning/docs +/planning/src + +*~ + +# Emacs +.#* + +# Catkin custom files +CATKIN_IGNORE \ No newline at end of file From eb7c50c98f7a6226919caea59b7aa19f3dd345af Mon Sep 17 00:00:00 2001 From: jorgenfj Date: Tue, 28 May 2024 07:43:39 +0200 Subject: [PATCH 04/53] removed unused main file --- image-filtering/src/image_filtering_node.cpp | 13 ------------- 1 file changed, 13 deletions(-) delete mode 100644 image-filtering/src/image_filtering_node.cpp diff --git a/image-filtering/src/image_filtering_node.cpp b/image-filtering/src/image_filtering_node.cpp deleted file mode 100644 index 075e394..0000000 --- a/image-filtering/src/image_filtering_node.cpp +++ /dev/null @@ -1,13 +0,0 @@ -#include - -int main(int argc, char * argv[]) -{ - rclcpp::init(argc, argv); - - rclcpp::NodeOptions options; - auto node = std::make_shared(options); - - rclcpp::spin(node); - rclcpp::shutdown(); - return 0; -} \ No newline at end of file From 385c042622c71ae723dd277e55f9b029e54c48c9 Mon Sep 17 00:00:00 2001 From: Andreas Kluge Svendsrud Date: Tue, 25 Mar 2025 19:25:07 +0100 Subject: [PATCH 05/53] ci: add ci workflows and config files --- .clang-format | 307 +++++++++++++++++++++++++ .github/workflows/code-coverage.yml | 13 ++ .github/workflows/industrial-ci.yml | 12 + .github/workflows/semantic-release.yml | 9 + .pre-commit-config.yaml | 85 +++++++ codecov.yml | 19 ++ ruff.toml | 6 + 7 files changed, 451 insertions(+) create mode 100644 .clang-format create mode 100644 .github/workflows/code-coverage.yml create mode 100644 .github/workflows/industrial-ci.yml create mode 100644 .github/workflows/semantic-release.yml create mode 100644 .pre-commit-config.yaml create mode 100644 codecov.yml create mode 100644 ruff.toml diff --git a/.clang-format b/.clang-format new file mode 100644 index 0000000..8340806 --- /dev/null +++ b/.clang-format @@ -0,0 +1,307 @@ +--- +BasedOnStyle: Chromium +AccessModifierOffset: -1 +AlignAfterOpenBracket: Align +AlignArrayOfStructures: None +AlignConsecutiveAssignments: + Enabled: false + AcrossEmptyLines: false + AcrossComments: false + AlignCompound: false + AlignFunctionPointers: false + PadOperators: true +AlignConsecutiveBitFields: + Enabled: false + AcrossEmptyLines: false + AcrossComments: false + AlignCompound: false + AlignFunctionPointers: false + PadOperators: false +AlignConsecutiveDeclarations: + Enabled: false + AcrossEmptyLines: false + AcrossComments: false + AlignCompound: false + AlignFunctionPointers: false + PadOperators: false +AlignConsecutiveMacros: + Enabled: false + AcrossEmptyLines: false + AcrossComments: false + AlignCompound: false + AlignFunctionPointers: false + PadOperators: false +AlignConsecutiveShortCaseStatements: + Enabled: false + AcrossEmptyLines: false + AcrossComments: false + AlignCaseArrows: false + AlignCaseColons: false +AlignConsecutiveTableGenBreakingDAGArgColons: + Enabled: false + AcrossEmptyLines: false + AcrossComments: false + AlignCompound: false + AlignFunctionPointers: false + PadOperators: false +AlignConsecutiveTableGenCondOperatorColons: + Enabled: false + AcrossEmptyLines: false + AcrossComments: false + AlignCompound: false + AlignFunctionPointers: false + PadOperators: false +AlignConsecutiveTableGenDefinitionColons: + Enabled: false + AcrossEmptyLines: false + AcrossComments: false + AlignCompound: false + AlignFunctionPointers: false + PadOperators: false +AlignEscapedNewlines: Left +AlignOperands: Align +AlignTrailingComments: + Kind: Always + OverEmptyLines: 0 +AllowAllArgumentsOnNextLine: true +AllowAllParametersOfDeclarationOnNextLine: false +AllowBreakBeforeNoexceptSpecifier: Never +AllowShortBlocksOnASingleLine: Never +AllowShortCaseExpressionOnASingleLine: true +AllowShortCaseLabelsOnASingleLine: false +AllowShortCompoundRequirementOnASingleLine: true +AllowShortEnumsOnASingleLine: true +AllowShortFunctionsOnASingleLine: Inline +AllowShortIfStatementsOnASingleLine: Never +AllowShortLambdasOnASingleLine: All +AllowShortLoopsOnASingleLine: false +AlwaysBreakAfterDefinitionReturnType: None +AlwaysBreakBeforeMultilineStrings: true +AttributeMacros: + - __capability +BinPackArguments: true +BinPackParameters: false +BitFieldColonSpacing: Both +BraceWrapping: + AfterCaseLabel: false + AfterClass: false + AfterControlStatement: Never + AfterEnum: false + AfterFunction: false + AfterNamespace: false + AfterObjCDeclaration: false + AfterStruct: false + AfterUnion: false + AfterExternBlock: false + BeforeCatch: false + BeforeElse: false + BeforeLambdaBody: false + BeforeWhile: false + IndentBraces: false + SplitEmptyFunction: true + SplitEmptyRecord: true + SplitEmptyNamespace: true +BreakAdjacentStringLiterals: true +BreakAfterAttributes: Leave +BreakAfterJavaFieldAnnotations: false +BreakAfterReturnType: None +BreakArrays: true +BreakBeforeBinaryOperators: None +BreakBeforeBraces: Attach +BreakBeforeConceptDeclarations: Always +BreakBeforeInlineASMColon: OnlyMultiline +BreakBeforeTernaryOperators: true +BreakConstructorInitializers: BeforeColon +BreakFunctionDefinitionParameters: false +BreakInheritanceList: BeforeColon +BreakStringLiterals: true +BreakTemplateDeclarations: Yes +ColumnLimit: 80 +CommentPragmas: "^ IWYU pragma:" +CompactNamespaces: false +ConstructorInitializerIndentWidth: 4 +ContinuationIndentWidth: 4 +Cpp11BracedListStyle: true +DerivePointerAlignment: false +DisableFormat: false +EmptyLineAfterAccessModifier: Never +EmptyLineBeforeAccessModifier: LogicalBlock +ExperimentalAutoDetectBinPacking: false +FixNamespaceComments: true +ForEachMacros: + - foreach + - Q_FOREACH + - BOOST_FOREACH +IfMacros: + - KJ_IF_MAYBE +IncludeBlocks: Preserve +IncludeCategories: + - Regex: ^ + Priority: 2 + SortPriority: 0 + CaseSensitive: false + - Regex: ^<.*\.h> + Priority: 1 + SortPriority: 0 + CaseSensitive: false + - Regex: ^<.* + Priority: 2 + SortPriority: 0 + CaseSensitive: false + - Regex: .* + Priority: 3 + SortPriority: 0 + CaseSensitive: false +IncludeIsMainRegex: ([-_](test|unittest))?$ +IncludeIsMainSourceRegex: "" +IndentAccessModifiers: false +IndentCaseBlocks: false +IndentCaseLabels: true +IndentExternBlock: AfterExternBlock +IndentGotoLabels: true +IndentPPDirectives: None +IndentRequiresClause: true +IndentWidth: 4 +IndentWrappedFunctionNames: false +InsertBraces: false +InsertNewlineAtEOF: false +InsertTrailingCommas: None +IntegerLiteralSeparator: + Binary: 0 + BinaryMinDigits: 0 + Decimal: 0 + DecimalMinDigits: 0 + Hex: 0 + HexMinDigits: 0 +JavaScriptQuotes: Leave +JavaScriptWrapImports: true +KeepEmptyLines: + AtEndOfFile: false + AtStartOfBlock: false + AtStartOfFile: true +LambdaBodyIndentation: Signature +Language: Cpp +LineEnding: DeriveLF +MacroBlockBegin: "" +MacroBlockEnd: "" +MainIncludeChar: Quote +MaxEmptyLinesToKeep: 1 +NamespaceIndentation: None +ObjCBinPackProtocolList: Never +ObjCBlockIndentWidth: 2 +ObjCBreakBeforeNestedBlockParam: true +ObjCSpaceAfterProperty: false +ObjCSpaceBeforeProtocolList: true +PPIndentWidth: -1 +PackConstructorInitializers: NextLine +PenaltyBreakAssignment: 2 +PenaltyBreakBeforeFirstCallParameter: 1 +PenaltyBreakComment: 300 +PenaltyBreakFirstLessLess: 120 +PenaltyBreakOpenParenthesis: 0 +PenaltyBreakScopeResolution: 500 +PenaltyBreakString: 1000 +PenaltyBreakTemplateDeclaration: 10 +PenaltyExcessCharacter: 1000000 +PenaltyIndentedWhitespace: 0 +PenaltyReturnTypeOnItsOwnLine: 200 +PointerAlignment: Left +QualifierAlignment: Leave +RawStringFormats: + - Language: Cpp + Delimiters: + - cc + - CC + - cpp + - Cpp + - CPP + - c++ + - C++ + CanonicalDelimiter: "" + BasedOnStyle: google + - Language: TextProto + Delimiters: + - pb + - PB + - proto + - PROTO + EnclosingFunctions: + - EqualsProto + - EquivToProto + - PARSE_PARTIAL_TEXT_PROTO + - PARSE_TEST_PROTO + - PARSE_TEXT_PROTO + - ParseTextOrDie + - ParseTextProtoOrDie + - ParseTestProto + - ParsePartialTestProto + CanonicalDelimiter: pb + BasedOnStyle: google +ReferenceAlignment: Pointer +ReflowComments: true +RemoveBracesLLVM: false +RemoveParentheses: Leave +RemoveSemicolon: false +RequiresClausePosition: OwnLine +RequiresExpressionIndentation: OuterScope +SeparateDefinitionBlocks: Leave +ShortNamespaceLines: 1 +SkipMacroDefinitionBody: false +SortIncludes: CaseSensitive +SortJavaStaticImport: Before +SortUsingDeclarations: LexicographicNumeric +SpaceAfterCStyleCast: false +SpaceAfterLogicalNot: false +SpaceAfterTemplateKeyword: true +SpaceAroundPointerQualifiers: Default +SpaceBeforeAssignmentOperators: true +SpaceBeforeCaseColon: false +SpaceBeforeCpp11BracedList: false +SpaceBeforeCtorInitializerColon: true +SpaceBeforeInheritanceColon: true +SpaceBeforeJsonColon: false +SpaceBeforeParens: ControlStatements +SpaceBeforeParensOptions: + AfterControlStatements: true + AfterForeachMacros: true + AfterFunctionDeclarationName: false + AfterFunctionDefinitionName: false + AfterIfMacros: true + AfterOverloadedOperator: false + AfterPlacementOperator: true + AfterRequiresInClause: false + AfterRequiresInExpression: false + BeforeNonEmptyParentheses: false +SpaceBeforeRangeBasedForLoopColon: true +SpaceBeforeSquareBrackets: false +SpaceInEmptyBlock: false +SpacesBeforeTrailingComments: 2 +SpacesInAngles: Never +SpacesInContainerLiterals: true +SpacesInLineCommentPrefix: + Minimum: 1 + Maximum: -1 +SpacesInParens: Never +SpacesInParensOptions: + ExceptDoubleParentheses: false + InConditionalStatements: false + InCStyleCasts: false + InEmptyParentheses: false + Other: false +SpacesInSquareBrackets: false +Standard: Auto +StatementAttributeLikeMacros: + - Q_EMIT +StatementMacros: + - Q_UNUSED + - QT_REQUIRE_VERSION +TabWidth: 8 +TableGenBreakInsideDAGArg: DontBreak +UseTab: Never +VerilogBreakBetweenInstancePorts: true +WhitespaceSensitiveMacros: + - BOOST_PP_STRINGIZE + - CF_SWIFT_NAME + - NS_SWIFT_NAME + - PP_STRINGIZE + - STRINGIZE diff --git a/.github/workflows/code-coverage.yml b/.github/workflows/code-coverage.yml new file mode 100644 index 0000000..a4e68bc --- /dev/null +++ b/.github/workflows/code-coverage.yml @@ -0,0 +1,13 @@ +name: Code Coverage +on: + push: + branches: + - main + pull_request: + branches: + - main +jobs: + call_reusable_workflow: + uses: vortexntnu/vortex-ci/.github/workflows/reusable-code-coverage.yml@main + secrets: + CODECOV_TOKEN: ${{ secrets.CODECOV_TOKEN }} diff --git a/.github/workflows/industrial-ci.yml b/.github/workflows/industrial-ci.yml new file mode 100644 index 0000000..67a4efe --- /dev/null +++ b/.github/workflows/industrial-ci.yml @@ -0,0 +1,12 @@ +name: Industrial CI + +on: + push: + workflow_dispatch: + schedule: + - cron: '0 1 * * *' # Runs daily to check for dependency issues or flaking tests +jobs: + call_reusable_workflow: + uses: vortexntnu/vortex-ci/.github/workflows/reusable-industrial-ci.yml@main + with: + ros_repo: '["testing", "main"]' diff --git a/.github/workflows/semantic-release.yml b/.github/workflows/semantic-release.yml new file mode 100644 index 0000000..70779d4 --- /dev/null +++ b/.github/workflows/semantic-release.yml @@ -0,0 +1,9 @@ +name: Semantic Release + +on: + push: + branches: + - main +jobs: + call_reusable_workflow: + uses: vortexntnu/vortex-ci/.github/workflows/reusable-semantic-release.yml@main diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml new file mode 100644 index 0000000..e1850c7 --- /dev/null +++ b/.pre-commit-config.yaml @@ -0,0 +1,85 @@ +# To use: +# +# pre-commit run -a +# +# Or: +# +# pre-commit install # (runs every time you commit in git) +# +# To update this file: +# +# pre-commit autoupdate +# +# See https://github.com/pre-commit/pre-commit + +repos: + # Standard hooks + - repo: https://github.com/pre-commit/pre-commit-hooks + rev: v5.0.0 + hooks: + - id: check-added-large-files + - id: check-ast + - id: check-case-conflict + - id: check-docstring-first + - id: check-merge-conflict + - id: check-symlinks + - id: check-xml + - id: check-yaml + args: ["--allow-multiple-documents"] + - id: debug-statements + - id: end-of-file-fixer + - id: mixed-line-ending + - id: trailing-whitespace + exclude_types: [rst] + - id: fix-byte-order-marker + - id: requirements-txt-fixer + # Python hooks + - repo: https://github.com/astral-sh/ruff-pre-commit + rev: v0.9.2 + hooks: + - id: ruff-format + - id: ruff + name: ruff-isort + args: [ + "--select=I", + "--fix" + ] + - id: ruff + name: ruff-pyupgrade + args: [ + "--select=UP", + "--fix" + ] + - id: ruff + name: ruff-pydocstyle + args: [ + "--select=D", + "--ignore=D100,D101,D102,D103,D104,D105,D106,D107,D401", + "--fix", + ] + stages: [pre-commit] + pass_filenames: true + - id: ruff + name: ruff-check + args: [ + "--select=F,PT,B,C4,T20,S,N", + "--ignore=T201,N812,B006,S101,S311,S607,S603", + "--fix" + ] + stages: [pre-commit] + pass_filenames: true + # C++ hooks + - repo: https://github.com/pre-commit/mirrors-clang-format + rev: v19.1.7 + hooks: + - id: clang-format + args: [--style=file] + # Spellcheck in comments and docs + - repo: https://github.com/codespell-project/codespell + rev: v2.3.0 + hooks: + - id: codespell + args: ['--write-changes', '--ignore-words-list=theses,fom'] + +ci: + autoupdate_schedule: quarterly diff --git a/codecov.yml b/codecov.yml new file mode 100644 index 0000000..afbbb46 --- /dev/null +++ b/codecov.yml @@ -0,0 +1,19 @@ +coverage: + precision: 2 + round: down + status: + project: + default: + informational: true + flags: + - unittests + patch: off +fixes: + - "ros_ws/src/vortex_image_filtering/::" +comment: + layout: "diff, flags, files" + behavior: default +flags: + unittests: + paths: + - "**" \ No newline at end of file diff --git a/ruff.toml b/ruff.toml new file mode 100644 index 0000000..bf3554d --- /dev/null +++ b/ruff.toml @@ -0,0 +1,6 @@ +[lint.pydocstyle] +convention = "google" + +[format] +# Keep quotes as is +quote-style = "preserve" From 8dcc03f91ad4e16686b15ea2b5429e76acba4715 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Tue, 25 Mar 2025 18:29:52 +0000 Subject: [PATCH 06/53] [pre-commit.ci] auto fixes from pre-commit.com hooks for more information, see https://pre-commit.ci --- .gitignore | 2 +- README.md | 4 +- codecov.yml | 2 +- .../image_filters/image_filtering_ros.hpp | 99 +++++----- .../image_filters/image_processing.hpp | 69 ++++--- .../launch/image_filtering_launch.py | 24 ++- .../params/image_filtering_params.yaml | 4 +- image-filtering/src/image_filtering_ros.cpp | 123 ++++++++----- image-filtering/src/image_processing.cpp | 170 ++++++++++-------- image-filtering/test/CMakeLists.txt | 4 +- 10 files changed, 281 insertions(+), 220 deletions(-) diff --git a/.gitignore b/.gitignore index 4aff857..5f73720 100644 --- a/.gitignore +++ b/.gitignore @@ -49,4 +49,4 @@ qtcreator-* .#* # Catkin custom files -CATKIN_IGNORE \ No newline at end of file +CATKIN_IGNORE diff --git a/README.md b/README.md index 4ec8fd0..c71a986 100644 --- a/README.md +++ b/README.md @@ -66,7 +66,7 @@ Implement your filter function. This function should take the `cv::Mat` objects void your_filter_function(const cv::Mat &original, cv::Mat &filtered, const FilterParams& filter_params) { // Access your filter-specific parameters like this: int example_param = filter_params.your_filter.example_param; - + // Implement your filtering logic here } ``` @@ -121,4 +121,4 @@ void ImageFilteringNode::set_filter_params(){ filter_params_ = params; // Update the filter parameters structure RCLCPP_INFO(this->get_logger(), "Filter parameters updated for your_filter."); } -``` \ No newline at end of file +``` diff --git a/codecov.yml b/codecov.yml index afbbb46..c4673e3 100644 --- a/codecov.yml +++ b/codecov.yml @@ -16,4 +16,4 @@ comment: flags: unittests: paths: - - "**" \ No newline at end of file + - "**" diff --git a/image-filtering/include/image_filters/image_filtering_ros.hpp b/image-filtering/include/image_filters/image_filtering_ros.hpp index d88a3a4..47702cd 100644 --- a/image-filtering/include/image_filters/image_filtering_ros.hpp +++ b/image-filtering/include/image_filters/image_filtering_ros.hpp @@ -1,68 +1,66 @@ #ifndef IMAGE_FILTERING_ROS_HPP #define IMAGE_FILTERING_ROS_HPP -#include #include -#include +#include #include +#include +#include #include "image_processing.hpp" -#include - - -namespace vortex::image_processing -{ -class ImageFilteringNode : public rclcpp::Node{ +namespace vortex::image_processing { +class ImageFilteringNode : public rclcpp::Node { + public: + explicit ImageFilteringNode(const rclcpp::NodeOptions& options); + ~ImageFilteringNode() {}; - -public: - explicit ImageFilteringNode(const rclcpp::NodeOptions & options); - ~ImageFilteringNode(){}; - - -private: + private: /** * @brief Subscribes to image topic - */ + */ rclcpp::Subscription::SharedPtr image_sub_; /** * @brief Publishes the filtered image - */ + */ rclcpp::Publisher::SharedPtr image_pub_; /** - * @brief Check and subscribe to image if not yet subscribed. Allows for dynaminc reconfiguration of image topic. - * If a new topic is set, the old subscription is cancelled and a new one is bound to the callback function. - * - */ + * @brief Check and subscribe to image if not yet subscribed. Allows for + * dynamic reconfiguration of image topic. If a new topic is set, the old + * subscription is cancelled and a new one is bound to the callback + * function. + * + */ void check_and_subscribe_to_image_topic(); /** * @brief Set the filter parameters for the FilterParams struct. - * - */ + * + */ void set_filter_params(); /** * @brief Initialize the parameter handler and a parameter event callback. - * - */ + * + */ void initialize_parameter_handler(); /** * @brief Callback function for parameter events. - * Checks for parameter changes that matches the nodes' namespace and invokes the relevant initializer functions to update member variables. - * + * Checks for parameter changes that matches the nodes' namespace and + * invokes the relevant initializer functions to update member variables. + * * @param event The parameter event. - */ - void on_parameter_event(const rcl_interfaces::msg::ParameterEvent &event); + */ + void on_parameter_event(const rcl_interfaces::msg::ParameterEvent& event); /** * @brief Manages parameter events for the node. * - * This handle is used to set up a mechanism to listen for and react to changes in parameters. - * Parameters can be used to configure the node's operational behavior dynamically, - * allowing adjustments without altering the code. The `param_handler_` is responsible for - * registering callbacks that are triggered on parameter changes, providing a centralized + * This handle is used to set up a mechanism to listen for and react to + * changes in parameters. Parameters can be used to configure the node's + * operational behavior dynamically, allowing adjustments without altering + * the code. The `param_handler_` is responsible for registering callbacks + * that are triggered on parameter changes, providing a centralized * management system within the node for such events. */ std::shared_ptr param_handler_; @@ -70,42 +68,41 @@ class ImageFilteringNode : public rclcpp::Node{ /** * @brief Handle to the registration of the parameter event callback. * - * Represents a token or reference to the specific callback registration made with - * the parameter event handler (`param_handler_`). This handle allows for management - * of the lifecycle of the callback, such as removing the callback if it's no longer needed. - * It ensures that the node can respond to parameter changes with the registered callback - * in an efficient and controlled manner. + * Represents a token or reference to the specific callback registration + * made with the parameter event handler (`param_handler_`). This handle + * allows for management of the lifecycle of the callback, such as removing + * the callback if it's no longer needed. It ensures that the node can + * respond to parameter changes with the registered callback in an efficient + * and controlled manner. */ rclcpp::ParameterEventCallbackHandle::SharedPtr param_cb_handle_; /** * @brief Callback function for image topic - * + * * @param msg The image message - */ + */ void image_callback(const sensor_msgs::msg::Image::SharedPtr msg); /** * @brief The image topic to subscribe to - * - */ + * + */ std::string image_topic_; /** * @brief The filter parameters - * - */ + * + */ FilterParams filter_params_; - + /** * @brief filter to apply - * - */ + * + */ std::string filter_; - }; -} // namespace vortex::image_processing - +} // namespace vortex::image_processing -#endif // IMAGE_FILTERING_ROS_HPP \ No newline at end of file +#endif // IMAGE_FILTERING_ROS_HPP diff --git a/image-filtering/include/image_filters/image_processing.hpp b/image-filtering/include/image_filters/image_processing.hpp index e87f395..ac224ae 100644 --- a/image-filtering/include/image_filters/image_processing.hpp +++ b/image-filtering/include/image_filters/image_processing.hpp @@ -1,16 +1,15 @@ #ifndef IMAGE_PROCESSING_HPP #define IMAGE_PROCESSING_HPP +#include +#include #include #include #include #include #include -#include -#include -namespace vortex::image_processing -{ +namespace vortex::image_processing { struct FlipParams { int flip_code; @@ -49,62 +48,80 @@ struct FilterParams { typedef void (*FilterFunction)(const FilterParams&, const cv::Mat&, cv::Mat&); /** - * Reads the filter_type from the FilterParams struct + * Reads the filter_type from the FilterParams struct * and calls the appropriate filter function from the filter_functions map. */ -void apply_filter(const std::string& filter, const FilterParams& params, const cv::Mat &original, cv::Mat &filtered); +void apply_filter(const std::string& filter, + const FilterParams& params, + const cv::Mat& original, + cv::Mat& filtered); /** * No filter, just copy the image */ -void no_filter(const FilterParams& params, const cv::Mat &original, cv::Mat &modified); +void no_filter(const FilterParams& params, + const cv::Mat& original, + cv::Mat& modified); /** * Flips the image */ -void flip_filter(const FilterParams& params, const cv::Mat &original, cv::Mat &modified); +void flip_filter(const FilterParams& params, + const cv::Mat& original, + cv::Mat& modified); /** * Makes edges harder */ -void sharpening_filter(const FilterParams& params, const cv::Mat &original, cv::Mat &modified); +void sharpening_filter(const FilterParams& params, + const cv::Mat& original, + cv::Mat& modified); /** * Makes edges harder in a smarter way */ -void unsharpening_filter(const FilterParams& params, const cv::Mat &original, cv::Mat &modified); +void unsharpening_filter(const FilterParams& params, + const cv::Mat& original, + cv::Mat& modified); /** * Expands the dark areas of the image */ -void eroding_filter(const FilterParams& params, const cv::Mat &original, cv::Mat &modified); +void eroding_filter(const FilterParams& params, + const cv::Mat& original, + cv::Mat& modified); /** * Expands the bright areas of the image */ -void dilating_filter(const FilterParams& params, const cv::Mat &original, cv::Mat &modified); +void dilating_filter(const FilterParams& params, + const cv::Mat& original, + cv::Mat& modified); /** * White Balancing Filter */ -void white_balance_filter(const FilterParams& params, const cv::Mat &original, cv::Mat &filtered); +void white_balance_filter(const FilterParams& params, + const cv::Mat& original, + cv::Mat& filtered); /** * A filter that worked well-ish in the mc-lab conditions easter 2023 * Uses a combination of dilation and unsharpening */ -void ebus_filter(const FilterParams& params, const cv::Mat &original, cv::Mat &filtered); +void ebus_filter(const FilterParams& params, + const cv::Mat& original, + cv::Mat& filtered); -const static std::map filter_functions ={ - {"no_filter", no_filter}, +const static std::map filter_functions = { + {"no_filter", no_filter}, {"flip", flip_filter}, - {"sharpening", sharpening_filter}, - {"unsharpening", unsharpening_filter}, - {"eroding", eroding_filter}, - {"dilating", dilating_filter}, - {"white_balancing", white_balance_filter}, - {"ebus", ebus_filter}}; - - -} // namespace image_processing -#endif // IMAGE_PROCESSING_HPP \ No newline at end of file + {"sharpening", sharpening_filter}, + {"unsharpening", unsharpening_filter}, + {"eroding", eroding_filter}, + {"dilating", dilating_filter}, + {"white_balancing", white_balance_filter}, + {"ebus", ebus_filter}}; + +} // namespace vortex::image_processing +#endif // IMAGE_PROCESSING_HPP diff --git a/image-filtering/launch/image_filtering_launch.py b/image-filtering/launch/image_filtering_launch.py index 74c4feb..a97ab1e 100644 --- a/image-filtering/launch/image_filtering_launch.py +++ b/image-filtering/launch/image_filtering_launch.py @@ -1,16 +1,22 @@ import os + from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription from launch_ros.actions import Node + def generate_launch_description(): image_filtering_node = Node( - package='image_filtering', - executable='image_filtering_node', - name='image_filtering_node', - parameters=[os.path.join(get_package_share_directory('image_filtering'),'params','image_filtering_params.yaml')], - output='screen', - ) - return LaunchDescription([ - image_filtering_node - ]) \ No newline at end of file + package='image_filtering', + executable='image_filtering_node', + name='image_filtering_node', + parameters=[ + os.path.join( + get_package_share_directory('image_filtering'), + 'params', + 'image_filtering_params.yaml', + ) + ], + output='screen', + ) + return LaunchDescription([image_filtering_node]) diff --git a/image-filtering/params/image_filtering_params.yaml b/image-filtering/params/image_filtering_params.yaml index db69b5e..d8013b8 100644 --- a/image-filtering/params/image_filtering_params.yaml +++ b/image-filtering/params/image_filtering_params.yaml @@ -19,5 +19,5 @@ image_filtering_node: blur_size: 30 mask_weight: 5 -# Filter params should reflect the FilterParams struct -# defined in /include/image_filters/image_processing.hpp \ No newline at end of file +# Filter params should reflect the FilterParams struct +# defined in /include/image_filters/image_processing.hpp diff --git a/image-filtering/src/image_filtering_ros.cpp b/image-filtering/src/image_filtering_ros.cpp index 6b42ddb..bcee311 100644 --- a/image-filtering/src/image_filtering_ros.cpp +++ b/image-filtering/src/image_filtering_ros.cpp @@ -5,9 +5,8 @@ using std::placeholders::_1; namespace vortex::image_processing { - -ImageFilteringNode::ImageFilteringNode(const rclcpp::NodeOptions & options) : Node("image_filtering_node", options) -{ +ImageFilteringNode::ImageFilteringNode(const rclcpp::NodeOptions& options) + : Node("image_filtering_node", options) { this->declare_parameter("sub_topic", "/flir_camera/image_raw"); this->declare_parameter("pub_topic", "/filtered_image"); this->declare_parameter("filter_params.filter_type", "ebus"); @@ -15,40 +14,55 @@ ImageFilteringNode::ImageFilteringNode(const rclcpp::NodeOptions & options) : No this->declare_parameter("filter_params.unsharpening.blur_size", 8); this->declare_parameter("filter_params.erosion.size", 1); this->declare_parameter("filter_params.dilation.size", 1); - this->declare_parameter("filter_params.white_balancing.contrast_percentage", 0.8); + this->declare_parameter( + "filter_params.white_balancing.contrast_percentage", 0.8); this->declare_parameter("filter_params.ebus.erosion_size", 2); this->declare_parameter("filter_params.ebus.blur_size", 30); this->declare_parameter("filter_params.ebus.mask_weight", 5); // Set up the QoS profile for the image subscriber rmw_qos_profile_t qos_profile = rmw_qos_profile_sensor_data; - auto qos_sensor_data = rclcpp::QoS(rclcpp::QoSInitialization(qos_profile.history, 1), qos_profile); + auto qos_sensor_data = rclcpp::QoS( + rclcpp::QoSInitialization(qos_profile.history, 1), qos_profile); check_and_subscribe_to_image_topic(); set_filter_params(); initialize_parameter_handler(); - image_pub_ = this->create_publisher("/filtered_image", qos_sensor_data); + image_pub_ = this->create_publisher( + "/filtered_image", qos_sensor_data); } -void ImageFilteringNode::set_filter_params(){ +void ImageFilteringNode::set_filter_params() { FilterParams params; - std::string filter = this->get_parameter("filter_params.filter_type").as_string(); - if(!filter_functions.contains(filter)){ - RCLCPP_ERROR_STREAM_THROTTLE(this->get_logger(), *this->get_clock(), 1000, "Invalid filter type: " << filter << " Setting to no_filter."); + std::string filter = + this->get_parameter("filter_params.filter_type").as_string(); + if (!filter_functions.contains(filter)) { + RCLCPP_ERROR_STREAM_THROTTLE( + this->get_logger(), *this->get_clock(), 1000, + "Invalid filter type: " << filter << " Setting to no_filter."); filter_ = "no_filter"; - } else{ + } else { filter_ = filter; } - params.flip.flip_code = this->get_parameter("filter_params.flip.flip_code").as_int(); - params.unsharpening.blur_size = this->get_parameter("filter_params.unsharpening.blur_size").as_int(); - params.eroding.size = this->get_parameter("filter_params.erosion.size").as_int(); - params.dilating.size = this->get_parameter("filter_params.dilation.size").as_int(); - params.white_balancing.contrast_percentage = this->get_parameter("filter_params.white_balancing.contrast_percentage").as_double(); - params.ebus.erosion_size = this->get_parameter("filter_params.ebus.erosion_size").as_int(); - params.ebus.blur_size = this->get_parameter("filter_params.ebus.blur_size").as_int(); - params.ebus.mask_weight = this->get_parameter("filter_params.ebus.mask_weight").as_int(); + params.flip.flip_code = + this->get_parameter("filter_params.flip.flip_code").as_int(); + params.unsharpening.blur_size = + this->get_parameter("filter_params.unsharpening.blur_size").as_int(); + params.eroding.size = + this->get_parameter("filter_params.erosion.size").as_int(); + params.dilating.size = + this->get_parameter("filter_params.dilation.size").as_int(); + params.white_balancing.contrast_percentage = + this->get_parameter("filter_params.white_balancing.contrast_percentage") + .as_double(); + params.ebus.erosion_size = + this->get_parameter("filter_params.ebus.erosion_size").as_int(); + params.ebus.blur_size = + this->get_parameter("filter_params.ebus.blur_size").as_int(); + params.ebus.mask_weight = + this->get_parameter("filter_params.ebus.mask_weight").as_int(); filter_params_ = params; RCLCPP_INFO(this->get_logger(), "Filter parameters updated."); } @@ -57,73 +71,84 @@ void ImageFilteringNode::check_and_subscribe_to_image_topic() { std::string image_topic = this->get_parameter("sub_topic").as_string(); if (image_topic_ != image_topic) { image_sub_ = this->create_subscription( - image_topic, 10, std::bind(&ImageFilteringNode::image_callback, this, _1)); + image_topic, 10, + std::bind(&ImageFilteringNode::image_callback, this, _1)); image_topic_ = image_topic; - RCLCPP_INFO(this->get_logger(), "Subscribed to image topic: %s", image_topic.c_str()); + RCLCPP_INFO(this->get_logger(), "Subscribed to image topic: %s", + image_topic.c_str()); } } void ImageFilteringNode::initialize_parameter_handler() { param_handler_ = std::make_shared(this); - + // Register the parameter event callback with the correct signature auto parameter_event_callback = - [this](const rcl_interfaces::msg::ParameterEvent & event) -> void { - this->on_parameter_event(event); - }; + [this](const rcl_interfaces::msg::ParameterEvent& event) -> void { + this->on_parameter_event(event); + }; // Register the callback with the parameter event handler - param_cb_handle_ = param_handler_->add_parameter_event_callback(parameter_event_callback); + param_cb_handle_ = + param_handler_->add_parameter_event_callback(parameter_event_callback); } -void ImageFilteringNode::on_parameter_event(const rcl_interfaces::msg::ParameterEvent & event) { - // Get the fully qualified name of the current node +void ImageFilteringNode::on_parameter_event( + const rcl_interfaces::msg::ParameterEvent& event) { + // Get the fully qualified name of the current node auto node_name = this->get_fully_qualified_name(); // Filter out events not related to this node if (event.node != node_name) { - return; // Early return if the event is not from this node + return; // Early return if the event is not from this node } RCLCPP_INFO(this->get_logger(), "Received parameter event"); for (const auto& changed_parameter : event.changed_parameters) { - if (changed_parameter.name.find("sub_topic") == 0) check_and_subscribe_to_image_topic(); - if (changed_parameter.name.find("filter_params") == 0) set_filter_params(); - } + if (changed_parameter.name.find("sub_topic") == 0) + check_and_subscribe_to_image_topic(); + if (changed_parameter.name.find("filter_params") == 0) + set_filter_params(); + } } -void ImageFilteringNode::image_callback(const sensor_msgs::msg::Image::SharedPtr msg) { +void ImageFilteringNode::image_callback( + const sensor_msgs::msg::Image::SharedPtr msg) { RCLCPP_INFO_ONCE(this->get_logger(), "Received image message."); cv_bridge::CvImagePtr cv_ptr; - try - { + try { cv_ptr = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::BGR8); if (cv_ptr->image.empty()) { - RCLCPP_WARN_STREAM_THROTTLE(this->get_logger(), *this->get_clock(), 1000, "Empty image received, skipping processing."); - return; + RCLCPP_WARN_STREAM_THROTTLE( + this->get_logger(), *this->get_clock(), 1000, + "Empty image received, skipping processing."); + return; } - } - catch (cv_bridge::Exception &e) - { - RCLCPP_WARN_STREAM_THROTTLE(this->get_logger(), *this->get_clock(), 1000, "cv_bridge exception: " << e.what()); + } catch (cv_bridge::Exception& e) { + RCLCPP_WARN_STREAM_THROTTLE(this->get_logger(), *this->get_clock(), + 1000, "cv_bridge exception: " << e.what()); return; } - + cv::Mat input_image = cv_ptr->image; cv::Mat filtered_image; apply_filter(filter_, filter_params_, input_image, filtered_image); -// auto message = cv_bridge::CvImage(msg->header, "bgr8", filtered_image).toImageMsg(); - -// image_pub_->publish(*message); + // auto message = cv_bridge::CvImage(msg->header, "bgr8", + // filtered_image).toImageMsg(); + + // image_pub_->publish(*message); // Create a unique pointer for the message - // RCLCPP_INFO_STREAM_THROTTLE(this->get_logger(), *this->get_clock(), 1000, "Addr of image message: " << msg.get()); + // RCLCPP_INFO_STREAM_THROTTLE(this->get_logger(), *this->get_clock(), 1000, + // "Addr of image message: " << msg.get()); auto message = std::make_unique(); - cv_bridge::CvImage(msg->header, "bgr8", filtered_image).toImageMsg(*message); + cv_bridge::CvImage(msg->header, "bgr8", filtered_image) + .toImageMsg(*message); - // RCLCPP_INFO_STREAM_THROTTLE(this->get_logger(), *this->get_clock(), 1000, "Addr of image message filtering: " << message.get()); + // RCLCPP_INFO_STREAM_THROTTLE(this->get_logger(), *this->get_clock(), 1000, + // "Addr of image message filtering: " << message.get()); // Publish the message using a unique pointer image_pub_->publish(std::move(message)); @@ -131,4 +156,4 @@ void ImageFilteringNode::image_callback(const sensor_msgs::msg::Image::SharedPtr RCLCPP_COMPONENTS_REGISTER_NODE(vortex::image_processing::ImageFilteringNode) -} // namespace vortex::image_processing +} // namespace vortex::image_processing diff --git a/image-filtering/src/image_processing.cpp b/image-filtering/src/image_processing.cpp index 19b9da3..5e1adce 100644 --- a/image-filtering/src/image_processing.cpp +++ b/image-filtering/src/image_processing.cpp @@ -1,104 +1,120 @@ #include #include -namespace vortex::image_processing -{ +namespace vortex::image_processing { -void no_filter([[maybe_unused]] const FilterParams& params, const cv::Mat &original, cv::Mat &filtered) -{ - original.copyTo(filtered); +void no_filter([[maybe_unused]] const FilterParams& params, + const cv::Mat& original, + cv::Mat& filtered) { + original.copyTo(filtered); } -void flip_filter([[maybe_unused]] const FilterParams& params, const cv::Mat &original, cv::Mat &filtered) -{ - int flip_code = params.flip.flip_code; // 0: x-axis, 1: y-axis, -1: both - cv::flip(original, filtered, flip_code); +void flip_filter([[maybe_unused]] const FilterParams& params, + const cv::Mat& original, + cv::Mat& filtered) { + int flip_code = params.flip.flip_code; // 0: x-axis, 1: y-axis, -1: both + cv::flip(original, filtered, flip_code); } -void sharpening_filter([[maybe_unused]] const FilterParams& params, const cv::Mat &original, cv::Mat &filtered) -{ - // Sharpen image - cv::Mat kernel = (cv::Mat_(3, 3) << 0, -1, 0, -1, 5, -1, 0, -1, 0); - cv::filter2D(original, filtered, -1, kernel); +void sharpening_filter([[maybe_unused]] const FilterParams& params, + const cv::Mat& original, + cv::Mat& filtered) { + // Sharpen image + cv::Mat kernel = (cv::Mat_(3, 3) << 0, -1, 0, -1, 5, -1, 0, -1, 0); + cv::filter2D(original, filtered, -1, kernel); } +void unsharpening_filter(const FilterParams& params, + const cv::Mat& original, + cv::Mat& filtered) { + int blur_size = params.unsharpening.blur_size; + // Create a blurred version of the image + cv::Mat blurred; + GaussianBlur(original, blurred, + cv::Size(2 * blur_size + 1, 2 * blur_size + 1), 0); -void unsharpening_filter(const FilterParams& params, const cv::Mat &original, cv::Mat &filtered) -{ - int blur_size = params.unsharpening.blur_size; - // Create a blurred version of the image - cv::Mat blurred; - GaussianBlur(original, blurred, cv::Size(2 * blur_size + 1, 2 * blur_size + 1), 0); + // Compute the unsharp mask + cv::Mat mask = original - blurred; + cv::Mat unsharp; - // Compute the unsharp mask - cv::Mat mask = original - blurred; - cv::Mat unsharp; - - addWeighted(original, 1, mask, 3, 0, filtered); + addWeighted(original, 1, mask, 3, 0, filtered); } -void eroding_filter(const FilterParams& params, const cv::Mat &original, cv::Mat &filtered) -{ - int erosion_size = params.eroding.size; - // Create a structuring element for dilation and erosion - cv::Mat element = cv::getStructuringElement(cv::MORPH_RECT, cv::Size(2 * erosion_size + 1, 2 * erosion_size + 1), cv::Point(erosion_size, erosion_size)); - - // Apply erosion to the image - cv::erode(original, filtered, element); +void eroding_filter(const FilterParams& params, + const cv::Mat& original, + cv::Mat& filtered) { + int erosion_size = params.eroding.size; + // Create a structuring element for dilation and erosion + cv::Mat element = cv::getStructuringElement( + cv::MORPH_RECT, cv::Size(2 * erosion_size + 1, 2 * erosion_size + 1), + cv::Point(erosion_size, erosion_size)); + + // Apply erosion to the image + cv::erode(original, filtered, element); } -void dilating_filter(const FilterParams& params, const cv::Mat &original, cv::Mat &filtered) -{ - int dilation_size = params.dilating.size; - // Create a structuring element for dilation and erosion - cv::Mat element = cv::getStructuringElement(cv::MORPH_RECT, cv::Size(2 * dilation_size + 1, 2 * dilation_size + 1), cv::Point(dilation_size, dilation_size)); - - // Apply dilation to the image - cv::dilate(original, filtered, element); +void dilating_filter(const FilterParams& params, + const cv::Mat& original, + cv::Mat& filtered) { + int dilation_size = params.dilating.size; + // Create a structuring element for dilation and erosion + cv::Mat element = cv::getStructuringElement( + cv::MORPH_RECT, cv::Size(2 * dilation_size + 1, 2 * dilation_size + 1), + cv::Point(dilation_size, dilation_size)); + + // Apply dilation to the image + cv::dilate(original, filtered, element); } -void white_balance_filter(const FilterParams& params, const cv::Mat &original, cv::Mat &filtered) -{ - double contrast_percentage = params.white_balancing.contrast_percentage; - cv::Ptr balance = cv::xphoto::createSimpleWB(); - balance->setP(contrast_percentage); - balance->balanceWhite(original, filtered); +void white_balance_filter(const FilterParams& params, + const cv::Mat& original, + cv::Mat& filtered) { + double contrast_percentage = params.white_balancing.contrast_percentage; + cv::Ptr balance = cv::xphoto::createSimpleWB(); + balance->setP(contrast_percentage); + balance->balanceWhite(original, filtered); } -void ebus_filter(const FilterParams& params, const cv::Mat &original, cv::Mat &filtered) -{ - int blur_size = params.ebus.blur_size; - int mask_weight = params.ebus.mask_weight; - // Erode image to make blacks more black - cv::Mat eroded; - - int erosion_size = params.eroding.size; - // Create a structuring element for dilation and erosion - cv::Mat element = cv::getStructuringElement(cv::MORPH_RECT, cv::Size(2 * erosion_size + 1, 2 * erosion_size + 1), cv::Point(erosion_size, erosion_size)); - - // Apply erosion to the image - cv::erode(original, eroded, element); - - // Make an unsharp mask from original image - cv::Mat blurred; - GaussianBlur(original, blurred, cv::Size(2 * blur_size + 1, 2 * blur_size + 1), 0); - - // Compute the unsharp mask - cv::Mat mask = original - blurred; - cv::Mat unsharp; - - // Add mask to the eroded image instead of the original - // Higher weight of mask will create a sharper but more noisy image - addWeighted(eroded, 1, mask, mask_weight, 0, filtered); +void ebus_filter(const FilterParams& params, + const cv::Mat& original, + cv::Mat& filtered) { + int blur_size = params.ebus.blur_size; + int mask_weight = params.ebus.mask_weight; + // Erode image to make blacks more black + cv::Mat eroded; + + int erosion_size = params.eroding.size; + // Create a structuring element for dilation and erosion + cv::Mat element = cv::getStructuringElement( + cv::MORPH_RECT, cv::Size(2 * erosion_size + 1, 2 * erosion_size + 1), + cv::Point(erosion_size, erosion_size)); + + // Apply erosion to the image + cv::erode(original, eroded, element); + + // Make an unsharp mask from original image + cv::Mat blurred; + GaussianBlur(original, blurred, + cv::Size(2 * blur_size + 1, 2 * blur_size + 1), 0); + + // Compute the unsharp mask + cv::Mat mask = original - blurred; + cv::Mat unsharp; + + // Add mask to the eroded image instead of the original + // Higher weight of mask will create a sharper but more noisy image + addWeighted(eroded, 1, mask, mask_weight, 0, filtered); } -void apply_filter(const std::string& filter, const FilterParams& params, const cv::Mat &original, cv::Mat &filtered) -{ - if(filter_functions.contains(filter)){ - ((filter_functions.at(filter)))(params, original, filtered); +void apply_filter(const std::string& filter, + const FilterParams& params, + const cv::Mat& original, + cv::Mat& filtered) { + if (filter_functions.contains(filter)) { + ((filter_functions.at(filter)))(params, original, filtered); } else { original.copyTo(filtered); // Default to no filter } } -} // namespace vortex::image_processing \ No newline at end of file +} // namespace vortex::image_processing diff --git a/image-filtering/test/CMakeLists.txt b/image-filtering/test/CMakeLists.txt index a866251..234509f 100644 --- a/image-filtering/test/CMakeLists.txt +++ b/image-filtering/test/CMakeLists.txt @@ -1,7 +1,7 @@ find_package(ament_cmake_gtest REQUIRED) -ament_add_gtest(${PROJECT_NAME}_test +ament_add_gtest(${PROJECT_NAME}_test image_processing_test.cpp - ../src/image_processing.cpp + ../src/image_processing.cpp ) target_link_libraries(${PROJECT_NAME}_test ${OpenCV_LIBS} # Link OpenCV libraries From 69cd612e957c0f52f88d260ca370f1c09c97cf70 Mon Sep 17 00:00:00 2001 From: Andreas Kluge Svendsrud <89779148+kluge7@users.noreply.github.com> Date: Wed, 26 Mar 2025 10:05:41 +0100 Subject: [PATCH 07/53] docs(README.md): add status badges for CI workflows --- README.md | 3 +++ 1 file changed, 3 insertions(+) diff --git a/README.md b/README.md index c71a986..dca01c0 100644 --- a/README.md +++ b/README.md @@ -1,4 +1,7 @@ # Image Filtering Node +[![Industrial CI](https://github.com/vortexntnu/vortex-image-filtering/actions/workflows/industrial-ci.yml/badge.svg)](https://github.com/vortexntnu/vortex-image-filtering/actions/workflows/industrial-ci.yml) +[![pre-commit.ci status](https://results.pre-commit.ci/badge/github/vortexntnu/vortex-image-filtering/main.svg)](https://results.pre-commit.ci/latest/github/vortexntnu/vortex-image-filtering/main) +[![codecov](https://codecov.io/github/vortexntnu/vortex-image-filtering/graph/badge.svg?token=6XHprkpUsR)](https://codecov.io/github/vortexntnu/vortex-image-filtering) The `image_filtering_node` is a ROS 2 node developed in the `vortex::image_filters` namespace. It is designed to subscribe to image topics, apply various image filters using OpenCV, and publish the filtered images back to ROS. From 250705509f5a745bce98d2dab91440e8730613da Mon Sep 17 00:00:00 2001 From: Jorgen Fjermedal Date: Mon, 14 Apr 2025 23:33:14 +0200 Subject: [PATCH 08/53] otsu filter and refactor --- image-filtering/CMakeLists.txt | 14 +- .../config/image_filtering_params.yaml | 33 +++++ .../image_filters/image_filtering_ros.hpp | 4 +- .../image_filters/image_processing.hpp | 34 ++++- ...ng_launch.py => image_filtering.launch.py} | 2 +- image-filtering/package.xml | 1 - .../params/image_filtering_params.yaml | 23 --- image-filtering/src/image_filtering_ros.cpp | 108 ++++++++------ image-filtering/src/image_processing.cpp | 140 +++++++++++++++++- image-filtering/test/CMakeLists.txt | 21 --- .../test/image_processing_test.cpp | 0 11 files changed, 260 insertions(+), 120 deletions(-) create mode 100644 image-filtering/config/image_filtering_params.yaml rename image-filtering/launch/{image_filtering_launch.py => image_filtering.launch.py} (95%) delete mode 100644 image-filtering/params/image_filtering_params.yaml delete mode 100644 image-filtering/test/CMakeLists.txt delete mode 100644 image-filtering/test/image_processing_test.cpp diff --git a/image-filtering/CMakeLists.txt b/image-filtering/CMakeLists.txt index cc6f6bd..c29732d 100644 --- a/image-filtering/CMakeLists.txt +++ b/image-filtering/CMakeLists.txt @@ -1,7 +1,6 @@ cmake_minimum_required(VERSION 3.8) project(image_filtering) -# === C++ standard === if(NOT CMAKE_CXX_STANDARD) set(CMAKE_CXX_STANDARD 20) endif() @@ -10,13 +9,14 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic) endif() -# find dependencies find_package(ament_cmake REQUIRED) find_package(rclcpp REQUIRED) find_package(rclcpp_components REQUIRED) find_package(OpenCV 4.5.4 REQUIRED) find_package(sensor_msgs REQUIRED) find_package(cv_bridge REQUIRED) +find_package(spdlog REQUIRED) +find_package(fmt REQUIRED) include_directories(include) @@ -42,11 +42,13 @@ ament_target_dependencies(${LIB_NAME} PUBLIC rclcpp_components cv_bridge sensor_msgs + spdlog + fmt ) rclcpp_components_register_node( ${LIB_NAME} - PLUGIN "vortex::image_processing::ImageFilteringNode" + PLUGIN "ImageFilteringNode" EXECUTABLE ${PROJECT_NAME}_node ) @@ -67,12 +69,8 @@ install( install(DIRECTORY launch - params + config DESTINATION share/${PROJECT_NAME}/ ) -if(BUILD_TESTING) - add_subdirectory(test) -endif() - ament_package() diff --git a/image-filtering/config/image_filtering_params.yaml b/image-filtering/config/image_filtering_params.yaml new file mode 100644 index 0000000..d5272f7 --- /dev/null +++ b/image-filtering/config/image_filtering_params.yaml @@ -0,0 +1,33 @@ +/**: + ros__parameters: + sub_topic: "/downwards_camera/image_raw" + pub_topic: "/filtered_image" + output_encoding: "mono8" + filter_params: + filter_type: "otsu" + flip: + flip_code: 1 + unsharpening: + blur_size: 8 + erosion: + size: 1 + dilation: + size: 1 + white_balancing: + contrast_percentage: 0.1 + ebus: + erosion_size: 2 + blur_size: 30 + mask_weight: 5 + otsu: + gsc_weight_r: 1.0 # Grayscale red weight + gsc_weight_g: 0.0 # Grayscale green weight + gsc_weight_b: 0.0 # Grayscale blue weight + gamma_auto_correction: true + gamma_auto_correction_weight: 4.0 + otsu_segmentation: true + erosion_size: 10 + dilation_size: 10 + +# Filter params should reflect the FilterParams struct +# defined in /include/image_filters/image_processing.hpp diff --git a/image-filtering/include/image_filters/image_filtering_ros.hpp b/image-filtering/include/image_filters/image_filtering_ros.hpp index 47702cd..7d2c15e 100644 --- a/image-filtering/include/image_filters/image_filtering_ros.hpp +++ b/image-filtering/include/image_filters/image_filtering_ros.hpp @@ -2,13 +2,13 @@ #define IMAGE_FILTERING_ROS_HPP #include +#include #include #include #include #include #include "image_processing.hpp" -namespace vortex::image_processing { class ImageFilteringNode : public rclcpp::Node { public: explicit ImageFilteringNode(const rclcpp::NodeOptions& options); @@ -103,6 +103,4 @@ class ImageFilteringNode : public rclcpp::Node { std::string filter_; }; -} // namespace vortex::image_processing - #endif // IMAGE_FILTERING_ROS_HPP diff --git a/image-filtering/include/image_filters/image_processing.hpp b/image-filtering/include/image_filters/image_processing.hpp index ac224ae..2453c9e 100644 --- a/image-filtering/include/image_filters/image_processing.hpp +++ b/image-filtering/include/image_filters/image_processing.hpp @@ -3,14 +3,13 @@ #include #include +#include #include #include #include #include #include -namespace vortex::image_processing { - struct FlipParams { int flip_code; }; @@ -36,6 +35,17 @@ struct EbusParams { int mask_weight; }; +struct OtsuParams { + bool gamma_auto_correction; + double gamma_auto_correction_weight; + bool otsu_segmentation; + double gsc_weight_r; + double gsc_weight_g; + double gsc_weight_b; + int erosion_size; + int dilation_size; +}; + struct FilterParams { FlipParams flip; UnsharpeningParams unsharpening; @@ -43,6 +53,7 @@ struct FilterParams { DilatingParams dilating; WhiteBalancingParams white_balancing; EbusParams ebus; + OtsuParams otsu; }; typedef void (*FilterFunction)(const FilterParams&, const cv::Mat&, cv::Mat&); @@ -87,14 +98,14 @@ void unsharpening_filter(const FilterParams& params, /** * Expands the dark areas of the image */ -void eroding_filter(const FilterParams& params, +void erosion_filter(const FilterParams& params, const cv::Mat& original, cv::Mat& modified); /** * Expands the bright areas of the image */ -void dilating_filter(const FilterParams& params, +void dilation_filter(const FilterParams& params, const cv::Mat& original, cv::Mat& modified); @@ -113,15 +124,22 @@ void ebus_filter(const FilterParams& params, const cv::Mat& original, cv::Mat& filtered); +/** + * A filter based on Otsu's method + */ +void otsu_segmentation_filter(const FilterParams& params, + const cv::Mat& original, + cv::Mat& output); + const static std::map filter_functions = { {"no_filter", no_filter}, {"flip", flip_filter}, {"sharpening", sharpening_filter}, {"unsharpening", unsharpening_filter}, - {"eroding", eroding_filter}, - {"dilating", dilating_filter}, + {"erosion", erosion_filter}, + {"dilation", dilation_filter}, {"white_balancing", white_balance_filter}, - {"ebus", ebus_filter}}; + {"ebus", ebus_filter}, + {"otsu", otsu_segmentation_filter}}; -} // namespace vortex::image_processing #endif // IMAGE_PROCESSING_HPP diff --git a/image-filtering/launch/image_filtering_launch.py b/image-filtering/launch/image_filtering.launch.py similarity index 95% rename from image-filtering/launch/image_filtering_launch.py rename to image-filtering/launch/image_filtering.launch.py index a97ab1e..eb01ef5 100644 --- a/image-filtering/launch/image_filtering_launch.py +++ b/image-filtering/launch/image_filtering.launch.py @@ -13,7 +13,7 @@ def generate_launch_description(): parameters=[ os.path.join( get_package_share_directory('image_filtering'), - 'params', + 'config', 'image_filtering_params.yaml', ) ], diff --git a/image-filtering/package.xml b/image-filtering/package.xml index 99a87ad..31aabc9 100644 --- a/image-filtering/package.xml +++ b/image-filtering/package.xml @@ -15,7 +15,6 @@ rclcpp_components cv_bridge sensor_msgs - image_transport rclcpp diff --git a/image-filtering/params/image_filtering_params.yaml b/image-filtering/params/image_filtering_params.yaml deleted file mode 100644 index d8013b8..0000000 --- a/image-filtering/params/image_filtering_params.yaml +++ /dev/null @@ -1,23 +0,0 @@ -image_filtering_node: - ros__parameters: - sub_topic: "/flir_camera/image_raw" - pub_topic: "/filtered_image" - filter_params: - filter_type: "ebus" - flip: - flip_code: 1 - unsharpening: - blur_size: 8 - erosion: - size: 1 - dilation: - size: 1 - white_balancing: - contrast_percentage: 0.8 - ebus: - erosion_size: 2 - blur_size: 30 - mask_weight: 5 - -# Filter params should reflect the FilterParams struct -# defined in /include/image_filters/image_processing.hpp diff --git a/image-filtering/src/image_filtering_ros.cpp b/image-filtering/src/image_filtering_ros.cpp index bcee311..b0e2fca 100644 --- a/image-filtering/src/image_filtering_ros.cpp +++ b/image-filtering/src/image_filtering_ros.cpp @@ -3,24 +3,31 @@ using std::placeholders::_1; -namespace vortex::image_processing { - ImageFilteringNode::ImageFilteringNode(const rclcpp::NodeOptions& options) : Node("image_filtering_node", options) { - this->declare_parameter("sub_topic", "/flir_camera/image_raw"); - this->declare_parameter("pub_topic", "/filtered_image"); - this->declare_parameter("filter_params.filter_type", "ebus"); - this->declare_parameter("filter_params.flip.flip_code", 0); - this->declare_parameter("filter_params.unsharpening.blur_size", 8); - this->declare_parameter("filter_params.erosion.size", 1); - this->declare_parameter("filter_params.dilation.size", 1); + this->declare_parameter("sub_topic"); + this->declare_parameter("pub_topic"); + this->declare_parameter("output_encoding"); + this->declare_parameter("filter_params.filter_type"); + this->declare_parameter("filter_params.flip.flip_code"); + this->declare_parameter("filter_params.unsharpening.blur_size"); + this->declare_parameter("filter_params.erosion.size"); + this->declare_parameter("filter_params.dilation.size"); + this->declare_parameter( + "filter_params.white_balancing.contrast_percentage"); + this->declare_parameter("filter_params.ebus.erosion_size"); + this->declare_parameter("filter_params.ebus.blur_size"); + this->declare_parameter("filter_params.ebus.mask_weight"); + this->declare_parameter("filter_params.otsu.gamma_auto_correction"); this->declare_parameter( - "filter_params.white_balancing.contrast_percentage", 0.8); - this->declare_parameter("filter_params.ebus.erosion_size", 2); - this->declare_parameter("filter_params.ebus.blur_size", 30); - this->declare_parameter("filter_params.ebus.mask_weight", 5); + "filter_params.otsu.gamma_auto_correction_weight"); + this->declare_parameter("filter_params.otsu.otsu_segmentation"); + this->declare_parameter("filter_params.otsu.gsc_weight_r"); + this->declare_parameter("filter_params.otsu.gsc_weight_g"); + this->declare_parameter("filter_params.otsu.gsc_weight_b"); + this->declare_parameter("filter_params.otsu.erosion_size"); + this->declare_parameter("filter_params.otsu.dilation_size"); - // Set up the QoS profile for the image subscriber rmw_qos_profile_t qos_profile = rmw_qos_profile_sensor_data; auto qos_sensor_data = rclcpp::QoS( rclcpp::QoSInitialization(qos_profile.history, 1), qos_profile); @@ -30,8 +37,9 @@ ImageFilteringNode::ImageFilteringNode(const rclcpp::NodeOptions& options) initialize_parameter_handler(); + std::string pub_topic = this->get_parameter("pub_topic").as_string(); image_pub_ = this->create_publisher( - "/filtered_image", qos_sensor_data); + pub_topic, qos_sensor_data); } void ImageFilteringNode::set_filter_params() { @@ -39,9 +47,9 @@ void ImageFilteringNode::set_filter_params() { std::string filter = this->get_parameter("filter_params.filter_type").as_string(); if (!filter_functions.contains(filter)) { - RCLCPP_ERROR_STREAM_THROTTLE( - this->get_logger(), *this->get_clock(), 1000, - "Invalid filter type: " << filter << " Setting to no_filter."); + spdlog::warn( + "Invalid filter type received: {}. Using default: no_filter.", + filter); filter_ = "no_filter"; } else { filter_ = filter; @@ -63,46 +71,63 @@ void ImageFilteringNode::set_filter_params() { this->get_parameter("filter_params.ebus.blur_size").as_int(); params.ebus.mask_weight = this->get_parameter("filter_params.ebus.mask_weight").as_int(); + params.otsu.gamma_auto_correction = + this->get_parameter("filter_params.otsu.gamma_auto_correction") + .as_bool(); + params.otsu.gamma_auto_correction_weight = + this->get_parameter("filter_params.otsu.gamma_auto_correction_weight") + .as_double(); + params.otsu.otsu_segmentation = + this->get_parameter("filter_params.otsu.otsu_segmentation").as_bool(); + params.otsu.gsc_weight_r = + this->get_parameter("filter_params.otsu.gsc_weight_r").as_double(); + params.otsu.gsc_weight_g = + this->get_parameter("filter_params.otsu.gsc_weight_g").as_double(); + params.otsu.gsc_weight_b = + this->get_parameter("filter_params.otsu.gsc_weight_b").as_double(); + params.otsu.erosion_size = + this->get_parameter("filter_params.otsu.erosion_size").as_int(); + params.otsu.dilation_size = + this->get_parameter("filter_params.otsu.dilation_size").as_int(); filter_params_ = params; - RCLCPP_INFO(this->get_logger(), "Filter parameters updated."); + spdlog::info("Filter parameters set: {}", filter); } void ImageFilteringNode::check_and_subscribe_to_image_topic() { std::string image_topic = this->get_parameter("sub_topic").as_string(); if (image_topic_ != image_topic) { + rmw_qos_profile_t qos_profile = rmw_qos_profile_sensor_data; + auto qos_sensor_data = rclcpp::QoS( + rclcpp::QoSInitialization(qos_profile.history, 1), qos_profile); + image_sub_ = this->create_subscription( - image_topic, 10, + image_topic, qos_sensor_data, std::bind(&ImageFilteringNode::image_callback, this, _1)); image_topic_ = image_topic; - RCLCPP_INFO(this->get_logger(), "Subscribed to image topic: %s", - image_topic.c_str()); + spdlog::info("Subscribed to image topic: {}", image_topic); } } void ImageFilteringNode::initialize_parameter_handler() { param_handler_ = std::make_shared(this); - // Register the parameter event callback with the correct signature auto parameter_event_callback = [this](const rcl_interfaces::msg::ParameterEvent& event) -> void { this->on_parameter_event(event); }; - // Register the callback with the parameter event handler param_cb_handle_ = param_handler_->add_parameter_event_callback(parameter_event_callback); } void ImageFilteringNode::on_parameter_event( const rcl_interfaces::msg::ParameterEvent& event) { - // Get the fully qualified name of the current node auto node_name = this->get_fully_qualified_name(); - // Filter out events not related to this node if (event.node != node_name) { - return; // Early return if the event is not from this node + return; } - RCLCPP_INFO(this->get_logger(), "Received parameter event"); + spdlog::info("Parameter event for node: {}", node_name); for (const auto& changed_parameter : event.changed_parameters) { if (changed_parameter.name.find("sub_topic") == 0) check_and_subscribe_to_image_topic(); @@ -113,22 +138,18 @@ void ImageFilteringNode::on_parameter_event( void ImageFilteringNode::image_callback( const sensor_msgs::msg::Image::SharedPtr msg) { - RCLCPP_INFO_ONCE(this->get_logger(), "Received image message."); cv_bridge::CvImagePtr cv_ptr; try { cv_ptr = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::BGR8); if (cv_ptr->image.empty()) { - RCLCPP_WARN_STREAM_THROTTLE( - this->get_logger(), *this->get_clock(), 1000, - "Empty image received, skipping processing."); + spdlog::error("Received empty image, skipping processing."); return; } } catch (cv_bridge::Exception& e) { - RCLCPP_WARN_STREAM_THROTTLE(this->get_logger(), *this->get_clock(), - 1000, "cv_bridge exception: " << e.what()); + spdlog::error("cv_bridge exception: {}", e.what()); return; } @@ -136,24 +157,13 @@ void ImageFilteringNode::image_callback( cv::Mat filtered_image; apply_filter(filter_, filter_params_, input_image, filtered_image); - // auto message = cv_bridge::CvImage(msg->header, "bgr8", - // filtered_image).toImageMsg(); - - // image_pub_->publish(*message); - // Create a unique pointer for the message - // RCLCPP_INFO_STREAM_THROTTLE(this->get_logger(), *this->get_clock(), 1000, - // "Addr of image message: " << msg.get()); + std::string output_encoding = + this->get_parameter("output_encoding").as_string(); auto message = std::make_unique(); - cv_bridge::CvImage(msg->header, "bgr8", filtered_image) + cv_bridge::CvImage(msg->header, output_encoding, filtered_image) .toImageMsg(*message); - // RCLCPP_INFO_STREAM_THROTTLE(this->get_logger(), *this->get_clock(), 1000, - // "Addr of image message filtering: " << message.get()); - - // Publish the message using a unique pointer image_pub_->publish(std::move(message)); } -RCLCPP_COMPONENTS_REGISTER_NODE(vortex::image_processing::ImageFilteringNode) - -} // namespace vortex::image_processing +RCLCPP_COMPONENTS_REGISTER_NODE(ImageFilteringNode) diff --git a/image-filtering/src/image_processing.cpp b/image-filtering/src/image_processing.cpp index 5e1adce..00b4aec 100644 --- a/image-filtering/src/image_processing.cpp +++ b/image-filtering/src/image_processing.cpp @@ -1,8 +1,6 @@ #include #include -namespace vortex::image_processing { - void no_filter([[maybe_unused]] const FilterParams& params, const cv::Mat& original, cv::Mat& filtered) { @@ -40,7 +38,7 @@ void unsharpening_filter(const FilterParams& params, addWeighted(original, 1, mask, 3, 0, filtered); } -void eroding_filter(const FilterParams& params, +void erosion_filter(const FilterParams& params, const cv::Mat& original, cv::Mat& filtered) { int erosion_size = params.eroding.size; @@ -53,7 +51,7 @@ void eroding_filter(const FilterParams& params, cv::erode(original, filtered, element); } -void dilating_filter(const FilterParams& params, +void dilation_filter(const FilterParams& params, const cv::Mat& original, cv::Mat& filtered) { int dilation_size = params.dilating.size; @@ -106,6 +104,138 @@ void ebus_filter(const FilterParams& params, addWeighted(eroded, 1, mask, mask_weight, 0, filtered); } +void applyGammaCorrection(cv::Mat& image, double gamma) { + // Create a lookup table for gamma correction + cv::Mat lookup(1, 256, CV_8U); + uchar* p = lookup.ptr(); + for (int i = 0; i < 256; ++i) { + p[i] = cv::saturate_cast(pow(i / 255.0, gamma) * 255.0); + } + + // Apply the gamma correction using the lookup table + cv::LUT(image, lookup, image); +} + +double calculateAutoGamma(const cv::Mat& image) { + // Convert the image to grayscale if it's not already + cv::Mat grayImage; + if (image.channels() == 3) { + cv::cvtColor(image, grayImage, cv::COLOR_BGR2GRAY); + } else { + grayImage = image; + } + + // Calculate the mean intensity + cv::Scalar meanIntensity = mean(grayImage); + + // The ideal mean intensity is 128 (midpoint for 8-bit grayscale images) + double idealMean = 128.0; + double currentMean = meanIntensity[0]; + + // Automatically set gamma value based on the mean intensity + double gamma; + if (currentMean > 0) { + gamma = log(idealMean / 255.0) / log(currentMean / 255.0); + } else { + gamma = 1.0; // Default gamma if the image has no intensity + } + + // Ensure gamma is within a reasonable range (e.g., between 0.1 and 3.0) + gamma = std::max(0.1, std::min(gamma, 3.0)); + + return gamma; +} + +void otsu_segmentation_filter(const FilterParams& params, + const cv::Mat& original, + cv::Mat& filtered) { + bool gamma_auto_correction = params.otsu.gamma_auto_correction; + double gamma_auto_correction_weight = + params.otsu.gamma_auto_correction_weight; + + bool otsu_segmentation = params.otsu.otsu_segmentation; + + cv::Mat grayImage; + + cv::Matx13f customWeights(params.otsu.gsc_weight_b, + params.otsu.gsc_weight_g, + params.otsu.gsc_weight_r); + cv::transform(original, filtered, customWeights); + + if (gamma_auto_correction) { + double autoGamma = + calculateAutoGamma(filtered) * gamma_auto_correction_weight; + + applyGammaCorrection(filtered, autoGamma); + } + + if (otsu_segmentation) { + // Calculate the histogram + int histSize = 256; + float range[] = {0, 256}; + const float* histRange = {range}; + cv::Mat hist; + calcHist(&filtered, 1, 0, cv::Mat(), hist, 1, &histSize, &histRange, + true, false); + + // Normalize histogram to get probabilities + hist /= filtered.total(); + + // Initialize variables for Otsu's method + std::vector sigma2_list(256, 0.0); + std::vector p(hist.begin(), + hist.end()); // Probabilities + + for (int th = 1; th < 256; ++th) { + // Calculate omega (weights) for foreground and background + float omega_fg = std::accumulate(p.begin(), p.begin() + th, 0.0f); + float omega_bg = std::accumulate(p.begin() + th, p.end(), 0.0f); + + // Calculate mu (means) for foreground and background + float mu_fg = 0, mu_bg = 0; + for (int i = 0; i < th; ++i) { + mu_fg += i * p[i]; + } + for (int i = th; i < 256; ++i) { + mu_bg += i * p[i]; + } + + if (omega_fg > 0) + mu_fg /= omega_fg; + if (omega_bg > 0) + mu_bg /= omega_bg; + + // Calculate sigma squared and store in list + sigma2_list[th] = omega_fg * omega_bg * pow(mu_fg - mu_bg, 2); + } + + // Find the threshold corresponding to the maximum sigma squared + int optimalThreshold = + std::max_element(sigma2_list.begin(), sigma2_list.end()) - + sigma2_list.begin(); + + // Apply the threshold to the image + // cv::Mat binaryImage; + cv::threshold(filtered, filtered, optimalThreshold, 255, + cv::THRESH_BINARY); + + // Apply erosion followed by dilation (opening) + cv::Mat openedImage; + int erosionSize = params.otsu.erosion_size; + cv::Mat erosionKernel = getStructuringElement( + cv::MORPH_CROSS, cv::Size(2 * erosionSize + 1, 2 * erosionSize + 1), + cv::Point(erosionSize, erosionSize)); + cv::erode(filtered, filtered, erosionKernel); + + int dilation_size = params.otsu.dilation_size; + cv::Mat dilationKernel = getStructuringElement( + cv::MORPH_CROSS, + cv::Size(2 * dilation_size + 1, 2 * dilation_size + 1), + cv::Point(dilation_size, dilation_size)); + cv::dilate(filtered, filtered, dilationKernel); + } +} + void apply_filter(const std::string& filter, const FilterParams& params, const cv::Mat& original, @@ -116,5 +246,3 @@ void apply_filter(const std::string& filter, original.copyTo(filtered); // Default to no filter } } - -} // namespace vortex::image_processing diff --git a/image-filtering/test/CMakeLists.txt b/image-filtering/test/CMakeLists.txt deleted file mode 100644 index 234509f..0000000 --- a/image-filtering/test/CMakeLists.txt +++ /dev/null @@ -1,21 +0,0 @@ -find_package(ament_cmake_gtest REQUIRED) -ament_add_gtest(${PROJECT_NAME}_test - image_processing_test.cpp - ../src/image_processing.cpp -) -target_link_libraries(${PROJECT_NAME}_test - ${OpenCV_LIBS} # Link OpenCV libraries - - - # Eigen3::Eigen # Link Eigen3 if your tests require it - # Add any other libraries your tests might require -) - -target_include_directories(${PROJECT_NAME}_test PUBLIC - $ - $ - ${OpenCV_INCLUDE_DIRS} # Ensure OpenCV include dirs are available to the test -) -ament_target_dependencies(${PROJECT_NAME}_test - cv_bridge -) diff --git a/image-filtering/test/image_processing_test.cpp b/image-filtering/test/image_processing_test.cpp deleted file mode 100644 index e69de29..0000000 From 74775ac2c91e93411f85db0662e4b539e45fbc8f Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Sat, 26 Apr 2025 14:26:29 +0200 Subject: [PATCH 09/53] [pre-commit.ci] pre-commit autoupdate (#4) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit updates: - [github.com/astral-sh/ruff-pre-commit: v0.9.2 → v0.11.4](https://github.com/astral-sh/ruff-pre-commit/compare/v0.9.2...v0.11.4) - [github.com/pre-commit/mirrors-clang-format: v19.1.7 → v20.1.0](https://github.com/pre-commit/mirrors-clang-format/compare/v19.1.7...v20.1.0) - [github.com/codespell-project/codespell: v2.3.0 → v2.4.1](https://github.com/codespell-project/codespell/compare/v2.3.0...v2.4.1) Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- .pre-commit-config.yaml | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index e1850c7..aaeb1d7 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -35,7 +35,7 @@ repos: - id: requirements-txt-fixer # Python hooks - repo: https://github.com/astral-sh/ruff-pre-commit - rev: v0.9.2 + rev: v0.11.4 hooks: - id: ruff-format - id: ruff @@ -70,13 +70,13 @@ repos: pass_filenames: true # C++ hooks - repo: https://github.com/pre-commit/mirrors-clang-format - rev: v19.1.7 + rev: v20.1.0 hooks: - id: clang-format args: [--style=file] # Spellcheck in comments and docs - repo: https://github.com/codespell-project/codespell - rev: v2.3.0 + rev: v2.4.1 hooks: - id: codespell args: ['--write-changes', '--ignore-words-list=theses,fom'] From af7438c1ccd21f8f6101d6bb34a2ff67d4a0b79b Mon Sep 17 00:00:00 2001 From: Jorgen Fjermedal Date: Sun, 14 Sep 2025 11:57:41 +0200 Subject: [PATCH 10/53] declare param function --- .../image_filters/image_filtering_ros.hpp | 6 ++++ image-filtering/src/image_filtering_ros.cpp | 29 ++++++++++--------- 2 files changed, 22 insertions(+), 13 deletions(-) diff --git a/image-filtering/include/image_filters/image_filtering_ros.hpp b/image-filtering/include/image_filters/image_filtering_ros.hpp index 7d2c15e..58fa853 100644 --- a/image-filtering/include/image_filters/image_filtering_ros.hpp +++ b/image-filtering/include/image_filters/image_filtering_ros.hpp @@ -33,6 +33,12 @@ class ImageFilteringNode : public rclcpp::Node { */ void check_and_subscribe_to_image_topic(); + /** + * @brief Declare the ros2 parameters used by the node. + * + */ + void declare_parameters(); + /** * @brief Set the filter parameters for the FilterParams struct. * diff --git a/image-filtering/src/image_filtering_ros.cpp b/image-filtering/src/image_filtering_ros.cpp index b0e2fca..1f3f15e 100644 --- a/image-filtering/src/image_filtering_ros.cpp +++ b/image-filtering/src/image_filtering_ros.cpp @@ -5,6 +5,22 @@ using std::placeholders::_1; ImageFilteringNode::ImageFilteringNode(const rclcpp::NodeOptions& options) : Node("image_filtering_node", options) { + + declare_parameters(); + check_and_subscribe_to_image_topic(); + set_filter_params(); + initialize_parameter_handler(); + + rmw_qos_profile_t qos_profile = rmw_qos_profile_sensor_data; + auto qos_sensor_data = rclcpp::QoS( + rclcpp::QoSInitialization(qos_profile.history, 1), qos_profile); + + std::string pub_topic = this->get_parameter("pub_topic").as_string(); + image_pub_ = this->create_publisher( + pub_topic, qos_sensor_data); +} + +void ImageFilteringNode::declare_parameters() { this->declare_parameter("sub_topic"); this->declare_parameter("pub_topic"); this->declare_parameter("output_encoding"); @@ -27,19 +43,6 @@ ImageFilteringNode::ImageFilteringNode(const rclcpp::NodeOptions& options) this->declare_parameter("filter_params.otsu.gsc_weight_b"); this->declare_parameter("filter_params.otsu.erosion_size"); this->declare_parameter("filter_params.otsu.dilation_size"); - - rmw_qos_profile_t qos_profile = rmw_qos_profile_sensor_data; - auto qos_sensor_data = rclcpp::QoS( - rclcpp::QoSInitialization(qos_profile.history, 1), qos_profile); - - check_and_subscribe_to_image_topic(); - set_filter_params(); - - initialize_parameter_handler(); - - std::string pub_topic = this->get_parameter("pub_topic").as_string(); - image_pub_ = this->create_publisher( - pub_topic, qos_sensor_data); } void ImageFilteringNode::set_filter_params() { From a52c845db47e476be3bcaf4ee96fb5f976496dd0 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Sun, 14 Sep 2025 10:00:18 +0000 Subject: [PATCH 11/53] [pre-commit.ci] auto fixes from pre-commit.com hooks for more information, see https://pre-commit.ci --- image-filtering/src/image_filtering_ros.cpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/image-filtering/src/image_filtering_ros.cpp b/image-filtering/src/image_filtering_ros.cpp index 1f3f15e..a6a8421 100644 --- a/image-filtering/src/image_filtering_ros.cpp +++ b/image-filtering/src/image_filtering_ros.cpp @@ -5,12 +5,11 @@ using std::placeholders::_1; ImageFilteringNode::ImageFilteringNode(const rclcpp::NodeOptions& options) : Node("image_filtering_node", options) { - declare_parameters(); check_and_subscribe_to_image_topic(); set_filter_params(); initialize_parameter_handler(); - + rmw_qos_profile_t qos_profile = rmw_qos_profile_sensor_data; auto qos_sensor_data = rclcpp::QoS( rclcpp::QoSInitialization(qos_profile.history, 1), qos_profile); From d6edc65c72ae672a9dd3dad2469ddeb482ad8d7c Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Mon, 7 Jul 2025 19:13:18 +0000 Subject: [PATCH 12/53] [pre-commit.ci] pre-commit autoupdate MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit updates: - [github.com/astral-sh/ruff-pre-commit: v0.11.4 → v0.12.2](https://github.com/astral-sh/ruff-pre-commit/compare/v0.11.4...v0.12.2) - [github.com/pre-commit/mirrors-clang-format: v20.1.0 → v20.1.7](https://github.com/pre-commit/mirrors-clang-format/compare/v20.1.0...v20.1.7) --- .pre-commit-config.yaml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index aaeb1d7..d145b28 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -35,7 +35,7 @@ repos: - id: requirements-txt-fixer # Python hooks - repo: https://github.com/astral-sh/ruff-pre-commit - rev: v0.11.4 + rev: v0.12.2 hooks: - id: ruff-format - id: ruff @@ -70,7 +70,7 @@ repos: pass_filenames: true # C++ hooks - repo: https://github.com/pre-commit/mirrors-clang-format - rev: v20.1.0 + rev: v20.1.7 hooks: - id: clang-format args: [--style=file] From 29291ad4519e7852fbb3d19c611636ed46a7476b Mon Sep 17 00:00:00 2001 From: Thomas Date: Wed, 22 Oct 2025 19:08:55 +0200 Subject: [PATCH 13/53] Adding the gauian blur but without the 5th step (declare and asign parameters) --- .vscode/c_cpp_properties.json | 21 +++++++++++ .vscode/settings.json | 10 ++++++ image-filtering/.vscode/c_cpp_properties.json | 21 +++++++++++ image-filtering/.vscode/settings.json | 13 +++++++ .../config/image_filtering_params.yaml | 6 ++-- .../image_filters/image_processing.hpp | 15 +++++++- image-filtering/log/COLCON_IGNORE | 0 image-filtering/log/latest | 1 + image-filtering/log/latest_list | 1 + .../list_2025-10-22_12-58-52/logger_all.log | 19 ++++++++++ image-filtering/src/image_processing.cpp | 14 ++++++++ log/COLCON_IGNORE | 0 log/latest | 1 + log/latest_list | 1 + log/list_2025-10-22_13-00-40/logger_all.log | 36 +++++++++++++++++++ 15 files changed, 155 insertions(+), 4 deletions(-) create mode 100644 .vscode/c_cpp_properties.json create mode 100644 .vscode/settings.json create mode 100644 image-filtering/.vscode/c_cpp_properties.json create mode 100644 image-filtering/.vscode/settings.json create mode 100644 image-filtering/log/COLCON_IGNORE create mode 120000 image-filtering/log/latest create mode 120000 image-filtering/log/latest_list create mode 100644 image-filtering/log/list_2025-10-22_12-58-52/logger_all.log create mode 100644 log/COLCON_IGNORE create mode 120000 log/latest create mode 120000 log/latest_list create mode 100644 log/list_2025-10-22_13-00-40/logger_all.log diff --git a/.vscode/c_cpp_properties.json b/.vscode/c_cpp_properties.json new file mode 100644 index 0000000..adb1194 --- /dev/null +++ b/.vscode/c_cpp_properties.json @@ -0,0 +1,21 @@ +{ + "configurations": [ + { + "browse": { + "databaseFilename": "${workspaceFolder}/.vscode/browse.vc.db", + "limitSymbolsToIncludedHeaders": false + }, + "includePath": [ + "/opt/ros/humble/include/**", + "/home/thomas/ros2_ws/src/vortex-image-filtering/image-filtering/include/**", + "/usr/include/**" + ], + "name": "ros2", + "intelliSenseMode": "gcc-x64", + "compilerPath": "/usr/bin/gcc", + "cStandard": "gnu11", + "cppStandard": "c++17" + } + ], + "version": 4 +} \ No newline at end of file diff --git a/.vscode/settings.json b/.vscode/settings.json new file mode 100644 index 0000000..593784e --- /dev/null +++ b/.vscode/settings.json @@ -0,0 +1,10 @@ +{ + "python.autoComplete.extraPaths": [ + "/opt/ros/humble/lib/python3.10/site-packages", + "/opt/ros/humble/local/lib/python3.10/dist-packages" + ], + "python.analysis.extraPaths": [ + "/opt/ros/humble/lib/python3.10/site-packages", + "/opt/ros/humble/local/lib/python3.10/dist-packages" + ] +} \ No newline at end of file diff --git a/image-filtering/.vscode/c_cpp_properties.json b/image-filtering/.vscode/c_cpp_properties.json new file mode 100644 index 0000000..adb1194 --- /dev/null +++ b/image-filtering/.vscode/c_cpp_properties.json @@ -0,0 +1,21 @@ +{ + "configurations": [ + { + "browse": { + "databaseFilename": "${workspaceFolder}/.vscode/browse.vc.db", + "limitSymbolsToIncludedHeaders": false + }, + "includePath": [ + "/opt/ros/humble/include/**", + "/home/thomas/ros2_ws/src/vortex-image-filtering/image-filtering/include/**", + "/usr/include/**" + ], + "name": "ros2", + "intelliSenseMode": "gcc-x64", + "compilerPath": "/usr/bin/gcc", + "cStandard": "gnu11", + "cppStandard": "c++17" + } + ], + "version": 4 +} \ No newline at end of file diff --git a/image-filtering/.vscode/settings.json b/image-filtering/.vscode/settings.json new file mode 100644 index 0000000..4657751 --- /dev/null +++ b/image-filtering/.vscode/settings.json @@ -0,0 +1,13 @@ +{ + "python.autoComplete.extraPaths": [ + "/opt/ros/humble/lib/python3.10/site-packages", + "/opt/ros/humble/local/lib/python3.10/dist-packages" + ], + "python.analysis.extraPaths": [ + "/opt/ros/humble/lib/python3.10/site-packages", + "/opt/ros/humble/local/lib/python3.10/dist-packages" + ], + "files.associations": { + "string_view": "cpp" + } +} \ No newline at end of file diff --git a/image-filtering/config/image_filtering_params.yaml b/image-filtering/config/image_filtering_params.yaml index d5272f7..630291b 100644 --- a/image-filtering/config/image_filtering_params.yaml +++ b/image-filtering/config/image_filtering_params.yaml @@ -1,10 +1,10 @@ /**: ros__parameters: - sub_topic: "/downwards_camera/image_raw" + sub_topic: "/fls_publisher/display" pub_topic: "/filtered_image" - output_encoding: "mono8" + output_encoding: "bgr8" filter_params: - filter_type: "otsu" + filter_type: "flip" flip: flip_code: 1 unsharpening: diff --git a/image-filtering/include/image_filters/image_processing.hpp b/image-filtering/include/image_filters/image_processing.hpp index 2453c9e..73ca604 100644 --- a/image-filtering/include/image_filters/image_processing.hpp +++ b/image-filtering/include/image_filters/image_processing.hpp @@ -46,6 +46,12 @@ struct OtsuParams { int dilation_size; }; +// Thomas' masterpiece +struct GaussianBlurParams{ + int blur_strength; + +}; + struct FilterParams { FlipParams flip; UnsharpeningParams unsharpening; @@ -54,6 +60,7 @@ struct FilterParams { WhiteBalancingParams white_balancing; EbusParams ebus; OtsuParams otsu; + GaussianBlurParams gausian_blur; }; typedef void (*FilterFunction)(const FilterParams&, const cv::Mat&, cv::Mat&); @@ -131,6 +138,11 @@ void otsu_segmentation_filter(const FilterParams& params, const cv::Mat& original, cv::Mat& output); +void gaussian_blur(const FilterParams& filter_params, + const cv::Mat &original, + cv::Mat &filtered); + + const static std::map filter_functions = { {"no_filter", no_filter}, {"flip", flip_filter}, @@ -140,6 +152,7 @@ const static std::map filter_functions = { {"dilation", dilation_filter}, {"white_balancing", white_balance_filter}, {"ebus", ebus_filter}, - {"otsu", otsu_segmentation_filter}}; + {"otsu", otsu_segmentation_filter}, + {"gaussian_blur", gaussian_blur}}; #endif // IMAGE_PROCESSING_HPP diff --git a/image-filtering/log/COLCON_IGNORE b/image-filtering/log/COLCON_IGNORE new file mode 100644 index 0000000..e69de29 diff --git a/image-filtering/log/latest b/image-filtering/log/latest new file mode 120000 index 0000000..1715667 --- /dev/null +++ b/image-filtering/log/latest @@ -0,0 +1 @@ +latest_list \ No newline at end of file diff --git a/image-filtering/log/latest_list b/image-filtering/log/latest_list new file mode 120000 index 0000000..b1c2701 --- /dev/null +++ b/image-filtering/log/latest_list @@ -0,0 +1 @@ +list_2025-10-22_12-58-52 \ No newline at end of file diff --git a/image-filtering/log/list_2025-10-22_12-58-52/logger_all.log b/image-filtering/log/list_2025-10-22_12-58-52/logger_all.log new file mode 100644 index 0000000..57f6a8a --- /dev/null +++ b/image-filtering/log/list_2025-10-22_12-58-52/logger_all.log @@ -0,0 +1,19 @@ +[0.403s] DEBUG:colcon:Command line arguments: ['/usr/bin/colcon', 'list', '-p', '--base-paths', '/home/thomas/ros2_ws/src/vortex-image-filtering/image-filtering', '--log-base', '/dev/null'] +[0.404s] DEBUG:colcon:Parsed command line arguments: Namespace(log_base=None, log_level=None, verb_name='list', build_base='build', ignore_user_meta=False, metas=['./colcon.meta'], base_paths=['/home/thomas/ros2_ws/src/vortex-image-filtering/image-filtering', '--log-base', '/dev/null'], packages_ignore=None, packages_ignore_regex=None, paths=None, packages_up_to=None, packages_up_to_regex=None, packages_above=None, packages_above_and_dependencies=None, packages_above_depth=None, packages_select_by_dep=None, packages_skip_by_dep=None, packages_skip_up_to=None, packages_select_build_failed=False, packages_skip_build_finished=False, packages_select_test_failures=False, packages_skip_test_passed=False, packages_select=None, packages_skip=None, packages_select_regex=None, packages_skip_regex=None, packages_start=None, packages_end=None, allow_overriding=[], topological_order=False, names_only=False, paths_only=True, topological_graph=False, topological_graph_dot=False, topological_graph_density=False, topological_graph_legend=False, topological_graph_dot_cluster=False, topological_graph_dot_include_skipped=False, mixin_files=None, mixin=None, verb_parser=, verb_extension=, main=>, mixin_verb=('list',)) +[1.029s] Level 1:colcon.colcon_core.package_discovery:discover_packages(colcon_meta) check parameters +[1.029s] Level 1:colcon.colcon_core.package_discovery:discover_packages(recursive) check parameters +[1.029s] Level 1:colcon.colcon_core.package_discovery:discover_packages(ignore) check parameters +[1.029s] Level 1:colcon.colcon_core.package_discovery:discover_packages(path) check parameters +[1.029s] Level 1:colcon.colcon_core.package_discovery:discover_packages(recursive) discover +[1.030s] INFO:colcon.colcon_core.package_discovery:Crawling recursively for packages in '/home/thomas/ros2_ws/src/vortex-image-filtering/image-filtering', '/home/thomas/ros2_ws/src/vortex-image-filtering/image-filtering/--log-base', '/dev/null' +[1.030s] Level 1:colcon.colcon_core.package_identification:_identify(/home/thomas/ros2_ws/src/vortex-image-filtering/image-filtering) by extensions ['ignore', 'ignore_ament_install'] +[1.031s] Level 1:colcon.colcon_core.package_identification:_identify(/home/thomas/ros2_ws/src/vortex-image-filtering/image-filtering) by extension 'ignore' +[1.031s] Level 1:colcon.colcon_core.package_identification:_identify(/home/thomas/ros2_ws/src/vortex-image-filtering/image-filtering) by extension 'ignore_ament_install' +[1.032s] Level 1:colcon.colcon_core.package_identification:_identify(/home/thomas/ros2_ws/src/vortex-image-filtering/image-filtering) by extensions ['colcon_pkg'] +[1.032s] Level 1:colcon.colcon_core.package_identification:_identify(/home/thomas/ros2_ws/src/vortex-image-filtering/image-filtering) by extension 'colcon_pkg' +[1.032s] Level 1:colcon.colcon_core.package_identification:_identify(/home/thomas/ros2_ws/src/vortex-image-filtering/image-filtering) by extensions ['colcon_meta'] +[1.032s] Level 1:colcon.colcon_core.package_identification:_identify(/home/thomas/ros2_ws/src/vortex-image-filtering/image-filtering) by extension 'colcon_meta' +[1.032s] Level 1:colcon.colcon_core.package_identification:_identify(/home/thomas/ros2_ws/src/vortex-image-filtering/image-filtering) by extensions ['ros'] +[1.032s] Level 1:colcon.colcon_core.package_identification:_identify(/home/thomas/ros2_ws/src/vortex-image-filtering/image-filtering) by extension 'ros' +[1.121s] DEBUG:colcon.colcon_core.package_identification:Package '/home/thomas/ros2_ws/src/vortex-image-filtering/image-filtering' with type 'ros.ament_cmake' and name 'image_filtering' +[1.121s] Level 1:colcon.colcon_core.package_discovery:discover_packages(recursive) using defaults diff --git a/image-filtering/src/image_processing.cpp b/image-filtering/src/image_processing.cpp index 00b4aec..60c5d53 100644 --- a/image-filtering/src/image_processing.cpp +++ b/image-filtering/src/image_processing.cpp @@ -236,6 +236,20 @@ void otsu_segmentation_filter(const FilterParams& params, } } +void your_filter_function(const FilterParams& filter_params, + const cv::Mat &original, + cv::Mat &filtered){ + // Access your filter-specific parameters like this: + int example_param = filter_params.your_filter.example_param; + + // Implement your filtering logic here + + int flip_code = params.flip.flip_code; // !! for testing only + cv::flip(original, filtered, flip_code); + + +} + void apply_filter(const std::string& filter, const FilterParams& params, const cv::Mat& original, diff --git a/log/COLCON_IGNORE b/log/COLCON_IGNORE new file mode 100644 index 0000000..e69de29 diff --git a/log/latest b/log/latest new file mode 120000 index 0000000..1715667 --- /dev/null +++ b/log/latest @@ -0,0 +1 @@ +latest_list \ No newline at end of file diff --git a/log/latest_list b/log/latest_list new file mode 120000 index 0000000..dd5d041 --- /dev/null +++ b/log/latest_list @@ -0,0 +1 @@ +list_2025-10-22_13-00-40 \ No newline at end of file diff --git a/log/list_2025-10-22_13-00-40/logger_all.log b/log/list_2025-10-22_13-00-40/logger_all.log new file mode 100644 index 0000000..d91e83d --- /dev/null +++ b/log/list_2025-10-22_13-00-40/logger_all.log @@ -0,0 +1,36 @@ +[0.380s] DEBUG:colcon:Command line arguments: ['/usr/bin/colcon', 'list', '-p', '--base-paths', '/home/thomas/ros2_ws/src/vortex-image-filtering', '--log-base', '/dev/null'] +[0.381s] DEBUG:colcon:Parsed command line arguments: Namespace(log_base=None, log_level=None, verb_name='list', build_base='build', ignore_user_meta=False, metas=['./colcon.meta'], base_paths=['/home/thomas/ros2_ws/src/vortex-image-filtering', '--log-base', '/dev/null'], packages_ignore=None, packages_ignore_regex=None, paths=None, packages_up_to=None, packages_up_to_regex=None, packages_above=None, packages_above_and_dependencies=None, packages_above_depth=None, packages_select_by_dep=None, packages_skip_by_dep=None, packages_skip_up_to=None, packages_select_build_failed=False, packages_skip_build_finished=False, packages_select_test_failures=False, packages_skip_test_passed=False, packages_select=None, packages_skip=None, packages_select_regex=None, packages_skip_regex=None, packages_start=None, packages_end=None, allow_overriding=[], topological_order=False, names_only=False, paths_only=True, topological_graph=False, topological_graph_dot=False, topological_graph_density=False, topological_graph_legend=False, topological_graph_dot_cluster=False, topological_graph_dot_include_skipped=False, mixin_files=None, mixin=None, verb_parser=, verb_extension=, main=>, mixin_verb=('list',)) +[0.967s] Level 1:colcon.colcon_core.package_discovery:discover_packages(colcon_meta) check parameters +[0.967s] Level 1:colcon.colcon_core.package_discovery:discover_packages(recursive) check parameters +[0.967s] Level 1:colcon.colcon_core.package_discovery:discover_packages(ignore) check parameters +[0.967s] Level 1:colcon.colcon_core.package_discovery:discover_packages(path) check parameters +[0.967s] Level 1:colcon.colcon_core.package_discovery:discover_packages(recursive) discover +[0.968s] INFO:colcon.colcon_core.package_discovery:Crawling recursively for packages in '/home/thomas/ros2_ws/src/vortex-image-filtering', '/home/thomas/ros2_ws/src/vortex-image-filtering/--log-base', '/dev/null' +[0.968s] Level 1:colcon.colcon_core.package_identification:_identify(/home/thomas/ros2_ws/src/vortex-image-filtering) by extensions ['ignore', 'ignore_ament_install'] +[0.969s] Level 1:colcon.colcon_core.package_identification:_identify(/home/thomas/ros2_ws/src/vortex-image-filtering) by extension 'ignore' +[0.970s] Level 1:colcon.colcon_core.package_identification:_identify(/home/thomas/ros2_ws/src/vortex-image-filtering) by extension 'ignore_ament_install' +[0.970s] Level 1:colcon.colcon_core.package_identification:_identify(/home/thomas/ros2_ws/src/vortex-image-filtering) by extensions ['colcon_pkg'] +[0.970s] Level 1:colcon.colcon_core.package_identification:_identify(/home/thomas/ros2_ws/src/vortex-image-filtering) by extension 'colcon_pkg' +[0.970s] Level 1:colcon.colcon_core.package_identification:_identify(/home/thomas/ros2_ws/src/vortex-image-filtering) by extensions ['colcon_meta'] +[0.970s] Level 1:colcon.colcon_core.package_identification:_identify(/home/thomas/ros2_ws/src/vortex-image-filtering) by extension 'colcon_meta' +[0.970s] Level 1:colcon.colcon_core.package_identification:_identify(/home/thomas/ros2_ws/src/vortex-image-filtering) by extensions ['ros'] +[0.970s] Level 1:colcon.colcon_core.package_identification:_identify(/home/thomas/ros2_ws/src/vortex-image-filtering) by extension 'ros' +[1.051s] Level 1:colcon.colcon_core.package_identification:_identify(/home/thomas/ros2_ws/src/vortex-image-filtering) by extensions ['cmake', 'python'] +[1.052s] Level 1:colcon.colcon_core.package_identification:_identify(/home/thomas/ros2_ws/src/vortex-image-filtering) by extension 'cmake' +[1.052s] Level 1:colcon.colcon_core.package_identification:_identify(/home/thomas/ros2_ws/src/vortex-image-filtering) by extension 'python' +[1.052s] Level 1:colcon.colcon_core.package_identification:_identify(/home/thomas/ros2_ws/src/vortex-image-filtering) by extensions ['python_setup_py'] +[1.052s] Level 1:colcon.colcon_core.package_identification:_identify(/home/thomas/ros2_ws/src/vortex-image-filtering) by extension 'python_setup_py' +[1.053s] Level 1:colcon.colcon_core.package_identification:_identify(/home/thomas/ros2_ws/src/vortex-image-filtering/image-filtering) by extensions ['ignore', 'ignore_ament_install'] +[1.053s] Level 1:colcon.colcon_core.package_identification:_identify(/home/thomas/ros2_ws/src/vortex-image-filtering/image-filtering) by extension 'ignore' +[1.054s] Level 1:colcon.colcon_core.package_identification:_identify(/home/thomas/ros2_ws/src/vortex-image-filtering/image-filtering) by extension 'ignore_ament_install' +[1.054s] Level 1:colcon.colcon_core.package_identification:_identify(/home/thomas/ros2_ws/src/vortex-image-filtering/image-filtering) by extensions ['colcon_pkg'] +[1.054s] Level 1:colcon.colcon_core.package_identification:_identify(/home/thomas/ros2_ws/src/vortex-image-filtering/image-filtering) by extension 'colcon_pkg' +[1.054s] Level 1:colcon.colcon_core.package_identification:_identify(/home/thomas/ros2_ws/src/vortex-image-filtering/image-filtering) by extensions ['colcon_meta'] +[1.054s] Level 1:colcon.colcon_core.package_identification:_identify(/home/thomas/ros2_ws/src/vortex-image-filtering/image-filtering) by extension 'colcon_meta' +[1.054s] Level 1:colcon.colcon_core.package_identification:_identify(/home/thomas/ros2_ws/src/vortex-image-filtering/image-filtering) by extensions ['ros'] +[1.054s] Level 1:colcon.colcon_core.package_identification:_identify(/home/thomas/ros2_ws/src/vortex-image-filtering/image-filtering) by extension 'ros' +[1.065s] DEBUG:colcon.colcon_core.package_identification:Package '/home/thomas/ros2_ws/src/vortex-image-filtering/image-filtering' with type 'ros.ament_cmake' and name 'image_filtering' +[1.066s] Level 1:colcon.colcon_core.package_identification:_identify(/home/thomas/ros2_ws/src/vortex-image-filtering/log) by extensions ['ignore', 'ignore_ament_install'] +[1.066s] Level 1:colcon.colcon_core.package_identification:_identify(/home/thomas/ros2_ws/src/vortex-image-filtering/log) by extension 'ignore' +[1.066s] Level 1:colcon.colcon_core.package_identification:_identify(/home/thomas/ros2_ws/src/vortex-image-filtering/log) ignored +[1.067s] Level 1:colcon.colcon_core.package_discovery:discover_packages(recursive) using defaults From 8719586881450ccc01a39d8572eb92ff4e165a19 Mon Sep 17 00:00:00 2001 From: Thomas Date: Sun, 26 Oct 2025 17:06:37 +0100 Subject: [PATCH 14/53] Adding future stuf --- .../config/image_filtering_params.yaml | 8 ++++-- .../image_filters/image_processing.hpp | 17 +++++++---- image-filtering/src/image_filtering_ros.cpp | 9 ++++++ image-filtering/src/image_processing.cpp | 28 +++++++++++++++---- 4 files changed, 48 insertions(+), 14 deletions(-) diff --git a/image-filtering/config/image_filtering_params.yaml b/image-filtering/config/image_filtering_params.yaml index 630291b..4723f12 100644 --- a/image-filtering/config/image_filtering_params.yaml +++ b/image-filtering/config/image_filtering_params.yaml @@ -3,8 +3,10 @@ sub_topic: "/fls_publisher/display" pub_topic: "/filtered_image" output_encoding: "bgr8" + input_encoding: "bgr8" + filter_params: - filter_type: "flip" + filter_type: "overlap" flip: flip_code: 1 unsharpening: @@ -26,8 +28,8 @@ gamma_auto_correction: true gamma_auto_correction_weight: 4.0 otsu_segmentation: true - erosion_size: 10 - dilation_size: 10 + erosion_size: 1 + dilation_size: 2 # Filter params should reflect the FilterParams struct # defined in /include/image_filters/image_processing.hpp diff --git a/image-filtering/include/image_filters/image_processing.hpp b/image-filtering/include/image_filters/image_processing.hpp index 73ca604..21515be 100644 --- a/image-filtering/include/image_filters/image_processing.hpp +++ b/image-filtering/include/image_filters/image_processing.hpp @@ -47,9 +47,9 @@ struct OtsuParams { }; // Thomas' masterpiece -struct GaussianBlurParams{ - int blur_strength; - +struct OverlapParams{ + bool is_first=true; + cv::Mat previous_image; }; struct FilterParams { @@ -60,7 +60,7 @@ struct FilterParams { WhiteBalancingParams white_balancing; EbusParams ebus; OtsuParams otsu; - GaussianBlurParams gausian_blur; + OverlapParams overlap; }; typedef void (*FilterFunction)(const FilterParams&, const cv::Mat&, cv::Mat&); @@ -138,7 +138,8 @@ void otsu_segmentation_filter(const FilterParams& params, const cv::Mat& original, cv::Mat& output); -void gaussian_blur(const FilterParams& filter_params, + +void overlap_filter(const FilterParams& filter_params, const cv::Mat &original, cv::Mat &filtered); @@ -153,6 +154,10 @@ const static std::map filter_functions = { {"white_balancing", white_balance_filter}, {"ebus", ebus_filter}, {"otsu", otsu_segmentation_filter}, - {"gaussian_blur", gaussian_blur}}; + {"overlap", overlap_filter} // This was done by the one and only Thomas +}; #endif // IMAGE_PROCESSING_HPP + + + diff --git a/image-filtering/src/image_filtering_ros.cpp b/image-filtering/src/image_filtering_ros.cpp index a6a8421..3d5c849 100644 --- a/image-filtering/src/image_filtering_ros.cpp +++ b/image-filtering/src/image_filtering_ros.cpp @@ -23,6 +23,7 @@ void ImageFilteringNode::declare_parameters() { this->declare_parameter("sub_topic"); this->declare_parameter("pub_topic"); this->declare_parameter("output_encoding"); + this->declare_parameter("input_encoding"); this->declare_parameter("filter_params.filter_type"); this->declare_parameter("filter_params.flip.flip_code"); this->declare_parameter("filter_params.unsharpening.blur_size"); @@ -42,6 +43,9 @@ void ImageFilteringNode::declare_parameters() { this->declare_parameter("filter_params.otsu.gsc_weight_b"); this->declare_parameter("filter_params.otsu.erosion_size"); this->declare_parameter("filter_params.otsu.dilation_size"); + + // this->declare_parameter(//Thomas has left a mark here + // "filter_params.gaussian_blur.blur_strength"); } void ImageFilteringNode::set_filter_params() { @@ -91,6 +95,8 @@ void ImageFilteringNode::set_filter_params() { this->get_parameter("filter_params.otsu.erosion_size").as_int(); params.otsu.dilation_size = this->get_parameter("filter_params.otsu.dilation_size").as_int(); + // params.gaussian_blur.blur_strength = // Thomas is everyware + // this->get_parameter("filter_params.gaussian_blur.blur_strength").as_int(); filter_params_ = params; spdlog::info("Filter parameters set: {}", filter); } @@ -142,6 +148,9 @@ void ImageFilteringNode::image_callback( const sensor_msgs::msg::Image::SharedPtr msg) { cv_bridge::CvImagePtr cv_ptr; + std::string input_encoding = + this->get_parameter("input_encoding").as_string(); + try { cv_ptr = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::BGR8); diff --git a/image-filtering/src/image_processing.cpp b/image-filtering/src/image_processing.cpp index 60c5d53..f244673 100644 --- a/image-filtering/src/image_processing.cpp +++ b/image-filtering/src/image_processing.cpp @@ -236,18 +236,36 @@ void otsu_segmentation_filter(const FilterParams& params, } } -void your_filter_function(const FilterParams& filter_params, +// Thomas was here +void overlap_filter(const FilterParams& filter_params, const cv::Mat &original, cv::Mat &filtered){ + static cv::Mat prev; + static bool has_prev = false; + + if (!has_prev) { + original.copyTo(filtered); // first call: just pass through + prev = original.clone(); // snapshot (clone so it’s independent) + has_prev = true; + return; + } + + + // cv::add(original, prev, filtered); + cv::cvtColor(original, gray, cv::COLOR_BGR2GRAY); + + cv::addWeighted(prev, 0.5, original, 0.5, 0.0, filtered); + // Access your filter-specific parameters like this: - int example_param = filter_params.your_filter.example_param; + // int example_param = filter_params.gaussian_blur.blur_strength; // Implement your filtering logic here - int flip_code = params.flip.flip_code; // !! for testing only - cv::flip(original, filtered, flip_code); + + // int flip_code = filter_params.flip.flip_code; // 0: x-axis, 1: y-axis, -1: both + // cv::flip(original, filtered, flip_code); + - } void apply_filter(const std::string& filter, From 45d7cf8fdf3ce78d040a17bfb81657a60ae0f205 Mon Sep 17 00:00:00 2001 From: Thomas Date: Sun, 2 Nov 2025 11:29:49 +0100 Subject: [PATCH 15/53] adding new files and filters --- .vscode/browse.vc.db-wal | 0 .vscode/settings.json | 5 +- image-filtering/CMakeLists.txt | 1 + .../config/image_filtering_params.yaml | 17 ++- .../image_filters/image_processing.hpp | 28 ++++- .../include/image_filters/utils.hpp | 18 +++ image-filtering/package.xml | 2 +- image-filtering/src/image_filtering_ros.cpp | 38 ++++-- image-filtering/src/image_processing.cpp | 116 +++++++++++++++--- image-filtering/src/utils.cpp | 21 ++++ 10 files changed, 206 insertions(+), 40 deletions(-) create mode 100644 .vscode/browse.vc.db-wal create mode 100644 image-filtering/include/image_filters/utils.hpp create mode 100644 image-filtering/src/utils.cpp diff --git a/.vscode/browse.vc.db-wal b/.vscode/browse.vc.db-wal new file mode 100644 index 0000000..e69de29 diff --git a/.vscode/settings.json b/.vscode/settings.json index 593784e..4657751 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -6,5 +6,8 @@ "python.analysis.extraPaths": [ "/opt/ros/humble/lib/python3.10/site-packages", "/opt/ros/humble/local/lib/python3.10/dist-packages" - ] + ], + "files.associations": { + "string_view": "cpp" + } } \ No newline at end of file diff --git a/image-filtering/CMakeLists.txt b/image-filtering/CMakeLists.txt index c29732d..ff4d3c8 100644 --- a/image-filtering/CMakeLists.txt +++ b/image-filtering/CMakeLists.txt @@ -25,6 +25,7 @@ set(LIB_NAME "${PROJECT_NAME}_component") add_library(${LIB_NAME} SHARED src/image_processing.cpp src/image_filtering_ros.cpp + src/utils.cpp ) target_link_libraries(${LIB_NAME} PUBLIC diff --git a/image-filtering/config/image_filtering_params.yaml b/image-filtering/config/image_filtering_params.yaml index 4723f12..ee6833d 100644 --- a/image-filtering/config/image_filtering_params.yaml +++ b/image-filtering/config/image_filtering_params.yaml @@ -2,11 +2,11 @@ ros__parameters: sub_topic: "/fls_publisher/display" pub_topic: "/filtered_image" - output_encoding: "bgr8" - input_encoding: "bgr8" + output_encoding: "mono8" + input_encoding: "" filter_params: - filter_type: "overlap" + filter_type: "binary" flip: flip_code: 1 unsharpening: @@ -28,8 +28,17 @@ gamma_auto_correction: true gamma_auto_correction_weight: 4.0 otsu_segmentation: true - erosion_size: 1 + erosion_size: 2 dilation_size: 2 + overlap: + percentage_threshold: 100.0 # Percentage to cap the picel intensity differance + median: # finds the median of each n x n square around each pixel + kernel_size: 5 # must be odd >= 3 + binary: + threshold: 20. + maxval: 255. + invert: false + # Filter params should reflect the FilterParams struct # defined in /include/image_filters/image_processing.hpp diff --git a/image-filtering/include/image_filters/image_processing.hpp b/image-filtering/include/image_filters/image_processing.hpp index 21515be..ed6bc73 100644 --- a/image-filtering/include/image_filters/image_processing.hpp +++ b/image-filtering/include/image_filters/image_processing.hpp @@ -48,8 +48,16 @@ struct OtsuParams { // Thomas' masterpiece struct OverlapParams{ - bool is_first=true; - cv::Mat previous_image; + double percentage_threshold; +}; +struct MedianParams{ + int kernel_size; +}; + +struct BinaryParams{ + double threshold; + double maxval; + bool invert; }; struct FilterParams { @@ -61,6 +69,8 @@ struct FilterParams { EbusParams ebus; OtsuParams otsu; OverlapParams overlap; + MedianParams median; + BinaryParams binary; }; typedef void (*FilterFunction)(const FilterParams&, const cv::Mat&, cv::Mat&); @@ -123,7 +133,7 @@ void white_balance_filter(const FilterParams& params, const cv::Mat& original, cv::Mat& filtered); -/** +/** * A filter that worked well-ish in the mc-lab conditions easter 2023 * Uses a combination of dilation and unsharpening */ @@ -144,6 +154,14 @@ void overlap_filter(const FilterParams& filter_params, cv::Mat &filtered); +void median_filter(const FilterParams& filter_params, + const cv::Mat& original, + cv::Mat& filtered); + +void binary_threshold(const FilterParams& filter_params, + const cv::Mat& original, + cv::Mat& filtered); + const static std::map filter_functions = { {"no_filter", no_filter}, {"flip", flip_filter}, @@ -154,7 +172,9 @@ const static std::map filter_functions = { {"white_balancing", white_balance_filter}, {"ebus", ebus_filter}, {"otsu", otsu_segmentation_filter}, - {"overlap", overlap_filter} // This was done by the one and only Thomas + {"overlap", overlap_filter}, // This was done by the one and only Thomas + {"median", median_filter}, + {"binary", binary_threshold}, }; #endif // IMAGE_PROCESSING_HPP diff --git a/image-filtering/include/image_filters/utils.hpp b/image-filtering/include/image_filters/utils.hpp new file mode 100644 index 0000000..f1b60bc --- /dev/null +++ b/image-filtering/include/image_filters/utils.hpp @@ -0,0 +1,18 @@ +#ifndef UTILITIES_HPP +#define UTILITIES_HPP + +#include +#include +#include +#include +#include +#include +#include +#include + + + + + + +#endif \ No newline at end of file diff --git a/image-filtering/package.xml b/image-filtering/package.xml index 31aabc9..8e40162 100644 --- a/image-filtering/package.xml +++ b/image-filtering/package.xml @@ -4,7 +4,7 @@ image_filtering 0.0.1 The image filtering package - Jorgen Fjermedal + Thomas Paulen MIT ament_cmake diff --git a/image-filtering/src/image_filtering_ros.cpp b/image-filtering/src/image_filtering_ros.cpp index 3d5c849..b51edef 100644 --- a/image-filtering/src/image_filtering_ros.cpp +++ b/image-filtering/src/image_filtering_ros.cpp @@ -44,8 +44,15 @@ void ImageFilteringNode::declare_parameters() { this->declare_parameter("filter_params.otsu.erosion_size"); this->declare_parameter("filter_params.otsu.dilation_size"); - // this->declare_parameter(//Thomas has left a mark here - // "filter_params.gaussian_blur.blur_strength"); + this->declare_parameter("filter_params.overlap.percentage_threshold"); //Thomas has left a mark here + + this->declare_parameter("filter_params.median.kernel_size"); + + this->declare_parameter("filter_params.binary.threshold"); + this->declare_parameter("filter_params.binary.maxval"); + this->declare_parameter("filter_params.binary.invert"); + + } void ImageFilteringNode::set_filter_params() { @@ -69,8 +76,7 @@ void ImageFilteringNode::set_filter_params() { params.dilating.size = this->get_parameter("filter_params.dilation.size").as_int(); params.white_balancing.contrast_percentage = - this->get_parameter("filter_params.white_balancing.contrast_percentage") - .as_double(); + this->get_parameter("filter_params.white_balancing.contrast_percentage").as_double(); params.ebus.erosion_size = this->get_parameter("filter_params.ebus.erosion_size").as_int(); params.ebus.blur_size = @@ -78,11 +84,9 @@ void ImageFilteringNode::set_filter_params() { params.ebus.mask_weight = this->get_parameter("filter_params.ebus.mask_weight").as_int(); params.otsu.gamma_auto_correction = - this->get_parameter("filter_params.otsu.gamma_auto_correction") - .as_bool(); + this->get_parameter("filter_params.otsu.gamma_auto_correction").as_bool(); params.otsu.gamma_auto_correction_weight = - this->get_parameter("filter_params.otsu.gamma_auto_correction_weight") - .as_double(); + this->get_parameter("filter_params.otsu.gamma_auto_correction_weight").as_double(); params.otsu.otsu_segmentation = this->get_parameter("filter_params.otsu.otsu_segmentation").as_bool(); params.otsu.gsc_weight_r = @@ -95,8 +99,16 @@ void ImageFilteringNode::set_filter_params() { this->get_parameter("filter_params.otsu.erosion_size").as_int(); params.otsu.dilation_size = this->get_parameter("filter_params.otsu.dilation_size").as_int(); - // params.gaussian_blur.blur_strength = // Thomas is everyware - // this->get_parameter("filter_params.gaussian_blur.blur_strength").as_int(); + params.overlap.percentage_threshold = // Thomas is everyware + this->get_parameter("filter_params.overlap.percentage_threshold").as_double(); + params.median.kernel_size = + this->get_parameter("filter_params.median.kernel_size").as_int(); + params.binary.threshold = + this->get_parameter("filter_params.binary.threshold").as_double(); + params.binary.maxval = + this->get_parameter("filter_params.binary.maxval").as_double(); + params.binary.invert = + this->get_parameter("filter_params.binary.invert").as_bool(); filter_params_ = params; spdlog::info("Filter parameters set: {}", filter); } @@ -150,9 +162,13 @@ void ImageFilteringNode::image_callback( std::string input_encoding = this->get_parameter("input_encoding").as_string(); + + if (input_encoding.empty()){ + input_encoding = msg->encoding; // Default to the input image encoding + } try { - cv_ptr = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::BGR8); + cv_ptr = cv_bridge::toCvCopy(msg, input_encoding); if (cv_ptr->image.empty()) { spdlog::error("Received empty image, skipping processing."); diff --git a/image-filtering/src/image_processing.cpp b/image-filtering/src/image_processing.cpp index f244673..28161da 100644 --- a/image-filtering/src/image_processing.cpp +++ b/image-filtering/src/image_processing.cpp @@ -238,36 +238,114 @@ void otsu_segmentation_filter(const FilterParams& params, // Thomas was here void overlap_filter(const FilterParams& filter_params, - const cv::Mat &original, - cv::Mat &filtered){ - static cv::Mat prev; + const cv::Mat& original, + cv::Mat& filtered) +{ + static cv::Mat prevR; // store previous R channel only static bool has_prev = false; - if (!has_prev) { - original.copyTo(filtered); // first call: just pass through - prev = original.clone(); // snapshot (clone so it’s independent) + // Extract current R channel + cv::Mat curR; + cv::extractChannel(original, curR, 2); // 0=B,1=G,2=R + + if (!has_prev || prevR.empty() || prevR.size()!=curR.size() || prevR.type()!=curR.type()) { + original.copyTo(filtered); // first call (or size/type change): pass through + prevR = curR.clone(); // cache R channel has_prev = true; return; } - - - // cv::add(original, prev, filtered); - cv::cvtColor(original, gray, cv::COLOR_BGR2GRAY); - cv::addWeighted(prev, 0.5, original, 0.5, 0.0, filtered); - - // Access your filter-specific parameters like this: - // int example_param = filter_params.gaussian_blur.blur_strength; + // |cur - prev| on the R channel + cv::Mat diff8u; + cv::absdiff(curR, prevR, diff8u); + + // % of full 8-bit range + cv::Mat percent32f; + diff8u.convertTo(percent32f, CV_32F, 100.0 / 255.0); + + // Mask: pixels whose % change > threshold + cv::Mat mask; + cv::threshold(percent32f, mask, filter_params.overlap.percentage_threshold, 255.0, cv::THRESH_BINARY); + mask.convertTo(mask, CV_8U); + + // Zero out those pixels in the R channel + filtered = original.clone(); + std::vector ch; + cv::split(filtered, ch); // ch[2] is R + ch[2].setTo(0, mask); + cv::merge(ch, filtered); + + // Update history (R channel only) + prevR = curR.clone(); +} + +void median_filter(const FilterParams& filter_params, + const cv::Mat& original, + cv::Mat& filtered){ + + CV_Assert(!original.empty()); + + // Validate & sanitize kernel size (must be odd and >= 3) + int k = filter_params.median.kernel_size; + if (k < 3) k = 3; + if ((k & 1) == 0) ++k; // make odd if even + + // cv::medianBlur is not suported for all depths "sais chat" + // (works for CV_8U, CV_16U, CV_32F) + const int depth = original.depth(); + const bool supported = (depth == CV_8U || depth == CV_16U || depth == CV_32F); + + const cv::Mat* src = &original; + cv::Mat tmp; + if (!supported) { + // Simple, safe conversion to 8-bit + original.convertTo(tmp, CV_8U); + src = &tmp; + } + + cv::medianBlur(*src, filtered, k); + + // If converted to 8U and want to preserve original depth, converts back here: + if (!supported) filtered.convertTo(filtered, depth); +} + + + +void binary_threshold(const FilterParams& filter_params, + const cv::Mat& original, + cv::Mat& filtered) +{ + + CV_Assert(!original.empty()); + + const double thresh = filter_params.binary.threshold; + const double maxval = filter_params.binary.maxval; + const bool invert = filter_params.binary.invert; - // Implement your filtering logic here + // 1) Ensure single-channel + cv::Mat gray; + if (original.channels() == 1) { + gray = original; + } else { + cv::cvtColor(original, gray, cv::COLOR_BGR2GRAY); + } - - // int flip_code = filter_params.flip.flip_code; // 0: x-axis, 1: y-axis, -1: both - // cv::flip(original, filtered, flip_code); - + // Standardize to 8-bit (safe for thresholding) + cv::Mat src8; + if (gray.depth() != CV_8U) { + // Adjust scaling here if grayscale is not already 0–255 + gray.convertTo(src8, CV_8U); + } else { + src8 = gray; + } + // Apply fixed threshold + const int type = invert ? cv::THRESH_BINARY_INV : cv::THRESH_BINARY; + cv::threshold(src8, filtered, thresh, maxval, type); } + + void apply_filter(const std::string& filter, const FilterParams& params, const cv::Mat& original, diff --git a/image-filtering/src/utils.cpp b/image-filtering/src/utils.cpp new file mode 100644 index 0000000..0cc4b51 --- /dev/null +++ b/image-filtering/src/utils.cpp @@ -0,0 +1,21 @@ +#include // Jørgen fix +#include + +// _.-. +// / 99\ +// ( `\ +// |\\ , ,| +// __ | \\____/ +// ,.--"`-.". / `---' +// _.-' '-/ | +// _.-" | '-. |_/_ +// ,__.-' _,.--\ \ (( /-\ +// ',_..--' `\ \ \\_ / +// `-, ) |\' +// | |-.,,-" ( +// | | `\ `',_ +// ) \ \,(\(\-' +// \ `-,_ +// \_(\-(\`-` +// " " + From 9e30fe6a0da071db2f2f5a26b3c18fa4c5d07f1d Mon Sep 17 00:00:00 2001 From: Thomas Date: Sun, 2 Nov 2025 12:02:34 +0100 Subject: [PATCH 16/53] remove .vscode files and cashed stuff --- .gitignore | 2 ++ .vscode/browse.vc.db-wal | 0 .vscode/c_cpp_properties.json | 21 --------------------- .vscode/settings.json | 13 ------------- image-filtering/src/utils.cpp | 2 +- 5 files changed, 3 insertions(+), 35 deletions(-) delete mode 100644 .vscode/browse.vc.db-wal delete mode 100644 .vscode/c_cpp_properties.json delete mode 100644 .vscode/settings.json diff --git a/.gitignore b/.gitignore index 5f73720..9405f84 100644 --- a/.gitignore +++ b/.gitignore @@ -50,3 +50,5 @@ qtcreator-* # Catkin custom files CATKIN_IGNORE + +.vscode/* \ No newline at end of file diff --git a/.vscode/browse.vc.db-wal b/.vscode/browse.vc.db-wal deleted file mode 100644 index e69de29..0000000 diff --git a/.vscode/c_cpp_properties.json b/.vscode/c_cpp_properties.json deleted file mode 100644 index adb1194..0000000 --- a/.vscode/c_cpp_properties.json +++ /dev/null @@ -1,21 +0,0 @@ -{ - "configurations": [ - { - "browse": { - "databaseFilename": "${workspaceFolder}/.vscode/browse.vc.db", - "limitSymbolsToIncludedHeaders": false - }, - "includePath": [ - "/opt/ros/humble/include/**", - "/home/thomas/ros2_ws/src/vortex-image-filtering/image-filtering/include/**", - "/usr/include/**" - ], - "name": "ros2", - "intelliSenseMode": "gcc-x64", - "compilerPath": "/usr/bin/gcc", - "cStandard": "gnu11", - "cppStandard": "c++17" - } - ], - "version": 4 -} \ No newline at end of file diff --git a/.vscode/settings.json b/.vscode/settings.json deleted file mode 100644 index 4657751..0000000 --- a/.vscode/settings.json +++ /dev/null @@ -1,13 +0,0 @@ -{ - "python.autoComplete.extraPaths": [ - "/opt/ros/humble/lib/python3.10/site-packages", - "/opt/ros/humble/local/lib/python3.10/dist-packages" - ], - "python.analysis.extraPaths": [ - "/opt/ros/humble/lib/python3.10/site-packages", - "/opt/ros/humble/local/lib/python3.10/dist-packages" - ], - "files.associations": { - "string_view": "cpp" - } -} \ No newline at end of file diff --git a/image-filtering/src/utils.cpp b/image-filtering/src/utils.cpp index 0cc4b51..934a4e8 100644 --- a/image-filtering/src/utils.cpp +++ b/image-filtering/src/utils.cpp @@ -2,7 +2,7 @@ #include // _.-. -// / 99\ +// / 66\ // ( `\ // |\\ , ,| // __ | \\____/ From 43c0d0825e747d53c0c0cdea85daecd90d88f669 Mon Sep 17 00:00:00 2001 From: Thomas Date: Sun, 2 Nov 2025 12:06:58 +0100 Subject: [PATCH 17/53] remove image_filtering.vscode files and cashed stuff --- image-filtering/.vscode/c_cpp_properties.json | 21 ------------------- image-filtering/.vscode/settings.json | 13 ------------ 2 files changed, 34 deletions(-) delete mode 100644 image-filtering/.vscode/c_cpp_properties.json delete mode 100644 image-filtering/.vscode/settings.json diff --git a/image-filtering/.vscode/c_cpp_properties.json b/image-filtering/.vscode/c_cpp_properties.json deleted file mode 100644 index adb1194..0000000 --- a/image-filtering/.vscode/c_cpp_properties.json +++ /dev/null @@ -1,21 +0,0 @@ -{ - "configurations": [ - { - "browse": { - "databaseFilename": "${workspaceFolder}/.vscode/browse.vc.db", - "limitSymbolsToIncludedHeaders": false - }, - "includePath": [ - "/opt/ros/humble/include/**", - "/home/thomas/ros2_ws/src/vortex-image-filtering/image-filtering/include/**", - "/usr/include/**" - ], - "name": "ros2", - "intelliSenseMode": "gcc-x64", - "compilerPath": "/usr/bin/gcc", - "cStandard": "gnu11", - "cppStandard": "c++17" - } - ], - "version": 4 -} \ No newline at end of file diff --git a/image-filtering/.vscode/settings.json b/image-filtering/.vscode/settings.json deleted file mode 100644 index 4657751..0000000 --- a/image-filtering/.vscode/settings.json +++ /dev/null @@ -1,13 +0,0 @@ -{ - "python.autoComplete.extraPaths": [ - "/opt/ros/humble/lib/python3.10/site-packages", - "/opt/ros/humble/local/lib/python3.10/dist-packages" - ], - "python.analysis.extraPaths": [ - "/opt/ros/humble/lib/python3.10/site-packages", - "/opt/ros/humble/local/lib/python3.10/dist-packages" - ], - "files.associations": { - "string_view": "cpp" - } -} \ No newline at end of file From d68360d2d4694806202dedb7ab32b8b971f638e8 Mon Sep 17 00:00:00 2001 From: Thomas Date: Sun, 2 Nov 2025 13:30:17 +0100 Subject: [PATCH 18/53] Making the utils file work --- .gitignore | 3 +- image-filtering/CMakeLists.txt | 2 +- .../config/image_filtering_params.yaml | 4 +- .../{utils.hpp => utilities.hpp} | 6 +- image-filtering/src/image_processing.cpp | 41 +--- image-filtering/src/utilities.cpp | 226 ++++++++++++++++++ image-filtering/src/utils.cpp | 21 -- 7 files changed, 235 insertions(+), 68 deletions(-) rename image-filtering/include/image_filters/{utils.hpp => utilities.hpp} (66%) create mode 100644 image-filtering/src/utilities.cpp delete mode 100644 image-filtering/src/utils.cpp diff --git a/.gitignore b/.gitignore index 9405f84..4f49ee8 100644 --- a/.gitignore +++ b/.gitignore @@ -51,4 +51,5 @@ qtcreator-* # Catkin custom files CATKIN_IGNORE -.vscode/* \ No newline at end of file +.vscode/* +image-filtering/.vscode/* \ No newline at end of file diff --git a/image-filtering/CMakeLists.txt b/image-filtering/CMakeLists.txt index ff4d3c8..e7e2440 100644 --- a/image-filtering/CMakeLists.txt +++ b/image-filtering/CMakeLists.txt @@ -25,7 +25,7 @@ set(LIB_NAME "${PROJECT_NAME}_component") add_library(${LIB_NAME} SHARED src/image_processing.cpp src/image_filtering_ros.cpp - src/utils.cpp + src/utilities.cpp ) target_link_libraries(${LIB_NAME} PUBLIC diff --git a/image-filtering/config/image_filtering_params.yaml b/image-filtering/config/image_filtering_params.yaml index ee6833d..b60ce9e 100644 --- a/image-filtering/config/image_filtering_params.yaml +++ b/image-filtering/config/image_filtering_params.yaml @@ -3,10 +3,10 @@ sub_topic: "/fls_publisher/display" pub_topic: "/filtered_image" output_encoding: "mono8" - input_encoding: "" + input_encoding: "bgr8" filter_params: - filter_type: "binary" + filter_type: "otsu" flip: flip_code: 1 unsharpening: diff --git a/image-filtering/include/image_filters/utils.hpp b/image-filtering/include/image_filters/utilities.hpp similarity index 66% rename from image-filtering/include/image_filters/utils.hpp rename to image-filtering/include/image_filters/utilities.hpp index f1b60bc..4fe1fea 100644 --- a/image-filtering/include/image_filters/utils.hpp +++ b/image-filtering/include/image_filters/utilities.hpp @@ -1,9 +1,7 @@ #ifndef UTILITIES_HPP #define UTILITIES_HPP -#include -#include -#include + #include #include #include @@ -12,6 +10,8 @@ +double calculateAutoGamma(const cv::Mat& image); +void applyGammaCorrection(cv::Mat& image, double gamma); diff --git a/image-filtering/src/image_processing.cpp b/image-filtering/src/image_processing.cpp index 28161da..693c3f8 100644 --- a/image-filtering/src/image_processing.cpp +++ b/image-filtering/src/image_processing.cpp @@ -1,4 +1,5 @@ #include +#include #include void no_filter([[maybe_unused]] const FilterParams& params, @@ -104,47 +105,7 @@ void ebus_filter(const FilterParams& params, addWeighted(eroded, 1, mask, mask_weight, 0, filtered); } -void applyGammaCorrection(cv::Mat& image, double gamma) { - // Create a lookup table for gamma correction - cv::Mat lookup(1, 256, CV_8U); - uchar* p = lookup.ptr(); - for (int i = 0; i < 256; ++i) { - p[i] = cv::saturate_cast(pow(i / 255.0, gamma) * 255.0); - } - - // Apply the gamma correction using the lookup table - cv::LUT(image, lookup, image); -} - -double calculateAutoGamma(const cv::Mat& image) { - // Convert the image to grayscale if it's not already - cv::Mat grayImage; - if (image.channels() == 3) { - cv::cvtColor(image, grayImage, cv::COLOR_BGR2GRAY); - } else { - grayImage = image; - } - // Calculate the mean intensity - cv::Scalar meanIntensity = mean(grayImage); - - // The ideal mean intensity is 128 (midpoint for 8-bit grayscale images) - double idealMean = 128.0; - double currentMean = meanIntensity[0]; - - // Automatically set gamma value based on the mean intensity - double gamma; - if (currentMean > 0) { - gamma = log(idealMean / 255.0) / log(currentMean / 255.0); - } else { - gamma = 1.0; // Default gamma if the image has no intensity - } - - // Ensure gamma is within a reasonable range (e.g., between 0.1 and 3.0) - gamma = std::max(0.1, std::min(gamma, 3.0)); - - return gamma; -} void otsu_segmentation_filter(const FilterParams& params, const cv::Mat& original, diff --git a/image-filtering/src/utilities.cpp b/image-filtering/src/utilities.cpp new file mode 100644 index 0000000..4fa2494 --- /dev/null +++ b/image-filtering/src/utilities.cpp @@ -0,0 +1,226 @@ +#include +#include + + + +// Picks gamma value considering the brightnes for choosing a gamma value +double calculateAutoGamma(const cv::Mat& image) { + // Convert the image to grayscale if it's not already + cv::Mat grayImage; + if (image.channels() == 3) { + cv::cvtColor(image, grayImage, cv::COLOR_BGR2GRAY); + } else { + grayImage = image; + } + + // Calculate the mean intensity + cv::Scalar meanIntensity = mean(grayImage); + + // The ideal mean intensity is 128 (midpoint for 8-bit grayscale images) + double idealMean = 128.0; + double currentMean = meanIntensity[0]; + + // Automatically set gamma value based on the mean intensity + double gamma; + if (currentMean > 0) { + gamma = log(idealMean / 255.0) / log(currentMean / 255.0); + } else { + gamma = 1.0; // Default gamma if the image has no intensity + } + + // Ensure gamma is within a reasonable range (e.g., between 0.1 and 3.0) + gamma = std::max(0.1, std::min(gamma, 3.0)); + + return gamma; +} + + + +// Apply gamma correction to an 8-bit image using a precomputed lookup table +void applyGammaCorrection(cv::Mat& image, double gamma) { + // Create a lookup table for gamma correction + cv::Mat lookup(1, 256, CV_8U); + uchar* p = lookup.ptr(); + for (int i = 0; i < 256; ++i) { + p[i] = cv::saturate_cast(pow(i / 255.0, gamma) * 255.0); + } + + // Apply the gamma correction using the lookup table + cv::LUT(image, lookup, image); +} + + + + + + +// void otsu_segmentation_filter(const FilterParams& params, +// const cv::Mat& original, +// cv::Mat& filtered) { +// bool gamma_auto_correction = params.otsu.gamma_auto_correction; +// double gamma_auto_correction_weight = +// params.otsu.gamma_auto_correction_weight; + +// bool otsu_segmentation = params.otsu.otsu_segmentation; + +// cv::Mat grayImage; + +// cv::Matx13f customWeights(params.otsu.gsc_weight_b, +// params.otsu.gsc_weight_g, +// params.otsu.gsc_weight_r); +// cv::transform(original, filtered, customWeights); + +// if (gamma_auto_correction) { +// double autoGamma = +// calculateAutoGamma(filtered) * gamma_auto_correction_weight; + +// applyGammaCorrection(filtered, autoGamma); +// } + +// if (otsu_segmentation) { +// // Calculate the histogram +// int histSize = 256; +// float range[] = {0, 256}; +// const float* histRange = {range}; +// cv::Mat hist; +// calcHist(&filtered, 1, 0, cv::Mat(), hist, 1, &histSize, &histRange, +// true, false); + +// // Normalize histogram to get probabilities +// hist /= filtered.total(); + +// // Initialize variables for Otsu's method +// std::vector sigma2_list(256, 0.0); +// std::vector p(hist.begin(), +// hist.end()); // Probabilities + +// for (int th = 1; th < 256; ++th) { +// // Calculate omega (weights) for foreground and background +// float omega_fg = std::accumulate(p.begin(), p.begin() + th, 0.0f); +// float omega_bg = std::accumulate(p.begin() + th, p.end(), 0.0f); + +// // Calculate mu (means) for foreground and background +// float mu_fg = 0, mu_bg = 0; +// for (int i = 0; i < th; ++i) { +// mu_fg += i * p[i]; +// } +// for (int i = th; i < 256; ++i) { +// mu_bg += i * p[i]; +// } + +// if (omega_fg > 0) +// mu_fg /= omega_fg; +// if (omega_bg > 0) +// mu_bg /= omega_bg; + +// // Calculate sigma squared and store in list +// sigma2_list[th] = omega_fg * omega_bg * pow(mu_fg - mu_bg, 2); +// } + +// // Find the threshold corresponding to the maximum sigma squared +// int optimalThreshold = +// std::max_element(sigma2_list.begin(), sigma2_list.end()) - +// sigma2_list.begin(); + +// // Apply the threshold to the image +// // cv::Mat binaryImage; +// cv::threshold(filtered, filtered, optimalThreshold, 255, +// cv::THRESH_BINARY); + +// // Apply erosion followed by dilation (opening) +// cv::Mat openedImage; +// int erosionSize = params.otsu.erosion_size; +// cv::Mat erosionKernel = getStructuringElement( +// cv::MORPH_CROSS, cv::Size(2 * erosionSize + 1, 2 * erosionSize + 1), +// cv::Point(erosionSize, erosionSize)); +// cv::erode(filtered, filtered, erosionKernel); + +// int dilation_size = params.otsu.dilation_size; +// cv::Mat dilationKernel = getStructuringElement( +// cv::MORPH_CROSS, +// cv::Size(2 * dilation_size + 1, 2 * dilation_size + 1), +// cv::Point(dilation_size, dilation_size)); +// cv::dilate(filtered, filtered, dilationKernel); +// } +// } + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +// _.-. +// / 66\ +// ( `\ +// |\\ , ,| +// __ | \\____/ +// ,.--"`-.". / `---' +// _.-' '-/ | +// _.-" | '-. |_/_ +// ,__.-' _,.--\ \ (( /-\ +// ',_..--' `\ \ \\_ / +// `-, ) |\' +// | |-.,,-" ( +// | | `\ `',_ +// ) \ \,(\(\-' +// \ `-,_ +// \_(\-(\`-` +// " " + diff --git a/image-filtering/src/utils.cpp b/image-filtering/src/utils.cpp deleted file mode 100644 index 934a4e8..0000000 --- a/image-filtering/src/utils.cpp +++ /dev/null @@ -1,21 +0,0 @@ -#include // Jørgen fix -#include - -// _.-. -// / 66\ -// ( `\ -// |\\ , ,| -// __ | \\____/ -// ,.--"`-.". / `---' -// _.-' '-/ | -// _.-" | '-. |_/_ -// ,__.-' _,.--\ \ (( /-\ -// ',_..--' `\ \ \\_ / -// `-, ) |\' -// | |-.,,-" ( -// | | `\ `',_ -// ) \ \,(\(\-' -// \ `-,_ -// \_(\-(\`-` -// " " - From 497d74d7013f05e2d3c617fd9c5084fb0611f354 Mon Sep 17 00:00:00 2001 From: Thomas Date: Sun, 2 Nov 2025 14:14:39 +0100 Subject: [PATCH 19/53] Starting the modulation prosses. made auto gamma modular --- .../include/image_filters/utilities.hpp | 6 +- image-filtering/src/image_processing.cpp | 5 +- image-filtering/src/utilities.cpp | 188 +++--------------- 3 files changed, 28 insertions(+), 171 deletions(-) diff --git a/image-filtering/include/image_filters/utilities.hpp b/image-filtering/include/image_filters/utilities.hpp index 4fe1fea..072cb8d 100644 --- a/image-filtering/include/image_filters/utilities.hpp +++ b/image-filtering/include/image_filters/utilities.hpp @@ -10,8 +10,10 @@ -double calculateAutoGamma(const cv::Mat& image); -void applyGammaCorrection(cv::Mat& image, double gamma); + +void applyAutoGamma(cv::Mat& image, double correction_weight); + +// weighted transform, gamma correction, thresholding, erosion og dilation diff --git a/image-filtering/src/image_processing.cpp b/image-filtering/src/image_processing.cpp index 693c3f8..34df935 100644 --- a/image-filtering/src/image_processing.cpp +++ b/image-filtering/src/image_processing.cpp @@ -124,10 +124,7 @@ void otsu_segmentation_filter(const FilterParams& params, cv::transform(original, filtered, customWeights); if (gamma_auto_correction) { - double autoGamma = - calculateAutoGamma(filtered) * gamma_auto_correction_weight; - - applyGammaCorrection(filtered, autoGamma); + applyAutoGamma(filtered, gamma_auto_correction_weight); } if (otsu_segmentation) { diff --git a/image-filtering/src/utilities.cpp b/image-filtering/src/utilities.cpp index 4fa2494..e11f940 100644 --- a/image-filtering/src/utilities.cpp +++ b/image-filtering/src/utilities.cpp @@ -3,8 +3,23 @@ -// Picks gamma value considering the brightnes for choosing a gamma value -double calculateAutoGamma(const cv::Mat& image) { + +// Apply a given gamma to an 8-bit image using a LUT +void applyGammaLUT(cv::Mat& image, double gamma) { + // Create a lookup table for gamma correction + cv::Mat lookup(1, 256, CV_8U); + uchar* p = lookup.ptr(); + for (int i = 0; i < 256; ++i) { + p[i] = cv::saturate_cast(pow(i / 255.0, gamma) * 255.0); + } + + // Apply the gamma correction using the lookup table + cv::LUT(image, lookup, image); +} + + +// Compute a gamma value that pushes the image mean toward mid-gray +double computeAutoGammaFromMean(const cv::Mat& image) { // Convert the image to grayscale if it's not already cv::Mat grayImage; if (image.channels() == 3) { @@ -35,170 +50,13 @@ double calculateAutoGamma(const cv::Mat& image) { } - -// Apply gamma correction to an 8-bit image using a precomputed lookup table -void applyGammaCorrection(cv::Mat& image, double gamma) { - // Create a lookup table for gamma correction - cv::Mat lookup(1, 256, CV_8U); - uchar* p = lookup.ptr(); - for (int i = 0; i < 256; ++i) { - p[i] = cv::saturate_cast(pow(i / 255.0, gamma) * 255.0); - } - - // Apply the gamma correction using the lookup table - cv::LUT(image, lookup, image); +// Auto-choose a gamma so dark images get lifted and bright images get toned down (expects mono8) +// - It sets the mean intensity to 255/2 ≃ 128 +// - The weight makes makes all the values weeker(<1) or stronger(>1) +void applyAutoGamma(cv::Mat& image, double correction_weight) { + double gamma = computeAutoGammaFromMean(image) * correction_weight; + applyGammaLUT(image, gamma); } - - - - - - -// void otsu_segmentation_filter(const FilterParams& params, -// const cv::Mat& original, -// cv::Mat& filtered) { -// bool gamma_auto_correction = params.otsu.gamma_auto_correction; -// double gamma_auto_correction_weight = -// params.otsu.gamma_auto_correction_weight; - -// bool otsu_segmentation = params.otsu.otsu_segmentation; - -// cv::Mat grayImage; - -// cv::Matx13f customWeights(params.otsu.gsc_weight_b, -// params.otsu.gsc_weight_g, -// params.otsu.gsc_weight_r); -// cv::transform(original, filtered, customWeights); - -// if (gamma_auto_correction) { -// double autoGamma = -// calculateAutoGamma(filtered) * gamma_auto_correction_weight; - -// applyGammaCorrection(filtered, autoGamma); -// } - -// if (otsu_segmentation) { -// // Calculate the histogram -// int histSize = 256; -// float range[] = {0, 256}; -// const float* histRange = {range}; -// cv::Mat hist; -// calcHist(&filtered, 1, 0, cv::Mat(), hist, 1, &histSize, &histRange, -// true, false); - -// // Normalize histogram to get probabilities -// hist /= filtered.total(); - -// // Initialize variables for Otsu's method -// std::vector sigma2_list(256, 0.0); -// std::vector p(hist.begin(), -// hist.end()); // Probabilities - -// for (int th = 1; th < 256; ++th) { -// // Calculate omega (weights) for foreground and background -// float omega_fg = std::accumulate(p.begin(), p.begin() + th, 0.0f); -// float omega_bg = std::accumulate(p.begin() + th, p.end(), 0.0f); - -// // Calculate mu (means) for foreground and background -// float mu_fg = 0, mu_bg = 0; -// for (int i = 0; i < th; ++i) { -// mu_fg += i * p[i]; -// } -// for (int i = th; i < 256; ++i) { -// mu_bg += i * p[i]; -// } - -// if (omega_fg > 0) -// mu_fg /= omega_fg; -// if (omega_bg > 0) -// mu_bg /= omega_bg; - -// // Calculate sigma squared and store in list -// sigma2_list[th] = omega_fg * omega_bg * pow(mu_fg - mu_bg, 2); -// } - -// // Find the threshold corresponding to the maximum sigma squared -// int optimalThreshold = -// std::max_element(sigma2_list.begin(), sigma2_list.end()) - -// sigma2_list.begin(); - -// // Apply the threshold to the image -// // cv::Mat binaryImage; -// cv::threshold(filtered, filtered, optimalThreshold, 255, -// cv::THRESH_BINARY); - -// // Apply erosion followed by dilation (opening) -// cv::Mat openedImage; -// int erosionSize = params.otsu.erosion_size; -// cv::Mat erosionKernel = getStructuringElement( -// cv::MORPH_CROSS, cv::Size(2 * erosionSize + 1, 2 * erosionSize + 1), -// cv::Point(erosionSize, erosionSize)); -// cv::erode(filtered, filtered, erosionKernel); - -// int dilation_size = params.otsu.dilation_size; -// cv::Mat dilationKernel = getStructuringElement( -// cv::MORPH_CROSS, -// cv::Size(2 * dilation_size + 1, 2 * dilation_size + 1), -// cv::Point(dilation_size, dilation_size)); -// cv::dilate(filtered, filtered, dilationKernel); -// } -// } - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - From c794f1fc2a93bd4109a667beef26470d2c7c1ad0 Mon Sep 17 00:00:00 2001 From: Thomas Date: Sun, 2 Nov 2025 15:36:47 +0100 Subject: [PATCH 20/53] Adding modular computation of otsu threshold --- .../include/image_filters/utilities.hpp | 7 ++- image-filtering/src/image_processing.cpp | 46 +++------------- image-filtering/src/utilities.cpp | 53 +++++++++++++++++++ 3 files changed, 65 insertions(+), 41 deletions(-) diff --git a/image-filtering/include/image_filters/utilities.hpp b/image-filtering/include/image_filters/utilities.hpp index 072cb8d..701fd56 100644 --- a/image-filtering/include/image_filters/utilities.hpp +++ b/image-filtering/include/image_filters/utilities.hpp @@ -1,7 +1,7 @@ #ifndef UTILITIES_HPP #define UTILITIES_HPP - +#include #include #include #include @@ -12,8 +12,11 @@ void applyAutoGamma(cv::Mat& image, double correction_weight); +void toWeightedGray(const cv::Mat& bgr, cv::Mat& gray, double wB, double wG, double wR); + +int computeOtsuThreshold(const cv::Mat& hist_prob); -// weighted transform, gamma correction, thresholding, erosion og dilation +// erosion og dilation diff --git a/image-filtering/src/image_processing.cpp b/image-filtering/src/image_processing.cpp index 34df935..c45e5a2 100644 --- a/image-filtering/src/image_processing.cpp +++ b/image-filtering/src/image_processing.cpp @@ -116,18 +116,17 @@ void otsu_segmentation_filter(const FilterParams& params, bool otsu_segmentation = params.otsu.otsu_segmentation; - cv::Mat grayImage; - cv::Matx13f customWeights(params.otsu.gsc_weight_b, + toWeightedGray(original, filtered, params.otsu.gsc_weight_b, params.otsu.gsc_weight_g, params.otsu.gsc_weight_r); - cv::transform(original, filtered, customWeights); - if (gamma_auto_correction) { - applyAutoGamma(filtered, gamma_auto_correction_weight); - } - if (otsu_segmentation) { + if (gamma_auto_correction) { + applyAutoGamma(filtered, gamma_auto_correction_weight); + } + + if (otsu_segmentation) { // Calculate the histogram int histSize = 256; float range[] = {0, 256}; @@ -139,38 +138,7 @@ void otsu_segmentation_filter(const FilterParams& params, // Normalize histogram to get probabilities hist /= filtered.total(); - // Initialize variables for Otsu's method - std::vector sigma2_list(256, 0.0); - std::vector p(hist.begin(), - hist.end()); // Probabilities - - for (int th = 1; th < 256; ++th) { - // Calculate omega (weights) for foreground and background - float omega_fg = std::accumulate(p.begin(), p.begin() + th, 0.0f); - float omega_bg = std::accumulate(p.begin() + th, p.end(), 0.0f); - - // Calculate mu (means) for foreground and background - float mu_fg = 0, mu_bg = 0; - for (int i = 0; i < th; ++i) { - mu_fg += i * p[i]; - } - for (int i = th; i < 256; ++i) { - mu_bg += i * p[i]; - } - - if (omega_fg > 0) - mu_fg /= omega_fg; - if (omega_bg > 0) - mu_bg /= omega_bg; - - // Calculate sigma squared and store in list - sigma2_list[th] = omega_fg * omega_bg * pow(mu_fg - mu_bg, 2); - } - - // Find the threshold corresponding to the maximum sigma squared - int optimalThreshold = - std::max_element(sigma2_list.begin(), sigma2_list.end()) - - sigma2_list.begin(); + int optimalThreshold = computeOtsuThreshold(hist); // Apply the threshold to the image // cv::Mat binaryImage; diff --git a/image-filtering/src/utilities.cpp b/image-filtering/src/utilities.cpp index e11f940..326548c 100644 --- a/image-filtering/src/utilities.cpp +++ b/image-filtering/src/utilities.cpp @@ -61,6 +61,59 @@ void applyAutoGamma(cv::Mat& image, double correction_weight) { +// Convert BGR image to single-channel grayscale using custom B,G,R weights +// weights = (b, g, r), e.g. (0.114f, 0.587f, 0.299f) +void toWeightedGray(const cv::Mat& bgr, cv::Mat& gray, double wB, double wG, double wR) { + cv::Matx13f customWeights(wB, wG, wR); + cv::transform(bgr, gray, customWeights); +} + + + + +// Computes the Otsu optimal threshold from a 256-bin normalized grayscale histogram. +// `hist_prob` must contain probabilities (sum ≈ 1). For every possible threshold t, +// the function splits the histogram into foreground [0, t) and background [t, 255], +// computes their weights and means, evaluates the between-class variance, and returns +// the t that maximizes it. A good threshold is one that makes two sizeable groups that +// are well separated in intensity: +// - Best regards ChatGPT +int computeOtsuThreshold(const cv::Mat& hist_prob) +{ + // Initialize variables for Otsu's method + std::vector sigma2_list(256, 0.0); + std::vector p(hist_prob.begin(), hist_prob.end()); // Probabilities + + for (int th = 1; th < 256; ++th) { + // Calculate omega (weights) for foreground and background + float omega_fg = std::accumulate(p.begin(), p.begin() + th, 0.0f); + float omega_bg = std::accumulate(p.begin() + th, p.end(), 0.0f); + + // Calculate mu (means) for foreground and background + float mu_fg = 0, mu_bg = 0; + for (int i = 0; i < th; ++i) { + mu_fg += i * p[i]; + } + for (int i = th; i < 256; ++i) { + mu_bg += i * p[i]; + } + + if (omega_fg > 0) + mu_fg /= omega_fg; + if (omega_bg > 0) + mu_bg /= omega_bg; + + // Calculate sigma squared and store in list + sigma2_list[th] = omega_fg * omega_bg * pow(mu_fg - mu_bg, 2); + } + + return int(std::max_element(sigma2_list.begin(), sigma2_list.end()) - sigma2_list.begin()); +} + + + + + From 2ab9709789ad4b9b83938b10cefe8b99e6683913 Mon Sep 17 00:00:00 2001 From: Thomas Paulen Date: Sun, 2 Nov 2025 19:55:45 +0100 Subject: [PATCH 21/53] test --- image-filtering/src/utilities.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/image-filtering/src/utilities.cpp b/image-filtering/src/utilities.cpp index 326548c..0a6a37c 100644 --- a/image-filtering/src/utilities.cpp +++ b/image-filtering/src/utilities.cpp @@ -119,7 +119,7 @@ int computeOtsuThreshold(const cv::Mat& hist_prob) // _.-. // / 66\ -// ( `\ +// ( `\ Hi, how you doin :) // |\\ , ,| // __ | \\____/ // ,.--"`-.". / `---' From 544c9ce11334e2b2e62b898976fc1551883b6e10 Mon Sep 17 00:00:00 2001 From: Thomas Paulen Date: Sun, 2 Nov 2025 22:30:46 +0100 Subject: [PATCH 22/53] adding last part has to be tested first --- .../image_filters/image_filtering_ros.hpp | 2 +- .../include/image_filters/utilities.hpp | 1 + image-filtering/src/utilities.cpp | 50 +++++++++++++++++++ 3 files changed, 52 insertions(+), 1 deletion(-) diff --git a/image-filtering/include/image_filters/image_filtering_ros.hpp b/image-filtering/include/image_filters/image_filtering_ros.hpp index 58fa853..eff7358 100644 --- a/image-filtering/include/image_filters/image_filtering_ros.hpp +++ b/image-filtering/include/image_filters/image_filtering_ros.hpp @@ -1,7 +1,7 @@ #ifndef IMAGE_FILTERING_ROS_HPP #define IMAGE_FILTERING_ROS_HPP -#include +#include // for jazzy this has to be hpp and for humble it has to be h #include #include #include diff --git a/image-filtering/include/image_filters/utilities.hpp b/image-filtering/include/image_filters/utilities.hpp index 701fd56..06414aa 100644 --- a/image-filtering/include/image_filters/utilities.hpp +++ b/image-filtering/include/image_filters/utilities.hpp @@ -16,6 +16,7 @@ void toWeightedGray(const cv::Mat& bgr, cv::Mat& gray, double wB, double wG, dou int computeOtsuThreshold(const cv::Mat& hist_prob); +// void apply_erosion(const cv::Mat& src, cv::Mat& dst, int size, int shape); // erosion og dilation diff --git a/image-filtering/src/utilities.cpp b/image-filtering/src/utilities.cpp index 0a6a37c..ee11327 100644 --- a/image-filtering/src/utilities.cpp +++ b/image-filtering/src/utilities.cpp @@ -109,6 +109,56 @@ int computeOtsuThreshold(const cv::Mat& hist_prob) return int(std::max_element(sigma2_list.begin(), sigma2_list.end()) - sigma2_list.begin()); } + + + + +// // Basic erosion +// void apply_erosion(const cv::Mat& src, +// cv::Mat& dst, +// int size, +// int shape = cv::MORPH_RECT) { +// cv::Mat kernel = cv::getStructuringElement( +// shape, +// cv::Size(2 * size + 1, 2 * size + 1), +// cv::Point(size, size)); +// cv::erode(src, dst, kernel); +// } + +// // Basic dilation +// void apply_dilation(const cv::Mat& src, +// cv::Mat& dst, +// int size, +// int shape = cv::MORPH_RECT) { +// cv::Mat kernel = cv::getStructuringElement( +// shape, +// cv::Size(2 * size + 1, 2 * size + 1), +// cv::Point(size, size)); +// cv::dilate(src, dst, kernel); +// } + + + + + + + + + + + + + + + + + + + + + + + From d9c206efe2cb0c8d37395797331ed1f189256b26 Mon Sep 17 00:00:00 2001 From: Thomas Date: Mon, 3 Nov 2025 11:45:02 +0100 Subject: [PATCH 23/53] Add dilation errotion and dilation --- .../include/image_filters/utilities.hpp | 3 +- image-filtering/src/image_processing.cpp | 15 ++---- image-filtering/src/utilities.cpp | 47 ++++++++++--------- 3 files changed, 31 insertions(+), 34 deletions(-) diff --git a/image-filtering/include/image_filters/utilities.hpp b/image-filtering/include/image_filters/utilities.hpp index 06414aa..65bf26f 100644 --- a/image-filtering/include/image_filters/utilities.hpp +++ b/image-filtering/include/image_filters/utilities.hpp @@ -16,7 +16,8 @@ void toWeightedGray(const cv::Mat& bgr, cv::Mat& gray, double wB, double wG, dou int computeOtsuThreshold(const cv::Mat& hist_prob); -// void apply_erosion(const cv::Mat& src, cv::Mat& dst, int size, int shape); +void apply_erosion(const cv::Mat& src, cv::Mat& dst, int size, int shape = cv::MORPH_CROSS); +void apply_dilation(const cv::Mat& src, cv::Mat& dst, int size, int shape=cv::MORPH_CROSS); // erosion og dilation diff --git a/image-filtering/src/image_processing.cpp b/image-filtering/src/image_processing.cpp index c45e5a2..52d6f2f 100644 --- a/image-filtering/src/image_processing.cpp +++ b/image-filtering/src/image_processing.cpp @@ -145,20 +145,15 @@ void otsu_segmentation_filter(const FilterParams& params, cv::threshold(filtered, filtered, optimalThreshold, 255, cv::THRESH_BINARY); + + // Apply erosion followed by dilation (opening) - cv::Mat openedImage; + int erosionSize = params.otsu.erosion_size; - cv::Mat erosionKernel = getStructuringElement( - cv::MORPH_CROSS, cv::Size(2 * erosionSize + 1, 2 * erosionSize + 1), - cv::Point(erosionSize, erosionSize)); - cv::erode(filtered, filtered, erosionKernel); + apply_erosion(filtered, filtered, erosionSize); int dilation_size = params.otsu.dilation_size; - cv::Mat dilationKernel = getStructuringElement( - cv::MORPH_CROSS, - cv::Size(2 * dilation_size + 1, 2 * dilation_size + 1), - cv::Point(dilation_size, dilation_size)); - cv::dilate(filtered, filtered, dilationKernel); + apply_dilation(filtered, filtered, dilation_size); } } diff --git a/image-filtering/src/utilities.cpp b/image-filtering/src/utilities.cpp index ee11327..170e5bc 100644 --- a/image-filtering/src/utilities.cpp +++ b/image-filtering/src/utilities.cpp @@ -113,29 +113,30 @@ int computeOtsuThreshold(const cv::Mat& hist_prob) -// // Basic erosion -// void apply_erosion(const cv::Mat& src, -// cv::Mat& dst, -// int size, -// int shape = cv::MORPH_RECT) { -// cv::Mat kernel = cv::getStructuringElement( -// shape, -// cv::Size(2 * size + 1, 2 * size + 1), -// cv::Point(size, size)); -// cv::erode(src, dst, kernel); -// } - -// // Basic dilation -// void apply_dilation(const cv::Mat& src, -// cv::Mat& dst, -// int size, -// int shape = cv::MORPH_RECT) { -// cv::Mat kernel = cv::getStructuringElement( -// shape, -// cv::Size(2 * size + 1, 2 * size + 1), -// cv::Point(size, size)); -// cv::dilate(src, dst, kernel); -// } +// Basic erosion +void apply_erosion(const cv::Mat& src, + cv::Mat& filtered, + int size, + int shape) { + cv::Mat kernel = cv::getStructuringElement( + shape, + cv::Size(2 * size + 1, 2 * size + 1), + cv::Point(size, size)); + cv::erode(src, filtered, kernel); +} + + +// Basic dilation +void apply_dilation(const cv::Mat& src, + cv::Mat& dst, + int size, + int shape) { + cv::Mat kernel = cv::getStructuringElement( + shape, + cv::Size(2 * size + 1, 2 * size + 1), + cv::Point(size, size)); + cv::dilate(src, dst, kernel); +} From ad04b9b644e04478f09a0a73e185e1ebebfa263f Mon Sep 17 00:00:00 2001 From: Thomas Date: Mon, 3 Nov 2025 12:17:13 +0100 Subject: [PATCH 24/53] Small changes --- image-filtering/config/image_filtering_params.yaml | 6 +++--- image-filtering/include/image_filters/utilities.hpp | 4 +--- image-filtering/src/image_processing.cpp | 3 ++- 3 files changed, 6 insertions(+), 7 deletions(-) diff --git a/image-filtering/config/image_filtering_params.yaml b/image-filtering/config/image_filtering_params.yaml index b60ce9e..b784709 100644 --- a/image-filtering/config/image_filtering_params.yaml +++ b/image-filtering/config/image_filtering_params.yaml @@ -3,10 +3,10 @@ sub_topic: "/fls_publisher/display" pub_topic: "/filtered_image" output_encoding: "mono8" - input_encoding: "bgr8" + input_encoding: "mono8" filter_params: - filter_type: "otsu" + filter_type: "erosion" flip: flip_code: 1 unsharpening: @@ -14,7 +14,7 @@ erosion: size: 1 dilation: - size: 1 + size: 3 white_balancing: contrast_percentage: 0.1 ebus: diff --git a/image-filtering/include/image_filters/utilities.hpp b/image-filtering/include/image_filters/utilities.hpp index 65bf26f..aee642b 100644 --- a/image-filtering/include/image_filters/utilities.hpp +++ b/image-filtering/include/image_filters/utilities.hpp @@ -16,10 +16,8 @@ void toWeightedGray(const cv::Mat& bgr, cv::Mat& gray, double wB, double wG, dou int computeOtsuThreshold(const cv::Mat& hist_prob); -void apply_erosion(const cv::Mat& src, cv::Mat& dst, int size, int shape = cv::MORPH_CROSS); +void apply_erosion(const cv::Mat& src, cv::Mat& dst, int size, int shape = cv::MORPH_CROSS); // TODO Apply these to the erotion and dilation filters void apply_dilation(const cv::Mat& src, cv::Mat& dst, int size, int shape=cv::MORPH_CROSS); -// erosion og dilation - #endif \ No newline at end of file diff --git a/image-filtering/src/image_processing.cpp b/image-filtering/src/image_processing.cpp index 52d6f2f..31a056e 100644 --- a/image-filtering/src/image_processing.cpp +++ b/image-filtering/src/image_processing.cpp @@ -50,6 +50,8 @@ void erosion_filter(const FilterParams& params, // Apply erosion to the image cv::erode(original, filtered, element); + + // apply_erosion(original, filtered, erosion_size, cv::MORPH_RECT); } void dilation_filter(const FilterParams& params, @@ -141,7 +143,6 @@ void otsu_segmentation_filter(const FilterParams& params, int optimalThreshold = computeOtsuThreshold(hist); // Apply the threshold to the image - // cv::Mat binaryImage; cv::threshold(filtered, filtered, optimalThreshold, 255, cv::THRESH_BINARY); From f4e9a4383e94335c7b9379de75010be61dbe4bbd Mon Sep 17 00:00:00 2001 From: Thomas Paulen Date: Mon, 3 Nov 2025 12:36:17 +0100 Subject: [PATCH 25/53] finishing the modularity phase --- .../config/image_filtering_params.yaml | 4 ++-- image-filtering/src/image_processing.cpp | 20 ++----------------- 2 files changed, 4 insertions(+), 20 deletions(-) diff --git a/image-filtering/config/image_filtering_params.yaml b/image-filtering/config/image_filtering_params.yaml index b784709..50982ef 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" + sub_topic: "/fls_publisher/display_mono" pub_topic: "/filtered_image" output_encoding: "mono8" input_encoding: "mono8" filter_params: - filter_type: "erosion" + filter_type: "dilation" flip: flip_code: 1 unsharpening: diff --git a/image-filtering/src/image_processing.cpp b/image-filtering/src/image_processing.cpp index 31a056e..3765be1 100644 --- a/image-filtering/src/image_processing.cpp +++ b/image-filtering/src/image_processing.cpp @@ -42,29 +42,13 @@ void unsharpening_filter(const FilterParams& params, void erosion_filter(const FilterParams& params, const cv::Mat& original, cv::Mat& filtered) { - int erosion_size = params.eroding.size; - // Create a structuring element for dilation and erosion - cv::Mat element = cv::getStructuringElement( - cv::MORPH_RECT, cv::Size(2 * erosion_size + 1, 2 * erosion_size + 1), - cv::Point(erosion_size, erosion_size)); - - // Apply erosion to the image - cv::erode(original, filtered, element); - - // apply_erosion(original, filtered, erosion_size, cv::MORPH_RECT); + apply_erosion(original, filtered, params.eroding.size, cv::MORPH_RECT); } void dilation_filter(const FilterParams& params, const cv::Mat& original, cv::Mat& filtered) { - int dilation_size = params.dilating.size; - // Create a structuring element for dilation and erosion - cv::Mat element = cv::getStructuringElement( - cv::MORPH_RECT, cv::Size(2 * dilation_size + 1, 2 * dilation_size + 1), - cv::Point(dilation_size, dilation_size)); - - // Apply dilation to the image - cv::dilate(original, filtered, element); + apply_dilation(original, filtered, params.dilating.size, cv::MORPH_RECT); } void white_balance_filter(const FilterParams& params, From d7fcc6ac2a0b68f47332de7c5b303bac542812f5 Mon Sep 17 00:00:00 2001 From: Thomas Paulen Date: Mon, 3 Nov 2025 13:24:37 +0100 Subject: [PATCH 26/53] adding last part of the modularity stuf --- .../image_filters/image_filtering_ros.hpp | 2 +- .../include/image_filters/utilities.hpp | 6 ++-- image-filtering/src/image_processing.cpp | 34 +++---------------- image-filtering/src/utilities.cpp | 14 ++++++++ 4 files changed, 23 insertions(+), 33 deletions(-) diff --git a/image-filtering/include/image_filters/image_filtering_ros.hpp b/image-filtering/include/image_filters/image_filtering_ros.hpp index eff7358..ae55aee 100644 --- a/image-filtering/include/image_filters/image_filtering_ros.hpp +++ b/image-filtering/include/image_filters/image_filtering_ros.hpp @@ -1,7 +1,7 @@ #ifndef IMAGE_FILTERING_ROS_HPP #define IMAGE_FILTERING_ROS_HPP -#include // for jazzy this has to be hpp and for humble it has to be h +#include // for jazzy this has to be hpp and for humble it has to be h #include #include #include diff --git a/image-filtering/include/image_filters/utilities.hpp b/image-filtering/include/image_filters/utilities.hpp index aee642b..195502b 100644 --- a/image-filtering/include/image_filters/utilities.hpp +++ b/image-filtering/include/image_filters/utilities.hpp @@ -16,8 +16,10 @@ void toWeightedGray(const cv::Mat& bgr, cv::Mat& gray, double wB, double wG, dou int computeOtsuThreshold(const cv::Mat& hist_prob); -void apply_erosion(const cv::Mat& src, cv::Mat& dst, int size, int shape = cv::MORPH_CROSS); // TODO Apply these to the erotion and dilation filters -void apply_dilation(const cv::Mat& src, cv::Mat& dst, int size, int shape=cv::MORPH_CROSS); +int applyOtsu(const cv::Mat& gray8u, cv::Mat& out, bool invert = false, double maxval = 255.0); + +void apply_erosion(const cv::Mat& src, cv::Mat& dst, int size, int shape = cv::MORPH_RECT); +void apply_dilation(const cv::Mat& src, cv::Mat& dst, int size, int shape=cv::MORPH_RECT); #endif \ No newline at end of file diff --git a/image-filtering/src/image_processing.cpp b/image-filtering/src/image_processing.cpp index 3765be1..a792913 100644 --- a/image-filtering/src/image_processing.cpp +++ b/image-filtering/src/image_processing.cpp @@ -69,13 +69,7 @@ void ebus_filter(const FilterParams& params, cv::Mat eroded; int erosion_size = params.eroding.size; - // Create a structuring element for dilation and erosion - cv::Mat element = cv::getStructuringElement( - cv::MORPH_RECT, cv::Size(2 * erosion_size + 1, 2 * erosion_size + 1), - cv::Point(erosion_size, erosion_size)); - - // Apply erosion to the image - cv::erode(original, eroded, element); + apply_erosion(original, eroded, erosion_size); // Make an unsharp mask from original image cv::Mat blurred; @@ -113,32 +107,12 @@ void otsu_segmentation_filter(const FilterParams& params, } if (otsu_segmentation) { - // Calculate the histogram - int histSize = 256; - float range[] = {0, 256}; - const float* histRange = {range}; - cv::Mat hist; - calcHist(&filtered, 1, 0, cv::Mat(), hist, 1, &histSize, &histRange, - true, false); - - // Normalize histogram to get probabilities - hist /= filtered.total(); - - int optimalThreshold = computeOtsuThreshold(hist); - - // Apply the threshold to the image - cv::threshold(filtered, filtered, optimalThreshold, 255, - cv::THRESH_BINARY); - - + applyOtsu(filtered, filtered, false, 255); // Apply erosion followed by dilation (opening) - int erosionSize = params.otsu.erosion_size; - apply_erosion(filtered, filtered, erosionSize); - - int dilation_size = params.otsu.dilation_size; - apply_dilation(filtered, filtered, dilation_size); + apply_erosion(filtered, filtered, params.otsu.erosion_size, cv::MORPH_CROSS); + apply_dilation(filtered, filtered, params.otsu.dilation_size, cv::MORPH_CROSS); } } diff --git a/image-filtering/src/utilities.cpp b/image-filtering/src/utilities.cpp index 170e5bc..daf07a0 100644 --- a/image-filtering/src/utilities.cpp +++ b/image-filtering/src/utilities.cpp @@ -112,6 +112,20 @@ int computeOtsuThreshold(const cv::Mat& hist_prob) +// Returns the Otsu threshold value chosen by OpenCV (0..255) and outputs the thresholded binary image +int applyOtsu(const cv::Mat& gray8u, cv::Mat& out, bool invert, double maxval) +{ + CV_Assert(gray8u.type() == CV_8UC1 && "applyOtsu expects 8-bit single-channel input"); + + int ttype = invert ? (cv::THRESH_BINARY_INV | cv::THRESH_OTSU) + : (cv::THRESH_BINARY | cv::THRESH_OTSU); + + double thresh = cv::threshold(gray8u, out, /*thresh ignored*/0.0, maxval, ttype); + return static_cast(std::round(thresh)); +} + + + // Basic erosion void apply_erosion(const cv::Mat& src, From 23a8a1b24d5e32adaee67c7e5d7f4f79163c7538 Mon Sep 17 00:00:00 2001 From: Thomas Paulen Date: Mon, 3 Nov 2025 13:32:25 +0100 Subject: [PATCH 27/53] its time to say goodbye --- .../include/image_filters/utilities.hpp | 2 +- image-filtering/src/utilities.cpp | 76 +++++++++---------- 2 files changed, 39 insertions(+), 39 deletions(-) diff --git a/image-filtering/include/image_filters/utilities.hpp b/image-filtering/include/image_filters/utilities.hpp index 195502b..fd32fc3 100644 --- a/image-filtering/include/image_filters/utilities.hpp +++ b/image-filtering/include/image_filters/utilities.hpp @@ -14,7 +14,7 @@ void applyAutoGamma(cv::Mat& image, double correction_weight); void toWeightedGray(const cv::Mat& bgr, cv::Mat& gray, double wB, double wG, double wR); -int computeOtsuThreshold(const cv::Mat& hist_prob); +// int computeOtsuThreshold(const cv::Mat& hist_prob); int applyOtsu(const cv::Mat& gray8u, cv::Mat& out, bool invert = false, double maxval = 255.0); diff --git a/image-filtering/src/utilities.cpp b/image-filtering/src/utilities.cpp index daf07a0..706b461 100644 --- a/image-filtering/src/utilities.cpp +++ b/image-filtering/src/utilities.cpp @@ -71,44 +71,44 @@ void toWeightedGray(const cv::Mat& bgr, cv::Mat& gray, double wB, double wG, dou -// Computes the Otsu optimal threshold from a 256-bin normalized grayscale histogram. -// `hist_prob` must contain probabilities (sum ≈ 1). For every possible threshold t, -// the function splits the histogram into foreground [0, t) and background [t, 255], -// computes their weights and means, evaluates the between-class variance, and returns -// the t that maximizes it. A good threshold is one that makes two sizeable groups that -// are well separated in intensity: -// - Best regards ChatGPT -int computeOtsuThreshold(const cv::Mat& hist_prob) -{ - // Initialize variables for Otsu's method - std::vector sigma2_list(256, 0.0); - std::vector p(hist_prob.begin(), hist_prob.end()); // Probabilities - - for (int th = 1; th < 256; ++th) { - // Calculate omega (weights) for foreground and background - float omega_fg = std::accumulate(p.begin(), p.begin() + th, 0.0f); - float omega_bg = std::accumulate(p.begin() + th, p.end(), 0.0f); - - // Calculate mu (means) for foreground and background - float mu_fg = 0, mu_bg = 0; - for (int i = 0; i < th; ++i) { - mu_fg += i * p[i]; - } - for (int i = th; i < 256; ++i) { - mu_bg += i * p[i]; - } - - if (omega_fg > 0) - mu_fg /= omega_fg; - if (omega_bg > 0) - mu_bg /= omega_bg; - - // Calculate sigma squared and store in list - sigma2_list[th] = omega_fg * omega_bg * pow(mu_fg - mu_bg, 2); - } - - return int(std::max_element(sigma2_list.begin(), sigma2_list.end()) - sigma2_list.begin()); -} +// // Computes the Otsu optimal threshold from a 256-bin normalized grayscale histogram. +// // `hist_prob` must contain probabilities (sum ≈ 1). For every possible threshold t, +// // the function splits the histogram into foreground [0, t) and background [t, 255], +// // computes their weights and means, evaluates the between-class variance, and returns +// // the t that maximizes it. A good threshold is one that makes two sizeable groups that +// // are well separated in intensity: +// // - Best regards ChatGPT +// int computeOtsuThreshold(const cv::Mat& hist_prob) // TODO it is time for you to go, its been nice knowing you, but you have been replased by opencv +// { +// // Initialize variables for Otsu's method +// std::vector sigma2_list(256, 0.0); +// std::vector p(hist_prob.begin(), hist_prob.end()); // Probabilities + +// for (int th = 1; th < 256; ++th) { +// // Calculate omega (weights) for foreground and background +// float omega_fg = std::accumulate(p.begin(), p.begin() + th, 0.0f); +// float omega_bg = std::accumulate(p.begin() + th, p.end(), 0.0f); + +// // Calculate mu (means) for foreground and background +// float mu_fg = 0, mu_bg = 0; +// for (int i = 0; i < th; ++i) { +// mu_fg += i * p[i]; +// } +// for (int i = th; i < 256; ++i) { +// mu_bg += i * p[i]; +// } + +// if (omega_fg > 0) +// mu_fg /= omega_fg; +// if (omega_bg > 0) +// mu_bg /= omega_bg; + +// // Calculate sigma squared and store in list +// sigma2_list[th] = omega_fg * omega_bg * pow(mu_fg - mu_bg, 2); +// } + +// return int(std::max_element(sigma2_list.begin(), sigma2_list.end()) - sigma2_list.begin()); +// } From 4a50ca1a59f8bda7d0524300e8d95273838a293d Mon Sep 17 00:00:00 2001 From: Thomas Date: Mon, 3 Nov 2025 17:38:07 +0100 Subject: [PATCH 28/53] Making median_binary filter from the paper (it is kind of ass) --- .../config/image_filtering_params.yaml | 8 +- .../image_filters/image_filtering_ros.hpp | 2 +- .../image_filters/image_processing.hpp | 10 +- .../include/image_filters/utilities.hpp | 13 +- image-filtering/src/image_filtering_ros.cpp | 12 +- image-filtering/src/image_processing.cpp | 36 ++--- image-filtering/src/utilities.cpp | 128 +++++++++++++++++- 7 files changed, 164 insertions(+), 45 deletions(-) diff --git a/image-filtering/config/image_filtering_params.yaml b/image-filtering/config/image_filtering_params.yaml index 50982ef..ad99ec7 100644 --- a/image-filtering/config/image_filtering_params.yaml +++ b/image-filtering/config/image_filtering_params.yaml @@ -6,7 +6,7 @@ input_encoding: "mono8" filter_params: - filter_type: "dilation" + filter_type: "median_binary" flip: flip_code: 1 unsharpening: @@ -32,8 +32,10 @@ dilation_size: 2 overlap: percentage_threshold: 100.0 # Percentage to cap the picel intensity differance - median: # finds the median of each n x n square around each pixel - kernel_size: 5 # must be odd >= 3 + median_binary: # finds the median of each n x n square around each pixel + kernel_size: 3 # must be odd >= 1 + threshold: 100 # [0, 255] + invert: false binary: threshold: 20. maxval: 255. diff --git a/image-filtering/include/image_filters/image_filtering_ros.hpp b/image-filtering/include/image_filters/image_filtering_ros.hpp index ae55aee..eff7358 100644 --- a/image-filtering/include/image_filters/image_filtering_ros.hpp +++ b/image-filtering/include/image_filters/image_filtering_ros.hpp @@ -1,7 +1,7 @@ #ifndef IMAGE_FILTERING_ROS_HPP #define IMAGE_FILTERING_ROS_HPP -#include // for jazzy this has to be hpp and for humble it has to be h +#include // for jazzy this has to be hpp and for humble it has to be h #include #include #include diff --git a/image-filtering/include/image_filters/image_processing.hpp b/image-filtering/include/image_filters/image_processing.hpp index ed6bc73..d74b4c1 100644 --- a/image-filtering/include/image_filters/image_processing.hpp +++ b/image-filtering/include/image_filters/image_processing.hpp @@ -50,8 +50,10 @@ struct OtsuParams { struct OverlapParams{ double percentage_threshold; }; -struct MedianParams{ +struct MedianBinaryParams{ int kernel_size; + int threshold; + bool invert; }; struct BinaryParams{ @@ -69,7 +71,7 @@ struct FilterParams { EbusParams ebus; OtsuParams otsu; OverlapParams overlap; - MedianParams median; + MedianBinaryParams median_binary; BinaryParams binary; }; @@ -154,7 +156,7 @@ void overlap_filter(const FilterParams& filter_params, cv::Mat &filtered); -void median_filter(const FilterParams& filter_params, +void median_binary_filter(const FilterParams& filter_params, const cv::Mat& original, cv::Mat& filtered); @@ -173,7 +175,7 @@ const static std::map filter_functions = { {"ebus", ebus_filter}, {"otsu", otsu_segmentation_filter}, {"overlap", overlap_filter}, // This was done by the one and only Thomas - {"median", median_filter}, + {"median_binary", median_binary_filter}, {"binary", binary_threshold}, }; diff --git a/image-filtering/include/image_filters/utilities.hpp b/image-filtering/include/image_filters/utilities.hpp index fd32fc3..cecd638 100644 --- a/image-filtering/include/image_filters/utilities.hpp +++ b/image-filtering/include/image_filters/utilities.hpp @@ -11,15 +11,22 @@ -void applyAutoGamma(cv::Mat& image, double correction_weight); -void toWeightedGray(const cv::Mat& bgr, cv::Mat& gray, double wB, double wG, double wR); +void apply_auto_gamma(cv::Mat& image, double correction_weight); +void to_weighted_gray(const cv::Mat& bgr, cv::Mat& gray, double wB, double wG, double wR); // int computeOtsuThreshold(const cv::Mat& hist_prob); -int applyOtsu(const cv::Mat& gray8u, cv::Mat& out, bool invert = false, double maxval = 255.0); +int apply_otsu(const cv::Mat& gray8u, cv::Mat& out, bool invert = false, double maxval = 255.0); void apply_erosion(const cv::Mat& src, cv::Mat& dst, int size, int shape = cv::MORPH_RECT); void apply_dilation(const cv::Mat& src, cv::Mat& dst, int size, int shape=cv::MORPH_RECT); +void apply_median(const cv::Mat& original, cv::Mat& filtered, int kernel_size); + +void apply_fixed_threshold(const cv::Mat& img, cv::Mat& filtered, int thresh, bool invert = false); + + +// TODO Fix, does not work properly (Chat has failed me) +void distance_field(const cv::Mat& binObstacles, cv::Mat& dist, bool obstaclesAreWhite = true, int type = cv::DIST_L2, int maskSize = 3); #endif \ No newline at end of file diff --git a/image-filtering/src/image_filtering_ros.cpp b/image-filtering/src/image_filtering_ros.cpp index b51edef..d9a9ff9 100644 --- a/image-filtering/src/image_filtering_ros.cpp +++ b/image-filtering/src/image_filtering_ros.cpp @@ -46,7 +46,9 @@ void ImageFilteringNode::declare_parameters() { this->declare_parameter("filter_params.overlap.percentage_threshold"); //Thomas has left a mark here - this->declare_parameter("filter_params.median.kernel_size"); + this->declare_parameter("filter_params.median_binary.kernel_size"); + this->declare_parameter("filter_params.median_binary.threshold"); + this->declare_parameter("filter_params.median_binary.invert"); this->declare_parameter("filter_params.binary.threshold"); this->declare_parameter("filter_params.binary.maxval"); @@ -101,8 +103,12 @@ void ImageFilteringNode::set_filter_params() { this->get_parameter("filter_params.otsu.dilation_size").as_int(); params.overlap.percentage_threshold = // Thomas is everyware this->get_parameter("filter_params.overlap.percentage_threshold").as_double(); - params.median.kernel_size = - this->get_parameter("filter_params.median.kernel_size").as_int(); + params.median_binary.kernel_size = + this->get_parameter("filter_params.median_binary.kernel_size").as_int(); + params.median_binary.threshold = + this->get_parameter("filter_params.median_binary.threshold").as_int(); + params.median_binary.invert = + this->get_parameter("filter_params.median_binary.invert").as_bool(); params.binary.threshold = this->get_parameter("filter_params.binary.threshold").as_double(); params.binary.maxval = diff --git a/image-filtering/src/image_processing.cpp b/image-filtering/src/image_processing.cpp index a792913..f14a4b8 100644 --- a/image-filtering/src/image_processing.cpp +++ b/image-filtering/src/image_processing.cpp @@ -97,17 +97,17 @@ void otsu_segmentation_filter(const FilterParams& params, bool otsu_segmentation = params.otsu.otsu_segmentation; - toWeightedGray(original, filtered, params.otsu.gsc_weight_b, + to_weighted_gray(original, filtered, params.otsu.gsc_weight_b, params.otsu.gsc_weight_g, params.otsu.gsc_weight_r); if (gamma_auto_correction) { - applyAutoGamma(filtered, gamma_auto_correction_weight); + apply_auto_gamma(filtered, gamma_auto_correction_weight); } if (otsu_segmentation) { - applyOtsu(filtered, filtered, false, 255); + apply_otsu(filtered, filtered, false, 255); // Apply erosion followed by dilation (opening) @@ -159,34 +159,12 @@ void overlap_filter(const FilterParams& filter_params, prevR = curR.clone(); } -void median_filter(const FilterParams& filter_params, +void median_binary_filter(const FilterParams& filter_params, const cv::Mat& original, cv::Mat& filtered){ - CV_Assert(!original.empty()); - - // Validate & sanitize kernel size (must be odd and >= 3) - int k = filter_params.median.kernel_size; - if (k < 3) k = 3; - if ((k & 1) == 0) ++k; // make odd if even - - // cv::medianBlur is not suported for all depths "sais chat" - // (works for CV_8U, CV_16U, CV_32F) - const int depth = original.depth(); - const bool supported = (depth == CV_8U || depth == CV_16U || depth == CV_32F); - - const cv::Mat* src = &original; - cv::Mat tmp; - if (!supported) { - // Simple, safe conversion to 8-bit - original.convertTo(tmp, CV_8U); - src = &tmp; - } - - cv::medianBlur(*src, filtered, k); - - // If converted to 8U and want to preserve original depth, converts back here: - if (!supported) filtered.convertTo(filtered, depth); + apply_median(original, filtered, filter_params.median_binary.kernel_size); + apply_fixed_threshold(filtered, filtered, filter_params.median_binary.threshold, filter_params.median_binary.invert); } @@ -236,3 +214,5 @@ void apply_filter(const std::string& filter, original.copyTo(filtered); // Default to no filter } } + + diff --git a/image-filtering/src/utilities.cpp b/image-filtering/src/utilities.cpp index 706b461..2518a2e 100644 --- a/image-filtering/src/utilities.cpp +++ b/image-filtering/src/utilities.cpp @@ -53,7 +53,7 @@ double computeAutoGammaFromMean(const cv::Mat& image) { // Auto-choose a gamma so dark images get lifted and bright images get toned down (expects mono8) // - It sets the mean intensity to 255/2 ≃ 128 // - The weight makes makes all the values weeker(<1) or stronger(>1) -void applyAutoGamma(cv::Mat& image, double correction_weight) { +void apply_auto_gamma(cv::Mat& image, double correction_weight) { double gamma = computeAutoGammaFromMean(image) * correction_weight; applyGammaLUT(image, gamma); } @@ -63,7 +63,7 @@ void applyAutoGamma(cv::Mat& image, double correction_weight) { // Convert BGR image to single-channel grayscale using custom B,G,R weights // weights = (b, g, r), e.g. (0.114f, 0.587f, 0.299f) -void toWeightedGray(const cv::Mat& bgr, cv::Mat& gray, double wB, double wG, double wR) { +void to_weighted_gray(const cv::Mat& bgr, cv::Mat& gray, double wB, double wG, double wR) { cv::Matx13f customWeights(wB, wG, wR); cv::transform(bgr, gray, customWeights); } @@ -113,7 +113,7 @@ void toWeightedGray(const cv::Mat& bgr, cv::Mat& gray, double wB, double wG, dou // Returns the Otsu threshold value chosen by OpenCV (0..255) and outputs the thresholded binary image -int applyOtsu(const cv::Mat& gray8u, cv::Mat& out, bool invert, double maxval) +int apply_otsu(const cv::Mat& gray8u, cv::Mat& out, bool invert, double maxval) { CV_Assert(gray8u.type() == CV_8UC1 && "applyOtsu expects 8-bit single-channel input"); @@ -154,9 +154,81 @@ void apply_dilation(const cv::Mat& src, +// Median filter that preserves original depth if it's unsupported by cv::medianBlur. +// Supported depths: CV_8U, CV_16U, CV_32F +// For others (e.g., CV_16S, CV_32S, CV_64F) we convert to CV_32F, filter, then convert back. +void apply_median(const cv::Mat& original, cv::Mat& filtered, int kernel_size) { + CV_Assert(!original.empty()); + // If caller passed 1, just copy + if (kernel_size == 1) { + original.copyTo(filtered); + return; + } + + // Sanitize kernel size: must be odd and >= 3 + if (kernel_size < 3) kernel_size = 3; + if ((kernel_size & 1) == 0) ++kernel_size; + + const int depth = original.depth(); + const bool supported = (depth == CV_8U || depth == CV_16U || depth == CV_32F); + + const cv::Mat* src = &original; + cv::Mat work, out; + + if (!supported) { + // Convert unsupported depths to CV_32F to avoid clipping (better than going to 8U). + original.convertTo(work, CV_32F); + src = &work; + } + + // Do the median blur (OpenCV supports multi-channel for these depths) + cv::medianBlur(*src, out, kernel_size); + + // Convert back to original depth if we promoted + if (!supported) { + out.convertTo(filtered, depth); + } else { + filtered = std::move(out); + } +} + + + +// Apply a fixed binary threshold. +// - Accepts grayscale or color input (auto-converts to gray). +// - Ensures 8-bit depth for thresholding. +// - Returns a 0/255 mask (CV_8U). +// - Set `invert=true` to get white background & black foreground. +void apply_fixed_threshold(const cv::Mat& img, cv::Mat& filtered, int thresh, bool invert) +{ + if (img.empty()) { + throw std::invalid_argument("applyFixedThreshold: input image is empty"); + } + if (thresh < 0 || thresh > 255) { + throw std::out_of_range("applyFixedThreshold: thresh must be in [0, 255]"); + } + + // Convert to grayscale + cv::Mat gray; + if (img.channels() == 3 || img.channels() == 4) { + cv::cvtColor(img, gray, cv::COLOR_BGR2GRAY); + } else { + gray = img; + } + + // Ensure 8-bit + if (gray.depth() != CV_8U) { + cv::Mat tmp; + cv::normalize(gray, tmp, 0, 255, cv::NORM_MINMAX); + tmp.convertTo(gray, CV_8U); + } + // Threshold + int type = invert ? (cv::THRESH_BINARY_INV) : (cv::THRESH_BINARY); + cv::threshold(gray, filtered, thresh, 255, type); +} @@ -166,9 +238,59 @@ void apply_dilation(const cv::Mat& src, +// Takes a strict binary obstacle mask (0/255) and returns a float image +// where each pixel is the distance (in pixels) to the nearest obstacle. +// Requirements: +// - binObstacles: single-channel CV_8U with values in {0, 255} only. +// - obstaclesAreWhite: +// true -> obstacles = 255, free = 0 +// false -> obstacles = 0, free = 255 +// Output: +// - dist: CV_32F distances to nearest obstacle (0 at obstacle pixels). +// Params: +// - type: CV_DIST_L1, CV_DIST_L2, CV_DIST_C, ... +// - maskSize: 3, 5, or CV_DIST_MASK_PRECISE (0) +void distance_field(const cv::Mat& binObstacles, + cv::Mat& dist, + bool obstaclesAreWhite, + int type, + int maskSize) +{ + if (binObstacles.empty()) + throw std::invalid_argument("distance_field_binary: input is empty"); + + if (binObstacles.channels() != 1) + throw std::invalid_argument("distance_field_binary: input must be single-channel"); + + if (binObstacles.depth() != CV_8U) + throw std::invalid_argument("distance_field_binary: input must be CV_8U (0/255)"); + + // Validate strict binary: values must be only 0 or 255 + { + cv::Mat notZero = (binObstacles != 0); + cv::Mat not255 = (binObstacles != 255); + cv::Mat invalid = notZero & not255; // pixels that are neither 0 nor 255 + if (cv::countNonZero(invalid) > 0) + throw std::invalid_argument("distance_field_binary: input must contain only 0 or 255 values"); + } + // OpenCV distanceTransform computes distance TO the nearest ZERO pixel + // for NON-ZERO regions. We want distance FROM obstacles. + // Build freeMask (255 = free, 0 = obstacles). + cv::Mat freeMask; + if (obstaclesAreWhite) { + // obstacles=255 -> free=0, invert to get free=255 + freeMask = 255 - binObstacles; + } else { + // obstacles=0, free=255 already correct + freeMask = binObstacles; + } + if (!(maskSize == 3 || maskSize == 5 || maskSize == cv::DIST_MASK_PRECISE)) + maskSize = 3; + cv::distanceTransform(freeMask, dist, type, maskSize); // dist is CV_32F +} From 6cc3f949402598d9a6e5e01426c34aeff0f2c161 Mon Sep 17 00:00:00 2001 From: Thomas Date: Wed, 5 Nov 2025 19:45:05 +0100 Subject: [PATCH 29/53] Filip said it was ok --- .../config/image_filtering_params.yaml | 8 +- .../image_filters/image_processing.hpp | 228 ++++++++++-------- .../include/image_filters/utilities.hpp | 2 +- image-filtering/src/image_filtering_ros.cpp | 20 +- image-filtering/src/image_processing.cpp | 81 ++++--- image-filtering/src/utilities.cpp | 6 + 6 files changed, 189 insertions(+), 156 deletions(-) diff --git a/image-filtering/config/image_filtering_params.yaml b/image-filtering/config/image_filtering_params.yaml index ad99ec7..857cb1e 100644 --- a/image-filtering/config/image_filtering_params.yaml +++ b/image-filtering/config/image_filtering_params.yaml @@ -3,10 +3,10 @@ sub_topic: "/fls_publisher/display_mono" pub_topic: "/filtered_image" output_encoding: "mono8" - input_encoding: "mono8" + input_encoding: "mono8" filter_params: - filter_type: "median_binary" + filter_type: "ebus" flip: flip_code: 1 unsharpening: @@ -37,9 +37,9 @@ threshold: 100 # [0, 255] invert: false binary: - threshold: 20. + threshold: 0.1 # in percent maxval: 255. - invert: false + invert: true # Filter params should reflect the FilterParams struct diff --git a/image-filtering/include/image_filters/image_processing.hpp b/image-filtering/include/image_filters/image_processing.hpp index d74b4c1..16d952e 100644 --- a/image-filtering/include/image_filters/image_processing.hpp +++ b/image-filtering/include/image_filters/image_processing.hpp @@ -10,159 +10,175 @@ #include #include -struct FlipParams { - int flip_code; -}; -struct UnsharpeningParams { - int blur_size; -}; - -struct ErodingParams { - int size; -}; - -struct DilatingParams { - int size; -}; - -struct WhiteBalancingParams { - double contrast_percentage; -}; -struct EbusParams { - int erosion_size; - int blur_size; - int mask_weight; -}; - -struct OtsuParams { - bool gamma_auto_correction; - double gamma_auto_correction_weight; - bool otsu_segmentation; - double gsc_weight_r; - double gsc_weight_g; - double gsc_weight_b; - int erosion_size; - int dilation_size; -}; - -// Thomas' masterpiece -struct OverlapParams{ - double percentage_threshold; -}; -struct MedianBinaryParams{ - int kernel_size; - int threshold; - bool invert; -}; - -struct BinaryParams{ - double threshold; - double maxval; - bool invert; -}; - -struct FilterParams { - FlipParams flip; - UnsharpeningParams unsharpening; - ErodingParams eroding; - DilatingParams dilating; - WhiteBalancingParams white_balancing; - EbusParams ebus; - OtsuParams otsu; - OverlapParams overlap; - MedianBinaryParams median_binary; - BinaryParams binary; +// struct FilterParams { +// FlipParams flip; +// UnsharpeningParams unsharpening; +// ErodingParams eroding; +// DilatingParams dilating; +// WhiteBalancingParams white_balancing; +// EbusParams ebus; +// OtsuParams otsu; +// OverlapParams overlap; +// MedianBinaryParams median_binary; +// BinaryParams binary; +// }; + +struct NoFilterStruct{}; + +struct FlipFiltersTructer { + int flip_code; }; -typedef void (*FilterFunction)(const FilterParams&, const cv::Mat&, cv::Mat&); - /** * Reads the filter_type from the FilterParams struct * and calls the appropriate filter function from the filter_functions map. */ -void apply_filter(const std::string& filter, - const FilterParams& params, - const cv::Mat& original, - cv::Mat& filtered); + +virtual class Filter{ + virtual void apply_filter(const cv::Mat& original, cv::Mat& filtered); +}; /** * No filter, just copy the image */ -void no_filter(const FilterParams& params, - const cv::Mat& original, - cv::Mat& modified); +class NoFilter(const NoFilterStruct& args): public Filter{ + void no_filter(const FilterParams& params, + const cv::Mat& original, + cv::Mat& modified); + private: + int flip_code; +}; +virtual class FilterClass(FilterStuct args); /** * Flips the image */ -void flip_filter(const FilterParams& params, - const cv::Mat& original, - cv::Mat& modified); +class Flip (Filter) { +public: + int flip_code; + + +}; /** * Makes edges harder */ -void sharpening_filter(const FilterParams& params, - const cv::Mat& original, - cv::Mat& modified); +class Sharpening { + void sharpening_filter(const FilterParams& params, + const cv::Mat& original, + cv::Mat& modified); +}; /** * Makes edges harder in a smarter way */ -void unsharpening_filter(const FilterParams& params, - const cv::Mat& original, - cv::Mat& modified); +class Unsharpening { +public: + int blur_size; + void unsharpening_filter(const FilterParams& params, + const cv::Mat& original, + cv::Mat& modified); +}; /** * Expands the dark areas of the image */ -void erosion_filter(const FilterParams& params, - const cv::Mat& original, - cv::Mat& modified); +class Erosion { +public: + int size; + void erosion_filter(const FilterParams& params, + const cv::Mat& original, + cv::Mat& modified); +}; /** * Expands the bright areas of the image */ -void dilation_filter(const FilterParams& params, - const cv::Mat& original, - cv::Mat& modified); +class Dilation { +public: + int size; + void dilation_filter(const FilterParams& params, + const cv::Mat& original, + cv::Mat& modified); +}; /** * White Balancing Filter */ -void white_balance_filter(const FilterParams& params, - const cv::Mat& original, - cv::Mat& filtered); +class WhiteBalance { +public: + double contrast_percentage; + void white_balance_filter(const FilterParams& params, + const cv::Mat& original, + cv::Mat& filtered); +}; -/** +/** * A filter that worked well-ish in the mc-lab conditions easter 2023 * Uses a combination of dilation and unsharpening */ -void ebus_filter(const FilterParams& params, - const cv::Mat& original, - cv::Mat& filtered); +class Ebus { +public: + int erosion_size; + int blur_size; + int mask_weight; + void ebus_filter(const FilterParams& params, + const cv::Mat& original, + cv::Mat& filtered); +}; /** * A filter based on Otsu's method */ -void otsu_segmentation_filter(const FilterParams& params, - const cv::Mat& original, - cv::Mat& output); +class OtsuSegmentation { +public: + bool gamma_auto_correction; + double gamma_auto_correction_weight; + bool otsu_segmentation; + double gsc_weight_r; + double gsc_weight_g; + double gsc_weight_b; + int erosion_size; + int dilation_size; + + void otsu_segmentation_filter(const FilterParams& params, + const cv::Mat& original, + cv::Mat& output); +}; + +class Overlap { +public: + // Thomas' masterpiece + double percentage_threshold; + + void overlap_filter(const FilterParams& filter_params, + const cv::Mat &original, + cv::Mat &filtered); +}; +class MedianBinary { +public: + int kernel_size; + int threshold; + bool invert; -void overlap_filter(const FilterParams& filter_params, - const cv::Mat &original, - cv::Mat &filtered); + void median_binary_filter(const FilterParams& filter_params, + const cv::Mat& original, + cv::Mat& filtered); +}; +class BinaryThreshold { +public: + double threshold; + double maxval; + bool invert; -void median_binary_filter(const FilterParams& filter_params, - const cv::Mat& original, - cv::Mat& filtered); + void binary_threshold(const FilterParams& filter_params, + const cv::Mat& original, + cv::Mat& filtered); +}; -void binary_threshold(const FilterParams& filter_params, - const cv::Mat& original, - cv::Mat& filtered); const static std::map filter_functions = { {"no_filter", no_filter}, @@ -174,7 +190,7 @@ const static std::map filter_functions = { {"white_balancing", white_balance_filter}, {"ebus", ebus_filter}, {"otsu", otsu_segmentation_filter}, - {"overlap", overlap_filter}, // This was done by the one and only Thomas + {"overlap", Overlap::overlap_filter}, // This was done by the one and only Thomas {"median_binary", median_binary_filter}, {"binary", binary_threshold}, }; diff --git a/image-filtering/include/image_filters/utilities.hpp b/image-filtering/include/image_filters/utilities.hpp index cecd638..e6abf31 100644 --- a/image-filtering/include/image_filters/utilities.hpp +++ b/image-filtering/include/image_filters/utilities.hpp @@ -27,6 +27,6 @@ void apply_fixed_threshold(const cv::Mat& img, cv::Mat& filtered, int thresh, bo // TODO Fix, does not work properly (Chat has failed me) -void distance_field(const cv::Mat& binObstacles, cv::Mat& dist, bool obstaclesAreWhite = true, int type = cv::DIST_L2, int maskSize = 3); +void distance_field(const cv::Mat& binObstacles, cv::Mat& dist, bool obstaclesAreWhite = true, int type = cv::DIST_L2, int maskSize = 3); // DIST_L2 is normal euclidian #endif \ No newline at end of file diff --git a/image-filtering/src/image_filtering_ros.cpp b/image-filtering/src/image_filtering_ros.cpp index d9a9ff9..f327cfa 100644 --- a/image-filtering/src/image_filtering_ros.cpp +++ b/image-filtering/src/image_filtering_ros.cpp @@ -46,13 +46,20 @@ void ImageFilteringNode::declare_parameters() { this->declare_parameter("filter_params.overlap.percentage_threshold"); //Thomas has left a mark here - this->declare_parameter("filter_params.median_binary.kernel_size"); - this->declare_parameter("filter_params.median_binary.threshold"); - this->declare_parameter("filter_params.median_binary.invert"); - this->declare_parameter("filter_params.binary.threshold"); this->declare_parameter("filter_params.binary.maxval"); this->declare_parameter("filter_params.binary.invert"); + GenericStruct args; + if(filter_type == OVERLAP){ + Filterstruct args; + this->declare_parameter("filter_params.median_binary.kernel_size"); + this->declare_parameter("filter_params.median_binary.threshold"); + this->declare_parameter("filter_params.median_binary.invert"); + args = overlapparms(args); + } + switch + this->filter_class_ = constructor(args) + } @@ -183,16 +190,17 @@ void ImageFilteringNode::image_callback( } catch (cv_bridge::Exception& e) { spdlog::error("cv_bridge exception: {}", e.what()); - return; + ; } cv::Mat input_image = cv_ptr->image; cv::Mat filtered_image; + apply_filter(filter_, filter_params_, input_image, filtered_image); std::string output_encoding = this->get_parameter("output_encoding").as_string(); - auto message = std::make_unique(); + auto message = std::make_unique(); cv_bridge::CvImage(msg->header, output_encoding, filtered_image) .toImageMsg(*message); diff --git a/image-filtering/src/image_processing.cpp b/image-filtering/src/image_processing.cpp index f14a4b8..da8a2cf 100644 --- a/image-filtering/src/image_processing.cpp +++ b/image-filtering/src/image_processing.cpp @@ -2,20 +2,21 @@ #include #include -void no_filter([[maybe_unused]] const FilterParams& params, - const cv::Mat& original, - cv::Mat& filtered) { + +void NoFilter::no_filter([[maybe_unused]] const FilterParams& params, + const cv::Mat& original, + cv::Mat& filtered) { original.copyTo(filtered); } -void flip_filter([[maybe_unused]] const FilterParams& params, +void Flip::flip_filter([[maybe_unused]] const FilterParams& params, const cv::Mat& original, cv::Mat& filtered) { int flip_code = params.flip.flip_code; // 0: x-axis, 1: y-axis, -1: both cv::flip(original, filtered, flip_code); } -void sharpening_filter([[maybe_unused]] const FilterParams& params, +void Sharpening::sharpening_filter([[maybe_unused]] const FilterParams& params, const cv::Mat& original, cv::Mat& filtered) { // Sharpen image @@ -23,7 +24,7 @@ void sharpening_filter([[maybe_unused]] const FilterParams& params, cv::filter2D(original, filtered, -1, kernel); } -void unsharpening_filter(const FilterParams& params, +void Unsharpening::unsharpening_filter(const FilterParams& params, const cv::Mat& original, cv::Mat& filtered) { int blur_size = params.unsharpening.blur_size; @@ -39,19 +40,19 @@ void unsharpening_filter(const FilterParams& params, addWeighted(original, 1, mask, 3, 0, filtered); } -void erosion_filter(const FilterParams& params, +void Erosion::erosion_filter(const FilterParams& params, const cv::Mat& original, cv::Mat& filtered) { apply_erosion(original, filtered, params.eroding.size, cv::MORPH_RECT); } -void dilation_filter(const FilterParams& params, +void Dilation::dilation_filter(const FilterParams& params, const cv::Mat& original, cv::Mat& filtered) { apply_dilation(original, filtered, params.dilating.size, cv::MORPH_RECT); } -void white_balance_filter(const FilterParams& params, +void WhiteBalance::white_balance_filter(const FilterParams& params, const cv::Mat& original, cv::Mat& filtered) { double contrast_percentage = params.white_balancing.contrast_percentage; @@ -60,7 +61,7 @@ void white_balance_filter(const FilterParams& params, balance->balanceWhite(original, filtered); } -void ebus_filter(const FilterParams& params, +void Ebus::ebus_filter(const FilterParams& params, const cv::Mat& original, cv::Mat& filtered) { int blur_size = params.ebus.blur_size; @@ -87,7 +88,7 @@ void ebus_filter(const FilterParams& params, -void otsu_segmentation_filter(const FilterParams& params, +void Otsu::otsu_segmentation_filter(const FilterParams& params, const cv::Mat& original, cv::Mat& filtered) { bool gamma_auto_correction = params.otsu.gamma_auto_correction; @@ -117,59 +118,62 @@ void otsu_segmentation_filter(const FilterParams& params, } // Thomas was here -void overlap_filter(const FilterParams& filter_params, +void Overlap::overlap_filter(const FilterParams& filter_params, const cv::Mat& original, cv::Mat& filtered) { - static cv::Mat prevR; // store previous R channel only + static cv::Mat prev; // previous mono frame static bool has_prev = false; - // Extract current R channel - cv::Mat curR; - cv::extractChannel(original, curR, 2); // 0=B,1=G,2=R - - if (!has_prev || prevR.empty() || prevR.size()!=curR.size() || prevR.type()!=curR.type()) { - original.copyTo(filtered); // first call (or size/type change): pass through - prevR = curR.clone(); // cache R channel + // Basic checks (keep it simple; require mono8) + if (original.empty()) + throw std::invalid_argument("overlap_filter_mono: input is empty"); + if (original.type() != CV_8UC1) + throw std::invalid_argument("overlap_filter_mono: input must be CV_8UC1 (mono8)"); + + if (!has_prev || prev.empty() + || prev.size() != original.size() + || prev.type() != original.type()) + { + original.copyTo(filtered); // pass-through on first frame / size change + prev = original.clone(); has_prev = true; return; } - // |cur - prev| on the R channel + // |cur - prev| cv::Mat diff8u; - cv::absdiff(curR, prevR, diff8u); + cv::absdiff(original, prev, diff8u); - // % of full 8-bit range - cv::Mat percent32f; - diff8u.convertTo(percent32f, CV_32F, 100.0 / 255.0); + // Convert percentage threshold to an 8-bit delta threshold + double pct = std::clamp(filter_params.overlap.percentage_threshold, 0.0, 100.0); + int delta_thr = static_cast(std::round(pct * 255.0 / 100.0)); - // Mask: pixels whose % change > threshold + // Mask: changed pixels cv::Mat mask; - cv::threshold(percent32f, mask, filter_params.overlap.percentage_threshold, 255.0, cv::THRESH_BINARY); - mask.convertTo(mask, CV_8U); + cv::threshold(diff8u, mask, delta_thr, 255, cv::THRESH_BINARY); - // Zero out those pixels in the R channel + // Zero out changed pixels filtered = original.clone(); - std::vector ch; - cv::split(filtered, ch); // ch[2] is R - ch[2].setTo(0, mask); - cv::merge(ch, filtered); + filtered.setTo(0, mask); + + // Update history + prev = original.clone(); - // Update history (R channel only) - prevR = curR.clone(); } -void median_binary_filter(const FilterParams& filter_params, +void MedianBinary::median_binary_filter(const FilterParams& filter_params, const cv::Mat& original, cv::Mat& filtered){ apply_median(original, filtered, filter_params.median_binary.kernel_size); apply_fixed_threshold(filtered, filtered, filter_params.median_binary.threshold, filter_params.median_binary.invert); + distance_field(filtered, filtered); } -void binary_threshold(const FilterParams& filter_params, +void BinaryThreshold::binary_threshold(const FilterParams& filter_params, const cv::Mat& original, cv::Mat& filtered) { @@ -204,8 +208,7 @@ void binary_threshold(const FilterParams& filter_params, -void apply_filter(const std::string& filter, - const FilterParams& params, +void Filter::apply_filter(const std::string& filter, const cv::Mat& original, cv::Mat& filtered) { if (filter_functions.contains(filter)) { diff --git a/image-filtering/src/utilities.cpp b/image-filtering/src/utilities.cpp index 2518a2e..49487fb 100644 --- a/image-filtering/src/utilities.cpp +++ b/image-filtering/src/utilities.cpp @@ -290,6 +290,12 @@ void distance_field(const cv::Mat& binObstacles, maskSize = 3; cv::distanceTransform(freeMask, dist, type, maskSize); // dist is CV_32F + + const float cap = 100.f; // pixel + cv::Mat clipped; cv::min(dist, cap, clipped); + clipped.convertTo(dist, CV_8U, 255.0f/cap); + // publish vis8u as "mono8" + } From df64d93085d1fc2991302a3bad11eec2ea4d3b40 Mon Sep 17 00:00:00 2001 From: Thomas Date: Fri, 7 Nov 2025 18:00:34 +0100 Subject: [PATCH 30/53] testing --- image-filtering/include/image_filters/image_processing.hpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/image-filtering/include/image_filters/image_processing.hpp b/image-filtering/include/image_filters/image_processing.hpp index 16d952e..5d53b62 100644 --- a/image-filtering/include/image_filters/image_processing.hpp +++ b/image-filtering/include/image_filters/image_processing.hpp @@ -24,9 +24,8 @@ // BinaryParams binary; // }; -struct NoFilterStruct{}; -struct FlipFiltersTructer { +struct FlipFilterParams { int flip_code; }; @@ -42,6 +41,8 @@ virtual class Filter{ /** * No filter, just copy the image */ +struct NoFilterParams{}; + class NoFilter(const NoFilterStruct& args): public Filter{ void no_filter(const FilterParams& params, const cv::Mat& original, From c8df273d7ae4ef289ce92e20a6f29475ff9d381f Mon Sep 17 00:00:00 2001 From: Thomas Date: Fri, 7 Nov 2025 18:10:47 +0100 Subject: [PATCH 31/53] Hei --- image-filtering/config/image_filtering_params.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/image-filtering/config/image_filtering_params.yaml b/image-filtering/config/image_filtering_params.yaml index 857cb1e..79d6d70 100644 --- a/image-filtering/config/image_filtering_params.yaml +++ b/image-filtering/config/image_filtering_params.yaml @@ -14,7 +14,7 @@ erosion: size: 1 dilation: - size: 3 + size: 2 white_balancing: contrast_percentage: 0.1 ebus: From 831412e5dc961d03543484c5eac233cac9b358a1 Mon Sep 17 00:00:00 2001 From: Thomas Date: Fri, 7 Nov 2025 18:11:34 +0100 Subject: [PATCH 32/53] tilbake --- image-filtering/config/image_filtering_params.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/image-filtering/config/image_filtering_params.yaml b/image-filtering/config/image_filtering_params.yaml index 79d6d70..857cb1e 100644 --- a/image-filtering/config/image_filtering_params.yaml +++ b/image-filtering/config/image_filtering_params.yaml @@ -14,7 +14,7 @@ erosion: size: 1 dilation: - size: 2 + size: 3 white_balancing: contrast_percentage: 0.1 ebus: From 4a297af5e39cd334c4030fa2ef5df2ff12bba32b Mon Sep 17 00:00:00 2001 From: Thomas Date: Wed, 12 Nov 2025 15:39:41 +0100 Subject: [PATCH 33/53] Basicaly changed the whole thing, trust me bro it is beter --- .../config/image_filtering_params.yaml | 62 ++--- .../image_filters/image_filtering_ros.hpp | 9 +- .../image_filters/image_processing.hpp | 231 ++++++------------ image-filtering/src/image_filtering_ros.cpp | 147 ++++++----- image-filtering/src/image_processing.cpp | 211 +--------------- 5 files changed, 212 insertions(+), 448 deletions(-) diff --git a/image-filtering/config/image_filtering_params.yaml b/image-filtering/config/image_filtering_params.yaml index 857cb1e..64f1426 100644 --- a/image-filtering/config/image_filtering_params.yaml +++ b/image-filtering/config/image_filtering_params.yaml @@ -7,39 +7,39 @@ filter_params: filter_type: "ebus" - flip: - flip_code: 1 + # flip: + # flip_code: 1 unsharpening: blur_size: 8 - erosion: - size: 1 - dilation: - size: 3 - white_balancing: - contrast_percentage: 0.1 - ebus: - erosion_size: 2 - blur_size: 30 - mask_weight: 5 - otsu: - gsc_weight_r: 1.0 # Grayscale red weight - gsc_weight_g: 0.0 # Grayscale green weight - gsc_weight_b: 0.0 # Grayscale blue weight - gamma_auto_correction: true - gamma_auto_correction_weight: 4.0 - otsu_segmentation: true - erosion_size: 2 - dilation_size: 2 - overlap: - percentage_threshold: 100.0 # Percentage to cap the picel intensity differance - median_binary: # finds the median of each n x n square around each pixel - kernel_size: 3 # must be odd >= 1 - threshold: 100 # [0, 255] - invert: false - binary: - threshold: 0.1 # in percent - maxval: 255. - invert: true + # erosion: + # size: 1 + # dilation: + # size: 3 + # white_balancing: + # contrast_percentage: 0.1 + # ebus: + # erosion_size: 2 + # blur_size: 30 + # mask_weight: 5 + # otsu: + # gsc_weight_r: 1.0 # Grayscale red weight + # gsc_weight_g: 0.0 # Grayscale green weight + # gsc_weight_b: 0.0 # Grayscale blue weight + # gamma_auto_correction: true + # gamma_auto_correction_weight: 4.0 + # otsu_segmentation: true + # erosion_size: 2 + # dilation_size: 2 + # overlap: + # percentage_threshold: 100.0 # Percentage to cap the picel intensity differance + # median_binary: # finds the median of each n x n square around each pixel + # kernel_size: 3 # must be odd >= 1 + # threshold: 100 # [0, 255] + # invert: false + # binary: + # threshold: 0.1 # in percent + # maxval: 255. + # invert: true # Filter params should reflect the FilterParams struct diff --git a/image-filtering/include/image_filters/image_filtering_ros.hpp b/image-filtering/include/image_filters/image_filtering_ros.hpp index eff7358..697153d 100644 --- a/image-filtering/include/image_filters/image_filtering_ros.hpp +++ b/image-filtering/include/image_filters/image_filtering_ros.hpp @@ -97,16 +97,11 @@ class ImageFilteringNode : public rclcpp::Node { std::string image_topic_; /** - * @brief The filter parameters + * @brief Pointer to the filter object * */ - FilterParams filter_params_; + std::unique_ptr filter_; - /** - * @brief filter to apply - * - */ - std::string filter_; }; #endif // IMAGE_FILTERING_ROS_HPP diff --git a/image-filtering/include/image_filters/image_processing.hpp b/image-filtering/include/image_filters/image_processing.hpp index 5d53b62..a527f0f 100644 --- a/image-filtering/include/image_filters/image_processing.hpp +++ b/image-filtering/include/image_filters/image_processing.hpp @@ -11,190 +11,119 @@ #include -// struct FilterParams { -// FlipParams flip; -// UnsharpeningParams unsharpening; -// ErodingParams eroding; -// DilatingParams dilating; -// WhiteBalancingParams white_balancing; -// EbusParams ebus; -// OtsuParams otsu; -// OverlapParams overlap; -// MedianBinaryParams median_binary; -// BinaryParams binary; -// }; +enum class FilterType { // TODO: Add filters here + NoFilter, + Flip, + Unsharpening, + Erosion, + Dilation, + WhiteBalancing, + Ebus, + Otsu, + Overlap, + MedianBinary, + Binary, + Unknown +}; + +inline std::string to_lower(std::string s) { + for (auto& c : s) c = static_cast(std::tolower(c)); + return s; +} + +inline FilterType parse_filter_type(std::string s) { // TODO: Also add filter-type here + s = to_lower(std::move(s)); + if (s == "no_filter") return FilterType::NoFilter; + if (s == "flip") return FilterType::Flip; + if (s == "unsharpening") return FilterType::Unsharpening; + if (s == "erosion") return FilterType::Erosion; + if (s == "dilation") return FilterType::Dilation; + if (s == "white_balancing")return FilterType::WhiteBalancing; + if (s == "ebus") return FilterType::Ebus; + if (s == "otsu") return FilterType::Otsu; + if (s == "overlap") return FilterType::Overlap; + if (s == "median_binary") return FilterType::MedianBinary; + if (s == "binary") return FilterType::Binary; + return FilterType::Unknown; +} + + + struct FlipFilterParams { int flip_code; }; -/** - * Reads the filter_type from the FilterParams struct - * and calls the appropriate filter function from the filter_functions map. - */ -virtual class Filter{ - virtual void apply_filter(const cv::Mat& original, cv::Mat& filtered); + +class Filter{ + public: + virtual ~Filter() = default; + virtual void apply_filter(const cv::Mat& original, cv::Mat& filtered) const = 0; + + protected: + Filter() = default; }; -/** - * No filter, just copy the image - */ +///////////////////////////// + struct NoFilterParams{}; -class NoFilter(const NoFilterStruct& args): public Filter{ - void no_filter(const FilterParams& params, - const cv::Mat& original, - cv::Mat& modified); - private: - int flip_code; + // (const NoFilterStruct& args) +class NoFilter: public Filter{ + public: + explicit NoFilter() = default;// No parameters to set + void apply_filter(const cv::Mat& original, cv::Mat& filtered) const override{ original.copyTo(filtered); }; }; -virtual class FilterClass(FilterStuct args); -/** - * Flips the image - */ -class Flip (Filter) { -public: - int flip_code; +//////////////////////////// - +struct UnsharpeningParams{ + int blur_size; }; -/** - * Makes edges harder - */ -class Sharpening { - void sharpening_filter(const FilterParams& params, - const cv::Mat& original, - cv::Mat& modified); -}; -/** - * Makes edges harder in a smarter way - */ -class Unsharpening { -public: - int blur_size; - void unsharpening_filter(const FilterParams& params, - const cv::Mat& original, - cv::Mat& modified); +class Unsharpening: public Filter{ + public: + explicit Unsharpening(UnsharpeningParams params): filter_params(params) {} + void apply_filter(const cv::Mat& original, cv::Mat& filtered) const override; + private: + UnsharpeningParams filter_params; }; -/** - * Expands the dark areas of the image - */ -class Erosion { -public: - int size; - void erosion_filter(const FilterParams& params, - const cv::Mat& original, - cv::Mat& modified); -}; -/** - * Expands the bright areas of the image - */ -class Dilation { -public: - int size; - void dilation_filter(const FilterParams& params, - const cv::Mat& original, - cv::Mat& modified); -}; -/** - * White Balancing Filter - */ -class WhiteBalance { -public: - double contrast_percentage; - void white_balance_filter(const FilterParams& params, - const cv::Mat& original, - cv::Mat& filtered); -}; -/** - * A filter that worked well-ish in the mc-lab conditions easter 2023 - * Uses a combination of dilation and unsharpening - */ -class Ebus { -public: - int erosion_size; - int blur_size; - int mask_weight; - void ebus_filter(const FilterParams& params, - const cv::Mat& original, - cv::Mat& filtered); +//////////////////////////////////// + +// TODO: add this structure for your filter + +// Template: +struct ExampleParams{ // Add filter variables here + int example_variable; + std::string example_string; }; -/** - * A filter based on Otsu's method - */ -class OtsuSegmentation { -public: - bool gamma_auto_correction; - double gamma_auto_correction_weight; - bool otsu_segmentation; - double gsc_weight_r; - double gsc_weight_g; - double gsc_weight_b; - int erosion_size; - int dilation_size; - - void otsu_segmentation_filter(const FilterParams& params, - const cv::Mat& original, - cv::Mat& output); +class Example: public Filter{ + public: + explicit Example(ExampleParams params): filter_params(params) {} + void apply_filter(const cv::Mat& original, cv::Mat& filtered) const override; // This is the filter itself + private: + ExampleParams filter_params; }; -class Overlap { -public: - // Thomas' masterpiece - double percentage_threshold; - void overlap_filter(const FilterParams& filter_params, - const cv::Mat &original, - cv::Mat &filtered); -}; -class MedianBinary { -public: - int kernel_size; - int threshold; - bool invert; - void median_binary_filter(const FilterParams& filter_params, - const cv::Mat& original, - cv::Mat& filtered); -}; -class BinaryThreshold { -public: - double threshold; - double maxval; - bool invert; - void binary_threshold(const FilterParams& filter_params, - const cv::Mat& original, - cv::Mat& filtered); -}; -const static std::map filter_functions = { - {"no_filter", no_filter}, - {"flip", flip_filter}, - {"sharpening", sharpening_filter}, - {"unsharpening", unsharpening_filter}, - {"erosion", erosion_filter}, - {"dilation", dilation_filter}, - {"white_balancing", white_balance_filter}, - {"ebus", ebus_filter}, - {"otsu", otsu_segmentation_filter}, - {"overlap", Overlap::overlap_filter}, // This was done by the one and only Thomas - {"median_binary", median_binary_filter}, - {"binary", binary_threshold}, -}; + + + + #endif // IMAGE_PROCESSING_HPP diff --git a/image-filtering/src/image_filtering_ros.cpp b/image-filtering/src/image_filtering_ros.cpp index f327cfa..7866254 100644 --- a/image-filtering/src/image_filtering_ros.cpp +++ b/image-filtering/src/image_filtering_ros.cpp @@ -44,7 +44,7 @@ void ImageFilteringNode::declare_parameters() { this->declare_parameter("filter_params.otsu.erosion_size"); this->declare_parameter("filter_params.otsu.dilation_size"); - this->declare_parameter("filter_params.overlap.percentage_threshold"); //Thomas has left a mark here + this->declare_parameter("filter_params.overlap.percentage_threshold"); this->declare_parameter("filter_params.binary.threshold"); this->declare_parameter("filter_params.binary.maxval"); @@ -65,65 +65,94 @@ void ImageFilteringNode::declare_parameters() { } void ImageFilteringNode::set_filter_params() { - FilterParams params; - std::string filter = - this->get_parameter("filter_params.filter_type").as_string(); - if (!filter_functions.contains(filter)) { - spdlog::warn( - "Invalid filter type received: {}. Using default: no_filter.", - filter); - filter_ = "no_filter"; - } else { - filter_ = filter; + std::string filter_type_string = this->get_parameter("filter_params.filter_type").as_string(); + FilterType filter_type = parse_filter_type(filter_type_string); + + // if () { + // spdlog::warn("Invalid filter type received: {}. Using default: no_filter.", filter_type_string); + // filter_ = "no_filter"; + // } else { + // filter_ = filter; + // } + + switch (filter_type) + { + case FilterType::Unknown: + { + spdlog::warn("Invalid filter type received: {}. Using default: no_filter.", filter_type_string); + // filter_ = "no_filter"; + filter_type = FilterType::NoFilter; + } + + case FilterType::NoFilter: + { + NoFilter filter_object; + filter_ = std::make_unique(filter_object); + break; } - params.flip.flip_code = - this->get_parameter("filter_params.flip.flip_code").as_int(); - params.unsharpening.blur_size = - this->get_parameter("filter_params.unsharpening.blur_size").as_int(); - params.eroding.size = - this->get_parameter("filter_params.erosion.size").as_int(); - params.dilating.size = - this->get_parameter("filter_params.dilation.size").as_int(); - params.white_balancing.contrast_percentage = - this->get_parameter("filter_params.white_balancing.contrast_percentage").as_double(); - params.ebus.erosion_size = - this->get_parameter("filter_params.ebus.erosion_size").as_int(); - params.ebus.blur_size = - this->get_parameter("filter_params.ebus.blur_size").as_int(); - params.ebus.mask_weight = - this->get_parameter("filter_params.ebus.mask_weight").as_int(); - params.otsu.gamma_auto_correction = - this->get_parameter("filter_params.otsu.gamma_auto_correction").as_bool(); - params.otsu.gamma_auto_correction_weight = - this->get_parameter("filter_params.otsu.gamma_auto_correction_weight").as_double(); - params.otsu.otsu_segmentation = - this->get_parameter("filter_params.otsu.otsu_segmentation").as_bool(); - params.otsu.gsc_weight_r = - this->get_parameter("filter_params.otsu.gsc_weight_r").as_double(); - params.otsu.gsc_weight_g = - this->get_parameter("filter_params.otsu.gsc_weight_g").as_double(); - params.otsu.gsc_weight_b = - this->get_parameter("filter_params.otsu.gsc_weight_b").as_double(); - params.otsu.erosion_size = - this->get_parameter("filter_params.otsu.erosion_size").as_int(); - params.otsu.dilation_size = - this->get_parameter("filter_params.otsu.dilation_size").as_int(); - params.overlap.percentage_threshold = // Thomas is everyware - this->get_parameter("filter_params.overlap.percentage_threshold").as_double(); - params.median_binary.kernel_size = - this->get_parameter("filter_params.median_binary.kernel_size").as_int(); - params.median_binary.threshold = - this->get_parameter("filter_params.median_binary.threshold").as_int(); - params.median_binary.invert = - this->get_parameter("filter_params.median_binary.invert").as_bool(); - params.binary.threshold = - this->get_parameter("filter_params.binary.threshold").as_double(); - params.binary.maxval = - this->get_parameter("filter_params.binary.maxval").as_double(); - params.binary.invert = - this->get_parameter("filter_params.binary.invert").as_bool(); - filter_params_ = params; - spdlog::info("Filter parameters set: {}", filter); + case FilterType::Unsharpening: + { + UnsharpeningParams params; + params.blur_size = this->get_parameter("filter_params.unsharpening.blur_size").as_int(); + Unsharpening filter_object(params); + + filter_ = std::make_unique(filter_object); + break; + } + + default: + spdlog::warn("Filterparams has not been set for your chosen filter {}. To fix this add your filter to ImageFilteringNode::set_filter_params()", filter_type_string); + break; + }; + spdlog::info("Filter parameters set: {}", filter_type_string); + + +// params.flip.flip_code = +// this->get_parameter("filter_params.flip.flip_code").as_int(); +// params.eroding.size = +// this->get_parameter("filter_params.erosion.size").as_int(); +// params.dilating.size = +// this->get_parameter("filter_params.dilation.size").as_int(); +// params.white_balancing.contrast_percentage = +// this->get_parameter("filter_params.white_balancing.contrast_percentage").as_double(); +// params.ebus.erosion_size = +// this->get_parameter("filter_params.ebus.erosion_size").as_int(); +// params.ebus.blur_size = +// this->get_parameter("filter_params.ebus.blur_size").as_int(); +// params.ebus.mask_weight = +// this->get_parameter("filter_params.ebus.mask_weight").as_int(); +// params.otsu.gamma_auto_correction = +// this->get_parameter("filter_params.otsu.gamma_auto_correction").as_bool(); +// params.otsu.gamma_auto_correction_weight = +// this->get_parameter("filter_params.otsu.gamma_auto_correction_weight").as_double(); +// params.otsu.otsu_segmentation = +// this->get_parameter("filter_params.otsu.otsu_segmentation").as_bool(); +// params.otsu.gsc_weight_r = +// this->get_parameter("filter_params.otsu.gsc_weight_r").as_double(); +// params.otsu.gsc_weight_g = +// this->get_parameter("filter_params.otsu.gsc_weight_g").as_double(); +// params.otsu.gsc_weight_b = +// this->get_parameter("filter_params.otsu.gsc_weight_b").as_double(); +// params.otsu.erosion_size = +// this->get_parameter("filter_params.otsu.erosion_size").as_int(); +// params.otsu.dilation_size = +// this->get_parameter("filter_params.otsu.dilation_size").as_int(); +// params.overlap.percentage_threshold = // Thomas is everyware +// this->get_parameter("filter_params.overlap.percentage_threshold").as_double(); +// params.median_binary.kernel_size = +// this->get_parameter("filter_params.median_binary.kernel_size").as_int(); +// params.median_binary.threshold = +// this->get_parameter("filter_params.median_binary.threshold").as_int(); +// params.median_binary.invert = +// this->get_parameter("filter_params.median_binary.invert").as_bool(); +// params.binary.threshold = +// this->get_parameter("filter_params.binary.threshold").as_double(); +// params.binary.maxval = +// this->get_parameter("filter_params.binary.maxval").as_double(); +// params.binary.invert = +// this->get_parameter("filter_params.binary.invert").as_bool(); +// filter_params_ = params; +// spdlog::info("Filter parameters set: {}", filter); } void ImageFilteringNode::check_and_subscribe_to_image_topic() { diff --git a/image-filtering/src/image_processing.cpp b/image-filtering/src/image_processing.cpp index da8a2cf..62751d3 100644 --- a/image-filtering/src/image_processing.cpp +++ b/image-filtering/src/image_processing.cpp @@ -3,31 +3,9 @@ #include -void NoFilter::no_filter([[maybe_unused]] const FilterParams& params, - const cv::Mat& original, - cv::Mat& filtered) { - original.copyTo(filtered); -} - -void Flip::flip_filter([[maybe_unused]] const FilterParams& params, - const cv::Mat& original, - cv::Mat& filtered) { - int flip_code = params.flip.flip_code; // 0: x-axis, 1: y-axis, -1: both - cv::flip(original, filtered, flip_code); -} - -void Sharpening::sharpening_filter([[maybe_unused]] const FilterParams& params, - const cv::Mat& original, - cv::Mat& filtered) { - // Sharpen image - cv::Mat kernel = (cv::Mat_(3, 3) << 0, -1, 0, -1, 5, -1, 0, -1, 0); - cv::filter2D(original, filtered, -1, kernel); -} -void Unsharpening::unsharpening_filter(const FilterParams& params, - const cv::Mat& original, - cv::Mat& filtered) { - int blur_size = params.unsharpening.blur_size; +void Unsharpening::apply_filter(const cv::Mat& original, cv::Mat& filtered) const{ + int blur_size = this->filter_params.blur_size; // Create a blurred version of the image cv::Mat blurred; GaussianBlur(original, blurred, @@ -40,182 +18,15 @@ void Unsharpening::unsharpening_filter(const FilterParams& params, addWeighted(original, 1, mask, 3, 0, filtered); } -void Erosion::erosion_filter(const FilterParams& params, - const cv::Mat& original, - cv::Mat& filtered) { - apply_erosion(original, filtered, params.eroding.size, cv::MORPH_RECT); -} - -void Dilation::dilation_filter(const FilterParams& params, - const cv::Mat& original, - cv::Mat& filtered) { - apply_dilation(original, filtered, params.dilating.size, cv::MORPH_RECT); -} - -void WhiteBalance::white_balance_filter(const FilterParams& params, - const cv::Mat& original, - cv::Mat& filtered) { - double contrast_percentage = params.white_balancing.contrast_percentage; - cv::Ptr balance = cv::xphoto::createSimpleWB(); - balance->setP(contrast_percentage); - balance->balanceWhite(original, filtered); -} - -void Ebus::ebus_filter(const FilterParams& params, - const cv::Mat& original, - cv::Mat& filtered) { - int blur_size = params.ebus.blur_size; - int mask_weight = params.ebus.mask_weight; - // Erode image to make blacks more black - cv::Mat eroded; - - int erosion_size = params.eroding.size; - apply_erosion(original, eroded, erosion_size); - - // Make an unsharp mask from original image - cv::Mat blurred; - GaussianBlur(original, blurred, - cv::Size(2 * blur_size + 1, 2 * blur_size + 1), 0); - - // Compute the unsharp mask - cv::Mat mask = original - blurred; - cv::Mat unsharp; - - // Add mask to the eroded image instead of the original - // Higher weight of mask will create a sharper but more noisy image - addWeighted(eroded, 1, mask, mask_weight, 0, filtered); -} - - - -void Otsu::otsu_segmentation_filter(const FilterParams& params, - const cv::Mat& original, - cv::Mat& filtered) { - bool gamma_auto_correction = params.otsu.gamma_auto_correction; - double gamma_auto_correction_weight = - params.otsu.gamma_auto_correction_weight; - - bool otsu_segmentation = params.otsu.otsu_segmentation; - - - to_weighted_gray(original, filtered, params.otsu.gsc_weight_b, - params.otsu.gsc_weight_g, - params.otsu.gsc_weight_r); - - - if (gamma_auto_correction) { - apply_auto_gamma(filtered, gamma_auto_correction_weight); - } - - if (otsu_segmentation) { - apply_otsu(filtered, filtered, false, 255); - - // Apply erosion followed by dilation (opening) - - apply_erosion(filtered, filtered, params.otsu.erosion_size, cv::MORPH_CROSS); - apply_dilation(filtered, filtered, params.otsu.dilation_size, cv::MORPH_CROSS); - } -} - -// Thomas was here -void Overlap::overlap_filter(const FilterParams& filter_params, - const cv::Mat& original, - cv::Mat& filtered) -{ - static cv::Mat prev; // previous mono frame - static bool has_prev = false; - - // Basic checks (keep it simple; require mono8) - if (original.empty()) - throw std::invalid_argument("overlap_filter_mono: input is empty"); - if (original.type() != CV_8UC1) - throw std::invalid_argument("overlap_filter_mono: input must be CV_8UC1 (mono8)"); - - if (!has_prev || prev.empty() - || prev.size() != original.size() - || prev.type() != original.type()) - { - original.copyTo(filtered); // pass-through on first frame / size change - prev = original.clone(); - has_prev = true; - return; - } - - // |cur - prev| - cv::Mat diff8u; - cv::absdiff(original, prev, diff8u); - - // Convert percentage threshold to an 8-bit delta threshold - double pct = std::clamp(filter_params.overlap.percentage_threshold, 0.0, 100.0); - int delta_thr = static_cast(std::round(pct * 255.0 / 100.0)); - - // Mask: changed pixels - cv::Mat mask; - cv::threshold(diff8u, mask, delta_thr, 255, cv::THRESH_BINARY); - - // Zero out changed pixels - filtered = original.clone(); - filtered.setTo(0, mask); - - // Update history - prev = original.clone(); - -} - -void MedianBinary::median_binary_filter(const FilterParams& filter_params, - const cv::Mat& original, - cv::Mat& filtered){ - apply_median(original, filtered, filter_params.median_binary.kernel_size); - apply_fixed_threshold(filtered, filtered, filter_params.median_binary.threshold, filter_params.median_binary.invert); - distance_field(filtered, filtered); -} - - - -void BinaryThreshold::binary_threshold(const FilterParams& filter_params, - const cv::Mat& original, - cv::Mat& filtered) -{ - - CV_Assert(!original.empty()); - - const double thresh = filter_params.binary.threshold; - const double maxval = filter_params.binary.maxval; - const bool invert = filter_params.binary.invert; - - // 1) Ensure single-channel - cv::Mat gray; - if (original.channels() == 1) { - gray = original; - } else { - cv::cvtColor(original, gray, cv::COLOR_BGR2GRAY); - } - - // Standardize to 8-bit (safe for thresholding) - cv::Mat src8; - if (gray.depth() != CV_8U) { - // Adjust scaling here if grayscale is not already 0–255 - gray.convertTo(src8, CV_8U); - } else { - src8 = gray; - } - - // Apply fixed threshold - const int type = invert ? cv::THRESH_BINARY_INV : cv::THRESH_BINARY; - cv::threshold(src8, filtered, thresh, maxval, type); -} - - - -void Filter::apply_filter(const std::string& filter, - const cv::Mat& original, - cv::Mat& filtered) { - if (filter_functions.contains(filter)) { - ((filter_functions.at(filter)))(params, original, filtered); - } else { - original.copyTo(filtered); // Default to no filter - } -} +// void Filter::apply_filter(const std::string& filter, +// const cv::Mat& original, +// cv::Mat& filtered) { +// if (filter_functions.contains(filter)) { +// ((filter_functions.at(filter)))(params, original, filtered); +// } else { +// original.copyTo(filtered); // Default to no filter +// } +// } From c978a3084e7e72261b987de922b776fed08fd7b9 Mon Sep 17 00:00:00 2001 From: Thomas Date: Wed, 12 Nov 2025 17:28:00 +0100 Subject: [PATCH 34/53] It is done --- .../config/image_filtering_params.yaml | 64 +++++++++---------- .../image_filters/image_filtering_ros.hpp | 2 +- image-filtering/src/image_filtering_ros.cpp | 32 +++------- image-filtering/src/image_processing.cpp | 1 + 4 files changed, 43 insertions(+), 56 deletions(-) diff --git a/image-filtering/config/image_filtering_params.yaml b/image-filtering/config/image_filtering_params.yaml index 64f1426..346c2d6 100644 --- a/image-filtering/config/image_filtering_params.yaml +++ b/image-filtering/config/image_filtering_params.yaml @@ -6,40 +6,40 @@ input_encoding: "mono8" filter_params: - filter_type: "ebus" - # flip: - # flip_code: 1 + filter_type: "unsharpening" + flip: + flip_code: 1 unsharpening: blur_size: 8 - # erosion: - # size: 1 - # dilation: - # size: 3 - # white_balancing: - # contrast_percentage: 0.1 - # ebus: - # erosion_size: 2 - # blur_size: 30 - # mask_weight: 5 - # otsu: - # gsc_weight_r: 1.0 # Grayscale red weight - # gsc_weight_g: 0.0 # Grayscale green weight - # gsc_weight_b: 0.0 # Grayscale blue weight - # gamma_auto_correction: true - # gamma_auto_correction_weight: 4.0 - # otsu_segmentation: true - # erosion_size: 2 - # dilation_size: 2 - # overlap: - # percentage_threshold: 100.0 # Percentage to cap the picel intensity differance - # median_binary: # finds the median of each n x n square around each pixel - # kernel_size: 3 # must be odd >= 1 - # threshold: 100 # [0, 255] - # invert: false - # binary: - # threshold: 0.1 # in percent - # maxval: 255. - # invert: true + erosion: + size: 1 + dilation: + size: 3 + white_balancing: + contrast_percentage: 0.1 + ebus: + erosion_size: 2 + blur_size: 30 + mask_weight: 5 + otsu: + gsc_weight_r: 1.0 # Grayscale red weight + gsc_weight_g: 0.0 # Grayscale green weight + gsc_weight_b: 0.0 # Grayscale blue weight + gamma_auto_correction: true + gamma_auto_correction_weight: 4.0 + otsu_segmentation: true + erosion_size: 2 + dilation_size: 2 + overlap: + percentage_threshold: 100.0 # Percentage to cap the picel intensity differance + median_binary: # finds the median of each n x n square around each pixel + kernel_size: 3 # must be odd >= 1 + threshold: 100 # [0, 255] + invert: false + binary: + threshold: 0.1 # in percent + maxval: 255. + invert: true # Filter params should reflect the FilterParams struct diff --git a/image-filtering/include/image_filters/image_filtering_ros.hpp b/image-filtering/include/image_filters/image_filtering_ros.hpp index 697153d..064a9d9 100644 --- a/image-filtering/include/image_filters/image_filtering_ros.hpp +++ b/image-filtering/include/image_filters/image_filtering_ros.hpp @@ -100,7 +100,7 @@ class ImageFilteringNode : public rclcpp::Node { * @brief Pointer to the filter object * */ - std::unique_ptr filter_; + std::unique_ptr filter_ptr; }; diff --git a/image-filtering/src/image_filtering_ros.cpp b/image-filtering/src/image_filtering_ros.cpp index 7866254..71e1fe1 100644 --- a/image-filtering/src/image_filtering_ros.cpp +++ b/image-filtering/src/image_filtering_ros.cpp @@ -49,18 +49,12 @@ void ImageFilteringNode::declare_parameters() { this->declare_parameter("filter_params.binary.threshold"); this->declare_parameter("filter_params.binary.maxval"); this->declare_parameter("filter_params.binary.invert"); - GenericStruct args; - if(filter_type == OVERLAP){ - Filterstruct args; - this->declare_parameter("filter_params.median_binary.kernel_size"); - this->declare_parameter("filter_params.median_binary.threshold"); - this->declare_parameter("filter_params.median_binary.invert"); - args = overlapparms(args); - } - switch - this->filter_class_ = constructor(args) - + this->declare_parameter("filter_params.median_binary.kernel_size"); + this->declare_parameter("filter_params.median_binary.threshold"); + this->declare_parameter("filter_params.median_binary.invert"); + + // TODO: Declare the parameters set for your filter here } @@ -68,12 +62,6 @@ void ImageFilteringNode::set_filter_params() { std::string filter_type_string = this->get_parameter("filter_params.filter_type").as_string(); FilterType filter_type = parse_filter_type(filter_type_string); - // if () { - // spdlog::warn("Invalid filter type received: {}. Using default: no_filter.", filter_type_string); - // filter_ = "no_filter"; - // } else { - // filter_ = filter; - // } switch (filter_type) { @@ -86,17 +74,15 @@ void ImageFilteringNode::set_filter_params() { case FilterType::NoFilter: { - NoFilter filter_object; - filter_ = std::make_unique(filter_object); + filter_ptr = std::make_unique(); break; } case FilterType::Unsharpening: { UnsharpeningParams params; params.blur_size = this->get_parameter("filter_params.unsharpening.blur_size").as_int(); - Unsharpening filter_object(params); - filter_ = std::make_unique(filter_object); + filter_ptr = std::make_unique(params); break; } @@ -225,11 +211,11 @@ void ImageFilteringNode::image_callback( cv::Mat input_image = cv_ptr->image; cv::Mat filtered_image; - apply_filter(filter_, filter_params_, input_image, filtered_image); + filter_ptr->apply_filter(input_image, filtered_image); std::string output_encoding = this->get_parameter("output_encoding").as_string(); - auto message = std::make_unique(); + auto message = std::make_unique(); cv_bridge::CvImage(msg->header, output_encoding, filtered_image) .toImageMsg(*message); diff --git a/image-filtering/src/image_processing.cpp b/image-filtering/src/image_processing.cpp index 62751d3..f645294 100644 --- a/image-filtering/src/image_processing.cpp +++ b/image-filtering/src/image_processing.cpp @@ -19,6 +19,7 @@ void Unsharpening::apply_filter(const cv::Mat& original, cv::Mat& filtered) cons } + // void Filter::apply_filter(const std::string& filter, // const cv::Mat& original, // cv::Mat& filtered) { From 4ec71f2a2e389f9a984831e7be141505c08c7343 Mon Sep 17 00:00:00 2001 From: Thomas Date: Thu, 13 Nov 2025 09:47:07 +0100 Subject: [PATCH 35/53] Adding the rest of the filters --- .../image_filters/image_processing.hpp | 197 +++++++++++++++++- image-filtering/src/image_filtering_ros.cpp | 167 ++++++++++----- image-filtering/src/image_processing.cpp | 177 +++++++++++++++- 3 files changed, 474 insertions(+), 67 deletions(-) diff --git a/image-filtering/include/image_filters/image_processing.hpp b/image-filtering/include/image_filters/image_processing.hpp index a527f0f..62a809f 100644 --- a/image-filtering/include/image_filters/image_processing.hpp +++ b/image-filtering/include/image_filters/image_processing.hpp @@ -50,13 +50,6 @@ inline FilterType parse_filter_type(std::string s) { // TODO: Also add filter-ty - -struct FlipFilterParams { - int flip_code; -}; - - - class Filter{ public: virtual ~Filter() = default; @@ -66,18 +59,23 @@ class Filter{ Filter() = default; }; + +///////////////////////////// +// No filter ///////////////////////////// struct NoFilterParams{}; - // (const NoFilterStruct& args) class NoFilter: public Filter{ public: explicit NoFilter() = default;// No parameters to set void apply_filter(const cv::Mat& original, cv::Mat& filtered) const override{ original.copyTo(filtered); }; }; + //////////////////////////// +// Unsharpening +///////////////////////////// struct UnsharpeningParams{ int blur_size; @@ -94,8 +92,189 @@ class Unsharpening: public Filter{ +///////////////////////////// +// Sharpening +///////////////////////////// + +struct FlipParams{ + int flip_code; +}; + +class Flip: public Filter{ + public: + explicit Flip(FlipParams params): filter_params(params) {} + void apply_filter(const cv::Mat& original, cv::Mat& filtered) const override; + private: + FlipParams filter_params; +}; + + + + + + +///////////////////////////// +// Sharpening +///////////////////////////// + +struct SharpeningParams{}; + +class Sharpening: public Filter{ +public: + explicit Sharpening(SharpeningParams params): filter_params(params) {} + void apply_filter(const cv::Mat& original, cv::Mat& filtered) const override; +private: + SharpeningParams filter_params; +}; +///////////////////////////// +// Erosion +///////////////////////////// + +struct ErosionParams{ + int kernel_size; // odd > 1 +}; + +class Erosion: public Filter{ +public: + explicit Erosion(ErosionParams params): filter_params(params) {} + void apply_filter(const cv::Mat& original, cv::Mat& filtered) const override; +private: + ErosionParams filter_params; +}; + +///////////////////////////// +// Dilation +///////////////////////////// + +struct DilationParams{ + int kernel_size = 3; +}; + +class Dilation: public Filter{ +public: + explicit Dilation(DilationParams params): filter_params(params) {} + void apply_filter(const cv::Mat& original, cv::Mat& filtered) const override; +private: + DilationParams filter_params; +}; + +///////////////////////////// +// White Balance +///////////////////////////// + +struct WhiteBalanceParams{ + double contrast_percentage; +}; + +class WhiteBalance: public Filter{ +public: + explicit WhiteBalance(WhiteBalanceParams params): filter_params(params) {} + void apply_filter(const cv::Mat& original, cv::Mat& filtered) const override; +private: + WhiteBalanceParams filter_params; +}; + +///////////////////////////// +// Ebus (dilation + unsharpening combo) +///////////////////////////// + +struct EbusParams{ + int erosion_size; + int blur_size; + int mask_weight; +}; + +class Ebus: public Filter{ +public: + explicit Ebus(EbusParams params): filter_params(params) {} + void apply_filter(const cv::Mat& original, cv::Mat& filtered) const override; +private: + EbusParams filter_params; +}; + +///////////////////////////// +// Otsu Segmentation +///////////////////////////// + +struct OtsuSegmentationParams{ + bool gamma_auto_correction; + double gamma_auto_correction_weight; + bool otsu_segmentation; + double gsc_weight_r; + double gsc_weight_g; + double gsc_weight_b; + int erosion_size; + int dilation_size; +}; + +class OtsuSegmentation: public Filter{ +public: + explicit OtsuSegmentation(OtsuSegmentationParams params): filter_params(params) {} + void apply_filter(const cv::Mat& original, cv::Mat& output) const override; +private: + OtsuSegmentationParams filter_params; +}; + +///////////////////////////// +// Overlap (blend/composite) +///////////////////////////// + +struct OverlapParams{ + double percentage_threshold; +}; -//////////////////////////////////// +class Overlap: public Filter{ +public: + explicit Overlap(OverlapParams params): filter_params(params) {} + void apply_filter(const cv::Mat& original, cv::Mat& filtered) const override; +private: + OverlapParams filter_params; +}; + +///////////////////////////// +// Median + Binary +///////////////////////////// + +struct MedianBinaryParams{ + int kernel_size; + int threshold; + bool invert; +}; + +class MedianBinary: public Filter{ +public: + explicit MedianBinary(MedianBinaryParams params): filter_params(params) {} + void apply_filter(const cv::Mat& original, cv::Mat& filtered) const override; +private: + MedianBinaryParams filter_params; +}; + +///////////////////////////// +// Binary Threshold +///////////////////////////// + +struct BinaryThresholdParams{ + double threshold; + double maxval; + bool invert; +}; + +class BinaryThreshold: public Filter{ +public: + explicit BinaryThreshold(BinaryThresholdParams params): filter_params(params) {} + void apply_filter(const cv::Mat& original, cv::Mat& filtered) const override; +private: + BinaryThresholdParams filter_params; +}; + + + + + + +///////////////////////////// +// Template +///////////////////////////// // TODO: add this structure for your filter diff --git a/image-filtering/src/image_filtering_ros.cpp b/image-filtering/src/image_filtering_ros.cpp index 71e1fe1..6bebed5 100644 --- a/image-filtering/src/image_filtering_ros.cpp +++ b/image-filtering/src/image_filtering_ros.cpp @@ -86,59 +86,128 @@ void ImageFilteringNode::set_filter_params() { break; } + case FilterType::Flip: + { + FlipParams params; + params.flip_code = + this->get_parameter("filter_params.flip.flip_code").as_int(); + + filter_ptr = std::make_unique(params); + break; + } + + case FilterType::Erosion: + { + ErosionParams params; + params.kernel_size = + this->get_parameter("filter_params.erosion.size").as_int(); + + filter_ptr = std::make_unique(params); + break; + } + + case FilterType::Dilation: + { + DilationParams params; + params.kernel_size = + this->get_parameter("filter_params.dilation.size").as_int(); + + filter_ptr = std::make_unique(params); + break; + } + + case FilterType::WhiteBalancing: + { + WhiteBalanceParams params; + params.contrast_percentage = + this->get_parameter("filter_params.white_balancing.contrast_percentage").as_double(); + + filter_ptr = std::make_unique(params); + break; + } + + case FilterType::Ebus: + { + EbusParams params; + params.erosion_size = + this->get_parameter("filter_params.ebus.erosion_size").as_int(); + params.blur_size = + this->get_parameter("filter_params.ebus.blur_size").as_int(); + params.mask_weight = + this->get_parameter("filter_params.ebus.mask_weight").as_int(); + + filter_ptr = std::make_unique(params); + break; + } + + case FilterType::Otsu: + { + OtsuSegmentationParams params; + params.gamma_auto_correction = + this->get_parameter("filter_params.otsu.gamma_auto_correction").as_bool(); + params.gamma_auto_correction_weight = + this->get_parameter("filter_params.otsu.gamma_auto_correction_weight").as_double(); + params.otsu_segmentation = + this->get_parameter("filter_params.otsu.otsu_segmentation").as_bool(); + params.gsc_weight_r = + this->get_parameter("filter_params.otsu.gsc_weight_r").as_double(); + params.gsc_weight_g = + this->get_parameter("filter_params.otsu.gsc_weight_g").as_double(); + params.gsc_weight_b = + this->get_parameter("filter_params.otsu.gsc_weight_b").as_double(); + params.erosion_size = + this->get_parameter("filter_params.otsu.erosion_size").as_int(); + params.dilation_size = + this->get_parameter("filter_params.otsu.dilation_size").as_int(); + + filter_ptr = std::make_unique(params); + break; + } + + case FilterType::Overlap: + { + OverlapParams params; + params.percentage_threshold = // Thomas is everyware + this->get_parameter("filter_params.overlap.percentage_threshold").as_double(); + + filter_ptr = std::make_unique(params); + break; + } + + case FilterType::MedianBinary: + { + MedianBinaryParams params; + params.kernel_size = + this->get_parameter("filter_params.median_binary.kernel_size").as_int(); + params.threshold = + this->get_parameter("filter_params.median_binary.threshold").as_int(); + params.invert = + this->get_parameter("filter_params.median_binary.invert").as_bool(); + + filter_ptr = std::make_unique(params); + break; + } + + case FilterType::Binary: + { + BinaryThresholdParams params; + params.threshold = + this->get_parameter("filter_params.binary.threshold").as_double(); + params.maxval = + this->get_parameter("filter_params.binary.maxval").as_double(); + params.invert = + this->get_parameter("filter_params.binary.invert").as_bool(); + + filter_ptr = std::make_unique(params); + break; + } + + default: spdlog::warn("Filterparams has not been set for your chosen filter {}. To fix this add your filter to ImageFilteringNode::set_filter_params()", filter_type_string); break; }; - spdlog::info("Filter parameters set: {}", filter_type_string); - - -// params.flip.flip_code = -// this->get_parameter("filter_params.flip.flip_code").as_int(); -// params.eroding.size = -// this->get_parameter("filter_params.erosion.size").as_int(); -// params.dilating.size = -// this->get_parameter("filter_params.dilation.size").as_int(); -// params.white_balancing.contrast_percentage = -// this->get_parameter("filter_params.white_balancing.contrast_percentage").as_double(); -// params.ebus.erosion_size = -// this->get_parameter("filter_params.ebus.erosion_size").as_int(); -// params.ebus.blur_size = -// this->get_parameter("filter_params.ebus.blur_size").as_int(); -// params.ebus.mask_weight = -// this->get_parameter("filter_params.ebus.mask_weight").as_int(); -// params.otsu.gamma_auto_correction = -// this->get_parameter("filter_params.otsu.gamma_auto_correction").as_bool(); -// params.otsu.gamma_auto_correction_weight = -// this->get_parameter("filter_params.otsu.gamma_auto_correction_weight").as_double(); -// params.otsu.otsu_segmentation = -// this->get_parameter("filter_params.otsu.otsu_segmentation").as_bool(); -// params.otsu.gsc_weight_r = -// this->get_parameter("filter_params.otsu.gsc_weight_r").as_double(); -// params.otsu.gsc_weight_g = -// this->get_parameter("filter_params.otsu.gsc_weight_g").as_double(); -// params.otsu.gsc_weight_b = -// this->get_parameter("filter_params.otsu.gsc_weight_b").as_double(); -// params.otsu.erosion_size = -// this->get_parameter("filter_params.otsu.erosion_size").as_int(); -// params.otsu.dilation_size = -// this->get_parameter("filter_params.otsu.dilation_size").as_int(); -// params.overlap.percentage_threshold = // Thomas is everyware -// this->get_parameter("filter_params.overlap.percentage_threshold").as_double(); -// params.median_binary.kernel_size = -// this->get_parameter("filter_params.median_binary.kernel_size").as_int(); -// params.median_binary.threshold = -// this->get_parameter("filter_params.median_binary.threshold").as_int(); -// params.median_binary.invert = -// this->get_parameter("filter_params.median_binary.invert").as_bool(); -// params.binary.threshold = -// this->get_parameter("filter_params.binary.threshold").as_double(); -// params.binary.maxval = -// this->get_parameter("filter_params.binary.maxval").as_double(); -// params.binary.invert = -// this->get_parameter("filter_params.binary.invert").as_bool(); -// filter_params_ = params; -// spdlog::info("Filter parameters set: {}", filter); + } void ImageFilteringNode::check_and_subscribe_to_image_topic() { diff --git a/image-filtering/src/image_processing.cpp b/image-filtering/src/image_processing.cpp index f645294..62065ae 100644 --- a/image-filtering/src/image_processing.cpp +++ b/image-filtering/src/image_processing.cpp @@ -20,14 +20,173 @@ void Unsharpening::apply_filter(const cv::Mat& original, cv::Mat& filtered) cons -// void Filter::apply_filter(const std::string& filter, -// const cv::Mat& original, -// cv::Mat& filtered) { -// if (filter_functions.contains(filter)) { -// ((filter_functions.at(filter)))(params, original, filtered); -// } else { -// original.copyTo(filtered); // Default to no filter -// } -// } +void Flip::apply_filter(const cv::Mat& original, cv::Mat& filtered) const { + int flip_code = this->filter_params.flip_code; // 0: x-axis, 1: y-axis, -1: both + cv::flip(original, filtered, flip_code); +} + + +void Sharpening::apply_filter(const cv::Mat& original, cv::Mat& filtered) const { + // Sharpen image + cv::Mat kernel = (cv::Mat_(3, 3) << 0, -1, 0, -1, 5, -1, 0, -1, 0); + cv::filter2D(original, filtered, -1, kernel); +} + +void Unsharpening::apply_filter(const cv::Mat& original, cv::Mat& filtered) const { + int blur_size = this->filter_params.blur_size; + // Create a blurred version of the image + cv::Mat blurred; + GaussianBlur(original, blurred, + cv::Size(2 * blur_size + 1, 2 * blur_size + 1), 0); + + // Compute the unsharp mask + cv::Mat mask = original - blurred; + cv::Mat unsharp; + + addWeighted(original, 1, mask, 3, 0, filtered); +} + +void Erosion::apply_filter(const cv::Mat& original, cv::Mat& filtered) const { + apply_erosion(original, filtered, this->filter_params.kernel_size, cv::MORPH_RECT); +} + +void Dilation::apply_filter(const cv::Mat& original, cv::Mat& filtered) const{ + apply_dilation(original, filtered, this->filter_params.kernel_size, cv::MORPH_RECT); +} + +void WhiteBalance::apply_filter(const cv::Mat& original, cv::Mat& filtered) const{ + double contrast_percentage = this->filter_params.contrast_percentage; + cv::Ptr balance = cv::xphoto::createSimpleWB(); + balance->setP(contrast_percentage); + balance->balanceWhite(original, filtered); +} + +void Ebus::apply_filter(const cv::Mat& original, cv::Mat& filtered) const{ + int blur_size = this->filter_params.blur_size; + int mask_weight = this->filter_params.mask_weight; + int erosion_size = this->filter_params.erosion_size; + // Erode image to make blacks more black + cv::Mat eroded; + + apply_erosion(original, eroded, erosion_size); + + // Make an unsharp mask from original image + cv::Mat blurred; + GaussianBlur(original, blurred, + cv::Size(2 * blur_size + 1, 2 * blur_size + 1), 0); + + // Compute the unsharp mask + cv::Mat mask = original - blurred; + cv::Mat unsharp; + + // Add mask to the eroded image instead of the original + // Higher weight of mask will create a sharper but more noisy image + addWeighted(eroded, 1, mask, mask_weight, 0, filtered); +} + + + +void OtsuSegmentation::apply_filter(const cv::Mat& original, cv::Mat& filtered) const{ + bool gamma_auto_correction = this->filter_params.gamma_auto_correction; + double gamma_auto_correction_weight = this->filter_params.gamma_auto_correction_weight; + + bool otsu_segmentation = this->filter_params.otsu_segmentation; + + + to_weighted_gray(original, filtered, this->filter_params.gsc_weight_b, + this->filter_params.gsc_weight_g, + this->filter_params.gsc_weight_r); + + + if (gamma_auto_correction) { + apply_auto_gamma(filtered, gamma_auto_correction_weight); + } + + if (otsu_segmentation) { + apply_otsu(filtered, filtered, false, 255); + + // Apply erosion followed by dilation (opening) + + apply_erosion(filtered, filtered, this->filter_params.erosion_size, cv::MORPH_CROSS); + apply_dilation(filtered, filtered, this->filter_params.dilation_size, cv::MORPH_CROSS); + } +} + +// Thomas was here +void Overlap::apply_filter(const cv::Mat& original, cv::Mat& filtered) const{ + static cv::Mat prevR; // store previous R channel only + static bool has_prev = false; + + // Extract current R channel + cv::Mat curR; + cv::extractChannel(original, curR, 2); // 0=B,1=G,2=R + + if (!has_prev || prevR.empty() || prevR.size()!=curR.size() || prevR.type()!=curR.type()) { + original.copyTo(filtered); // first call (or size/type change): pass through + prevR = curR.clone(); // cache R channel + has_prev = true; + return; + } + + // |cur - prev| on the R channel + cv::Mat diff8u; + cv::absdiff(curR, prevR, diff8u); + + // % of full 8-bit range + cv::Mat percent32f; + diff8u.convertTo(percent32f, CV_32F, 100.0 / 255.0); + + // Mask: pixels whose % change > threshold + cv::Mat mask; + cv::threshold(percent32f, mask, this->filter_params.percentage_threshold, 255.0, cv::THRESH_BINARY); + mask.convertTo(mask, CV_8U); + + // Zero out those pixels in the R channel + filtered = original.clone(); + std::vector ch; + cv::split(filtered, ch); // ch[2] is R + ch[2].setTo(0, mask); + cv::merge(ch, filtered); + + // Update history (R channel only) + prevR = curR.clone(); +} + +void MedianBinary::apply_filter(const cv::Mat& original, cv::Mat& filtered) const{ + + apply_median(original, filtered, this->filter_params.kernel_size); + apply_fixed_threshold(filtered, filtered, this->filter_params.threshold, this->filter_params.invert); +} + + + +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 maxval = this->filter_params.maxval; + const bool invert = this->filter_params.invert; + + // 1) Ensure single-channel + cv::Mat gray; + if (original.channels() == 1) { + gray = original; + } else { + cv::cvtColor(original, gray, cv::COLOR_BGR2GRAY); + } + // Standardize to 8-bit (safe for thresholding) + cv::Mat src8; + if (gray.depth() != CV_8U) { + // Adjust scaling here if grayscale is not already 0–255 + gray.convertTo(src8, CV_8U); + } else { + src8 = gray; + } + // Apply fixed threshold + const int type = invert ? cv::THRESH_BINARY_INV : cv::THRESH_BINARY; + cv::threshold(src8, filtered, thresh, maxval, type); +} \ No newline at end of file From 4596c5622635d617ad33c2254ee1fee61ab8b6fe Mon Sep 17 00:00:00 2001 From: Thomas Paulen Date: Tue, 18 Nov 2025 10:16:52 +0100 Subject: [PATCH 36/53] changed(not realy fixed) some bugs --- .../config/image_filtering_params.yaml | 2 +- .../include/image_filters/utilities.hpp | 2 -- image-filtering/src/image_filtering_ros.cpp | 21 +++++++++++++++---- image-filtering/src/image_processing.cpp | 15 ------------- 4 files changed, 18 insertions(+), 22 deletions(-) diff --git a/image-filtering/config/image_filtering_params.yaml b/image-filtering/config/image_filtering_params.yaml index 346c2d6..a75dbd8 100644 --- a/image-filtering/config/image_filtering_params.yaml +++ b/image-filtering/config/image_filtering_params.yaml @@ -6,7 +6,7 @@ input_encoding: "mono8" filter_params: - filter_type: "unsharpening" + filter_type: "overlap" flip: flip_code: 1 unsharpening: diff --git a/image-filtering/include/image_filters/utilities.hpp b/image-filtering/include/image_filters/utilities.hpp index e6abf31..f652f91 100644 --- a/image-filtering/include/image_filters/utilities.hpp +++ b/image-filtering/include/image_filters/utilities.hpp @@ -14,8 +14,6 @@ void apply_auto_gamma(cv::Mat& image, double correction_weight); void to_weighted_gray(const cv::Mat& bgr, cv::Mat& gray, double wB, double wG, double wR); -// int computeOtsuThreshold(const cv::Mat& hist_prob); - int apply_otsu(const cv::Mat& gray8u, cv::Mat& out, bool invert = false, double maxval = 255.0); void apply_erosion(const cv::Mat& src, cv::Mat& dst, int size, int shape = cv::MORPH_RECT); diff --git a/image-filtering/src/image_filtering_ros.cpp b/image-filtering/src/image_filtering_ros.cpp index 6bebed5..8e585ae 100644 --- a/image-filtering/src/image_filtering_ros.cpp +++ b/image-filtering/src/image_filtering_ros.cpp @@ -54,7 +54,7 @@ void ImageFilteringNode::declare_parameters() { this->declare_parameter("filter_params.median_binary.threshold"); this->declare_parameter("filter_params.median_binary.invert"); - // TODO: Declare the parameters set for your filter here + // TODO: Declare parameters set for your filter here } @@ -167,7 +167,7 @@ void ImageFilteringNode::set_filter_params() { case FilterType::Overlap: { OverlapParams params; - params.percentage_threshold = // Thomas is everyware + params.percentage_threshold = this->get_parameter("filter_params.overlap.percentage_threshold").as_double(); filter_ptr = std::make_unique(params); @@ -202,12 +202,25 @@ void ImageFilteringNode::set_filter_params() { break; } + // TODO: Add your filter case here as such: + // case FilterType::Example: + // { + // ExampleParams params; + // params.example_variable = + // this->get_parameter("filter_params.example.example_variable").as_int(); + // params.example_string = + // this->get_parameter("filter_params.example.example_string").as_string(); + + // filter_ptr = std::make_unique(params); + // break; + // } + default: - spdlog::warn("Filterparams has not been set for your chosen filter {}. To fix this add your filter to ImageFilteringNode::set_filter_params()", filter_type_string); + spdlog::warn("Filterparams has not been set for your chosen filter {}. To fix this add your filter to ImageFilteringNode::set_filter_params(). Defaulting to no_filter", filter_type_string); + filter_ptr = std::make_unique(); break; }; - } void ImageFilteringNode::check_and_subscribe_to_image_topic() { diff --git a/image-filtering/src/image_processing.cpp b/image-filtering/src/image_processing.cpp index 62065ae..0ed0dc5 100644 --- a/image-filtering/src/image_processing.cpp +++ b/image-filtering/src/image_processing.cpp @@ -4,21 +4,6 @@ -void Unsharpening::apply_filter(const cv::Mat& original, cv::Mat& filtered) const{ - int blur_size = this->filter_params.blur_size; - // Create a blurred version of the image - cv::Mat blurred; - GaussianBlur(original, blurred, - cv::Size(2 * blur_size + 1, 2 * blur_size + 1), 0); - - // Compute the unsharp mask - cv::Mat mask = original - blurred; - cv::Mat unsharp; - - addWeighted(original, 1, mask, 3, 0, filtered); -} - - void Flip::apply_filter(const cv::Mat& original, cv::Mat& filtered) const { int flip_code = this->filter_params.flip_code; // 0: x-axis, 1: y-axis, -1: both From fdb5e6b3578c8e30f907c5300d56d3f53781a103 Mon Sep 17 00:00:00 2001 From: Thomas Date: Mon, 5 Jan 2026 11:07:17 +0100 Subject: [PATCH 37/53] removing unesesary code --- image-filtering/src/image_processing.cpp | 21 +++------------------ 1 file changed, 3 insertions(+), 18 deletions(-) diff --git a/image-filtering/src/image_processing.cpp b/image-filtering/src/image_processing.cpp index 62065ae..b7f7884 100644 --- a/image-filtering/src/image_processing.cpp +++ b/image-filtering/src/image_processing.cpp @@ -3,23 +3,6 @@ #include - -void Unsharpening::apply_filter(const cv::Mat& original, cv::Mat& filtered) const{ - int blur_size = this->filter_params.blur_size; - // Create a blurred version of the image - cv::Mat blurred; - GaussianBlur(original, blurred, - cv::Size(2 * blur_size + 1, 2 * blur_size + 1), 0); - - // Compute the unsharp mask - cv::Mat mask = original - blurred; - cv::Mat unsharp; - - addWeighted(original, 1, mask, 3, 0, filtered); -} - - - void Flip::apply_filter(const cv::Mat& original, cv::Mat& filtered) const { int flip_code = this->filter_params.flip_code; // 0: x-axis, 1: y-axis, -1: both cv::flip(original, filtered, flip_code); @@ -87,12 +70,14 @@ void Ebus::apply_filter(const cv::Mat& original, cv::Mat& filtered) const{ void OtsuSegmentation::apply_filter(const cv::Mat& original, cv::Mat& filtered) const{ + + bool gamma_auto_correction = this->filter_params.gamma_auto_correction; double gamma_auto_correction_weight = this->filter_params.gamma_auto_correction_weight; bool otsu_segmentation = this->filter_params.otsu_segmentation; - + // if (original.type) to_weighted_gray(original, filtered, this->filter_params.gsc_weight_b, this->filter_params.gsc_weight_g, this->filter_params.gsc_weight_r); From 6c29ba52d445de01828143036c31ca1ed6833a53 Mon Sep 17 00:00:00 2001 From: Thomas Date: Mon, 5 Jan 2026 15:43:30 +0100 Subject: [PATCH 38/53] Documenting the code with examples --- README.md | 157 +++++---- .../config/image_filtering_params.yaml | 7 +- image-filtering/image_processing.hpp | 310 ++++++++++++++++++ .../image_filters/image_processing.hpp | 9 +- .../include/image_filters/utilities.hpp | 8 + image-filtering/src/image_filtering_ros.cpp | 27 +- image-filtering/src/image_processing.cpp | 15 +- image-filtering/src/utilities.cpp | 62 ++++ 8 files changed, 520 insertions(+), 75 deletions(-) create mode 100644 image-filtering/image_processing.hpp diff --git a/README.md b/README.md index dca01c0..7b66a71 100644 --- a/README.md +++ b/README.md @@ -32,96 +32,141 @@ Parameters can be set through a YAML file or dynamically adjusted at runtime. ## Implementing New Filters -To extend the functionality of the `image_filtering_node` by adding new filters, follow these steps to ensure compatibility and integration with the existing codebase: +To extend the functionality of the `image_filtering_node` by adding new filters, follow these steps to ensure compatibility and integration with the existing codebase. There should be //TODO comments where you add your filter: -### Step 1: Define Filter Parameters +### Step 1: Add filter to Enum -Each filter should have its own set of parameters encapsulated in a structure. Define this structure within the `vortex::image_filters` namespace. +You should define your filtertype in the filtertype enum in [image_processing.hpp](image-filtering/include/image_filters/image_processing.hpp) ```cpp -struct YourFilterParams { - // Add necessary parameters here - int example_param; +enum class FilterType { + NoFilter, + Flip, + Unsharpening, + Erosion, + Dilation, + ... + // Add your filter here }; ``` -### Step 2: Add to FilterParams Structure - -Integrate your new filter parameters structure into the existing `FilterParams` structure. This allows the `apply_filter` function to access the parameters specific to your filter. +### Step 2: Filter string +To access the filter trough the yamal file we need to access it trough a string. You need to add it as a string to parse_filter_type in [image_processing.hpp](image-filtering/include/image_filters/image_processing.hpp) ```cpp -struct FilterParams { - std::string filter_type; - UnsharpeningFilterParams unsharpening; - ErodingFilterParams eroding; - DilatingFilterParams dilating; - WhiteBalancingFilterParams white_balancing; - EbusFilterParams ebus; - YourFilterParams your_filter; // Add your filter params here -}; +inline FilterType parse_filter_type(std::string s) { + s = to_lower(std::move(s)); + if (s == "no_filter") return FilterType::NoFilter; + if (s == "flip") return FilterType::Flip; + if (s == "unsharpening") return FilterType::Unsharpening; + ... + // Add your filter type here: + + return FilterType::Unknown; + +} ``` -### Step 3: Create the Filter Function -Implement your filter function. This function should take the `cv::Mat` objects for the input and output images and a `const FilterParams&` which includes your specific filter parameters. Make sure to use your parameter structure within this function. +### Step 3: Define Filter Parameters -```cpp -void your_filter_function(const cv::Mat &original, cv::Mat &filtered, const FilterParams& filter_params) { - // Access your filter-specific parameters like this: - int example_param = filter_params.your_filter.example_param; +Each filter should have its own set of parameters encapsulated in a structure. Define this structure within [image_processing.hpp](image-filtering/include/image_filters/image_processing.hpp). - // Implement your filtering logic here -} +```cpp +struct ExampleParams{ + // Add necessary filter parameters here + int example_int; + std::string example_string; +}; ``` -### Step 4: Register the Filter Function - -Add an entry to the `filter_functions` map for your new filter. This step is crucial as it links the filter name (as a string) to the corresponding filter function pointer. +### Step 4: Add filter class +Add a Class for your filter inhereting from the Filter class, with the same exact structure as shown below. This should also be in [image_processing.hpp](image-filtering/include/image_filters/image_processing.hpp) ```cpp -std::map filter_functions = { - {"no_filter", no_filter}, - {"sharpening", sharpening_filter}, - {"unsharpening", unsharpening_filter}, - {"eroding", eroding_filter}, - {"dilating", dilating_filter}, - {"white_balancing", white_balance_filter}, - {"ebus", ebus_filter}, - {"your_filter", your_filter_function} // Add your filter here +class Example: public Filter{ + public: + explicit Example(ExampleParams params): filter_params(params) {} + void apply_filter(const cv::Mat& original, cv::Mat& filtered) const override; // This is the filter itself + private: + ExampleParams filter_params; }; ``` +Here you can add other filter spesific stuff like storing variables that needs to change between runs and so on. -### Step 5: Declare and Assign Parameters -Declare the new filter parameters in the ROS 2 node constructor and assign these parameters to the `FilterParams` structure within the `set_filter_params` function. -#### In the Node Constructor +### Step 5: Create the Filter Function -In the constructor of your ROS 2 node, declare each of the new filter parameters using the `declare_parameter` function. This sets the default values and prepares the node to accept these parameters at runtime through command line or a YAML configuration file. +Implement your filter function in [image_processing.cpp](image-filtering/src/image_processing.cpp). This function should take inn the `cv::Mat` objects for the input and the filtered image, and change the filtered one according to your need. ```cpp -ImageFilteringNode::ImageFilteringNode() : Node("image_filtering_node") -{ - this->declare_parameter("filter_params.your_filter.example_param", "default_value"); - ... - // Other parameters declarations +void Example::apply_filter(const cv::Mat& original, cv::Mat& filtered) const{ + std::string example_str = this->filter_params.example_string; + int example_int = this->filter_params.example_int; + DoExample(original,filtered, example_str, example_int); } ``` +*If you need a helperfunction go to the [helperfunction](#adding-helperfunctions) section of this page. -#### In the set_filter_params Function +### Step 6: Add to config file -In the set_filter_params function, retrieve and assign the parameters to the corresponding fields in the FilterParams structure. Ensure to handle cases where the parameter might not be set or provided. +In the [image_filtering_params.yaml](image-filtering/config/image_filtering_params.yaml) file you add your filter and filterparameters for easily interfacing with the filters: -```cpp +```yaml + filter_params: + filter_type: "example" + + flip: + flip_code: 1 + ... + # Add your filter type here -void ImageFilteringNode::set_filter_params(){ - FilterParams params = filter_params_; // assuming filter_params_ is already defined in your class + example: + example_int: 5 + example_string: "This is an example" +``` - params.your_filter.example_param = this->get_parameter("filter_params.your_filter.example_param").as_string(); + +### Step 7: Declare and Assign Parameters + +In the constructor of your ROS 2 node, declare each of the new filter parameters using the `declare_parameter` function in [image_filtering_ros.cpp](image-filtering/src/image_filtering_ros.cpp). This sets the default values and prepares the node to accept these parameters at runtime through command line or the YAML configuration file. + +```cpp +void ImageFilteringNode::declare_parameters() { + // Declare your parameters here + this->declare_parameter("filter_params.example.example_int"); + this->declare_parameter("filter_params.example.example_string"); +} +``` + +Then in the same file you make a new case in `set_filter_params` for your filter, to set the variables you just declared. +```cpp +void ImageFilteringNode::set_filter_params() { ... - // Retrieve other parameters and handle cases where parameters might not be provided - filter_params_ = params; // Update the filter parameters structure - RCLCPP_INFO(this->get_logger(), "Filter parameters updated for your_filter."); + switch (filter_type){ + + ... + // Add case here + case FilterType::Example: + { + ExampleParams params; + params.example_int = + this->get_parameter("filter_params.example.example_int").as_int(); + params.example_string = + this->get_parameter("filter_params.example.example_string").as_string(); + + filter_ptr = std::make_unique(params); + break; + } + + } } ``` + + + +#### Adding Helperfunctions + +If you need helperfunctions for your filter, you can add the declaration to [utilities.hpp](image-filtering/include/image_filters/utilities.hpp), and then add the function defenition to [utilities.cpp](image-filtering/src/utilities.cpp). There wil be TODO comments where you can add them. These cunctions are allredy included in the image_prosessing files. \ No newline at end of file diff --git a/image-filtering/config/image_filtering_params.yaml b/image-filtering/config/image_filtering_params.yaml index bd9b95c..3959d0b 100644 --- a/image-filtering/config/image_filtering_params.yaml +++ b/image-filtering/config/image_filtering_params.yaml @@ -6,7 +6,7 @@ input_encoding: "mono8" filter_params: - filter_type: "flip" + filter_type: "example" flip: flip_code: 1 unsharpening: @@ -40,6 +40,11 @@ threshold: 0.1 # in percent maxval: 255. invert: true + + # TODO: add your filterparameters here + example: + example_int: 5 + example_string: "This is an example" # Filter params should reflect the FilterParams struct diff --git a/image-filtering/image_processing.hpp b/image-filtering/image_processing.hpp new file mode 100644 index 0000000..7fb400c --- /dev/null +++ b/image-filtering/image_processing.hpp @@ -0,0 +1,310 @@ +#ifndef IMAGE_PROCESSING_HPP +#define IMAGE_PROCESSING_HPP + +#include +#include +#include +#include +#include +#include +#include +#include + + +enum class FilterType { // TODO: Add filters here + NoFilter, + Flip, + Unsharpening, + Erosion, + Dilation, + WhiteBalancing, + Ebus, + Otsu, + Overlap, + MedianBinary, + Binary, + Unknown +}; + +inline std::string to_lower(std::string s) { + for (auto& c : s) c = static_cast(std::tolower(c)); + return s; +} + +inline FilterType parse_filter_type(std::string s) { // TODO: Also add filter-type here + s = to_lower(std::move(s)); + if (s == "no_filter") return FilterType::NoFilter; + if (s == "flip") return FilterType::Flip; + if (s == "unsharpening") return FilterType::Unsharpening; + if (s == "erosion") return FilterType::Erosion; + if (s == "dilation") return FilterType::Dilation; + if (s == "white_balancing")return FilterType::WhiteBalancing; + if (s == "ebus") return FilterType::Ebus; + if (s == "otsu") return FilterType::Otsu; + if (s == "overlap") return FilterType::Overlap; + if (s == "median_binary") return FilterType::MedianBinary; + if (s == "binary") return FilterType::Binary; + return FilterType::Unknown; +} + + + + +class Filter{ + public: + virtual ~Filter() = default; + virtual void apply_filter(const cv::Mat& original, cv::Mat& filtered) const = 0; + + protected: + Filter() = default; +}; + + +///////////////////////////// +// No filter +///////////////////////////// + +struct NoFilterParams{}; + +class NoFilter: public Filter{ + public: + explicit NoFilter() = default;// No parameters to set + void apply_filter(const cv::Mat& original, cv::Mat& filtered) const override{ original.copyTo(filtered); }; +}; + + +//////////////////////////// +// Unsharpening +///////////////////////////// + +struct UnsharpeningParams{ + int blur_size; +}; + + +class Unsharpening: public Filter{ + public: + explicit Unsharpening(UnsharpeningParams params): filter_params(params) {} + void apply_filter(const cv::Mat& original, cv::Mat& filtered) const override; + private: + UnsharpeningParams filter_params; +}; + + + +///////////////////////////// +// Sharpening +///////////////////////////// + +struct FlipParams{ + int flip_code; +}; + +class Flip: public Filter{ + public: + explicit Flip(FlipParams params): filter_params(params) {} + void apply_filter(const cv::Mat& original, cv::Mat& filtered) const override; + private: + FlipParams filter_params; +}; + + + + + + +///////////////////////////// +// Sharpening +///////////////////////////// + +struct SharpeningParams{}; + +class Sharpening: public Filter{ +public: + explicit Sharpening(SharpeningParams params): filter_params(params) {} + void apply_filter(const cv::Mat& original, cv::Mat& filtered) const override; +private: + SharpeningParams filter_params; +}; +///////////////////////////// +// Erosion +///////////////////////////// + +struct ErosionParams{ + int kernel_size; // odd > 1 +}; + +class Erosion: public Filter{ +public: + explicit Erosion(ErosionParams params): filter_params(params) {} + void apply_filter(const cv::Mat& original, cv::Mat& filtered) const override; +private: + ErosionParams filter_params; +}; + +///////////////////////////// +// Dilation +///////////////////////////// + +struct DilationParams{ + int kernel_size = 3; +}; + +class Dilation: public Filter{ +public: + explicit Dilation(DilationParams params): filter_params(params) {} + void apply_filter(const cv::Mat& original, cv::Mat& filtered) const override; +private: + DilationParams filter_params; +}; + +///////////////////////////// +// White Balance +///////////////////////////// + +struct WhiteBalanceParams{ + double contrast_percentage; +}; + +class WhiteBalance: public Filter{ +public: + explicit WhiteBalance(WhiteBalanceParams params): filter_params(params) {} + void apply_filter(const cv::Mat& original, cv::Mat& filtered) const override; +private: + WhiteBalanceParams filter_params; +}; + +///////////////////////////// +// Ebus (dilation + unsharpening combo) +///////////////////////////// + +struct EbusParams{ + int erosion_size; + int blur_size; + int mask_weight; +}; + +class Ebus: public Filter{ +public: + explicit Ebus(EbusParams params): filter_params(params) {} + void apply_filter(const cv::Mat& original, cv::Mat& filtered) const override; +private: + EbusParams filter_params; +}; + +///////////////////////////// +// Otsu Segmentation +///////////////////////////// + +struct OtsuSegmentationParams{ + bool gamma_auto_correction; + double gamma_auto_correction_weight; + bool otsu_segmentation; + double gsc_weight_r; + double gsc_weight_g; + double gsc_weight_b; + int erosion_size; + int dilation_size; +}; + +class OtsuSegmentation: public Filter{ +public: + explicit OtsuSegmentation(OtsuSegmentationParams params): filter_params(params) {} + void apply_filter(const cv::Mat& original, cv::Mat& output) const override; +private: + OtsuSegmentationParams filter_params; +}; + +///////////////////////////// +// Overlap (blend/composite) +///////////////////////////// + +struct OverlapParams{ + double percentage_threshold; +}; + +class Overlap: public Filter{ +public: + explicit Overlap(OverlapParams params): filter_params(params) {} + void apply_filter(const cv::Mat& original, cv::Mat& filtered) const override; +private: + OverlapParams filter_params; +}; + +///////////////////////////// +// Median + Binary +///////////////////////////// + +struct MedianBinaryParams{ + int kernel_size; + int threshold; + bool invert; +}; + +class MedianBinary: public Filter{ +public: + explicit MedianBinary(MedianBinaryParams params): filter_params(params) {} + void apply_filter(const cv::Mat& original, cv::Mat& filtered) const override; +private: + MedianBinaryParams filter_params; +}; + +///////////////////////////// +// Binary Threshold +///////////////////////////// + +struct BinaryThresholdParams{ + double threshold; + double maxval; + bool invert; +}; + +class BinaryThreshold: public Filter{ +public: + explicit BinaryThreshold(BinaryThresholdParams params): filter_params(params) {} + void apply_filter(const cv::Mat& original, cv::Mat& filtered) const override; +private: + BinaryThresholdParams filter_params; +}; + + + + + + +///////////////////////////// +// Example +///////////////////////////// + +// TODO: add this structure for your filter + +// Example: +struct ExampleParams{ // Add filter variables here + int example_variable; + std::string example_string; +}; + +class Example: public Filter{ + public: + explicit Example(ExampleParams params): filter_params(params) {} + void apply_filter(const cv::Mat& original, cv::Mat& filtered) const override; // This is the filter itself + private: + ExampleParams filter_params; +}; + + + + + + + + + + + + + +#endif // IMAGE_PROCESSING_HPP + + + diff --git a/image-filtering/include/image_filters/image_processing.hpp b/image-filtering/include/image_filters/image_processing.hpp index 62a809f..a5416ac 100644 --- a/image-filtering/include/image_filters/image_processing.hpp +++ b/image-filtering/include/image_filters/image_processing.hpp @@ -23,6 +23,8 @@ enum class FilterType { // TODO: Add filters here Overlap, MedianBinary, Binary, + + Example, Unknown }; @@ -44,6 +46,7 @@ inline FilterType parse_filter_type(std::string s) { // TODO: Also add filter-ty if (s == "overlap") return FilterType::Overlap; if (s == "median_binary") return FilterType::MedianBinary; if (s == "binary") return FilterType::Binary; + if (s == "example") return FilterType::Example; return FilterType::Unknown; } @@ -273,14 +276,14 @@ class BinaryThreshold: public Filter{ ///////////////////////////// -// Template +// Example ///////////////////////////// // TODO: add this structure for your filter -// Template: +// Example: struct ExampleParams{ // Add filter variables here - int example_variable; + int example_int; std::string example_string; }; diff --git a/image-filtering/include/image_filters/utilities.hpp b/image-filtering/include/image_filters/utilities.hpp index f652f91..3426ead 100644 --- a/image-filtering/include/image_filters/utilities.hpp +++ b/image-filtering/include/image_filters/utilities.hpp @@ -27,4 +27,12 @@ void apply_fixed_threshold(const cv::Mat& img, cv::Mat& filtered, int thresh, bo // TODO Fix, does not work properly (Chat has failed me) void distance_field(const cv::Mat& binObstacles, cv::Mat& dist, bool obstaclesAreWhite = true, int type = cv::DIST_L2, int maskSize = 3); // DIST_L2 is normal euclidian + + + + + +// TODO: If you need a helper function have the signature here +void DoExample(const cv::Mat& original, cv::Mat& filtered, std::string example_string, int example_int); + #endif \ No newline at end of file diff --git a/image-filtering/src/image_filtering_ros.cpp b/image-filtering/src/image_filtering_ros.cpp index 8e585ae..8fb080e 100644 --- a/image-filtering/src/image_filtering_ros.cpp +++ b/image-filtering/src/image_filtering_ros.cpp @@ -54,7 +54,10 @@ void ImageFilteringNode::declare_parameters() { this->declare_parameter("filter_params.median_binary.threshold"); this->declare_parameter("filter_params.median_binary.invert"); + // TODO: Declare parameters set for your filter here + this->declare_parameter("filter_params.example.example_int"); + this->declare_parameter("filter_params.example.example_string"); } @@ -202,18 +205,18 @@ void ImageFilteringNode::set_filter_params() { break; } - // TODO: Add your filter case here as such: - // case FilterType::Example: - // { - // ExampleParams params; - // params.example_variable = - // this->get_parameter("filter_params.example.example_variable").as_int(); - // params.example_string = - // this->get_parameter("filter_params.example.example_string").as_string(); - - // filter_ptr = std::make_unique(params); - // break; - // } + // TODO: Add your filter case here: + case FilterType::Example: + { + ExampleParams params; + params.example_int = + this->get_parameter("filter_params.example.example_int").as_int(); + params.example_string = + this->get_parameter("filter_params.example.example_string").as_string(); + + filter_ptr = std::make_unique(params); + break; + } default: diff --git a/image-filtering/src/image_processing.cpp b/image-filtering/src/image_processing.cpp index 0ed0dc5..bffefb2 100644 --- a/image-filtering/src/image_processing.cpp +++ b/image-filtering/src/image_processing.cpp @@ -169,9 +169,18 @@ void BinaryThreshold::apply_filter(const cv::Mat& original, cv::Mat& filtered) c gray.convertTo(src8, CV_8U); } else { src8 = gray; - } - + } // Apply fixed threshold const int type = invert ? cv::THRESH_BINARY_INV : cv::THRESH_BINARY; cv::threshold(src8, filtered, thresh, maxval, type); -} \ No newline at end of file +} + + + + +// TODO: Implement your filter here +void Example::apply_filter(const cv::Mat& original, cv::Mat& filtered) const{ + std::string example_str = this->filter_params.example_string; + int example_int = this->filter_params.example_int; + DoExample(original,filtered, example_str, example_int); +} diff --git a/image-filtering/src/utilities.cpp b/image-filtering/src/utilities.cpp index 49487fb..7bfdee7 100644 --- a/image-filtering/src/utilities.cpp +++ b/image-filtering/src/utilities.cpp @@ -301,6 +301,68 @@ void distance_field(const cv::Mat& binObstacles, +// TODO: If you need a helper function define it here like this + +void DoExample(const cv::Mat& original, + cv::Mat& filtered, + std::string example_string, + int example_int) +{ + CV_Assert(!original.empty()); + CV_Assert(original.type() == CV_8UC1); // mono8 + + filtered = original.clone(); + + // Two centered lines: number above string + const std::string line1 = std::to_string(example_int); + const std::string line2 = example_string; + + // Text style + const int fontFace = cv::FONT_HERSHEY_SIMPLEX; + const double fontScale = 1.0; + const int thickness = 2; + const int lineType = cv::LINE_AA; // smoother; use cv::LINE_8 for hard edges + const int lineGapPx = 10; // vertical gap between lines in pixels + + // Measure both lines + int base1 = 0, base2 = 0; + const cv::Size sz1 = cv::getTextSize(line1, fontFace, fontScale, thickness, &base1); + const cv::Size sz2 = cv::getTextSize(line2, fontFace, fontScale, thickness, &base2); + + // Total block size (approx). Heights don't include baseline, so we add baselines for safety. + const int blockW = std::max(sz1.width, sz2.width); + const int blockH = (sz1.height + base1) + lineGapPx + (sz2.height + base2); + + // Top-left of the text block so the whole block is centered + const int blockX = (filtered.cols - blockW) / 2; + const int blockY = (filtered.rows - blockH) / 2; + + // Baseline positions for each line (y is baseline in putText) + const int x1 = blockX + (blockW - sz1.width) / 2; + const int y1 = blockY + sz1.height; // baseline ~ top + height + + const int x2 = blockX + (blockW - sz2.width) / 2; + const int y2 = y1 + base1 + lineGapPx + sz2.height; + + // Clamp to keep text inside image if needed + auto clamp = [](int v, int lo, int hi) { return std::max(lo, std::min(v, hi)); }; + + const int x1c = clamp(x1, 0, std::max(0, filtered.cols - sz1.width)); + const int y1c = clamp(y1, sz1.height, std::max(sz1.height, filtered.rows - base1)); + + const int x2c = clamp(x2, 0, std::max(0, filtered.cols - sz2.width)); + const int y2c = clamp(y2, sz2.height, std::max(sz2.height, filtered.rows - base2)); + + // Draw white text on mono8 + cv::putText(filtered, line1, cv::Point(x1c, y1c), + fontFace, fontScale, cv::Scalar(255), thickness, lineType); + + cv::putText(filtered, line2, cv::Point(x2c, y2c), + fontFace, fontScale, cv::Scalar(255), thickness, lineType); +} + + + From 1737631ffa040cf3dcdc76840ab52001de51f699 Mon Sep 17 00:00:00 2001 From: Thomas Date: Mon, 5 Jan 2026 17:01:41 +0100 Subject: [PATCH 39/53] refactoring --- .../config/image_filtering_params.yaml | 10 ++-- .../image_filters/image_processing.hpp | 16 ++++-- image-filtering/src/image_processing.cpp | 51 ++++++++++--------- image-filtering/src/utilities.cpp | 41 --------------- 4 files changed, 44 insertions(+), 74 deletions(-) diff --git a/image-filtering/config/image_filtering_params.yaml b/image-filtering/config/image_filtering_params.yaml index 3959d0b..9324499 100644 --- a/image-filtering/config/image_filtering_params.yaml +++ b/image-filtering/config/image_filtering_params.yaml @@ -16,7 +16,7 @@ dilation: size: 3 white_balancing: - contrast_percentage: 0.1 + contrast_percentage: 10.0 ebus: erosion_size: 2 blur_size: 30 @@ -31,20 +31,20 @@ erosion_size: 2 dilation_size: 2 overlap: - percentage_threshold: 100.0 # Percentage to cap the picel intensity differance + percentage_threshold: 20.0 # Percentage (0-100) to cap the pixcel intensity differance median_binary: # finds the median of each n x n square around each pixel kernel_size: 3 # must be odd >= 1 threshold: 100 # [0, 255] invert: false binary: - threshold: 0.1 # in percent + threshold: 20. # in percent maxval: 255. invert: true # TODO: add your filterparameters here example: - example_int: 5 - example_string: "This is an example" + example_int: 1 + example_string: "Get filtered" # Filter params should reflect the FilterParams struct diff --git a/image-filtering/include/image_filters/image_processing.hpp b/image-filtering/include/image_filters/image_processing.hpp index a5416ac..c836fcf 100644 --- a/image-filtering/include/image_filters/image_processing.hpp +++ b/image-filtering/include/image_filters/image_processing.hpp @@ -222,18 +222,26 @@ class OtsuSegmentation: public Filter{ // Overlap (blend/composite) ///////////////////////////// -struct OverlapParams{ - double percentage_threshold; +struct OverlapParams { + double percentage_threshold; // 0..100 (percent) }; -class Overlap: public Filter{ +class Overlap : public Filter { public: - explicit Overlap(OverlapParams params): filter_params(params) {} + explicit Overlap(OverlapParams params) + : filter_params(params), has_prev(false) {} + void apply_filter(const cv::Mat& original, cv::Mat& filtered) const override; + private: OverlapParams filter_params; + + // Cached previous mono8 frame + mutable cv::Mat prev; + mutable bool has_prev; }; + ///////////////////////////// // Median + Binary ///////////////////////////// diff --git a/image-filtering/src/image_processing.cpp b/image-filtering/src/image_processing.cpp index bffefb2..93b04fc 100644 --- a/image-filtering/src/image_processing.cpp +++ b/image-filtering/src/image_processing.cpp @@ -81,6 +81,7 @@ void OtsuSegmentation::apply_filter(const cv::Mat& original, cv::Mat& filtered) to_weighted_gray(original, filtered, this->filter_params.gsc_weight_b, this->filter_params.gsc_weight_g, this->filter_params.gsc_weight_r); + original.copyTo(filtered); if (gamma_auto_correction) { @@ -97,46 +98,48 @@ void OtsuSegmentation::apply_filter(const cv::Mat& original, cv::Mat& filtered) } } -// Thomas was here -void Overlap::apply_filter(const cv::Mat& original, cv::Mat& filtered) const{ - static cv::Mat prevR; // store previous R channel only - static bool has_prev = false; - // Extract current R channel - cv::Mat curR; - cv::extractChannel(original, curR, 2); // 0=B,1=G,2=R - if (!has_prev || prevR.empty() || prevR.size()!=curR.size() || prevR.type()!=curR.type()) { - original.copyTo(filtered); // first call (or size/type change): pass through - prevR = curR.clone(); // cache R channel +void Overlap::apply_filter(const cv::Mat& original, cv::Mat& filtered) const +{ + // mono8 only + CV_Assert(!original.empty()); + CV_Assert(original.type() == CV_8UC1); + + double thr_percent = filter_params.percentage_threshold; + + // First frame (or size/type change): passthrough + cache + if (!has_prev || prev.empty() || + prev.size() != original.size() || prev.type() != original.type()) + { + original.copyTo(filtered); + prev = original.clone(); has_prev = true; return; } - // |cur - prev| on the R channel + // Absolute difference (still mono8) cv::Mat diff8u; - cv::absdiff(curR, prevR, diff8u); + cv::absdiff(original, prev, diff8u); - // % of full 8-bit range + // Convert to percent of 8-bit range (0..100) cv::Mat percent32f; diff8u.convertTo(percent32f, CV_32F, 100.0 / 255.0); - // Mask: pixels whose % change > threshold - cv::Mat mask; - cv::threshold(percent32f, mask, this->filter_params.percentage_threshold, 255.0, cv::THRESH_BINARY); - mask.convertTo(mask, CV_8U); + // Build mask where change > threshold + cv::Mat mask8u; + cv::threshold(percent32f, mask8u, thr_percent, 255.0, cv::THRESH_BINARY); + mask8u.convertTo(mask8u, CV_8U); - // Zero out those pixels in the R channel + // Output: same as original except zero-out "changed" pixels filtered = original.clone(); - std::vector ch; - cv::split(filtered, ch); // ch[2] is R - ch[2].setTo(0, mask); - cv::merge(ch, filtered); + filtered.setTo(0, mask8u); - // Update history (R channel only) - prevR = curR.clone(); + // Update history (write to cached previous) + prev = original.clone(); } + void MedianBinary::apply_filter(const cv::Mat& original, cv::Mat& filtered) const{ apply_median(original, filtered, this->filter_params.kernel_size); diff --git a/image-filtering/src/utilities.cpp b/image-filtering/src/utilities.cpp index 7bfdee7..dbfbbce 100644 --- a/image-filtering/src/utilities.cpp +++ b/image-filtering/src/utilities.cpp @@ -71,47 +71,6 @@ void to_weighted_gray(const cv::Mat& bgr, cv::Mat& gray, double wB, double wG, d -// // Computes the Otsu optimal threshold from a 256-bin normalized grayscale histogram. -// // `hist_prob` must contain probabilities (sum ≈ 1). For every possible threshold t, -// // the function splits the histogram into foreground [0, t) and background [t, 255], -// // computes their weights and means, evaluates the between-class variance, and returns -// // the t that maximizes it. A good threshold is one that makes two sizeable groups that -// // are well separated in intensity: -// // - Best regards ChatGPT -// int computeOtsuThreshold(const cv::Mat& hist_prob) // TODO it is time for you to go, its been nice knowing you, but you have been replased by opencv -// { -// // Initialize variables for Otsu's method -// std::vector sigma2_list(256, 0.0); -// std::vector p(hist_prob.begin(), hist_prob.end()); // Probabilities - -// for (int th = 1; th < 256; ++th) { -// // Calculate omega (weights) for foreground and background -// float omega_fg = std::accumulate(p.begin(), p.begin() + th, 0.0f); -// float omega_bg = std::accumulate(p.begin() + th, p.end(), 0.0f); - -// // Calculate mu (means) for foreground and background -// float mu_fg = 0, mu_bg = 0; -// for (int i = 0; i < th; ++i) { -// mu_fg += i * p[i]; -// } -// for (int i = th; i < 256; ++i) { -// mu_bg += i * p[i]; -// } - -// if (omega_fg > 0) -// mu_fg /= omega_fg; -// if (omega_bg > 0) -// mu_bg /= omega_bg; - -// // Calculate sigma squared and store in list -// sigma2_list[th] = omega_fg * omega_bg * pow(mu_fg - mu_bg, 2); -// } - -// return int(std::max_element(sigma2_list.begin(), sigma2_list.end()) - sigma2_list.begin()); -// } - - - // Returns the Otsu threshold value chosen by OpenCV (0..255) and outputs the thresholded binary image int apply_otsu(const cv::Mat& gray8u, cv::Mat& out, bool invert, double maxval) { From c06fb2f0cf9e090cfc53dff0dd0773c98923ce4f Mon Sep 17 00:00:00 2001 From: Thomas Date: Tue, 6 Jan 2026 16:02:38 +0100 Subject: [PATCH 40/53] Fixing mono vs bgr for otsu --- image-filtering/config/image_filtering_params.yaml | 2 +- image-filtering/src/image_processing.cpp | 14 ++++++++------ 2 files changed, 9 insertions(+), 7 deletions(-) diff --git a/image-filtering/config/image_filtering_params.yaml b/image-filtering/config/image_filtering_params.yaml index 9324499..f86472b 100644 --- a/image-filtering/config/image_filtering_params.yaml +++ b/image-filtering/config/image_filtering_params.yaml @@ -2,8 +2,8 @@ ros__parameters: sub_topic: "/fls_publisher/display_mono" pub_topic: "/filtered_image" - output_encoding: "mono8" input_encoding: "mono8" + output_encoding: "mono8" filter_params: filter_type: "example" diff --git a/image-filtering/src/image_processing.cpp b/image-filtering/src/image_processing.cpp index 93b04fc..0c707af 100644 --- a/image-filtering/src/image_processing.cpp +++ b/image-filtering/src/image_processing.cpp @@ -77,12 +77,14 @@ void OtsuSegmentation::apply_filter(const cv::Mat& original, cv::Mat& filtered) bool otsu_segmentation = this->filter_params.otsu_segmentation; - - to_weighted_gray(original, filtered, this->filter_params.gsc_weight_b, - this->filter_params.gsc_weight_g, - this->filter_params.gsc_weight_r); - original.copyTo(filtered); - + if (original.type() == CV_8UC3) { // if the image type is bgr8 + to_weighted_gray(original, filtered, + this->filter_params.gsc_weight_b, + this->filter_params.gsc_weight_g, + this->filter_params.gsc_weight_r); + } + else if (original.type() == CV_8UC1){ original.copyTo(filtered); } // if its mono8 + else {std::cout << "your image type is not matching this filter" << std::endl;} if (gamma_auto_correction) { apply_auto_gamma(filtered, gamma_auto_correction_weight); From 2a28bc943833066ec1e1e03c13ec4d8ba39dc489 Mon Sep 17 00:00:00 2001 From: Thomas Date: Tue, 6 Jan 2026 19:38:43 +0100 Subject: [PATCH 41/53] Adding final tutches, like coloring coments ++ --- README.md | 2 +- image-filtering/CMakeLists.txt | 1 + image-filtering/src/image_filtering_ros.cpp | 13 ++++++++++--- image-filtering/src/utilities.cpp | 2 -- 4 files changed, 12 insertions(+), 6 deletions(-) diff --git a/README.md b/README.md index 7b66a71..fa340e6 100644 --- a/README.md +++ b/README.md @@ -169,4 +169,4 @@ void ImageFilteringNode::set_filter_params() { #### Adding Helperfunctions -If you need helperfunctions for your filter, you can add the declaration to [utilities.hpp](image-filtering/include/image_filters/utilities.hpp), and then add the function defenition to [utilities.cpp](image-filtering/src/utilities.cpp). There wil be TODO comments where you can add them. These cunctions are allredy included in the image_prosessing files. \ No newline at end of file +If you need helperfunctions for your filter, you can add the declaration to [utilities.hpp](image-filtering/include/image_filters/utilities.hpp), and then add the function defenition to [utilities.cpp](image-filtering/src/utilities.cpp). There wil be TODO comments where you can add them. These functions are alredy included in the image_prosessing files. \ No newline at end of file diff --git a/image-filtering/CMakeLists.txt b/image-filtering/CMakeLists.txt index c29732d..e7e2440 100644 --- a/image-filtering/CMakeLists.txt +++ b/image-filtering/CMakeLists.txt @@ -25,6 +25,7 @@ set(LIB_NAME "${PROJECT_NAME}_component") add_library(${LIB_NAME} SHARED src/image_processing.cpp src/image_filtering_ros.cpp + src/utilities.cpp ) target_link_libraries(${LIB_NAME} PUBLIC diff --git a/image-filtering/src/image_filtering_ros.cpp b/image-filtering/src/image_filtering_ros.cpp index 8fb080e..9b1fa68 100644 --- a/image-filtering/src/image_filtering_ros.cpp +++ b/image-filtering/src/image_filtering_ros.cpp @@ -70,7 +70,7 @@ void ImageFilteringNode::set_filter_params() { { case FilterType::Unknown: { - spdlog::warn("Invalid filter type received: {}. Using default: no_filter.", filter_type_string); + spdlog::warn("\033[33mInvalid filter type received: {}. Using default: no_filter.\033[0m", filter_type_string); // filter_ = "no_filter"; filter_type = FilterType::NoFilter; } @@ -220,10 +220,17 @@ void ImageFilteringNode::set_filter_params() { default: - spdlog::warn("Filterparams has not been set for your chosen filter {}. To fix this add your filter to ImageFilteringNode::set_filter_params(). Defaulting to no_filter", filter_type_string); + ; filter_ptr = std::make_unique(); - break; + spdlog::warn("\033[33m Filterparams has not been set for your chosen filter {}. " + "To fix this add your filter to ImageFilteringNode::set_filter_params(). " + "Defaulting to no_filter. \033[0m", + filter_type_string); }; + + if (filter_type != FilterType::Unknown){ + spdlog::info("\033[32m Using filter: {} \033[0m", filter_type_string); + } } void ImageFilteringNode::check_and_subscribe_to_image_topic() { diff --git a/image-filtering/src/utilities.cpp b/image-filtering/src/utilities.cpp index dbfbbce..5eddcb0 100644 --- a/image-filtering/src/utilities.cpp +++ b/image-filtering/src/utilities.cpp @@ -267,8 +267,6 @@ void DoExample(const cv::Mat& original, std::string example_string, int example_int) { - CV_Assert(!original.empty()); - CV_Assert(original.type() == CV_8UC1); // mono8 filtered = original.clone(); From 1cae05027c62a7de7d756ec7a14531ed8c325ced Mon Sep 17 00:00:00 2001 From: Thomas Date: Wed, 7 Jan 2026 13:21:01 +0100 Subject: [PATCH 42/53] i fixed a warning coming from the dinosaur from toy story in asci art, blus some finishing tuches in comenting and so on --- .../image_filters/image_processing.hpp | 53 +++++++++++++------ .../include/image_filters/utilities.hpp | 21 ++++++-- image-filtering/src/image_filtering_ros.cpp | 7 ++- image-filtering/src/image_processing.cpp | 2 +- image-filtering/src/utilities.cpp | 15 ++---- 5 files changed, 61 insertions(+), 37 deletions(-) diff --git a/image-filtering/include/image_filters/image_processing.hpp b/image-filtering/include/image_filters/image_processing.hpp index c836fcf..2631fb5 100644 --- a/image-filtering/include/image_filters/image_processing.hpp +++ b/image-filtering/include/image_filters/image_processing.hpp @@ -11,6 +11,7 @@ #include + enum class FilterType { // TODO: Add filters here NoFilter, Flip, @@ -28,28 +29,47 @@ enum class FilterType { // TODO: Add filters here Unknown }; +static constexpr std::pair kFilterMap[] = { + {"no_filter", FilterType::NoFilter}, + {"flip", FilterType::Flip}, + {"unsharpening", FilterType::Unsharpening}, + {"erosion", FilterType::Erosion}, + {"dilation", FilterType::Dilation}, + {"white_balancing", FilterType::WhiteBalancing}, + {"ebus", FilterType::Ebus}, + {"otsu", FilterType::Otsu}, + {"overlap", FilterType::Overlap}, + {"median_binary", FilterType::MedianBinary}, + {"binary", FilterType::Binary}, + + // TODO: Also add your filter here + {"example", FilterType::Example}, + {"unknown", FilterType::Unknown} +}; + + inline std::string to_lower(std::string s) { - for (auto& c : s) c = static_cast(std::tolower(c)); + for (char& ch : s) { + ch = static_cast(std::tolower(static_cast(ch))); + } return s; } -inline FilterType parse_filter_type(std::string s) { // TODO: Also add filter-type here - s = to_lower(std::move(s)); - if (s == "no_filter") return FilterType::NoFilter; - if (s == "flip") return FilterType::Flip; - if (s == "unsharpening") return FilterType::Unsharpening; - if (s == "erosion") return FilterType::Erosion; - if (s == "dilation") return FilterType::Dilation; - if (s == "white_balancing")return FilterType::WhiteBalancing; - if (s == "ebus") return FilterType::Ebus; - if (s == "otsu") return FilterType::Otsu; - if (s == "overlap") return FilterType::Overlap; - if (s == "median_binary") return FilterType::MedianBinary; - if (s == "binary") return FilterType::Binary; - if (s == "example") return FilterType::Example; +inline FilterType parse_filter_type(std::string s) { + s = to_lower(std::move(s)); + + for (auto [name, type] : kFilterMap) { + if (s == name) return type; + } + std::cout << "\033[33m No string connected to that filtertype: '" << s << "'. This might be misspelling or you need to add the filtertype to kFilterMap in image_processing.hpp\033[0m"; return FilterType::Unknown; } +inline std::string_view filtertype_to_string(FilterType t) { + for (auto [name, type] : kFilterMap) {if (t == type) return name;} + std::cout << "\033[33m No string connected to your filtertype. To fix this add the string and filtertype to kFilterMap\033[0m"; + return "unknown"; +} @@ -282,13 +302,12 @@ class BinaryThreshold: public Filter{ +// TODO: add this structure for your filter ///////////////////////////// // Example ///////////////////////////// -// TODO: add this structure for your filter - // Example: struct ExampleParams{ // Add filter variables here int example_int; diff --git a/image-filtering/include/image_filters/utilities.hpp b/image-filtering/include/image_filters/utilities.hpp index 3426ead..08c36ac 100644 --- a/image-filtering/include/image_filters/utilities.hpp +++ b/image-filtering/include/image_filters/utilities.hpp @@ -9,22 +9,35 @@ #include - - +// Auto-choose a gamma so dark images get lifted and bright images get toned down (expects mono8) void apply_auto_gamma(cv::Mat& image, double correction_weight); + +// Converts a BGR image to grayscale using custom channel weights (wB, wG, wR). void to_weighted_gray(const cv::Mat& bgr, cv::Mat& gray, double wB, double wG, double wR); +// Applies Otsu’s automatic thresholding and returning the threshold int apply_otsu(const cv::Mat& gray8u, cv::Mat& out, bool invert = false, double maxval = 255.0); +// Performs morphological erosion (shrinks bright regions / removes small white noise) void apply_erosion(const cv::Mat& src, cv::Mat& dst, int size, int shape = cv::MORPH_RECT); + +// Performs morphological dilation (grows bright regions / fills small holes) opposite of erotion void apply_dilation(const cv::Mat& src, cv::Mat& dst, int size, int shape=cv::MORPH_RECT); +// Applies a median blur with `kernel_size` to reduce salt-and-pepper noise while preserving edges. void apply_median(const cv::Mat& original, cv::Mat& filtered, int kernel_size); + + +// Apply a fixed binary threshold. +// - Accepts grayscale or color input (auto-converts to gray). +// - Ensures 8-bit depth for thresholding. +// - Returns a 0/255 mask (CV_8U). +// - Set `invert=true` to get white background & black foreground. void apply_fixed_threshold(const cv::Mat& img, cv::Mat& filtered, int thresh, bool invert = false); -// TODO Fix, does not work properly (Chat has failed me) +// This does not work properly (Chat has failed me) void distance_field(const cv::Mat& binObstacles, cv::Mat& dist, bool obstaclesAreWhite = true, int type = cv::DIST_L2, int maskSize = 3); // DIST_L2 is normal euclidian @@ -33,6 +46,6 @@ void distance_field(const cv::Mat& binObstacles, cv::Mat& dist, bool obstaclesAr // TODO: If you need a helper function have the signature here -void DoExample(const cv::Mat& original, cv::Mat& filtered, std::string example_string, int example_int); +void apply_example(const cv::Mat& original, cv::Mat& filtered, std::string example_string, int example_int); #endif \ No newline at end of file diff --git a/image-filtering/src/image_filtering_ros.cpp b/image-filtering/src/image_filtering_ros.cpp index 9b1fa68..8a0ca79 100644 --- a/image-filtering/src/image_filtering_ros.cpp +++ b/image-filtering/src/image_filtering_ros.cpp @@ -71,8 +71,8 @@ void ImageFilteringNode::set_filter_params() { case FilterType::Unknown: { spdlog::warn("\033[33mInvalid filter type received: {}. Using default: no_filter.\033[0m", filter_type_string); - // filter_ = "no_filter"; filter_type = FilterType::NoFilter; + [[fallthrough]]; } case FilterType::NoFilter: @@ -226,11 +226,10 @@ void ImageFilteringNode::set_filter_params() { "To fix this add your filter to ImageFilteringNode::set_filter_params(). " "Defaulting to no_filter. \033[0m", filter_type_string); + filter_type = FilterType::NoFilter; }; - if (filter_type != FilterType::Unknown){ - spdlog::info("\033[32m Using filter: {} \033[0m", filter_type_string); - } + spdlog::info("\033[32m Using filter: {} \033[0m", filtertype_to_string(filter_type)); } void ImageFilteringNode::check_and_subscribe_to_image_topic() { diff --git a/image-filtering/src/image_processing.cpp b/image-filtering/src/image_processing.cpp index 0c707af..b259c71 100644 --- a/image-filtering/src/image_processing.cpp +++ b/image-filtering/src/image_processing.cpp @@ -187,5 +187,5 @@ void BinaryThreshold::apply_filter(const cv::Mat& original, cv::Mat& filtered) c void Example::apply_filter(const cv::Mat& original, cv::Mat& filtered) const{ std::string example_str = this->filter_params.example_string; int example_int = this->filter_params.example_int; - DoExample(original,filtered, example_str, example_int); + apply_example(original,filtered, example_str, example_int); } diff --git a/image-filtering/src/utilities.cpp b/image-filtering/src/utilities.cpp index 5eddcb0..be9036d 100644 --- a/image-filtering/src/utilities.cpp +++ b/image-filtering/src/utilities.cpp @@ -209,6 +209,7 @@ void apply_fixed_threshold(const cv::Mat& img, cv::Mat& filtered, int thresh, bo // Params: // - type: CV_DIST_L1, CV_DIST_L2, CV_DIST_C, ... // - maskSize: 3, 5, or CV_DIST_MASK_PRECISE (0) +// Best regards, ChatGPT void distance_field(const cv::Mat& binObstacles, cv::Mat& dist, bool obstaclesAreWhite, @@ -262,7 +263,7 @@ void distance_field(const cv::Mat& binObstacles, // TODO: If you need a helper function define it here like this -void DoExample(const cv::Mat& original, +void apply_example(const cv::Mat& original, cv::Mat& filtered, std::string example_string, int example_int) @@ -321,23 +322,15 @@ void DoExample(const cv::Mat& original, - - - - - - - - // _.-. -// / 66\ +// / 66\ // ( `\ Hi, how you doin :) // |\\ , ,| // __ | \\____/ // ,.--"`-.". / `---' // _.-' '-/ | // _.-" | '-. |_/_ -// ,__.-' _,.--\ \ (( /-\ +// ,__.-' _,.--\ \ (( /-\ // ',_..--' `\ \ \\_ / // `-, ) |\' // | |-.,,-" ( From 213dc8b8706315b2ed0c54509798bbfdbe81f105 Mon Sep 17 00:00:00 2001 From: Thomas Date: Wed, 7 Jan 2026 13:28:47 +0100 Subject: [PATCH 43/53] Maching the read me --- README.md | 23 +++++++++++------------ 1 file changed, 11 insertions(+), 12 deletions(-) diff --git a/README.md b/README.md index fa340e6..7c7278f 100644 --- a/README.md +++ b/README.md @@ -34,7 +34,7 @@ Parameters can be set through a YAML file or dynamically adjusted at runtime. To extend the functionality of the `image_filtering_node` by adding new filters, follow these steps to ensure compatibility and integration with the existing codebase. There should be //TODO comments where you add your filter: -### Step 1: Add filter to Enum +### Step 1: Filter Enum You should define your filtertype in the filtertype enum in [image_processing.hpp](image-filtering/include/image_filters/image_processing.hpp) @@ -51,20 +51,19 @@ enum class FilterType { ``` ### Step 2: Filter string -To access the filter trough the yamal file we need to access it trough a string. You need to add it as a string to parse_filter_type in [image_processing.hpp](image-filtering/include/image_filters/image_processing.hpp) +To access the filter trough the yamal file we need to access it trough a string. You need to add it as a string to map to the Enum in [image_processing.hpp](image-filtering/include/image_filters/image_processing.hpp) ```cpp -inline FilterType parse_filter_type(std::string s) { - s = to_lower(std::move(s)); - if (s == "no_filter") return FilterType::NoFilter; - if (s == "flip") return FilterType::Flip; - if (s == "unsharpening") return FilterType::Unsharpening; +static constexpr std::pair kFilterMap[] = { + {"no_filter", FilterType::NoFilter}, + {"flip", FilterType::Flip}, + {"unsharpening", FilterType::Unsharpening}, ... - // Add your filter type here: - return FilterType::Unknown; - -} + // Add your filter here + {"example", FilterType::Example}, + {"unknown", FilterType::Unknown} +}; ``` @@ -82,7 +81,7 @@ struct ExampleParams{ ### Step 4: Add filter class -Add a Class for your filter inhereting from the Filter class, with the same exact structure as shown below. This should also be in [image_processing.hpp](image-filtering/include/image_filters/image_processing.hpp) +Below the filter parameters add a Class for your filter inhereting from the Filter class, with the same structure as shown below. This should also be in [image_processing.hpp](image-filtering/include/image_filters/image_processing.hpp) ```cpp class Example: public Filter{ public: From 61844d9900f64d6347df605e2cb3c54ae0e7a02c Mon Sep 17 00:00:00 2001 From: Thomas Date: Wed, 7 Jan 2026 13:43:55 +0100 Subject: [PATCH 44/53] sadest chapter of vortex history: unfortunately the dinosaur from toy story has to go (cant merge to main with it) --- image-filtering/src/utilities.cpp | 22 ---------------------- 1 file changed, 22 deletions(-) diff --git a/image-filtering/src/utilities.cpp b/image-filtering/src/utilities.cpp index be9036d..8ee7510 100644 --- a/image-filtering/src/utilities.cpp +++ b/image-filtering/src/utilities.cpp @@ -318,25 +318,3 @@ void apply_example(const cv::Mat& original, cv::putText(filtered, line2, cv::Point(x2c, y2c), fontFace, fontScale, cv::Scalar(255), thickness, lineType); } - - - - -// _.-. -// / 66\ -// ( `\ Hi, how you doin :) -// |\\ , ,| -// __ | \\____/ -// ,.--"`-.". / `---' -// _.-' '-/ | -// _.-" | '-. |_/_ -// ,__.-' _,.--\ \ (( /-\ -// ',_..--' `\ \ \\_ / -// `-, ) |\' -// | |-.,,-" ( -// | | `\ `',_ -// ) \ \,(\(\-' -// \ `-,_ -// \_(\-(\`-` -// " " - From 8904c7ccad6014a0d5d9a252dfd6142295f85c5c Mon Sep 17 00:00:00 2001 From: Thomas Date: Wed, 7 Jan 2026 13:55:22 +0100 Subject: [PATCH 45/53] spelling mistake --- README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/README.md b/README.md index 7c7278f..22f396c 100644 --- a/README.md +++ b/README.md @@ -168,4 +168,4 @@ void ImageFilteringNode::set_filter_params() { #### Adding Helperfunctions -If you need helperfunctions for your filter, you can add the declaration to [utilities.hpp](image-filtering/include/image_filters/utilities.hpp), and then add the function defenition to [utilities.cpp](image-filtering/src/utilities.cpp). There wil be TODO comments where you can add them. These functions are alredy included in the image_prosessing files. \ No newline at end of file +If you need helperfunctions for your filter, you can add the declaration to [utilities.hpp](image-filtering/include/image_filters/utilities.hpp), and then add the function defenition to [utilities.cpp](image-filtering/src/utilities.cpp). There will be TODO comments where you can add them. These functions are alredy included in the image_prosessing files. \ No newline at end of file From 82a5e2a7068cc059db503058255991eb40d510ca Mon Sep 17 00:00:00 2001 From: Thomas Date: Wed, 7 Jan 2026 14:33:02 +0100 Subject: [PATCH 46/53] Spelling --- README.md | 14 +++++++------- image-filtering/config/image_filtering_params.yaml | 2 +- .../include/image_filters/image_processing.hpp | 4 ++-- .../include/image_filters/utilities.hpp | 4 ++-- image-filtering/src/utilities.cpp | 2 +- 5 files changed, 13 insertions(+), 13 deletions(-) diff --git a/README.md b/README.md index 22f396c..fdc230a 100644 --- a/README.md +++ b/README.md @@ -51,7 +51,7 @@ enum class FilterType { ``` ### Step 2: Filter string -To access the filter trough the yamal file we need to access it trough a string. You need to add it as a string to map to the Enum in [image_processing.hpp](image-filtering/include/image_filters/image_processing.hpp) +To access the filter through the yaml file we need to access it through a string. You need to add it as a string to map to the Enum in [image_processing.hpp](image-filtering/include/image_filters/image_processing.hpp) ```cpp static constexpr std::pair kFilterMap[] = { @@ -81,7 +81,7 @@ struct ExampleParams{ ### Step 4: Add filter class -Below the filter parameters add a Class for your filter inhereting from the Filter class, with the same structure as shown below. This should also be in [image_processing.hpp](image-filtering/include/image_filters/image_processing.hpp) +Below the filter parameters add a Class for your filter inheriting from the Filter class, with the same structure as shown below. This should also be in [image_processing.hpp](image-filtering/include/image_filters/image_processing.hpp) ```cpp class Example: public Filter{ public: @@ -91,13 +91,13 @@ class Example: public Filter{ ExampleParams filter_params; }; ``` -Here you can add other filter spesific stuff like storing variables that needs to change between runs and so on. +Here you can add other filter specific stuff like storing variables that need to change between runs and so on. ### Step 5: Create the Filter Function -Implement your filter function in [image_processing.cpp](image-filtering/src/image_processing.cpp). This function should take inn the `cv::Mat` objects for the input and the filtered image, and change the filtered one according to your need. +Implement your filter function in [image_processing.cpp](image-filtering/src/image_processing.cpp). This function should take in the `cv::Mat` objects for the input and the filtered image, and change the filtered one according to your needs. ```cpp void Example::apply_filter(const cv::Mat& original, cv::Mat& filtered) const{ @@ -106,7 +106,7 @@ void Example::apply_filter(const cv::Mat& original, cv::Mat& filtered) const{ DoExample(original,filtered, example_str, example_int); } ``` -*If you need a helperfunction go to the [helperfunction](#adding-helperfunctions) section of this page. +*If you need a helper function go to the [helperfunction](#adding-helper-functions) section of this page. ### Step 6: Add to config file @@ -166,6 +166,6 @@ void ImageFilteringNode::set_filter_params() { -#### Adding Helperfunctions +#### Adding Helper functions -If you need helperfunctions for your filter, you can add the declaration to [utilities.hpp](image-filtering/include/image_filters/utilities.hpp), and then add the function defenition to [utilities.cpp](image-filtering/src/utilities.cpp). There will be TODO comments where you can add them. These functions are alredy included in the image_prosessing files. \ No newline at end of file +If you need helper functions for your filter, you can add the declaration to [utilities.hpp](image-filtering/include/image_filters/utilities.hpp), and then add the function definition to [utilities.cpp](image-filtering/src/utilities.cpp). There will be TODO comments where you can add them. These functions are already included in the image_processing files. \ No newline at end of file diff --git a/image-filtering/config/image_filtering_params.yaml b/image-filtering/config/image_filtering_params.yaml index f86472b..172f180 100644 --- a/image-filtering/config/image_filtering_params.yaml +++ b/image-filtering/config/image_filtering_params.yaml @@ -31,7 +31,7 @@ erosion_size: 2 dilation_size: 2 overlap: - percentage_threshold: 20.0 # Percentage (0-100) to cap the pixcel intensity differance + 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 kernel_size: 3 # must be odd >= 1 threshold: 100 # [0, 255] diff --git a/image-filtering/include/image_filters/image_processing.hpp b/image-filtering/include/image_filters/image_processing.hpp index 2631fb5..0ce9f9d 100644 --- a/image-filtering/include/image_filters/image_processing.hpp +++ b/image-filtering/include/image_filters/image_processing.hpp @@ -61,13 +61,13 @@ inline FilterType parse_filter_type(std::string s) { for (auto [name, type] : kFilterMap) { if (s == name) return type; } - std::cout << "\033[33m No string connected to that filtertype: '" << s << "'. This might be misspelling or you need to add the filtertype to kFilterMap in image_processing.hpp\033[0m"; + std::cout << "\033[33m No string connected to that filter type: '" << s << "'. This might be misspelling or you need to add the filter type to kFilterMap in image_processing.hpp\033[0m"; return FilterType::Unknown; } inline std::string_view filtertype_to_string(FilterType t) { for (auto [name, type] : kFilterMap) {if (t == type) return name;} - std::cout << "\033[33m No string connected to your filtertype. To fix this add the string and filtertype to kFilterMap\033[0m"; + std::cout << "\033[33m No string connected to your filter type. To fix this add the string and filter type to kFilterMap\033[0m"; return "unknown"; } diff --git a/image-filtering/include/image_filters/utilities.hpp b/image-filtering/include/image_filters/utilities.hpp index 08c36ac..3a1198a 100644 --- a/image-filtering/include/image_filters/utilities.hpp +++ b/image-filtering/include/image_filters/utilities.hpp @@ -21,7 +21,7 @@ int apply_otsu(const cv::Mat& gray8u, cv::Mat& out, bool invert = false, double // Performs morphological erosion (shrinks bright regions / removes small white noise) void apply_erosion(const cv::Mat& src, cv::Mat& dst, int size, int shape = cv::MORPH_RECT); -// Performs morphological dilation (grows bright regions / fills small holes) opposite of erotion +// Performs morphological dilation (grows bright regions / fills small holes) opposite of erosion void apply_dilation(const cv::Mat& src, cv::Mat& dst, int size, int shape=cv::MORPH_RECT); // Applies a median blur with `kernel_size` to reduce salt-and-pepper noise while preserving edges. @@ -38,7 +38,7 @@ void apply_fixed_threshold(const cv::Mat& img, cv::Mat& filtered, int thresh, bo // This does not work properly (Chat has failed me) -void distance_field(const cv::Mat& binObstacles, cv::Mat& dist, bool obstaclesAreWhite = true, int type = cv::DIST_L2, int maskSize = 3); // DIST_L2 is normal euclidian +void distance_field(const cv::Mat& binObstacles, cv::Mat& dist, bool obstaclesAreWhite = true, int type = cv::DIST_L2, int maskSize = 3); // DIST_L2 is normal euclidean diff --git a/image-filtering/src/utilities.cpp b/image-filtering/src/utilities.cpp index 8ee7510..a0c0a49 100644 --- a/image-filtering/src/utilities.cpp +++ b/image-filtering/src/utilities.cpp @@ -52,7 +52,7 @@ double computeAutoGammaFromMean(const cv::Mat& image) { // Auto-choose a gamma so dark images get lifted and bright images get toned down (expects mono8) // - It sets the mean intensity to 255/2 ≃ 128 -// - The weight makes makes all the values weeker(<1) or stronger(>1) +// - The correction weight makes all the values weaker(<1) or stronger(>1) void apply_auto_gamma(cv::Mat& image, double correction_weight) { double gamma = computeAutoGammaFromMean(image) * correction_weight; applyGammaLUT(image, gamma); From 5490448be347a0f933549ca5af5204baaa184cc2 Mon Sep 17 00:00:00 2001 From: Thomas Date: Wed, 7 Jan 2026 15:15:36 +0100 Subject: [PATCH 47/53] Fixing with precommit --- .gitignore | 2 +- README.md | 20 +- .../config/image_filtering_params.yaml | 10 +- image-filtering/image_processing.hpp | 301 ++++++++-------- .../image_filters/image_filtering_ros.hpp | 3 +- .../image_filters/image_processing.hpp | 308 +++++++++-------- .../include/image_filters/utilities.hpp | 70 ++-- image-filtering/src/image_filtering_ros.cpp | 325 +++++++++--------- image-filtering/src/image_processing.cpp | 100 +++--- image-filtering/src/utilities.cpp | 181 +++++----- 10 files changed, 668 insertions(+), 652 deletions(-) diff --git a/.gitignore b/.gitignore index 6c03073..e429bd6 100644 --- a/.gitignore +++ b/.gitignore @@ -107,4 +107,4 @@ CATKIN_IGNORE .vscode/ log/ -image-filtering/.vscode/* \ No newline at end of file +image-filtering/.vscode/* diff --git a/README.md b/README.md index fdc230a..1e2a04c 100644 --- a/README.md +++ b/README.md @@ -39,12 +39,12 @@ To extend the functionality of the `image_filtering_node` by adding new filters, You should define your filtertype in the filtertype enum in [image_processing.hpp](image-filtering/include/image_filters/image_processing.hpp) ```cpp -enum class FilterType { - NoFilter, - Flip, - Unsharpening, - Erosion, - Dilation, +enum class FilterType { + NoFilter, + Flip, + Unsharpening, + Erosion, + Dilation, ... // Add your filter here }; @@ -59,7 +59,7 @@ static constexpr std::pair kFilterMap[] = { {"flip", FilterType::Flip}, {"unsharpening", FilterType::Unsharpening}, ... - + // Add your filter here {"example", FilterType::Example}, {"unknown", FilterType::Unknown} @@ -72,7 +72,7 @@ static constexpr std::pair kFilterMap[] = { Each filter should have its own set of parameters encapsulated in a structure. Define this structure within [image_processing.hpp](image-filtering/include/image_filters/image_processing.hpp). ```cpp -struct ExampleParams{ +struct ExampleParams{ // Add necessary filter parameters here int example_int; std::string example_string; @@ -116,7 +116,7 @@ In the [image_filtering_params.yaml](image-filtering/config/image_filtering_para ```yaml filter_params: filter_type: "example" - + flip: flip_code: 1 ... @@ -168,4 +168,4 @@ void ImageFilteringNode::set_filter_params() { #### Adding Helper functions -If you need helper functions for your filter, you can add the declaration to [utilities.hpp](image-filtering/include/image_filters/utilities.hpp), and then add the function definition to [utilities.cpp](image-filtering/src/utilities.cpp). There will be TODO comments where you can add them. These functions are already included in the image_processing files. \ No newline at end of file +If you need helper functions for your filter, you can add the declaration to [utilities.hpp](image-filtering/include/image_filters/utilities.hpp), and then add the function definition to [utilities.cpp](image-filtering/src/utilities.cpp). There will be TODO comments where you can add them. These functions are already included in the image_processing files. diff --git a/image-filtering/config/image_filtering_params.yaml b/image-filtering/config/image_filtering_params.yaml index 172f180..2c34393 100644 --- a/image-filtering/config/image_filtering_params.yaml +++ b/image-filtering/config/image_filtering_params.yaml @@ -2,9 +2,9 @@ ros__parameters: sub_topic: "/fls_publisher/display_mono" pub_topic: "/filtered_image" - input_encoding: "mono8" + input_encoding: "mono8" output_encoding: "mono8" - + filter_params: filter_type: "example" flip: @@ -32,15 +32,15 @@ dilation_size: 2 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 + median_binary: # finds the median of each n x n square around each pixel kernel_size: 3 # must be odd >= 1 threshold: 100 # [0, 255] invert: false - binary: + binary: threshold: 20. # in percent maxval: 255. invert: true - + # TODO: add your filterparameters here example: example_int: 1 diff --git a/image-filtering/image_processing.hpp b/image-filtering/image_processing.hpp index 7fb400c..78b0920 100644 --- a/image-filtering/image_processing.hpp +++ b/image-filtering/image_processing.hpp @@ -10,135 +10,146 @@ #include #include - -enum class FilterType { // TODO: Add filters here - NoFilter, - Flip, - Unsharpening, - Erosion, - Dilation, - WhiteBalancing, - Ebus, - Otsu, - Overlap, - MedianBinary, - Binary, - Unknown +enum class FilterType { // TODO: Add filters here + NoFilter, + Flip, + Unsharpening, + Erosion, + Dilation, + WhiteBalancing, + Ebus, + Otsu, + Overlap, + MedianBinary, + Binary, + Unknown }; inline std::string to_lower(std::string s) { - for (auto& c : s) c = static_cast(std::tolower(c)); - return s; + for (auto& c : s) + c = static_cast(std::tolower(c)); + return s; } -inline FilterType parse_filter_type(std::string s) { // TODO: Also add filter-type here - s = to_lower(std::move(s)); - if (s == "no_filter") return FilterType::NoFilter; - if (s == "flip") return FilterType::Flip; - if (s == "unsharpening") return FilterType::Unsharpening; - if (s == "erosion") return FilterType::Erosion; - if (s == "dilation") return FilterType::Dilation; - if (s == "white_balancing")return FilterType::WhiteBalancing; - if (s == "ebus") return FilterType::Ebus; - if (s == "otsu") return FilterType::Otsu; - if (s == "overlap") return FilterType::Overlap; - if (s == "median_binary") return FilterType::MedianBinary; - if (s == "binary") return FilterType::Binary; +inline FilterType parse_filter_type( + std::string s) { // TODO: Also add filter-type here + s = to_lower(std::move(s)); + if (s == "no_filter") + return FilterType::NoFilter; + if (s == "flip") + return FilterType::Flip; + if (s == "unsharpening") + return FilterType::Unsharpening; + if (s == "erosion") + return FilterType::Erosion; + if (s == "dilation") + return FilterType::Dilation; + if (s == "white_balancing") + return FilterType::WhiteBalancing; + if (s == "ebus") + return FilterType::Ebus; + if (s == "otsu") + return FilterType::Otsu; + if (s == "overlap") + return FilterType::Overlap; + if (s == "median_binary") + return FilterType::MedianBinary; + if (s == "binary") + return FilterType::Binary; return FilterType::Unknown; } +class Filter { + public: + virtual ~Filter() = default; + virtual void apply_filter(const cv::Mat& original, + cv::Mat& filtered) const = 0; - - -class Filter{ - public: - virtual ~Filter() = default; - virtual void apply_filter(const cv::Mat& original, cv::Mat& filtered) const = 0; - - protected: - Filter() = default; + protected: + Filter() = default; }; - ///////////////////////////// // No filter ///////////////////////////// -struct NoFilterParams{}; +struct NoFilterParams {}; -class NoFilter: public Filter{ - public: - explicit NoFilter() = default;// No parameters to set - void apply_filter(const cv::Mat& original, cv::Mat& filtered) const override{ original.copyTo(filtered); }; +class NoFilter : public Filter { + public: + explicit NoFilter() = default; // No parameters to set + void apply_filter(const cv::Mat& original, + cv::Mat& filtered) const override { + original.copyTo(filtered); + }; }; - //////////////////////////// // Unsharpening ///////////////////////////// -struct UnsharpeningParams{ +struct UnsharpeningParams { int blur_size; }; +class Unsharpening : public Filter { + public: + explicit Unsharpening(UnsharpeningParams params) : filter_params(params) {} + void apply_filter(const cv::Mat& original, + cv::Mat& filtered) const override; -class Unsharpening: public Filter{ - public: - explicit Unsharpening(UnsharpeningParams params): filter_params(params) {} - void apply_filter(const cv::Mat& original, cv::Mat& filtered) const override; - private: - UnsharpeningParams filter_params; + private: + UnsharpeningParams filter_params; }; - - ///////////////////////////// // Sharpening ///////////////////////////// -struct FlipParams{ +struct FlipParams { int flip_code; }; -class Flip: public Filter{ - public: - explicit Flip(FlipParams params): filter_params(params) {} - void apply_filter(const cv::Mat& original, cv::Mat& filtered) const override; - private: - FlipParams filter_params; -}; - - - - +class Flip : public Filter { + public: + explicit Flip(FlipParams params) : filter_params(params) {} + void apply_filter(const cv::Mat& original, + cv::Mat& filtered) const override; + private: + FlipParams filter_params; +}; ///////////////////////////// // Sharpening ///////////////////////////// -struct SharpeningParams{}; +struct SharpeningParams {}; + +class Sharpening : public Filter { + public: + explicit Sharpening(SharpeningParams params) : filter_params(params) {} + void apply_filter(const cv::Mat& original, + cv::Mat& filtered) const override; -class Sharpening: public Filter{ -public: - explicit Sharpening(SharpeningParams params): filter_params(params) {} - void apply_filter(const cv::Mat& original, cv::Mat& filtered) const override; -private: + private: SharpeningParams filter_params; }; ///////////////////////////// // Erosion ///////////////////////////// -struct ErosionParams{ - int kernel_size; // odd > 1 +struct ErosionParams { + int kernel_size; // odd > 1 }; -class Erosion: public Filter{ -public: - explicit Erosion(ErosionParams params): filter_params(params) {} - void apply_filter(const cv::Mat& original, cv::Mat& filtered) const override; -private: +class Erosion : public Filter { + public: + explicit Erosion(ErosionParams params) : filter_params(params) {} + void apply_filter(const cv::Mat& original, + cv::Mat& filtered) const override; + + private: ErosionParams filter_params; }; @@ -146,15 +157,17 @@ class Erosion: public Filter{ // Dilation ///////////////////////////// -struct DilationParams{ +struct DilationParams { int kernel_size = 3; }; -class Dilation: public Filter{ -public: - explicit Dilation(DilationParams params): filter_params(params) {} - void apply_filter(const cv::Mat& original, cv::Mat& filtered) const override; -private: +class Dilation : public Filter { + public: + explicit Dilation(DilationParams params) : filter_params(params) {} + void apply_filter(const cv::Mat& original, + cv::Mat& filtered) const override; + + private: DilationParams filter_params; }; @@ -162,15 +175,17 @@ class Dilation: public Filter{ // White Balance ///////////////////////////// -struct WhiteBalanceParams{ +struct WhiteBalanceParams { double contrast_percentage; }; -class WhiteBalance: public Filter{ -public: - explicit WhiteBalance(WhiteBalanceParams params): filter_params(params) {} - void apply_filter(const cv::Mat& original, cv::Mat& filtered) const override; -private: +class WhiteBalance : public Filter { + public: + explicit WhiteBalance(WhiteBalanceParams params) : filter_params(params) {} + void apply_filter(const cv::Mat& original, + cv::Mat& filtered) const override; + + private: WhiteBalanceParams filter_params; }; @@ -178,17 +193,19 @@ class WhiteBalance: public Filter{ // Ebus (dilation + unsharpening combo) ///////////////////////////// -struct EbusParams{ +struct EbusParams { int erosion_size; int blur_size; int mask_weight; }; -class Ebus: public Filter{ -public: - explicit Ebus(EbusParams params): filter_params(params) {} - void apply_filter(const cv::Mat& original, cv::Mat& filtered) const override; -private: +class Ebus : public Filter { + public: + explicit Ebus(EbusParams params) : filter_params(params) {} + void apply_filter(const cv::Mat& original, + cv::Mat& filtered) const override; + + private: EbusParams filter_params; }; @@ -196,7 +213,7 @@ class Ebus: public Filter{ // Otsu Segmentation ///////////////////////////// -struct OtsuSegmentationParams{ +struct OtsuSegmentationParams { bool gamma_auto_correction; double gamma_auto_correction_weight; bool otsu_segmentation; @@ -207,11 +224,13 @@ struct OtsuSegmentationParams{ int dilation_size; }; -class OtsuSegmentation: public Filter{ -public: - explicit OtsuSegmentation(OtsuSegmentationParams params): filter_params(params) {} +class OtsuSegmentation : public Filter { + public: + explicit OtsuSegmentation(OtsuSegmentationParams params) + : filter_params(params) {} void apply_filter(const cv::Mat& original, cv::Mat& output) const override; -private: + + private: OtsuSegmentationParams filter_params; }; @@ -219,15 +238,17 @@ class OtsuSegmentation: public Filter{ // Overlap (blend/composite) ///////////////////////////// -struct OverlapParams{ +struct OverlapParams { double percentage_threshold; }; -class Overlap: public Filter{ -public: - explicit Overlap(OverlapParams params): filter_params(params) {} - void apply_filter(const cv::Mat& original, cv::Mat& filtered) const override; -private: +class Overlap : public Filter { + public: + explicit Overlap(OverlapParams params) : filter_params(params) {} + void apply_filter(const cv::Mat& original, + cv::Mat& filtered) const override; + + private: OverlapParams filter_params; }; @@ -235,17 +256,19 @@ class Overlap: public Filter{ // Median + Binary ///////////////////////////// -struct MedianBinaryParams{ +struct MedianBinaryParams { int kernel_size; int threshold; bool invert; }; -class MedianBinary: public Filter{ -public: - explicit MedianBinary(MedianBinaryParams params): filter_params(params) {} - void apply_filter(const cv::Mat& original, cv::Mat& filtered) const override; -private: +class MedianBinary : public Filter { + public: + explicit MedianBinary(MedianBinaryParams params) : filter_params(params) {} + void apply_filter(const cv::Mat& original, + cv::Mat& filtered) const override; + + private: MedianBinaryParams filter_params; }; @@ -253,25 +276,23 @@ class MedianBinary: public Filter{ // Binary Threshold ///////////////////////////// -struct BinaryThresholdParams{ +struct BinaryThresholdParams { double threshold; double maxval; bool invert; }; -class BinaryThreshold: public Filter{ -public: - explicit BinaryThreshold(BinaryThresholdParams params): filter_params(params) {} - void apply_filter(const cv::Mat& original, cv::Mat& filtered) const override; -private: +class BinaryThreshold : public Filter { + public: + explicit BinaryThreshold(BinaryThresholdParams params) + : filter_params(params) {} + void apply_filter(const cv::Mat& original, + cv::Mat& filtered) const override; + + private: BinaryThresholdParams filter_params; }; - - - - - ///////////////////////////// // Example ///////////////////////////// @@ -279,32 +300,18 @@ class BinaryThreshold: public Filter{ // TODO: add this structure for your filter // Example: -struct ExampleParams{ // Add filter variables here +struct ExampleParams { // Add filter variables here int example_variable; std::string example_string; }; -class Example: public Filter{ - public: - explicit Example(ExampleParams params): filter_params(params) {} - void apply_filter(const cv::Mat& original, cv::Mat& filtered) const override; // This is the filter itself - private: - ExampleParams filter_params; +class Example : public Filter { + public: + explicit Example(ExampleParams params) : filter_params(params) {} + void apply_filter(const cv::Mat& original, cv::Mat& filtered) + const override; // This is the filter itself + private: + ExampleParams filter_params; }; - - - - - - - - - - - - #endif // IMAGE_PROCESSING_HPP - - - diff --git a/image-filtering/include/image_filters/image_filtering_ros.hpp b/image-filtering/include/image_filters/image_filtering_ros.hpp index 064a9d9..645f80e 100644 --- a/image-filtering/include/image_filters/image_filtering_ros.hpp +++ b/image-filtering/include/image_filters/image_filtering_ros.hpp @@ -1,7 +1,7 @@ #ifndef IMAGE_FILTERING_ROS_HPP #define IMAGE_FILTERING_ROS_HPP -#include // for jazzy this has to be hpp and for humble it has to be h +#include // for jazzy this has to be hpp and for humble it has to be h #include #include #include @@ -101,7 +101,6 @@ class ImageFilteringNode : public rclcpp::Node { * */ std::unique_ptr filter_ptr; - }; #endif // IMAGE_FILTERING_ROS_HPP diff --git a/image-filtering/include/image_filters/image_processing.hpp b/image-filtering/include/image_filters/image_processing.hpp index 0ce9f9d..dc4528e 100644 --- a/image-filtering/include/image_filters/image_processing.hpp +++ b/image-filtering/include/image_filters/image_processing.hpp @@ -10,158 +10,161 @@ #include #include - - -enum class FilterType { // TODO: Add filters here - NoFilter, - Flip, - Unsharpening, - Erosion, - Dilation, - WhiteBalancing, - Ebus, - Otsu, - Overlap, - MedianBinary, - Binary, - - Example, - Unknown +enum class FilterType { // TODO: Add filters here + NoFilter, + Flip, + Unsharpening, + Erosion, + Dilation, + WhiteBalancing, + Ebus, + Otsu, + Overlap, + MedianBinary, + Binary, + + Example, + Unknown }; static constexpr std::pair kFilterMap[] = { - {"no_filter", FilterType::NoFilter}, - {"flip", FilterType::Flip}, - {"unsharpening", FilterType::Unsharpening}, - {"erosion", FilterType::Erosion}, - {"dilation", FilterType::Dilation}, + {"no_filter", FilterType::NoFilter}, + {"flip", FilterType::Flip}, + {"unsharpening", FilterType::Unsharpening}, + {"erosion", FilterType::Erosion}, + {"dilation", FilterType::Dilation}, {"white_balancing", FilterType::WhiteBalancing}, - {"ebus", FilterType::Ebus}, - {"otsu", FilterType::Otsu}, - {"overlap", FilterType::Overlap}, - {"median_binary", FilterType::MedianBinary}, - {"binary", FilterType::Binary}, - - // TODO: Also add your filter here - {"example", FilterType::Example}, - {"unknown", FilterType::Unknown} -}; + {"ebus", FilterType::Ebus}, + {"otsu", FilterType::Otsu}, + {"overlap", FilterType::Overlap}, + {"median_binary", FilterType::MedianBinary}, + {"binary", FilterType::Binary}, + // TODO: Also add your filter here + {"example", FilterType::Example}, + {"unknown", FilterType::Unknown}}; inline std::string to_lower(std::string s) { - for (char& ch : s) { - ch = static_cast(std::tolower(static_cast(ch))); - } - return s; + for (char& ch : s) { + ch = static_cast(std::tolower(static_cast(ch))); + } + return s; } inline FilterType parse_filter_type(std::string s) { s = to_lower(std::move(s)); for (auto [name, type] : kFilterMap) { - if (s == name) return type; + if (s == name) + return type; } - std::cout << "\033[33m No string connected to that filter type: '" << s << "'. This might be misspelling or you need to add the filter type to kFilterMap in image_processing.hpp\033[0m"; + std::cout << "\033[33m No string connected to that filter type: '" << s + << "'. This might be misspelling or you need to add the filter " + "type to kFilterMap in image_processing.hpp\033[0m"; return FilterType::Unknown; } inline std::string_view filtertype_to_string(FilterType t) { - for (auto [name, type] : kFilterMap) {if (t == type) return name;} - std::cout << "\033[33m No string connected to your filter type. To fix this add the string and filter type to kFilterMap\033[0m"; + for (auto [name, type] : kFilterMap) { + if (t == type) + return name; + } + std::cout << "\033[33m No string connected to your filter type. To fix " + "this add the string and filter type to kFilterMap\033[0m"; return "unknown"; } +class Filter { + public: + virtual ~Filter() = default; + virtual void apply_filter(const cv::Mat& original, + cv::Mat& filtered) const = 0; - -class Filter{ - public: - virtual ~Filter() = default; - virtual void apply_filter(const cv::Mat& original, cv::Mat& filtered) const = 0; - - protected: - Filter() = default; + protected: + Filter() = default; }; - ///////////////////////////// // No filter ///////////////////////////// -struct NoFilterParams{}; +struct NoFilterParams {}; -class NoFilter: public Filter{ - public: - explicit NoFilter() = default;// No parameters to set - void apply_filter(const cv::Mat& original, cv::Mat& filtered) const override{ original.copyTo(filtered); }; +class NoFilter : public Filter { + public: + explicit NoFilter() = default; // No parameters to set + void apply_filter(const cv::Mat& original, + cv::Mat& filtered) const override { + original.copyTo(filtered); + }; }; - //////////////////////////// // Unsharpening ///////////////////////////// -struct UnsharpeningParams{ +struct UnsharpeningParams { int blur_size; }; +class Unsharpening : public Filter { + public: + explicit Unsharpening(UnsharpeningParams params) : filter_params(params) {} + void apply_filter(const cv::Mat& original, + cv::Mat& filtered) const override; -class Unsharpening: public Filter{ - public: - explicit Unsharpening(UnsharpeningParams params): filter_params(params) {} - void apply_filter(const cv::Mat& original, cv::Mat& filtered) const override; - private: - UnsharpeningParams filter_params; + private: + UnsharpeningParams filter_params; }; - - ///////////////////////////// // Sharpening ///////////////////////////// -struct FlipParams{ +struct FlipParams { int flip_code; }; -class Flip: public Filter{ - public: - explicit Flip(FlipParams params): filter_params(params) {} - void apply_filter(const cv::Mat& original, cv::Mat& filtered) const override; - private: - FlipParams filter_params; -}; - - - - +class Flip : public Filter { + public: + explicit Flip(FlipParams params) : filter_params(params) {} + void apply_filter(const cv::Mat& original, + cv::Mat& filtered) const override; + private: + FlipParams filter_params; +}; ///////////////////////////// // Sharpening ///////////////////////////// -struct SharpeningParams{}; +struct SharpeningParams {}; + +class Sharpening : public Filter { + public: + explicit Sharpening(SharpeningParams params) : filter_params(params) {} + void apply_filter(const cv::Mat& original, + cv::Mat& filtered) const override; -class Sharpening: public Filter{ -public: - explicit Sharpening(SharpeningParams params): filter_params(params) {} - void apply_filter(const cv::Mat& original, cv::Mat& filtered) const override; -private: + private: SharpeningParams filter_params; }; ///////////////////////////// // Erosion ///////////////////////////// -struct ErosionParams{ - int kernel_size; // odd > 1 +struct ErosionParams { + int kernel_size; // odd > 1 }; -class Erosion: public Filter{ -public: - explicit Erosion(ErosionParams params): filter_params(params) {} - void apply_filter(const cv::Mat& original, cv::Mat& filtered) const override; -private: +class Erosion : public Filter { + public: + explicit Erosion(ErosionParams params) : filter_params(params) {} + void apply_filter(const cv::Mat& original, + cv::Mat& filtered) const override; + + private: ErosionParams filter_params; }; @@ -169,15 +172,17 @@ class Erosion: public Filter{ // Dilation ///////////////////////////// -struct DilationParams{ +struct DilationParams { int kernel_size = 3; }; -class Dilation: public Filter{ -public: - explicit Dilation(DilationParams params): filter_params(params) {} - void apply_filter(const cv::Mat& original, cv::Mat& filtered) const override; -private: +class Dilation : public Filter { + public: + explicit Dilation(DilationParams params) : filter_params(params) {} + void apply_filter(const cv::Mat& original, + cv::Mat& filtered) const override; + + private: DilationParams filter_params; }; @@ -185,15 +190,17 @@ class Dilation: public Filter{ // White Balance ///////////////////////////// -struct WhiteBalanceParams{ +struct WhiteBalanceParams { double contrast_percentage; }; -class WhiteBalance: public Filter{ -public: - explicit WhiteBalance(WhiteBalanceParams params): filter_params(params) {} - void apply_filter(const cv::Mat& original, cv::Mat& filtered) const override; -private: +class WhiteBalance : public Filter { + public: + explicit WhiteBalance(WhiteBalanceParams params) : filter_params(params) {} + void apply_filter(const cv::Mat& original, + cv::Mat& filtered) const override; + + private: WhiteBalanceParams filter_params; }; @@ -201,17 +208,19 @@ class WhiteBalance: public Filter{ // Ebus (dilation + unsharpening combo) ///////////////////////////// -struct EbusParams{ +struct EbusParams { int erosion_size; int blur_size; int mask_weight; }; -class Ebus: public Filter{ -public: - explicit Ebus(EbusParams params): filter_params(params) {} - void apply_filter(const cv::Mat& original, cv::Mat& filtered) const override; -private: +class Ebus : public Filter { + public: + explicit Ebus(EbusParams params) : filter_params(params) {} + void apply_filter(const cv::Mat& original, + cv::Mat& filtered) const override; + + private: EbusParams filter_params; }; @@ -219,7 +228,7 @@ class Ebus: public Filter{ // Otsu Segmentation ///////////////////////////// -struct OtsuSegmentationParams{ +struct OtsuSegmentationParams { bool gamma_auto_correction; double gamma_auto_correction_weight; bool otsu_segmentation; @@ -230,11 +239,13 @@ struct OtsuSegmentationParams{ int dilation_size; }; -class OtsuSegmentation: public Filter{ -public: - explicit OtsuSegmentation(OtsuSegmentationParams params): filter_params(params) {} +class OtsuSegmentation : public Filter { + public: + explicit OtsuSegmentation(OtsuSegmentationParams params) + : filter_params(params) {} void apply_filter(const cv::Mat& original, cv::Mat& output) const override; -private: + + private: OtsuSegmentationParams filter_params; }; @@ -243,17 +254,18 @@ class OtsuSegmentation: public Filter{ ///////////////////////////// struct OverlapParams { - double percentage_threshold; // 0..100 (percent) + double percentage_threshold; // 0..100 (percent) }; class Overlap : public Filter { -public: + public: explicit Overlap(OverlapParams params) : filter_params(params), has_prev(false) {} - void apply_filter(const cv::Mat& original, cv::Mat& filtered) const override; + void apply_filter(const cv::Mat& original, + cv::Mat& filtered) const override; -private: + private: OverlapParams filter_params; // Cached previous mono8 frame @@ -261,22 +273,23 @@ class Overlap : public Filter { mutable bool has_prev; }; - ///////////////////////////// // Median + Binary ///////////////////////////// -struct MedianBinaryParams{ +struct MedianBinaryParams { int kernel_size; int threshold; bool invert; }; -class MedianBinary: public Filter{ -public: - explicit MedianBinary(MedianBinaryParams params): filter_params(params) {} - void apply_filter(const cv::Mat& original, cv::Mat& filtered) const override; -private: +class MedianBinary : public Filter { + public: + explicit MedianBinary(MedianBinaryParams params) : filter_params(params) {} + void apply_filter(const cv::Mat& original, + cv::Mat& filtered) const override; + + private: MedianBinaryParams filter_params; }; @@ -284,24 +297,23 @@ class MedianBinary: public Filter{ // Binary Threshold ///////////////////////////// -struct BinaryThresholdParams{ +struct BinaryThresholdParams { double threshold; double maxval; bool invert; }; -class BinaryThreshold: public Filter{ -public: - explicit BinaryThreshold(BinaryThresholdParams params): filter_params(params) {} - void apply_filter(const cv::Mat& original, cv::Mat& filtered) const override; -private: +class BinaryThreshold : public Filter { + public: + explicit BinaryThreshold(BinaryThresholdParams params) + : filter_params(params) {} + void apply_filter(const cv::Mat& original, + cv::Mat& filtered) const override; + + private: BinaryThresholdParams filter_params; }; - - - - // TODO: add this structure for your filter ///////////////////////////// @@ -309,32 +321,18 @@ class BinaryThreshold: public Filter{ ///////////////////////////// // Example: -struct ExampleParams{ // Add filter variables here +struct ExampleParams { // Add filter variables here int example_int; std::string example_string; }; -class Example: public Filter{ - public: - explicit Example(ExampleParams params): filter_params(params) {} - void apply_filter(const cv::Mat& original, cv::Mat& filtered) const override; // This is the filter itself - private: - ExampleParams filter_params; +class Example : public Filter { + public: + explicit Example(ExampleParams params) : filter_params(params) {} + void apply_filter(const cv::Mat& original, cv::Mat& filtered) + const override; // This is the filter itself + private: + ExampleParams filter_params; }; - - - - - - - - - - - - #endif // IMAGE_PROCESSING_HPP - - - diff --git a/image-filtering/include/image_filters/utilities.hpp b/image-filtering/include/image_filters/utilities.hpp index 3a1198a..2298cfa 100644 --- a/image-filtering/include/image_filters/utilities.hpp +++ b/image-filtering/include/image_filters/utilities.hpp @@ -8,44 +8,62 @@ #include #include - -// Auto-choose a gamma so dark images get lifted and bright images get toned down (expects mono8) +// Auto-choose a gamma so dark images get lifted and bright images get toned +// down (expects mono8) void apply_auto_gamma(cv::Mat& image, double correction_weight); // Converts a BGR image to grayscale using custom channel weights (wB, wG, wR). -void to_weighted_gray(const cv::Mat& bgr, cv::Mat& gray, double wB, double wG, double wR); +void to_weighted_gray(const cv::Mat& bgr, + cv::Mat& gray, + double wB, + double wG, + double wR); // Applies Otsu’s automatic thresholding and returning the threshold -int apply_otsu(const cv::Mat& gray8u, cv::Mat& out, bool invert = false, double maxval = 255.0); - -// Performs morphological erosion (shrinks bright regions / removes small white noise) -void apply_erosion(const cv::Mat& src, cv::Mat& dst, int size, int shape = cv::MORPH_RECT); - -// Performs morphological dilation (grows bright regions / fills small holes) opposite of erosion -void apply_dilation(const cv::Mat& src, cv::Mat& dst, int size, int shape=cv::MORPH_RECT); - -// Applies a median blur with `kernel_size` to reduce salt-and-pepper noise while preserving edges. +int apply_otsu(const cv::Mat& gray8u, + cv::Mat& out, + bool invert = false, + double maxval = 255.0); + +// Performs morphological erosion (shrinks bright regions / removes small white +// noise) +void apply_erosion(const cv::Mat& src, + cv::Mat& dst, + int size, + int shape = cv::MORPH_RECT); + +// Performs morphological dilation (grows bright regions / fills small holes) +// opposite of erosion +void apply_dilation(const cv::Mat& src, + cv::Mat& dst, + int size, + int shape = cv::MORPH_RECT); + +// Applies a median blur with `kernel_size` to reduce salt-and-pepper noise +// while preserving edges. void apply_median(const cv::Mat& original, cv::Mat& filtered, int kernel_size); - - // Apply a fixed binary threshold. // - Accepts grayscale or color input (auto-converts to gray). // - Ensures 8-bit depth for thresholding. // - Returns a 0/255 mask (CV_8U). // - Set `invert=true` to get white background & black foreground. -void apply_fixed_threshold(const cv::Mat& img, cv::Mat& filtered, int thresh, bool invert = false); - - -// This does not work properly (Chat has failed me) -void distance_field(const cv::Mat& binObstacles, cv::Mat& dist, bool obstaclesAreWhite = true, int type = cv::DIST_L2, int maskSize = 3); // DIST_L2 is normal euclidean - - - - - +void apply_fixed_threshold(const cv::Mat& img, + cv::Mat& filtered, + int thresh, + bool invert = false); + +// This does not work properly (Chat has failed me) +void distance_field(const cv::Mat& binObstacles, + cv::Mat& dist, + bool obstaclesAreWhite = true, + int type = cv::DIST_L2, + int maskSize = 3); // DIST_L2 is normal euclidean // TODO: If you need a helper function have the signature here -void apply_example(const cv::Mat& original, cv::Mat& filtered, std::string example_string, int example_int); +void apply_example(const cv::Mat& original, + cv::Mat& filtered, + std::string example_string, + int example_int); -#endif \ No newline at end of file +#endif diff --git a/image-filtering/src/image_filtering_ros.cpp b/image-filtering/src/image_filtering_ros.cpp index 8a0ca79..7168efa 100644 --- a/image-filtering/src/image_filtering_ros.cpp +++ b/image-filtering/src/image_filtering_ros.cpp @@ -43,9 +43,10 @@ void ImageFilteringNode::declare_parameters() { this->declare_parameter("filter_params.otsu.gsc_weight_b"); this->declare_parameter("filter_params.otsu.erosion_size"); this->declare_parameter("filter_params.otsu.dilation_size"); - - this->declare_parameter("filter_params.overlap.percentage_threshold"); - + + this->declare_parameter( + "filter_params.overlap.percentage_threshold"); + this->declare_parameter("filter_params.binary.threshold"); this->declare_parameter("filter_params.binary.maxval"); this->declare_parameter("filter_params.binary.invert"); @@ -53,183 +54,193 @@ void ImageFilteringNode::declare_parameters() { this->declare_parameter("filter_params.median_binary.kernel_size"); this->declare_parameter("filter_params.median_binary.threshold"); this->declare_parameter("filter_params.median_binary.invert"); - // TODO: Declare parameters set for your filter here this->declare_parameter("filter_params.example.example_int"); - this->declare_parameter("filter_params.example.example_string"); - + this->declare_parameter( + "filter_params.example.example_string"); } void ImageFilteringNode::set_filter_params() { - std::string filter_type_string = this->get_parameter("filter_params.filter_type").as_string(); + std::string filter_type_string = + this->get_parameter("filter_params.filter_type").as_string(); FilterType filter_type = parse_filter_type(filter_type_string); - - switch (filter_type) - { - case FilterType::Unknown: - { - spdlog::warn("\033[33mInvalid filter type received: {}. Using default: no_filter.\033[0m", filter_type_string); - filter_type = FilterType::NoFilter; - [[fallthrough]]; - } - - case FilterType::NoFilter: - { - filter_ptr = std::make_unique(); - break; - } - case FilterType::Unsharpening: - { - UnsharpeningParams params; - params.blur_size = this->get_parameter("filter_params.unsharpening.blur_size").as_int(); - - filter_ptr = std::make_unique(params); - break; - } + switch (filter_type) { + case FilterType::Unknown: { + spdlog::warn( + "\033[33mInvalid filter type received: {}. Using default: " + "no_filter.\033[0m", + filter_type_string); + filter_type = FilterType::NoFilter; + [[fallthrough]]; + } - case FilterType::Flip: - { - FlipParams params; - params.flip_code = - this->get_parameter("filter_params.flip.flip_code").as_int(); + case FilterType::NoFilter: { + filter_ptr = std::make_unique(); + break; + } + case FilterType::Unsharpening: { + UnsharpeningParams params; + params.blur_size = + this->get_parameter("filter_params.unsharpening.blur_size") + .as_int(); + + filter_ptr = std::make_unique(params); + break; + } - filter_ptr = std::make_unique(params); - break; - } + case FilterType::Flip: { + FlipParams params; + params.flip_code = + this->get_parameter("filter_params.flip.flip_code").as_int(); - case FilterType::Erosion: - { - ErosionParams params; - params.kernel_size = - this->get_parameter("filter_params.erosion.size").as_int(); + filter_ptr = std::make_unique(params); + break; + } - filter_ptr = std::make_unique(params); - break; - } + case FilterType::Erosion: { + ErosionParams params; + params.kernel_size = + this->get_parameter("filter_params.erosion.size").as_int(); - case FilterType::Dilation: - { - DilationParams params; - params.kernel_size = - this->get_parameter("filter_params.dilation.size").as_int(); + filter_ptr = std::make_unique(params); + break; + } - filter_ptr = std::make_unique(params); - break; - } + case FilterType::Dilation: { + DilationParams params; + params.kernel_size = + this->get_parameter("filter_params.dilation.size").as_int(); - case FilterType::WhiteBalancing: - { - WhiteBalanceParams params; - params.contrast_percentage = - this->get_parameter("filter_params.white_balancing.contrast_percentage").as_double(); + filter_ptr = std::make_unique(params); + break; + } - filter_ptr = std::make_unique(params); - break; - } + case FilterType::WhiteBalancing: { + WhiteBalanceParams params; + params.contrast_percentage = + this->get_parameter( + "filter_params.white_balancing.contrast_percentage") + .as_double(); - case FilterType::Ebus: - { - EbusParams params; - params.erosion_size = - this->get_parameter("filter_params.ebus.erosion_size").as_int(); - params.blur_size = - this->get_parameter("filter_params.ebus.blur_size").as_int(); - params.mask_weight = - this->get_parameter("filter_params.ebus.mask_weight").as_int(); - - filter_ptr = std::make_unique(params); - break; - } + filter_ptr = std::make_unique(params); + break; + } - case FilterType::Otsu: - { - OtsuSegmentationParams params; - params.gamma_auto_correction = - this->get_parameter("filter_params.otsu.gamma_auto_correction").as_bool(); - params.gamma_auto_correction_weight = - this->get_parameter("filter_params.otsu.gamma_auto_correction_weight").as_double(); - params.otsu_segmentation = - this->get_parameter("filter_params.otsu.otsu_segmentation").as_bool(); - params.gsc_weight_r = - this->get_parameter("filter_params.otsu.gsc_weight_r").as_double(); - params.gsc_weight_g = - this->get_parameter("filter_params.otsu.gsc_weight_g").as_double(); - params.gsc_weight_b = - this->get_parameter("filter_params.otsu.gsc_weight_b").as_double(); - params.erosion_size = - this->get_parameter("filter_params.otsu.erosion_size").as_int(); - params.dilation_size = - this->get_parameter("filter_params.otsu.dilation_size").as_int(); - - filter_ptr = std::make_unique(params); - break; - } + case FilterType::Ebus: { + EbusParams params; + params.erosion_size = + this->get_parameter("filter_params.ebus.erosion_size").as_int(); + params.blur_size = + this->get_parameter("filter_params.ebus.blur_size").as_int(); + params.mask_weight = + this->get_parameter("filter_params.ebus.mask_weight").as_int(); + + filter_ptr = std::make_unique(params); + break; + } - case FilterType::Overlap: - { - OverlapParams params; - params.percentage_threshold = - this->get_parameter("filter_params.overlap.percentage_threshold").as_double(); + case FilterType::Otsu: { + OtsuSegmentationParams params; + params.gamma_auto_correction = + this->get_parameter("filter_params.otsu.gamma_auto_correction") + .as_bool(); + params.gamma_auto_correction_weight = + this->get_parameter( + "filter_params.otsu.gamma_auto_correction_weight") + .as_double(); + params.otsu_segmentation = + this->get_parameter("filter_params.otsu.otsu_segmentation") + .as_bool(); + params.gsc_weight_r = + this->get_parameter("filter_params.otsu.gsc_weight_r") + .as_double(); + params.gsc_weight_g = + this->get_parameter("filter_params.otsu.gsc_weight_g") + .as_double(); + params.gsc_weight_b = + this->get_parameter("filter_params.otsu.gsc_weight_b") + .as_double(); + params.erosion_size = + this->get_parameter("filter_params.otsu.erosion_size").as_int(); + params.dilation_size = + this->get_parameter("filter_params.otsu.dilation_size") + .as_int(); + + filter_ptr = std::make_unique(params); + break; + } - filter_ptr = std::make_unique(params); - break; - } + case FilterType::Overlap: { + OverlapParams params; + params.percentage_threshold = + this->get_parameter( + "filter_params.overlap.percentage_threshold") + .as_double(); - case FilterType::MedianBinary: - { - MedianBinaryParams params; - params.kernel_size = - this->get_parameter("filter_params.median_binary.kernel_size").as_int(); - params.threshold = - this->get_parameter("filter_params.median_binary.threshold").as_int(); - params.invert = - this->get_parameter("filter_params.median_binary.invert").as_bool(); - - filter_ptr = std::make_unique(params); - break; - } + filter_ptr = std::make_unique(params); + break; + } - case FilterType::Binary: - { - BinaryThresholdParams params; - params.threshold = - this->get_parameter("filter_params.binary.threshold").as_double(); - params.maxval = - this->get_parameter("filter_params.binary.maxval").as_double(); - params.invert = - this->get_parameter("filter_params.binary.invert").as_bool(); - - filter_ptr = std::make_unique(params); - break; - } + case FilterType::MedianBinary: { + MedianBinaryParams params; + params.kernel_size = + this->get_parameter("filter_params.median_binary.kernel_size") + .as_int(); + params.threshold = + this->get_parameter("filter_params.median_binary.threshold") + .as_int(); + params.invert = + this->get_parameter("filter_params.median_binary.invert") + .as_bool(); + + filter_ptr = std::make_unique(params); + break; + } - // TODO: Add your filter case here: - case FilterType::Example: - { - ExampleParams params; - params.example_int = - this->get_parameter("filter_params.example.example_int").as_int(); - params.example_string = - this->get_parameter("filter_params.example.example_string").as_string(); - - filter_ptr = std::make_unique(params); - break; - } + case FilterType::Binary: { + BinaryThresholdParams params; + params.threshold = + this->get_parameter("filter_params.binary.threshold") + .as_double(); + params.maxval = + this->get_parameter("filter_params.binary.maxval").as_double(); + params.invert = + this->get_parameter("filter_params.binary.invert").as_bool(); + + filter_ptr = std::make_unique(params); + break; + } + // TODO: Add your filter case here: + case FilterType::Example: { + ExampleParams params; + params.example_int = + this->get_parameter("filter_params.example.example_int") + .as_int(); + params.example_string = + this->get_parameter("filter_params.example.example_string") + .as_string(); + + filter_ptr = std::make_unique(params); + break; + } - default: - ; - filter_ptr = std::make_unique(); - spdlog::warn("\033[33m Filterparams has not been set for your chosen filter {}. " - "To fix this add your filter to ImageFilteringNode::set_filter_params(). " - "Defaulting to no_filter. \033[0m", - filter_type_string); - filter_type = FilterType::NoFilter; + default:; + filter_ptr = std::make_unique(); + spdlog::warn( + "\033[33m Filterparams has not been set for your chosen filter " + "{}. " + "To fix this add your filter to " + "ImageFilteringNode::set_filter_params(). " + "Defaulting to no_filter. \033[0m", + filter_type_string); + filter_type = FilterType::NoFilter; }; - spdlog::info("\033[32m Using filter: {} \033[0m", filtertype_to_string(filter_type)); + spdlog::info("\033[32m Using filter: {} \033[0m", + filtertype_to_string(filter_type)); } void ImageFilteringNode::check_and_subscribe_to_image_topic() { @@ -279,11 +290,11 @@ void ImageFilteringNode::image_callback( const sensor_msgs::msg::Image::SharedPtr msg) { cv_bridge::CvImagePtr cv_ptr; - std::string input_encoding = + std::string input_encoding = this->get_parameter("input_encoding").as_string(); - - if (input_encoding.empty()){ - input_encoding = msg->encoding; // Default to the input image encoding + + if (input_encoding.empty()) { + input_encoding = msg->encoding; // Default to the input image encoding } try { diff --git a/image-filtering/src/image_processing.cpp b/image-filtering/src/image_processing.cpp index b259c71..73afd0d 100644 --- a/image-filtering/src/image_processing.cpp +++ b/image-filtering/src/image_processing.cpp @@ -2,22 +2,21 @@ #include #include - - - void Flip::apply_filter(const cv::Mat& original, cv::Mat& filtered) const { - int flip_code = this->filter_params.flip_code; // 0: x-axis, 1: y-axis, -1: both + int flip_code = + this->filter_params.flip_code; // 0: x-axis, 1: y-axis, -1: both cv::flip(original, filtered, flip_code); } - -void Sharpening::apply_filter(const cv::Mat& original, cv::Mat& filtered) const { +void Sharpening::apply_filter(const cv::Mat& original, + cv::Mat& filtered) const { // Sharpen image cv::Mat kernel = (cv::Mat_(3, 3) << 0, -1, 0, -1, 5, -1, 0, -1, 0); cv::filter2D(original, filtered, -1, kernel); } -void Unsharpening::apply_filter(const cv::Mat& original, cv::Mat& filtered) const { +void Unsharpening::apply_filter(const cv::Mat& original, + cv::Mat& filtered) const { int blur_size = this->filter_params.blur_size; // Create a blurred version of the image cv::Mat blurred; @@ -32,21 +31,24 @@ void Unsharpening::apply_filter(const cv::Mat& original, cv::Mat& filtered) cons } void Erosion::apply_filter(const cv::Mat& original, cv::Mat& filtered) const { - apply_erosion(original, filtered, this->filter_params.kernel_size, cv::MORPH_RECT); + apply_erosion(original, filtered, this->filter_params.kernel_size, + cv::MORPH_RECT); } -void Dilation::apply_filter(const cv::Mat& original, cv::Mat& filtered) const{ - apply_dilation(original, filtered, this->filter_params.kernel_size, cv::MORPH_RECT); +void Dilation::apply_filter(const cv::Mat& original, cv::Mat& filtered) const { + apply_dilation(original, filtered, this->filter_params.kernel_size, + cv::MORPH_RECT); } -void WhiteBalance::apply_filter(const cv::Mat& original, cv::Mat& filtered) const{ +void WhiteBalance::apply_filter(const cv::Mat& original, + cv::Mat& filtered) const { double contrast_percentage = this->filter_params.contrast_percentage; cv::Ptr balance = cv::xphoto::createSimpleWB(); balance->setP(contrast_percentage); balance->balanceWhite(original, filtered); } -void Ebus::apply_filter(const cv::Mat& original, cv::Mat& filtered) const{ +void Ebus::apply_filter(const cv::Mat& original, cv::Mat& filtered) const { int blur_size = this->filter_params.blur_size; int mask_weight = this->filter_params.mask_weight; int erosion_size = this->filter_params.erosion_size; @@ -69,41 +71,42 @@ void Ebus::apply_filter(const cv::Mat& original, cv::Mat& filtered) const{ addWeighted(eroded, 1, mask, mask_weight, 0, filtered); } - - -void OtsuSegmentation::apply_filter(const cv::Mat& original, cv::Mat& filtered) const{ +void OtsuSegmentation::apply_filter(const cv::Mat& original, + cv::Mat& filtered) const { bool gamma_auto_correction = this->filter_params.gamma_auto_correction; - double gamma_auto_correction_weight = this->filter_params.gamma_auto_correction_weight; + double gamma_auto_correction_weight = + this->filter_params.gamma_auto_correction_weight; bool otsu_segmentation = this->filter_params.otsu_segmentation; - if (original.type() == CV_8UC3) { // if the image type is bgr8 - to_weighted_gray(original, filtered, - this->filter_params.gsc_weight_b, - this->filter_params.gsc_weight_g, - this->filter_params.gsc_weight_r); + if (original.type() == CV_8UC3) { // if the image type is bgr8 + to_weighted_gray(original, filtered, this->filter_params.gsc_weight_b, + this->filter_params.gsc_weight_g, + this->filter_params.gsc_weight_r); + } else if (original.type() == CV_8UC1) { + original.copyTo(filtered); + } // if its mono8 + else { + std::cout << "your image type is not matching this filter" << std::endl; } - else if (original.type() == CV_8UC1){ original.copyTo(filtered); } // if its mono8 - else {std::cout << "your image type is not matching this filter" << std::endl;} - if (gamma_auto_correction) { + if (gamma_auto_correction) { apply_auto_gamma(filtered, gamma_auto_correction_weight); - } - - if (otsu_segmentation) { + } + + if (otsu_segmentation) { apply_otsu(filtered, filtered, false, 255); // Apply erosion followed by dilation (opening) - apply_erosion(filtered, filtered, this->filter_params.erosion_size, cv::MORPH_CROSS); - apply_dilation(filtered, filtered, this->filter_params.dilation_size, cv::MORPH_CROSS); + apply_erosion(filtered, filtered, this->filter_params.erosion_size, + cv::MORPH_CROSS); + apply_dilation(filtered, filtered, this->filter_params.dilation_size, + cv::MORPH_CROSS); } } - - -void Overlap::apply_filter(const cv::Mat& original, cv::Mat& filtered) const -{ +void Overlap::apply_filter(const cv::Mat& original, cv::Mat& filtered) const { // mono8 only CV_Assert(!original.empty()); CV_Assert(original.type() == CV_8UC1); @@ -111,9 +114,8 @@ void Overlap::apply_filter(const cv::Mat& original, cv::Mat& filtered) const double thr_percent = filter_params.percentage_threshold; // First frame (or size/type change): passthrough + cache - if (!has_prev || prev.empty() || - prev.size() != original.size() || prev.type() != original.type()) - { + if (!has_prev || prev.empty() || prev.size() != original.size() || + prev.type() != original.type()) { original.copyTo(filtered); prev = original.clone(); has_prev = true; @@ -141,23 +143,20 @@ void Overlap::apply_filter(const cv::Mat& original, cv::Mat& filtered) const prev = original.clone(); } - -void MedianBinary::apply_filter(const cv::Mat& original, cv::Mat& filtered) const{ - +void MedianBinary::apply_filter(const cv::Mat& original, + cv::Mat& filtered) const { apply_median(original, filtered, this->filter_params.kernel_size); - apply_fixed_threshold(filtered, filtered, this->filter_params.threshold, this->filter_params.invert); + apply_fixed_threshold(filtered, filtered, this->filter_params.threshold, + this->filter_params.invert); } - - -void BinaryThreshold::apply_filter(const cv::Mat& original, cv::Mat& filtered) const -{ - +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 maxval = this->filter_params.maxval; - const bool invert = this->filter_params.invert; + const bool invert = this->filter_params.invert; // 1) Ensure single-channel cv::Mat gray; @@ -174,18 +173,15 @@ void BinaryThreshold::apply_filter(const cv::Mat& original, cv::Mat& filtered) c gray.convertTo(src8, CV_8U); } else { src8 = gray; - } + } // Apply fixed threshold const int type = invert ? cv::THRESH_BINARY_INV : cv::THRESH_BINARY; cv::threshold(src8, filtered, thresh, maxval, type); } - - - // TODO: Implement your filter here -void Example::apply_filter(const cv::Mat& original, cv::Mat& filtered) const{ +void Example::apply_filter(const cv::Mat& original, cv::Mat& filtered) const { std::string example_str = this->filter_params.example_string; int example_int = this->filter_params.example_int; - apply_example(original,filtered, example_str, example_int); + apply_example(original, filtered, example_str, example_int); } diff --git a/image-filtering/src/utilities.cpp b/image-filtering/src/utilities.cpp index a0c0a49..15cd95d 100644 --- a/image-filtering/src/utilities.cpp +++ b/image-filtering/src/utilities.cpp @@ -1,9 +1,6 @@ #include #include - - - // Apply a given gamma to an 8-bit image using a LUT void applyGammaLUT(cv::Mat& image, double gamma) { // Create a lookup table for gamma correction @@ -17,7 +14,6 @@ void applyGammaLUT(cv::Mat& image, double gamma) { cv::LUT(image, lookup, image); } - // Compute a gamma value that pushes the image mean toward mid-gray double computeAutoGammaFromMean(const cv::Mat& image) { // Convert the image to grayscale if it's not already @@ -49,8 +45,8 @@ double computeAutoGammaFromMean(const cv::Mat& image) { return gamma; } - -// Auto-choose a gamma so dark images get lifted and bright images get toned down (expects mono8) +// Auto-choose a gamma so dark images get lifted and bright images get toned +// down (expects mono8) // - It sets the mean intensity to 255/2 ≃ 128 // - The correction weight makes all the values weaker(<1) or stronger(>1) void apply_auto_gamma(cv::Mat& image, double correction_weight) { @@ -58,68 +54,54 @@ void apply_auto_gamma(cv::Mat& image, double correction_weight) { applyGammaLUT(image, gamma); } - - - // Convert BGR image to single-channel grayscale using custom B,G,R weights // weights = (b, g, r), e.g. (0.114f, 0.587f, 0.299f) -void to_weighted_gray(const cv::Mat& bgr, cv::Mat& gray, double wB, double wG, double wR) { +void to_weighted_gray(const cv::Mat& bgr, + cv::Mat& gray, + double wB, + double wG, + double wR) { cv::Matx13f customWeights(wB, wG, wR); cv::transform(bgr, gray, customWeights); } - - - -// Returns the Otsu threshold value chosen by OpenCV (0..255) and outputs the thresholded binary image -int apply_otsu(const cv::Mat& gray8u, cv::Mat& out, bool invert, double maxval) -{ - CV_Assert(gray8u.type() == CV_8UC1 && "applyOtsu expects 8-bit single-channel input"); +// Returns the Otsu threshold value chosen by OpenCV (0..255) and outputs the +// thresholded binary image +int apply_otsu(const cv::Mat& gray8u, + cv::Mat& out, + bool invert, + double maxval) { + CV_Assert(gray8u.type() == CV_8UC1 && + "applyOtsu expects 8-bit single-channel input"); int ttype = invert ? (cv::THRESH_BINARY_INV | cv::THRESH_OTSU) - : (cv::THRESH_BINARY | cv::THRESH_OTSU); + : (cv::THRESH_BINARY | cv::THRESH_OTSU); - double thresh = cv::threshold(gray8u, out, /*thresh ignored*/0.0, maxval, ttype); + double thresh = + cv::threshold(gray8u, out, /*thresh ignored*/ 0.0, maxval, ttype); return static_cast(std::round(thresh)); } - - - // Basic erosion -void apply_erosion(const cv::Mat& src, - cv::Mat& filtered, - int size, - int shape) { +void apply_erosion(const cv::Mat& src, cv::Mat& filtered, int size, int shape) { cv::Mat kernel = cv::getStructuringElement( - shape, - cv::Size(2 * size + 1, 2 * size + 1), - cv::Point(size, size)); + shape, cv::Size(2 * size + 1, 2 * size + 1), cv::Point(size, size)); cv::erode(src, filtered, kernel); } - // Basic dilation -void apply_dilation(const cv::Mat& src, - cv::Mat& dst, - int size, - int shape) { +void apply_dilation(const cv::Mat& src, cv::Mat& dst, int size, int shape) { cv::Mat kernel = cv::getStructuringElement( - shape, - cv::Size(2 * size + 1, 2 * size + 1), - cv::Point(size, size)); + shape, cv::Size(2 * size + 1, 2 * size + 1), cv::Point(size, size)); cv::dilate(src, dst, kernel); } - - -// Median filter that preserves original depth if it's unsupported by cv::medianBlur. -// Supported depths: CV_8U, CV_16U, CV_32F -// For others (e.g., CV_16S, CV_32S, CV_64F) we convert to CV_32F, filter, then convert back. +// Median filter that preserves original depth if it's unsupported by +// cv::medianBlur. Supported depths: CV_8U, CV_16U, CV_32F For others (e.g., +// CV_16S, CV_32S, CV_64F) we convert to CV_32F, filter, then convert back. void apply_median(const cv::Mat& original, cv::Mat& filtered, int kernel_size) { CV_Assert(!original.empty()); - // If caller passed 1, just copy if (kernel_size == 1) { original.copyTo(filtered); @@ -127,17 +109,21 @@ void apply_median(const cv::Mat& original, cv::Mat& filtered, int kernel_size) { } // Sanitize kernel size: must be odd and >= 3 - if (kernel_size < 3) kernel_size = 3; - if ((kernel_size & 1) == 0) ++kernel_size; + if (kernel_size < 3) + kernel_size = 3; + if ((kernel_size & 1) == 0) + ++kernel_size; const int depth = original.depth(); - const bool supported = (depth == CV_8U || depth == CV_16U || depth == CV_32F); + const bool supported = + (depth == CV_8U || depth == CV_16U || depth == CV_32F); const cv::Mat* src = &original; cv::Mat work, out; if (!supported) { - // Convert unsupported depths to CV_32F to avoid clipping (better than going to 8U). + // Convert unsupported depths to CV_32F to avoid clipping (better than + // going to 8U). original.convertTo(work, CV_32F); src = &work; } @@ -153,20 +139,22 @@ void apply_median(const cv::Mat& original, cv::Mat& filtered, int kernel_size) { } } - - // Apply a fixed binary threshold. // - Accepts grayscale or color input (auto-converts to gray). // - Ensures 8-bit depth for thresholding. // - Returns a 0/255 mask (CV_8U). // - Set `invert=true` to get white background & black foreground. -void apply_fixed_threshold(const cv::Mat& img, cv::Mat& filtered, int thresh, bool invert) -{ +void apply_fixed_threshold(const cv::Mat& img, + cv::Mat& filtered, + int thresh, + bool invert) { if (img.empty()) { - throw std::invalid_argument("applyFixedThreshold: input image is empty"); + throw std::invalid_argument( + "applyFixedThreshold: input image is empty"); } if (thresh < 0 || thresh > 255) { - throw std::out_of_range("applyFixedThreshold: thresh must be in [0, 255]"); + throw std::out_of_range( + "applyFixedThreshold: thresh must be in [0, 255]"); } // Convert to grayscale @@ -189,15 +177,7 @@ void apply_fixed_threshold(const cv::Mat& img, cv::Mat& filtered, int thresh, bo cv::threshold(gray, filtered, thresh, 255, type); } - - - - - - - - -// Takes a strict binary obstacle mask (0/255) and returns a float image +// Takes a strict binary obstacle mask (0/255) and returns a float image // where each pixel is the distance (in pixels) to the nearest obstacle. // Requirements: // - binObstacles: single-channel CV_8U with values in {0, 255} only. @@ -211,27 +191,31 @@ void apply_fixed_threshold(const cv::Mat& img, cv::Mat& filtered, int thresh, bo // - maskSize: 3, 5, or CV_DIST_MASK_PRECISE (0) // Best regards, ChatGPT void distance_field(const cv::Mat& binObstacles, - cv::Mat& dist, - bool obstaclesAreWhite, - int type, - int maskSize) -{ + cv::Mat& dist, + bool obstaclesAreWhite, + int type, + int maskSize) { if (binObstacles.empty()) throw std::invalid_argument("distance_field_binary: input is empty"); if (binObstacles.channels() != 1) - throw std::invalid_argument("distance_field_binary: input must be single-channel"); + throw std::invalid_argument( + "distance_field_binary: input must be single-channel"); if (binObstacles.depth() != CV_8U) - throw std::invalid_argument("distance_field_binary: input must be CV_8U (0/255)"); + throw std::invalid_argument( + "distance_field_binary: input must be CV_8U (0/255)"); // Validate strict binary: values must be only 0 or 255 { cv::Mat notZero = (binObstacles != 0); cv::Mat not255 = (binObstacles != 255); - cv::Mat invalid = notZero & not255; // pixels that are neither 0 nor 255 + cv::Mat invalid = + notZero & not255; // pixels that are neither 0 nor 255 if (cv::countNonZero(invalid) > 0) - throw std::invalid_argument("distance_field_binary: input must contain only 0 or 255 values"); + throw std::invalid_argument( + "distance_field_binary: input must contain only 0 or 255 " + "values"); } // OpenCV distanceTransform computes distance TO the nearest ZERO pixel @@ -249,26 +233,21 @@ void distance_field(const cv::Mat& binObstacles, if (!(maskSize == 3 || maskSize == 5 || maskSize == cv::DIST_MASK_PRECISE)) maskSize = 3; - cv::distanceTransform(freeMask, dist, type, maskSize); // dist is CV_32F + cv::distanceTransform(freeMask, dist, type, maskSize); // dist is CV_32F - const float cap = 100.f; // pixel - cv::Mat clipped; cv::min(dist, cap, clipped); - clipped.convertTo(dist, CV_8U, 255.0f/cap); + const float cap = 100.f; // pixel + cv::Mat clipped; + cv::min(dist, cap, clipped); + clipped.convertTo(dist, CV_8U, 255.0f / cap); // publish vis8u as "mono8" - } - - - // TODO: If you need a helper function define it here like this void apply_example(const cv::Mat& original, - cv::Mat& filtered, - std::string example_string, - int example_int) -{ - + cv::Mat& filtered, + std::string example_string, + int example_int) { filtered = original.clone(); // Two centered lines: number above string @@ -279,15 +258,19 @@ void apply_example(const cv::Mat& original, const int fontFace = cv::FONT_HERSHEY_SIMPLEX; const double fontScale = 1.0; const int thickness = 2; - const int lineType = cv::LINE_AA; // smoother; use cv::LINE_8 for hard edges - const int lineGapPx = 10; // vertical gap between lines in pixels + const int lineType = + cv::LINE_AA; // smoother; use cv::LINE_8 for hard edges + const int lineGapPx = 10; // vertical gap between lines in pixels // Measure both lines int base1 = 0, base2 = 0; - const cv::Size sz1 = cv::getTextSize(line1, fontFace, fontScale, thickness, &base1); - const cv::Size sz2 = cv::getTextSize(line2, fontFace, fontScale, thickness, &base2); + const cv::Size sz1 = + cv::getTextSize(line1, fontFace, fontScale, thickness, &base1); + const cv::Size sz2 = + cv::getTextSize(line2, fontFace, fontScale, thickness, &base2); - // Total block size (approx). Heights don't include baseline, so we add baselines for safety. + // Total block size (approx). Heights don't include baseline, so we add + // baselines for safety. const int blockW = std::max(sz1.width, sz2.width); const int blockH = (sz1.height + base1) + lineGapPx + (sz2.height + base2); @@ -297,24 +280,28 @@ void apply_example(const cv::Mat& original, // Baseline positions for each line (y is baseline in putText) const int x1 = blockX + (blockW - sz1.width) / 2; - const int y1 = blockY + sz1.height; // baseline ~ top + height + const int y1 = blockY + sz1.height; // baseline ~ top + height const int x2 = blockX + (blockW - sz2.width) / 2; const int y2 = y1 + base1 + lineGapPx + sz2.height; // Clamp to keep text inside image if needed - auto clamp = [](int v, int lo, int hi) { return std::max(lo, std::min(v, hi)); }; + auto clamp = [](int v, int lo, int hi) { + return std::max(lo, std::min(v, hi)); + }; const int x1c = clamp(x1, 0, std::max(0, filtered.cols - sz1.width)); - const int y1c = clamp(y1, sz1.height, std::max(sz1.height, filtered.rows - base1)); + const int y1c = + clamp(y1, sz1.height, std::max(sz1.height, filtered.rows - base1)); const int x2c = clamp(x2, 0, std::max(0, filtered.cols - sz2.width)); - const int y2c = clamp(y2, sz2.height, std::max(sz2.height, filtered.rows - base2)); + const int y2c = + clamp(y2, sz2.height, std::max(sz2.height, filtered.rows - base2)); // Draw white text on mono8 - cv::putText(filtered, line1, cv::Point(x1c, y1c), - fontFace, fontScale, cv::Scalar(255), thickness, lineType); + cv::putText(filtered, line1, cv::Point(x1c, y1c), fontFace, fontScale, + cv::Scalar(255), thickness, lineType); - cv::putText(filtered, line2, cv::Point(x2c, y2c), - fontFace, fontScale, cv::Scalar(255), thickness, lineType); + cv::putText(filtered, line2, cv::Point(x2c, y2c), fontFace, fontScale, + cv::Scalar(255), thickness, lineType); } From 90dac028f6a9e67caeafef9c6f035fc26d0e629b Mon Sep 17 00:00:00 2001 From: Thomas Date: Thu, 8 Jan 2026 15:25:14 +0100 Subject: [PATCH 48/53] Fixing some stuf, not all is done? --- README.md | 4 ++-- image-filtering/config/image_filtering_params.yaml | 2 +- image-filtering/image_processing.hpp | 6 +++--- .../include/image_filters/image_filtering_ros.hpp | 8 +++++--- .../include/image_filters/image_processing.hpp | 14 ++++++++------ .../include/image_filters/utilities.hpp | 9 +++++---- image-filtering/src/image_filtering_ros.cpp | 4 ++-- image-filtering/src/image_processing.cpp | 2 +- image-filtering/src/utilities.cpp | 2 +- 9 files changed, 28 insertions(+), 23 deletions(-) diff --git a/README.md b/README.md index 1e2a04c..38252a1 100644 --- a/README.md +++ b/README.md @@ -32,7 +32,7 @@ Parameters can be set through a YAML file or dynamically adjusted at runtime. ## Implementing New Filters -To extend the functionality of the `image_filtering_node` by adding new filters, follow these steps to ensure compatibility and integration with the existing codebase. There should be //TODO comments where you add your filter: +To extend the functionality of the `image_filtering_node` by adding new filters, follow these steps to ensure compatibility and integration with the existing codebase. There should be //TODO(Vortex) comments where you add your filter: ### Step 1: Filter Enum @@ -168,4 +168,4 @@ void ImageFilteringNode::set_filter_params() { #### Adding Helper functions -If you need helper functions for your filter, you can add the declaration to [utilities.hpp](image-filtering/include/image_filters/utilities.hpp), and then add the function definition to [utilities.cpp](image-filtering/src/utilities.cpp). There will be TODO comments where you can add them. These functions are already included in the image_processing files. +If you need helper functions for your filter, you can add the declaration to [utilities.hpp](image-filtering/include/image_filters/utilities.hpp), and then add the function definition to [utilities.cpp](image-filtering/src/utilities.cpp). There will be TODO(Vortex) comments where you can add them. These functions are already included in the image_processing files. diff --git a/image-filtering/config/image_filtering_params.yaml b/image-filtering/config/image_filtering_params.yaml index 2c34393..d860153 100644 --- a/image-filtering/config/image_filtering_params.yaml +++ b/image-filtering/config/image_filtering_params.yaml @@ -41,7 +41,7 @@ maxval: 255. invert: true - # TODO: add your filterparameters here + # TODO(Vortex): add your filterparameters here example: example_int: 1 example_string: "Get filtered" diff --git a/image-filtering/image_processing.hpp b/image-filtering/image_processing.hpp index 78b0920..a6e37e3 100644 --- a/image-filtering/image_processing.hpp +++ b/image-filtering/image_processing.hpp @@ -10,7 +10,7 @@ #include #include -enum class FilterType { // TODO: Add filters here +enum class FilterType { // TODO(Vortex): Add filters here NoFilter, Flip, Unsharpening, @@ -32,7 +32,7 @@ inline std::string to_lower(std::string s) { } inline FilterType parse_filter_type( - std::string s) { // TODO: Also add filter-type here + std::string s) { // TODO(Vortex): Also add filter-type here s = to_lower(std::move(s)); if (s == "no_filter") return FilterType::NoFilter; @@ -297,7 +297,7 @@ class BinaryThreshold : public Filter { // Example ///////////////////////////// -// TODO: add this structure for your filter +// TODO(Vortex): add this structure for your filter // Example: struct ExampleParams { // Add filter variables here diff --git a/image-filtering/include/image_filters/image_filtering_ros.hpp b/image-filtering/include/image_filters/image_filtering_ros.hpp index 645f80e..3972d63 100644 --- a/image-filtering/include/image_filters/image_filtering_ros.hpp +++ b/image-filtering/include/image_filters/image_filtering_ros.hpp @@ -1,12 +1,14 @@ -#ifndef IMAGE_FILTERING_ROS_HPP -#define IMAGE_FILTERING_ROS_HPP +#ifndef IMAGE_FILTERS__IMAGE_FILTERING_ROS_HPP_ +#define IMAGE_FILTERS__IMAGE_FILTERING_ROS_HPP_ #include // for jazzy this has to be hpp and for humble it has to be h #include +#include #include #include #include #include +#include #include "image_processing.hpp" class ImageFilteringNode : public rclcpp::Node { @@ -103,4 +105,4 @@ class ImageFilteringNode : public rclcpp::Node { std::unique_ptr filter_ptr; }; -#endif // IMAGE_FILTERING_ROS_HPP +#endif // IMAGE_FILTERS__IMAGE_FILTERING_ROS_HPP_ diff --git a/image-filtering/include/image_filters/image_processing.hpp b/image-filtering/include/image_filters/image_processing.hpp index dc4528e..023dd00 100644 --- a/image-filtering/include/image_filters/image_processing.hpp +++ b/image-filtering/include/image_filters/image_processing.hpp @@ -1,5 +1,5 @@ -#ifndef IMAGE_PROCESSING_HPP -#define IMAGE_PROCESSING_HPP +#ifndef IMAGE_FILTERING__IMAGE_PROCESSING_HPP_ +#define IMAGE_FILTERING__IMAGE_PROCESSING_HPP_ #include #include @@ -9,8 +9,10 @@ #include #include #include +#include +#include -enum class FilterType { // TODO: Add filters here +enum class FilterType { // TODO(Vortex): Add filters here NoFilter, Flip, Unsharpening, @@ -40,7 +42,7 @@ static constexpr std::pair kFilterMap[] = { {"median_binary", FilterType::MedianBinary}, {"binary", FilterType::Binary}, - // TODO: Also add your filter here + // TODO(Vortex): Also add your filter here {"example", FilterType::Example}, {"unknown", FilterType::Unknown}}; @@ -314,7 +316,7 @@ class BinaryThreshold : public Filter { BinaryThresholdParams filter_params; }; -// TODO: add this structure for your filter +// TODO(Vortex): add this structure for your filter ///////////////////////////// // Example @@ -335,4 +337,4 @@ class Example : public Filter { ExampleParams filter_params; }; -#endif // IMAGE_PROCESSING_HPP +#endif // IMAGE_FILTERING__IMAGE_PROCESSING_HPP_ diff --git a/image-filtering/include/image_filters/utilities.hpp b/image-filtering/include/image_filters/utilities.hpp index 2298cfa..abf1cfe 100644 --- a/image-filtering/include/image_filters/utilities.hpp +++ b/image-filtering/include/image_filters/utilities.hpp @@ -1,5 +1,5 @@ -#ifndef UTILITIES_HPP -#define UTILITIES_HPP +#ifndef IMAGE_FILTERS__UTILITIES_HPP_ +#define IMAGE_FILTERS__UTILITIES_HPP_ #include #include @@ -7,6 +7,7 @@ #include #include #include +#include // Auto-choose a gamma so dark images get lifted and bright images get toned // down (expects mono8) @@ -60,10 +61,10 @@ void distance_field(const cv::Mat& binObstacles, int type = cv::DIST_L2, int maskSize = 3); // DIST_L2 is normal euclidean -// TODO: If you need a helper function have the signature here +// TODO(Vortex): If you need a helper function have the signature here void apply_example(const cv::Mat& original, cv::Mat& filtered, std::string example_string, int example_int); -#endif +#endif // IMAGE_FILTERS__UTILITIES_HPP_ diff --git a/image-filtering/src/image_filtering_ros.cpp b/image-filtering/src/image_filtering_ros.cpp index 7168efa..69801d3 100644 --- a/image-filtering/src/image_filtering_ros.cpp +++ b/image-filtering/src/image_filtering_ros.cpp @@ -55,7 +55,7 @@ void ImageFilteringNode::declare_parameters() { this->declare_parameter("filter_params.median_binary.threshold"); this->declare_parameter("filter_params.median_binary.invert"); - // TODO: Declare parameters set for your filter here + // TODO(Vortex): Declare parameters set for your filter here this->declare_parameter("filter_params.example.example_int"); this->declare_parameter( "filter_params.example.example_string"); @@ -213,7 +213,7 @@ void ImageFilteringNode::set_filter_params() { break; } - // TODO: Add your filter case here: + // TODO(Vortex): Add your filter case here: case FilterType::Example: { ExampleParams params; params.example_int = diff --git a/image-filtering/src/image_processing.cpp b/image-filtering/src/image_processing.cpp index 73afd0d..048e3fc 100644 --- a/image-filtering/src/image_processing.cpp +++ b/image-filtering/src/image_processing.cpp @@ -179,7 +179,7 @@ void BinaryThreshold::apply_filter(const cv::Mat& original, cv::threshold(src8, filtered, thresh, maxval, type); } -// TODO: Implement your filter here +// TODO(Vortex): Implement your filter here void Example::apply_filter(const cv::Mat& original, cv::Mat& filtered) const { std::string example_str = this->filter_params.example_string; int example_int = this->filter_params.example_int; diff --git a/image-filtering/src/utilities.cpp b/image-filtering/src/utilities.cpp index 15cd95d..78415f3 100644 --- a/image-filtering/src/utilities.cpp +++ b/image-filtering/src/utilities.cpp @@ -242,7 +242,7 @@ void distance_field(const cv::Mat& binObstacles, // publish vis8u as "mono8" } -// TODO: If you need a helper function define it here like this +// TODO(Vortex): If you need a helper function define it here like this void apply_example(const cv::Mat& original, cv::Mat& filtered, From c031c914c6b1557146749b8a7ab25ad86ce43e9d Mon Sep 17 00:00:00 2001 From: Thomas Date: Thu, 8 Jan 2026 15:38:22 +0100 Subject: [PATCH 49/53] Final changes i hope --- .../config/image_filtering_params.yaml | 2 +- image-filtering/image_processing.hpp | 317 ------------------ .../image_filters/image_filtering_ros.hpp | 2 +- .../image_filters/image_processing.hpp | 7 +- image-filtering/src/image_filtering_ros.cpp | 3 +- image-filtering/src/image_processing.cpp | 3 +- 6 files changed, 7 insertions(+), 327 deletions(-) delete mode 100644 image-filtering/image_processing.hpp diff --git a/image-filtering/config/image_filtering_params.yaml b/image-filtering/config/image_filtering_params.yaml index d860153..44e89e2 100644 --- a/image-filtering/config/image_filtering_params.yaml +++ b/image-filtering/config/image_filtering_params.yaml @@ -6,7 +6,7 @@ output_encoding: "mono8" filter_params: - filter_type: "example" + filter_type: "otsu" flip: flip_code: 1 unsharpening: diff --git a/image-filtering/image_processing.hpp b/image-filtering/image_processing.hpp deleted file mode 100644 index a6e37e3..0000000 --- a/image-filtering/image_processing.hpp +++ /dev/null @@ -1,317 +0,0 @@ -#ifndef IMAGE_PROCESSING_HPP -#define IMAGE_PROCESSING_HPP - -#include -#include -#include -#include -#include -#include -#include -#include - -enum class FilterType { // TODO(Vortex): Add filters here - NoFilter, - Flip, - Unsharpening, - Erosion, - Dilation, - WhiteBalancing, - Ebus, - Otsu, - Overlap, - MedianBinary, - Binary, - Unknown -}; - -inline std::string to_lower(std::string s) { - for (auto& c : s) - c = static_cast(std::tolower(c)); - return s; -} - -inline FilterType parse_filter_type( - std::string s) { // TODO(Vortex): Also add filter-type here - s = to_lower(std::move(s)); - if (s == "no_filter") - return FilterType::NoFilter; - if (s == "flip") - return FilterType::Flip; - if (s == "unsharpening") - return FilterType::Unsharpening; - if (s == "erosion") - return FilterType::Erosion; - if (s == "dilation") - return FilterType::Dilation; - if (s == "white_balancing") - return FilterType::WhiteBalancing; - if (s == "ebus") - return FilterType::Ebus; - if (s == "otsu") - return FilterType::Otsu; - if (s == "overlap") - return FilterType::Overlap; - if (s == "median_binary") - return FilterType::MedianBinary; - if (s == "binary") - return FilterType::Binary; - return FilterType::Unknown; -} - -class Filter { - public: - virtual ~Filter() = default; - virtual void apply_filter(const cv::Mat& original, - cv::Mat& filtered) const = 0; - - protected: - Filter() = default; -}; - -///////////////////////////// -// No filter -///////////////////////////// - -struct NoFilterParams {}; - -class NoFilter : public Filter { - public: - explicit NoFilter() = default; // No parameters to set - void apply_filter(const cv::Mat& original, - cv::Mat& filtered) const override { - original.copyTo(filtered); - }; -}; - -//////////////////////////// -// Unsharpening -///////////////////////////// - -struct UnsharpeningParams { - int blur_size; -}; - -class Unsharpening : public Filter { - public: - explicit Unsharpening(UnsharpeningParams params) : filter_params(params) {} - void apply_filter(const cv::Mat& original, - cv::Mat& filtered) const override; - - private: - UnsharpeningParams filter_params; -}; - -///////////////////////////// -// Sharpening -///////////////////////////// - -struct FlipParams { - int flip_code; -}; - -class Flip : public Filter { - public: - explicit Flip(FlipParams params) : filter_params(params) {} - void apply_filter(const cv::Mat& original, - cv::Mat& filtered) const override; - - private: - FlipParams filter_params; -}; - -///////////////////////////// -// Sharpening -///////////////////////////// - -struct SharpeningParams {}; - -class Sharpening : public Filter { - public: - explicit Sharpening(SharpeningParams params) : filter_params(params) {} - void apply_filter(const cv::Mat& original, - cv::Mat& filtered) const override; - - private: - SharpeningParams filter_params; -}; -///////////////////////////// -// Erosion -///////////////////////////// - -struct ErosionParams { - int kernel_size; // odd > 1 -}; - -class Erosion : public Filter { - public: - explicit Erosion(ErosionParams params) : filter_params(params) {} - void apply_filter(const cv::Mat& original, - cv::Mat& filtered) const override; - - private: - ErosionParams filter_params; -}; - -///////////////////////////// -// Dilation -///////////////////////////// - -struct DilationParams { - int kernel_size = 3; -}; - -class Dilation : public Filter { - public: - explicit Dilation(DilationParams params) : filter_params(params) {} - void apply_filter(const cv::Mat& original, - cv::Mat& filtered) const override; - - private: - DilationParams filter_params; -}; - -///////////////////////////// -// White Balance -///////////////////////////// - -struct WhiteBalanceParams { - double contrast_percentage; -}; - -class WhiteBalance : public Filter { - public: - explicit WhiteBalance(WhiteBalanceParams params) : filter_params(params) {} - void apply_filter(const cv::Mat& original, - cv::Mat& filtered) const override; - - private: - WhiteBalanceParams filter_params; -}; - -///////////////////////////// -// Ebus (dilation + unsharpening combo) -///////////////////////////// - -struct EbusParams { - int erosion_size; - int blur_size; - int mask_weight; -}; - -class Ebus : public Filter { - public: - explicit Ebus(EbusParams params) : filter_params(params) {} - void apply_filter(const cv::Mat& original, - cv::Mat& filtered) const override; - - private: - EbusParams filter_params; -}; - -///////////////////////////// -// Otsu Segmentation -///////////////////////////// - -struct OtsuSegmentationParams { - bool gamma_auto_correction; - double gamma_auto_correction_weight; - bool otsu_segmentation; - double gsc_weight_r; - double gsc_weight_g; - double gsc_weight_b; - int erosion_size; - int dilation_size; -}; - -class OtsuSegmentation : public Filter { - public: - explicit OtsuSegmentation(OtsuSegmentationParams params) - : filter_params(params) {} - void apply_filter(const cv::Mat& original, cv::Mat& output) const override; - - private: - OtsuSegmentationParams filter_params; -}; - -///////////////////////////// -// Overlap (blend/composite) -///////////////////////////// - -struct OverlapParams { - double percentage_threshold; -}; - -class Overlap : public Filter { - public: - explicit Overlap(OverlapParams params) : filter_params(params) {} - void apply_filter(const cv::Mat& original, - cv::Mat& filtered) const override; - - private: - OverlapParams filter_params; -}; - -///////////////////////////// -// Median + Binary -///////////////////////////// - -struct MedianBinaryParams { - int kernel_size; - int threshold; - bool invert; -}; - -class MedianBinary : public Filter { - public: - explicit MedianBinary(MedianBinaryParams params) : filter_params(params) {} - void apply_filter(const cv::Mat& original, - cv::Mat& filtered) const override; - - private: - MedianBinaryParams filter_params; -}; - -///////////////////////////// -// Binary Threshold -///////////////////////////// - -struct BinaryThresholdParams { - double threshold; - double maxval; - bool invert; -}; - -class BinaryThreshold : public Filter { - public: - explicit BinaryThreshold(BinaryThresholdParams params) - : filter_params(params) {} - void apply_filter(const cv::Mat& original, - cv::Mat& filtered) const override; - - private: - BinaryThresholdParams filter_params; -}; - -///////////////////////////// -// Example -///////////////////////////// - -// TODO(Vortex): add this structure for your filter - -// Example: -struct ExampleParams { // Add filter variables here - int example_variable; - std::string example_string; -}; - -class Example : public Filter { - public: - explicit Example(ExampleParams params) : filter_params(params) {} - void apply_filter(const cv::Mat& original, cv::Mat& filtered) - const override; // This is the filter itself - private: - ExampleParams filter_params; -}; - -#endif // IMAGE_PROCESSING_HPP diff --git a/image-filtering/include/image_filters/image_filtering_ros.hpp b/image-filtering/include/image_filters/image_filtering_ros.hpp index 3972d63..b8203d7 100644 --- a/image-filtering/include/image_filters/image_filtering_ros.hpp +++ b/image-filtering/include/image_filters/image_filtering_ros.hpp @@ -14,7 +14,7 @@ class ImageFilteringNode : public rclcpp::Node { public: explicit ImageFilteringNode(const rclcpp::NodeOptions& options); - ~ImageFilteringNode() {}; + ~ImageFilteringNode() {} private: /** diff --git a/image-filtering/include/image_filters/image_processing.hpp b/image-filtering/include/image_filters/image_processing.hpp index 023dd00..027fb7d 100644 --- a/image-filtering/include/image_filters/image_processing.hpp +++ b/image-filtering/include/image_filters/image_processing.hpp @@ -1,5 +1,5 @@ -#ifndef IMAGE_FILTERING__IMAGE_PROCESSING_HPP_ -#define IMAGE_FILTERING__IMAGE_PROCESSING_HPP_ +#ifndef IMAGE_FILTERS__IMAGE_PROCESSING_HPP_ +#define IMAGE_FILTERS__IMAGE_PROCESSING_HPP_ #include #include @@ -94,7 +94,6 @@ struct NoFilterParams {}; class NoFilter : public Filter { public: - explicit NoFilter() = default; // No parameters to set void apply_filter(const cv::Mat& original, cv::Mat& filtered) const override { original.copyTo(filtered); @@ -337,4 +336,4 @@ class Example : public Filter { ExampleParams filter_params; }; -#endif // IMAGE_FILTERING__IMAGE_PROCESSING_HPP_ +#endif // IMAGE_FILTERS__IMAGE_PROCESSING_HPP_ diff --git a/image-filtering/src/image_filtering_ros.cpp b/image-filtering/src/image_filtering_ros.cpp index 69801d3..8a59eff 100644 --- a/image-filtering/src/image_filtering_ros.cpp +++ b/image-filtering/src/image_filtering_ros.cpp @@ -237,7 +237,7 @@ void ImageFilteringNode::set_filter_params() { "Defaulting to no_filter. \033[0m", filter_type_string); filter_type = FilterType::NoFilter; - }; + } spdlog::info("\033[32m Using filter: {} \033[0m", filtertype_to_string(filter_type)); @@ -304,7 +304,6 @@ void ImageFilteringNode::image_callback( spdlog::error("Received empty image, skipping processing."); return; } - } catch (cv_bridge::Exception& e) { spdlog::error("cv_bridge exception: {}", e.what()); ; diff --git a/image-filtering/src/image_processing.cpp b/image-filtering/src/image_processing.cpp index 048e3fc..6860c29 100644 --- a/image-filtering/src/image_processing.cpp +++ b/image-filtering/src/image_processing.cpp @@ -86,9 +86,8 @@ void OtsuSegmentation::apply_filter(const cv::Mat& original, } else if (original.type() == CV_8UC1) { original.copyTo(filtered); } // if its mono8 - else { + else std::cout << "your image type is not matching this filter" << std::endl; - } if (gamma_auto_correction) { apply_auto_gamma(filtered, gamma_auto_correction_weight); From 9779ff94deaac4f12d20d1fc7c3225f9d509bc1c Mon Sep 17 00:00:00 2001 From: Thomas Date: Thu, 8 Jan 2026 15:59:54 +0100 Subject: [PATCH 50/53] We are saved, the dinosaur from toystory is back to life, and beter than ever --- .../config/image_filtering_params.yaml | 2 +- image-filtering/src/utilities.cpp | 20 +++++++++++++++++++ 2 files changed, 21 insertions(+), 1 deletion(-) diff --git a/image-filtering/config/image_filtering_params.yaml b/image-filtering/config/image_filtering_params.yaml index 44e89e2..d860153 100644 --- a/image-filtering/config/image_filtering_params.yaml +++ b/image-filtering/config/image_filtering_params.yaml @@ -6,7 +6,7 @@ output_encoding: "mono8" filter_params: - filter_type: "otsu" + filter_type: "example" flip: flip_code: 1 unsharpening: diff --git a/image-filtering/src/utilities.cpp b/image-filtering/src/utilities.cpp index 78415f3..a3cef1d 100644 --- a/image-filtering/src/utilities.cpp +++ b/image-filtering/src/utilities.cpp @@ -305,3 +305,23 @@ void apply_example(const cv::Mat& original, cv::putText(filtered, line2, cv::Point(x2c, y2c), fontFace, fontScale, cv::Scalar(255), thickness, lineType); } + +/* + _.-. + / 66\ + ( `\ Hi, how you doing :) + |\\ , ,| + __ | \\____/ + ,.--"`-.". / `---' + _.-' '-/ | + _.-" | '-. |_/_ + ,__.-' _,.--\ \ (( /-\ + ',_..--' `\ \ \\_ / + `-, ) |\' + | |-.,,-" ( + | | `\ `',_ + ) \ \,(\(\-' + \ `-,_ + \_(\-(\`-` + " " +*/ From fd73f5ea01e4dafdb9a86a1759cc280084e90694 Mon Sep 17 00:00:00 2001 From: Thomas Date: Thu, 8 Jan 2026 16:54:50 +0100 Subject: [PATCH 51/53] Removing log files --- image-filtering/log/COLCON_IGNORE | 0 image-filtering/log/latest | 1 - image-filtering/log/latest_list | 1 - .../list_2025-10-22_12-58-52/logger_all.log | 19 ---------- log/COLCON_IGNORE | 0 log/latest | 1 - log/latest_list | 1 - log/list_2025-10-22_13-00-40/logger_all.log | 36 ------------------- 8 files changed, 59 deletions(-) delete mode 100644 image-filtering/log/COLCON_IGNORE delete mode 120000 image-filtering/log/latest delete mode 120000 image-filtering/log/latest_list delete mode 100644 image-filtering/log/list_2025-10-22_12-58-52/logger_all.log delete mode 100644 log/COLCON_IGNORE delete mode 120000 log/latest delete mode 120000 log/latest_list delete mode 100644 log/list_2025-10-22_13-00-40/logger_all.log diff --git a/image-filtering/log/COLCON_IGNORE b/image-filtering/log/COLCON_IGNORE deleted file mode 100644 index e69de29..0000000 diff --git a/image-filtering/log/latest b/image-filtering/log/latest deleted file mode 120000 index 1715667..0000000 --- a/image-filtering/log/latest +++ /dev/null @@ -1 +0,0 @@ -latest_list \ No newline at end of file diff --git a/image-filtering/log/latest_list b/image-filtering/log/latest_list deleted file mode 120000 index b1c2701..0000000 --- a/image-filtering/log/latest_list +++ /dev/null @@ -1 +0,0 @@ -list_2025-10-22_12-58-52 \ No newline at end of file diff --git a/image-filtering/log/list_2025-10-22_12-58-52/logger_all.log b/image-filtering/log/list_2025-10-22_12-58-52/logger_all.log deleted file mode 100644 index 57f6a8a..0000000 --- a/image-filtering/log/list_2025-10-22_12-58-52/logger_all.log +++ /dev/null @@ -1,19 +0,0 @@ -[0.403s] DEBUG:colcon:Command line arguments: ['/usr/bin/colcon', 'list', '-p', '--base-paths', '/home/thomas/ros2_ws/src/vortex-image-filtering/image-filtering', '--log-base', '/dev/null'] -[0.404s] DEBUG:colcon:Parsed command line arguments: Namespace(log_base=None, log_level=None, verb_name='list', build_base='build', ignore_user_meta=False, metas=['./colcon.meta'], base_paths=['/home/thomas/ros2_ws/src/vortex-image-filtering/image-filtering', '--log-base', '/dev/null'], packages_ignore=None, packages_ignore_regex=None, paths=None, packages_up_to=None, packages_up_to_regex=None, packages_above=None, packages_above_and_dependencies=None, packages_above_depth=None, packages_select_by_dep=None, packages_skip_by_dep=None, packages_skip_up_to=None, packages_select_build_failed=False, packages_skip_build_finished=False, packages_select_test_failures=False, packages_skip_test_passed=False, packages_select=None, packages_skip=None, packages_select_regex=None, packages_skip_regex=None, packages_start=None, packages_end=None, allow_overriding=[], topological_order=False, names_only=False, paths_only=True, topological_graph=False, topological_graph_dot=False, topological_graph_density=False, topological_graph_legend=False, topological_graph_dot_cluster=False, topological_graph_dot_include_skipped=False, mixin_files=None, mixin=None, verb_parser=, verb_extension=, main=>, mixin_verb=('list',)) -[1.029s] Level 1:colcon.colcon_core.package_discovery:discover_packages(colcon_meta) check parameters -[1.029s] Level 1:colcon.colcon_core.package_discovery:discover_packages(recursive) check parameters -[1.029s] Level 1:colcon.colcon_core.package_discovery:discover_packages(ignore) check parameters -[1.029s] Level 1:colcon.colcon_core.package_discovery:discover_packages(path) check parameters -[1.029s] Level 1:colcon.colcon_core.package_discovery:discover_packages(recursive) discover -[1.030s] INFO:colcon.colcon_core.package_discovery:Crawling recursively for packages in '/home/thomas/ros2_ws/src/vortex-image-filtering/image-filtering', '/home/thomas/ros2_ws/src/vortex-image-filtering/image-filtering/--log-base', '/dev/null' -[1.030s] Level 1:colcon.colcon_core.package_identification:_identify(/home/thomas/ros2_ws/src/vortex-image-filtering/image-filtering) by extensions ['ignore', 'ignore_ament_install'] -[1.031s] Level 1:colcon.colcon_core.package_identification:_identify(/home/thomas/ros2_ws/src/vortex-image-filtering/image-filtering) by extension 'ignore' -[1.031s] Level 1:colcon.colcon_core.package_identification:_identify(/home/thomas/ros2_ws/src/vortex-image-filtering/image-filtering) by extension 'ignore_ament_install' -[1.032s] Level 1:colcon.colcon_core.package_identification:_identify(/home/thomas/ros2_ws/src/vortex-image-filtering/image-filtering) by extensions ['colcon_pkg'] -[1.032s] Level 1:colcon.colcon_core.package_identification:_identify(/home/thomas/ros2_ws/src/vortex-image-filtering/image-filtering) by extension 'colcon_pkg' -[1.032s] Level 1:colcon.colcon_core.package_identification:_identify(/home/thomas/ros2_ws/src/vortex-image-filtering/image-filtering) by extensions ['colcon_meta'] -[1.032s] Level 1:colcon.colcon_core.package_identification:_identify(/home/thomas/ros2_ws/src/vortex-image-filtering/image-filtering) by extension 'colcon_meta' -[1.032s] Level 1:colcon.colcon_core.package_identification:_identify(/home/thomas/ros2_ws/src/vortex-image-filtering/image-filtering) by extensions ['ros'] -[1.032s] Level 1:colcon.colcon_core.package_identification:_identify(/home/thomas/ros2_ws/src/vortex-image-filtering/image-filtering) by extension 'ros' -[1.121s] DEBUG:colcon.colcon_core.package_identification:Package '/home/thomas/ros2_ws/src/vortex-image-filtering/image-filtering' with type 'ros.ament_cmake' and name 'image_filtering' -[1.121s] Level 1:colcon.colcon_core.package_discovery:discover_packages(recursive) using defaults diff --git a/log/COLCON_IGNORE b/log/COLCON_IGNORE deleted file mode 100644 index e69de29..0000000 diff --git a/log/latest b/log/latest deleted file mode 120000 index 1715667..0000000 --- a/log/latest +++ /dev/null @@ -1 +0,0 @@ -latest_list \ No newline at end of file diff --git a/log/latest_list b/log/latest_list deleted file mode 120000 index dd5d041..0000000 --- a/log/latest_list +++ /dev/null @@ -1 +0,0 @@ -list_2025-10-22_13-00-40 \ No newline at end of file diff --git a/log/list_2025-10-22_13-00-40/logger_all.log b/log/list_2025-10-22_13-00-40/logger_all.log deleted file mode 100644 index d91e83d..0000000 --- a/log/list_2025-10-22_13-00-40/logger_all.log +++ /dev/null @@ -1,36 +0,0 @@ -[0.380s] DEBUG:colcon:Command line arguments: ['/usr/bin/colcon', 'list', '-p', '--base-paths', '/home/thomas/ros2_ws/src/vortex-image-filtering', '--log-base', '/dev/null'] -[0.381s] DEBUG:colcon:Parsed command line arguments: Namespace(log_base=None, log_level=None, verb_name='list', build_base='build', ignore_user_meta=False, metas=['./colcon.meta'], base_paths=['/home/thomas/ros2_ws/src/vortex-image-filtering', '--log-base', '/dev/null'], packages_ignore=None, packages_ignore_regex=None, paths=None, packages_up_to=None, packages_up_to_regex=None, packages_above=None, packages_above_and_dependencies=None, packages_above_depth=None, packages_select_by_dep=None, packages_skip_by_dep=None, packages_skip_up_to=None, packages_select_build_failed=False, packages_skip_build_finished=False, packages_select_test_failures=False, packages_skip_test_passed=False, packages_select=None, packages_skip=None, packages_select_regex=None, packages_skip_regex=None, packages_start=None, packages_end=None, allow_overriding=[], topological_order=False, names_only=False, paths_only=True, topological_graph=False, topological_graph_dot=False, topological_graph_density=False, topological_graph_legend=False, topological_graph_dot_cluster=False, topological_graph_dot_include_skipped=False, mixin_files=None, mixin=None, verb_parser=, verb_extension=, main=>, mixin_verb=('list',)) -[0.967s] Level 1:colcon.colcon_core.package_discovery:discover_packages(colcon_meta) check parameters -[0.967s] Level 1:colcon.colcon_core.package_discovery:discover_packages(recursive) check parameters -[0.967s] Level 1:colcon.colcon_core.package_discovery:discover_packages(ignore) check parameters -[0.967s] Level 1:colcon.colcon_core.package_discovery:discover_packages(path) check parameters -[0.967s] Level 1:colcon.colcon_core.package_discovery:discover_packages(recursive) discover -[0.968s] INFO:colcon.colcon_core.package_discovery:Crawling recursively for packages in '/home/thomas/ros2_ws/src/vortex-image-filtering', '/home/thomas/ros2_ws/src/vortex-image-filtering/--log-base', '/dev/null' -[0.968s] Level 1:colcon.colcon_core.package_identification:_identify(/home/thomas/ros2_ws/src/vortex-image-filtering) by extensions ['ignore', 'ignore_ament_install'] -[0.969s] Level 1:colcon.colcon_core.package_identification:_identify(/home/thomas/ros2_ws/src/vortex-image-filtering) by extension 'ignore' -[0.970s] Level 1:colcon.colcon_core.package_identification:_identify(/home/thomas/ros2_ws/src/vortex-image-filtering) by extension 'ignore_ament_install' -[0.970s] Level 1:colcon.colcon_core.package_identification:_identify(/home/thomas/ros2_ws/src/vortex-image-filtering) by extensions ['colcon_pkg'] -[0.970s] Level 1:colcon.colcon_core.package_identification:_identify(/home/thomas/ros2_ws/src/vortex-image-filtering) by extension 'colcon_pkg' -[0.970s] Level 1:colcon.colcon_core.package_identification:_identify(/home/thomas/ros2_ws/src/vortex-image-filtering) by extensions ['colcon_meta'] -[0.970s] Level 1:colcon.colcon_core.package_identification:_identify(/home/thomas/ros2_ws/src/vortex-image-filtering) by extension 'colcon_meta' -[0.970s] Level 1:colcon.colcon_core.package_identification:_identify(/home/thomas/ros2_ws/src/vortex-image-filtering) by extensions ['ros'] -[0.970s] Level 1:colcon.colcon_core.package_identification:_identify(/home/thomas/ros2_ws/src/vortex-image-filtering) by extension 'ros' -[1.051s] Level 1:colcon.colcon_core.package_identification:_identify(/home/thomas/ros2_ws/src/vortex-image-filtering) by extensions ['cmake', 'python'] -[1.052s] Level 1:colcon.colcon_core.package_identification:_identify(/home/thomas/ros2_ws/src/vortex-image-filtering) by extension 'cmake' -[1.052s] Level 1:colcon.colcon_core.package_identification:_identify(/home/thomas/ros2_ws/src/vortex-image-filtering) by extension 'python' -[1.052s] Level 1:colcon.colcon_core.package_identification:_identify(/home/thomas/ros2_ws/src/vortex-image-filtering) by extensions ['python_setup_py'] -[1.052s] Level 1:colcon.colcon_core.package_identification:_identify(/home/thomas/ros2_ws/src/vortex-image-filtering) by extension 'python_setup_py' -[1.053s] Level 1:colcon.colcon_core.package_identification:_identify(/home/thomas/ros2_ws/src/vortex-image-filtering/image-filtering) by extensions ['ignore', 'ignore_ament_install'] -[1.053s] Level 1:colcon.colcon_core.package_identification:_identify(/home/thomas/ros2_ws/src/vortex-image-filtering/image-filtering) by extension 'ignore' -[1.054s] Level 1:colcon.colcon_core.package_identification:_identify(/home/thomas/ros2_ws/src/vortex-image-filtering/image-filtering) by extension 'ignore_ament_install' -[1.054s] Level 1:colcon.colcon_core.package_identification:_identify(/home/thomas/ros2_ws/src/vortex-image-filtering/image-filtering) by extensions ['colcon_pkg'] -[1.054s] Level 1:colcon.colcon_core.package_identification:_identify(/home/thomas/ros2_ws/src/vortex-image-filtering/image-filtering) by extension 'colcon_pkg' -[1.054s] Level 1:colcon.colcon_core.package_identification:_identify(/home/thomas/ros2_ws/src/vortex-image-filtering/image-filtering) by extensions ['colcon_meta'] -[1.054s] Level 1:colcon.colcon_core.package_identification:_identify(/home/thomas/ros2_ws/src/vortex-image-filtering/image-filtering) by extension 'colcon_meta' -[1.054s] Level 1:colcon.colcon_core.package_identification:_identify(/home/thomas/ros2_ws/src/vortex-image-filtering/image-filtering) by extensions ['ros'] -[1.054s] Level 1:colcon.colcon_core.package_identification:_identify(/home/thomas/ros2_ws/src/vortex-image-filtering/image-filtering) by extension 'ros' -[1.065s] DEBUG:colcon.colcon_core.package_identification:Package '/home/thomas/ros2_ws/src/vortex-image-filtering/image-filtering' with type 'ros.ament_cmake' and name 'image_filtering' -[1.066s] Level 1:colcon.colcon_core.package_identification:_identify(/home/thomas/ros2_ws/src/vortex-image-filtering/log) by extensions ['ignore', 'ignore_ament_install'] -[1.066s] Level 1:colcon.colcon_core.package_identification:_identify(/home/thomas/ros2_ws/src/vortex-image-filtering/log) by extension 'ignore' -[1.066s] Level 1:colcon.colcon_core.package_identification:_identify(/home/thomas/ros2_ws/src/vortex-image-filtering/log) ignored -[1.067s] Level 1:colcon.colcon_core.package_discovery:discover_packages(recursive) using defaults From bc9b5f3bddb050aed4759d51c11f20fd7598a4f8 Mon Sep 17 00:00:00 2001 From: Thomas Date: Fri, 9 Jan 2026 11:01:00 +0100 Subject: [PATCH 52/53] =?UTF-8?q?Fixing=20some=20of=20j=C3=B8rgens=20compl?= =?UTF-8?q?aints?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- README.md | 6 +++--- image-filtering/config/image_filtering_params.yaml | 2 +- .../include/image_filters/image_filtering_ros.hpp | 2 +- image-filtering/include/image_filters/image_processing.hpp | 6 +++--- image-filtering/include/image_filters/ros_utils.hpp | 0 image-filtering/include/image_filters/utilities.hpp | 2 +- image-filtering/src/image_filtering_ros.cpp | 4 ++-- image-filtering/src/image_processing.cpp | 2 +- image-filtering/src/ros_utils.cpp | 0 image-filtering/src/utilities.cpp | 2 +- 10 files changed, 13 insertions(+), 13 deletions(-) create mode 100644 image-filtering/include/image_filters/ros_utils.hpp create mode 100644 image-filtering/src/ros_utils.cpp diff --git a/README.md b/README.md index 38252a1..53560ea 100644 --- a/README.md +++ b/README.md @@ -32,7 +32,7 @@ Parameters can be set through a YAML file or dynamically adjusted at runtime. ## Implementing New Filters -To extend the functionality of the `image_filtering_node` by adding new filters, follow these steps to ensure compatibility and integration with the existing codebase. There should be //TODO(Vortex) comments where you add your filter: +To extend the functionality of the `image_filtering_node` by adding new filters, follow these steps to ensure compatibility and integration with the existing codebase. There should be //TODO(New filter) comments where you add your filter: ### Step 1: Filter Enum @@ -130,7 +130,7 @@ In the [image_filtering_params.yaml](image-filtering/config/image_filtering_para ### Step 7: Declare and Assign Parameters -In the constructor of your ROS 2 node, declare each of the new filter parameters using the `declare_parameter` function in [image_filtering_ros.cpp](image-filtering/src/image_filtering_ros.cpp). This sets the default values and prepares the node to accept these parameters at runtime through command line or the YAML configuration file. +In the constructor of your ROS 2 node, declare each of the new filter parameters using the `declare_parameter` function in [image_filtering_ros.cpp](image-filtering/src/image_filtering_ros.cpp). This declares the ros2 parameters and prepares the node to accept them at runtime through command line or the YAML configuration file. ```cpp void ImageFilteringNode::declare_parameters() { @@ -168,4 +168,4 @@ void ImageFilteringNode::set_filter_params() { #### Adding Helper functions -If you need helper functions for your filter, you can add the declaration to [utilities.hpp](image-filtering/include/image_filters/utilities.hpp), and then add the function definition to [utilities.cpp](image-filtering/src/utilities.cpp). There will be TODO(Vortex) comments where you can add them. These functions are already included in the image_processing files. +If you need helper functions for your filter, you can add the declaration to [utilities.hpp](image-filtering/include/image_filters/utilities.hpp), and then add the function definition to [utilities.cpp](image-filtering/src/utilities.cpp). There will be TODO(New filter) comments where you can add them. These functions are already included in the image_processing files. diff --git a/image-filtering/config/image_filtering_params.yaml b/image-filtering/config/image_filtering_params.yaml index d860153..b69cf75 100644 --- a/image-filtering/config/image_filtering_params.yaml +++ b/image-filtering/config/image_filtering_params.yaml @@ -41,7 +41,7 @@ maxval: 255. invert: true - # TODO(Vortex): add your filterparameters here + # TODO(New filter): add your filterparameters here example: example_int: 1 example_string: "Get filtered" diff --git a/image-filtering/include/image_filters/image_filtering_ros.hpp b/image-filtering/include/image_filters/image_filtering_ros.hpp index b8203d7..10fd982 100644 --- a/image-filtering/include/image_filters/image_filtering_ros.hpp +++ b/image-filtering/include/image_filters/image_filtering_ros.hpp @@ -1,7 +1,7 @@ #ifndef IMAGE_FILTERS__IMAGE_FILTERING_ROS_HPP_ #define IMAGE_FILTERS__IMAGE_FILTERING_ROS_HPP_ -#include // for jazzy this has to be hpp and for humble it has to be h +#include #include #include #include diff --git a/image-filtering/include/image_filters/image_processing.hpp b/image-filtering/include/image_filters/image_processing.hpp index 027fb7d..59046f1 100644 --- a/image-filtering/include/image_filters/image_processing.hpp +++ b/image-filtering/include/image_filters/image_processing.hpp @@ -12,7 +12,7 @@ #include #include -enum class FilterType { // TODO(Vortex): Add filters here +enum class FilterType { // TODO(New filter): Add filters here NoFilter, Flip, Unsharpening, @@ -42,7 +42,7 @@ static constexpr std::pair kFilterMap[] = { {"median_binary", FilterType::MedianBinary}, {"binary", FilterType::Binary}, - // TODO(Vortex): Also add your filter here + // TODO(New filter): Also add your filter here {"example", FilterType::Example}, {"unknown", FilterType::Unknown}}; @@ -315,7 +315,7 @@ class BinaryThreshold : public Filter { BinaryThresholdParams filter_params; }; -// TODO(Vortex): add this structure for your filter +// TODO(New filter): add this structure for your filter ///////////////////////////// // Example diff --git a/image-filtering/include/image_filters/ros_utils.hpp b/image-filtering/include/image_filters/ros_utils.hpp new file mode 100644 index 0000000..e69de29 diff --git a/image-filtering/include/image_filters/utilities.hpp b/image-filtering/include/image_filters/utilities.hpp index abf1cfe..2ad41f8 100644 --- a/image-filtering/include/image_filters/utilities.hpp +++ b/image-filtering/include/image_filters/utilities.hpp @@ -61,7 +61,7 @@ void distance_field(const cv::Mat& binObstacles, int type = cv::DIST_L2, int maskSize = 3); // DIST_L2 is normal euclidean -// TODO(Vortex): If you need a helper function have the signature here +// TODO(New filter): If you need a helper function have the signature here void apply_example(const cv::Mat& original, cv::Mat& filtered, std::string example_string, diff --git a/image-filtering/src/image_filtering_ros.cpp b/image-filtering/src/image_filtering_ros.cpp index 8a59eff..0502ba3 100644 --- a/image-filtering/src/image_filtering_ros.cpp +++ b/image-filtering/src/image_filtering_ros.cpp @@ -55,7 +55,7 @@ void ImageFilteringNode::declare_parameters() { this->declare_parameter("filter_params.median_binary.threshold"); this->declare_parameter("filter_params.median_binary.invert"); - // TODO(Vortex): Declare parameters set for your filter here + // TODO(New filter): Declare parameters set for your filter here this->declare_parameter("filter_params.example.example_int"); this->declare_parameter( "filter_params.example.example_string"); @@ -213,7 +213,7 @@ void ImageFilteringNode::set_filter_params() { break; } - // TODO(Vortex): Add your filter case here: + // TODO(New filter): Add your filter case here: case FilterType::Example: { ExampleParams params; params.example_int = diff --git a/image-filtering/src/image_processing.cpp b/image-filtering/src/image_processing.cpp index 6860c29..cbdb774 100644 --- a/image-filtering/src/image_processing.cpp +++ b/image-filtering/src/image_processing.cpp @@ -178,7 +178,7 @@ void BinaryThreshold::apply_filter(const cv::Mat& original, cv::threshold(src8, filtered, thresh, maxval, type); } -// TODO(Vortex): Implement your filter here +// TODO(New filter): Implement your filter here void Example::apply_filter(const cv::Mat& original, cv::Mat& filtered) const { std::string example_str = this->filter_params.example_string; int example_int = this->filter_params.example_int; diff --git a/image-filtering/src/ros_utils.cpp b/image-filtering/src/ros_utils.cpp new file mode 100644 index 0000000..e69de29 diff --git a/image-filtering/src/utilities.cpp b/image-filtering/src/utilities.cpp index a3cef1d..696d4a6 100644 --- a/image-filtering/src/utilities.cpp +++ b/image-filtering/src/utilities.cpp @@ -242,7 +242,7 @@ void distance_field(const cv::Mat& binObstacles, // publish vis8u as "mono8" } -// TODO(Vortex): If you need a helper function define it here like this +// TODO(New filter): If you need a helper function define it here like this void apply_example(const cv::Mat& original, cv::Mat& filtered, From e8d5e0f422fb071bfc816d0225f6e4fd134ddc16 Mon Sep 17 00:00:00 2001 From: Thomas Date: Fri, 9 Jan 2026 13:20:06 +0100 Subject: [PATCH 53/53] fixing swich case for filtertype and try catch for running the filter, and adding somme util files for ros only --- .../image_filters/image_filtering_ros.hpp | 3 ++ .../image_filtering_ros_utils.hpp | 3 ++ .../image_filters/image_processing.hpp | 10 ++-- image-filtering/src/image_filtering_ros.cpp | 50 ++++++++++++------- .../image_filtering_ros_utils.cpp} | 0 image-filtering/src/image_processing.cpp | 2 +- image-filtering/src/ros_utils.cpp | 0 7 files changed, 44 insertions(+), 24 deletions(-) create mode 100644 image-filtering/include/image_filters/image_filtering_ros_utils.hpp rename image-filtering/{include/image_filters/ros_utils.hpp => src/image_filtering_ros_utils.cpp} (100%) delete mode 100644 image-filtering/src/ros_utils.cpp diff --git a/image-filtering/include/image_filters/image_filtering_ros.hpp b/image-filtering/include/image_filters/image_filtering_ros.hpp index 10fd982..fcc6a08 100644 --- a/image-filtering/include/image_filters/image_filtering_ros.hpp +++ b/image-filtering/include/image_filters/image_filtering_ros.hpp @@ -3,6 +3,8 @@ #include #include +#include +#include #include #include #include @@ -11,6 +13,7 @@ #include #include "image_processing.hpp" + class ImageFilteringNode : public rclcpp::Node { public: explicit ImageFilteringNode(const rclcpp::NodeOptions& options); diff --git a/image-filtering/include/image_filters/image_filtering_ros_utils.hpp b/image-filtering/include/image_filters/image_filtering_ros_utils.hpp new file mode 100644 index 0000000..eae73c4 --- /dev/null +++ b/image-filtering/include/image_filters/image_filtering_ros_utils.hpp @@ -0,0 +1,3 @@ +// FilterType ImageFilteringNode::create_binary_filter(){ + +// } \ No newline at end of file diff --git a/image-filtering/include/image_filters/image_processing.hpp b/image-filtering/include/image_filters/image_processing.hpp index 59046f1..3b50e83 100644 --- a/image-filtering/include/image_filters/image_processing.hpp +++ b/image-filtering/include/image_filters/image_processing.hpp @@ -9,6 +9,8 @@ #include #include #include +#include +#include #include #include @@ -60,9 +62,7 @@ inline FilterType parse_filter_type(std::string s) { if (s == name) return type; } - std::cout << "\033[33m No string connected to that filter type: '" << s - << "'. This might be misspelling or you need to add the filter " - "type to kFilterMap in image_processing.hpp\033[0m"; + spdlog::warn(fmt::format(fmt::fg(fmt::rgb(200, 180, 50)), "No string connected to that filter type: '{}'. This might be misspelling or you need to add the filter type to kFilterMap in image_processing.hpp", s)); return FilterType::Unknown; } @@ -71,8 +71,8 @@ inline std::string_view filtertype_to_string(FilterType t) { if (t == type) return name; } - std::cout << "\033[33m No string connected to your filter type. To fix " - "this add the string and filter type to kFilterMap\033[0m"; + spdlog::warn(fmt::format(fmt::fg(fmt::rgb(200, 180, 50)), " No string connected to your filter type. To fix " + "this add the string and filter type to kFilterMap")); return "unknown"; } diff --git a/image-filtering/src/image_filtering_ros.cpp b/image-filtering/src/image_filtering_ros.cpp index 0502ba3..3594f05 100644 --- a/image-filtering/src/image_filtering_ros.cpp +++ b/image-filtering/src/image_filtering_ros.cpp @@ -67,14 +67,6 @@ void ImageFilteringNode::set_filter_params() { FilterType filter_type = parse_filter_type(filter_type_string); switch (filter_type) { - case FilterType::Unknown: { - spdlog::warn( - "\033[33mInvalid filter type received: {}. Using default: " - "no_filter.\033[0m", - filter_type_string); - filter_type = FilterType::NoFilter; - [[fallthrough]]; - } case FilterType::NoFilter: { filter_ptr = std::make_unique(); @@ -228,19 +220,31 @@ void ImageFilteringNode::set_filter_params() { } default:; + if (filter_type == FilterType::Unknown){ + spdlog::warn(fmt::format(fmt::fg(fmt::rgb(200, 180, 50)), + "Invalid filter type received: {}. Using default: no_filter.", + filter_type_string)); + } + else{ + spdlog::warn(fmt::format(fmt::fg(fmt::rgb(200, 180, 50)), + "Filterparams has not been set for your chosen filter " + "{}. " + "To fix this add your filter to " + "ImageFilteringNode::set_filter_params(). " + "Using default: no_filter.", + filter_type_string)); + } + filter_ptr = std::make_unique(); - spdlog::warn( - "\033[33m Filterparams has not been set for your chosen filter " - "{}. " - "To fix this add your filter to " - "ImageFilteringNode::set_filter_params(). " - "Defaulting to no_filter. \033[0m", - filter_type_string); filter_type = FilterType::NoFilter; + + return; } - spdlog::info("\033[32m Using filter: {} \033[0m", - filtertype_to_string(filter_type)); + + + spdlog::info(fmt::format(fmt::fg(fmt::rgb(31, 161, 221)),"Using filter: {}", + filter_type_string)); } void ImageFilteringNode::check_and_subscribe_to_image_topic() { @@ -312,7 +316,17 @@ void ImageFilteringNode::image_callback( cv::Mat input_image = cv_ptr->image; cv::Mat filtered_image; - filter_ptr->apply_filter(input_image, filtered_image); + try{ + filter_ptr->apply_filter(input_image, filtered_image); + } + catch (const cv::Exception& e) { + spdlog::error(fmt::format(fmt::fg(fmt::rgb(31, 161, 221)), "OpenCV error while applying filter: {}", e.what())); + filtered_image = input_image.clone(); // fallback to no filter + } + catch (const std::exception& e) { + spdlog::error(fmt::format(fmt::fg(fmt::rgb(31, 161, 221)),"Error while applying filter: {}", e.what())); + filtered_image = input_image.clone(); + } std::string output_encoding = this->get_parameter("output_encoding").as_string(); diff --git a/image-filtering/include/image_filters/ros_utils.hpp b/image-filtering/src/image_filtering_ros_utils.cpp similarity index 100% rename from image-filtering/include/image_filters/ros_utils.hpp rename to image-filtering/src/image_filtering_ros_utils.cpp diff --git a/image-filtering/src/image_processing.cpp b/image-filtering/src/image_processing.cpp index cbdb774..a3c3a98 100644 --- a/image-filtering/src/image_processing.cpp +++ b/image-filtering/src/image_processing.cpp @@ -87,7 +87,7 @@ void OtsuSegmentation::apply_filter(const cv::Mat& original, original.copyTo(filtered); } // if its mono8 else - std::cout << "your image type is not matching this filter" << std::endl; + spdlog::error("your image type is not matching this filter"); if (gamma_auto_correction) { apply_auto_gamma(filtered, gamma_auto_correction_weight); diff --git a/image-filtering/src/ros_utils.cpp b/image-filtering/src/ros_utils.cpp deleted file mode 100644 index e69de29..0000000