diff --git a/CMakeLists.txt b/CMakeLists.txt index e4f8f6ff..6c696d6c 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -66,6 +66,7 @@ generate_dynamic_reconfigure_options( cfg/HSVColorFilter.cfg cfg/LabColorFilter.cfg cfg/WatershedSegmentation.cfg + cfg/BlobDetection.cfg ) ## Generate messages in the 'msg' folder @@ -102,6 +103,9 @@ add_message_files( Contour.msg ContourArray.msg ContourArrayStamped.msg + Blob.msg + BlobArray.msg + BlobArrayStamped.msg ) add_service_files( @@ -327,6 +331,10 @@ opencv_apps_add_nodelet(segment_objects src/nodelet/segment_objects_nodelet.cpp) # ./videostab.cpp opencv_apps_add_nodelet(watershed_segmentation src/nodelet/watershed_segmentation_nodelet.cpp) # ./watershed.cpp +if(OpenCV_VERSION VERSION_GREATER "3.0") + opencv_apps_add_nodelet(blob_detection src/nodelet/blob_detection_nodelet.cpp) # ./blob_detection.cpp +endif() + # ros examples opencv_apps_add_nodelet(simple_example src/nodelet/simple_example_nodelet.cpp) opencv_apps_add_nodelet(simple_compressed_example src/nodelet/simple_compressed_example_nodelet.cpp) @@ -387,9 +395,12 @@ if(CATKIN_ENABLE_TESTING) message(WARNING "roslaunch_add_file check fails with unsupported doc attributes ${roslaunch_VERSION}") else() file(GLOB LAUNCH_FILES launch/*.launch) + if(NOT OpenCV_VERSION VERSION_GREATER "3.0") + list(REMOVE_ITEM LAUNCH_FILES ${CMAKE_CURRENT_SOURCE_DIR}/launch/blob_detection.launch) + endif() foreach(LAUNCH_FILE ${LAUNCH_FILES}) roslaunch_add_file_check(${LAUNCH_FILE}) endforeach() endif() add_subdirectory(test) -endif() +endif() \ No newline at end of file diff --git a/cfg/BlobDetection.cfg b/cfg/BlobDetection.cfg new file mode 100755 index 00000000..9138e35e --- /dev/null +++ b/cfg/BlobDetection.cfg @@ -0,0 +1,72 @@ +#! /usr/bin/env python +# Software License Agreement (BSD License) +# +# Copyright (c) 2022, Hui Shi, University of Tartu. +# All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# * Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# * Redistributions in binary form must reproduce the above +# copyright notice, this list of conditions and the following +# disclaimer in the documentation and/or other materials provided +# with the distribution. +# * Neither the name of Hui Shi, University of Tartu nor the names of its +# contributors may be used to endorse or promote products derived +# from this software without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. + + +PACKAGE = "opencv_apps" + +from dynamic_reconfigure.parameter_generator_catkin import * + +gen = ParameterGenerator() +gen.add("use_camera_info", bool_t, 0, "Indicates that the camera_info topic should be subscribed to to get the default input_frame_id. Otherwise the frame from the image message will be used.", False) + +gen.add("hue_lower_limit", int_t, 0, "The minimum allowed field value Hue", 0, 0, 179) +gen.add("hue_upper_limit", int_t, 0, "The maximum allowed field value Hue", 179, 0, 179) +gen.add("sat_lower_limit", int_t, 0, "The minimum allowed field value Saturation", 0, 0, 255) +gen.add("sat_upper_limit", int_t, 0, "The maximum allowed field value Saturation", 255, 0, 255) +gen.add("val_lower_limit", int_t, 0, "The minimum allowed field value Value", 0, 0, 255) +gen.add("val_upper_limit", int_t, 0, "The maximum allowed field value Value", 255, 0, 255) + +morphology_ex_type = gen.enum([ gen.const("off", str_t, "off", "no morphological operation"), + gen.const("opening", str_t, "opening", "morphological operation type opening (erosion followed by dilation)"), + gen.const("closing", str_t, "closing", "morphological operation type closing (dilation followed by erosion)")], + "An enum for morphological operation type") +gen.add("morphology_ex_type", str_t, 0, "morphological operation type", "off", edit_method=morphology_ex_type) +gen.add("morphology_ex_kernel_size", int_t, 0, "morphological operation kernel size (should be odd number)", 3, 0, 100) + +gen.add("filter_by_color", bool_t, 0, "filter by color, use True/False to turn on/off the filtration", True) +gen.add("blob_color", int_t, 0, "Use blob_color = 0 to extract dark blobs and blob_color = 255 to extract light blobs.", 255, 0, 255) +gen.add("filter_by_area", bool_t, 0, "filter by area, use True/False to turn on/off the filtration", True) +gen.add("min_area", double_t, 0, "Extracted blobs have an area between minArea (inclusive) and maxArea (exclusive).", 0, 0, 40000) +gen.add("max_area", double_t, 0, "Extracted blobs have an area between minArea (inclusive) and maxArea (exclusive).", 40000, 0, 40000) +gen.add("min_dist_between_blobs", double_t, 0, "The blobs located closer than minDistBetweenBlobs are merged.", 0, 0, 200) +gen.add("filter_by_circularity", bool_t, 0, "filter by circularity, use True/False to turn on/off the filtration", False) +gen.add("min_circularity", double_t, 0, "Extracted blobs have circularity between minCircularity (inclusive) and maxCircularity (exclusive).", 0, 0, 1) +gen.add("max_circularity", double_t, 0, "Extracted blobs have circularity between minCircularity (inclusive) and maxCircularity (exclusive).", 1, 0, 1) +gen.add("filter_by_inertia", bool_t, 0, "filter by inertia, use True/False to turn on/off the filtration", False) +gen.add("min_inertia_ratio", double_t, 0, "Extracted blobs have this ratio between minInertiaRatio (inclusive) and maxInertiaRatio (exclusive).", 0, 0, 1) +gen.add("max_inertia_ratio", double_t, 0, "Extracted blobs have this ratio between minInertiaRatio (inclusive) and maxInertiaRatio (exclusive).", 1, 0, 1) +gen.add("filter_by_convexity", bool_t, 0, "filter by convexity, use True/False to turn on/off the filtration", False) +gen.add("min_convexity", double_t, 0, "Extracted blobs have convexity between minConvexity (inclusive) and maxConvexity (exclusive).", 0, 0, 1) +gen.add("max_convexity", double_t, 0, "Extracted blobs have convexity between minConvexity (inclusive) and maxConvexity (exclusive).", 1, 0, 1) + +exit(gen.generate(PACKAGE, "blob_detection", "BlobDetection")) \ No newline at end of file diff --git a/config/blob_detection.rviz b/config/blob_detection.rviz new file mode 100644 index 00000000..25f00cf9 --- /dev/null +++ b/config/blob_detection.rviz @@ -0,0 +1,180 @@ +Panels: + - Class: rviz/Displays + Help Height: 172 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /Status1 + - /Blob Detection Demo1 + - /Thresholded Image1 + - /Thresholded Image With Mask1 + - /MorphologyEx Image1 + Splitter Ratio: 0.5 + Tree Height: 848 + - Class: rviz/Selection + Name: Selection + - Class: rviz/Tool Properties + Expanded: + - /2D Pose Estimate1 + - /2D Nav Goal1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: rviz/Time + Experimental: false + Name: Time + SyncMode: 0 + SyncSource: Blob Detection Demo +Preferences: + PromptSaveOnExit: true +Toolbars: + toolButtonStyle: 2 +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.029999999329447746 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + - Class: rviz/Image + Enabled: true + Image Topic: /blob_detection/image + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: Blob Detection Demo + Normalize Range: true + Queue Size: 2 + Transport Hint: raw + Unreliable: false + Value: true + - Class: rviz/Image + Enabled: true + Image Topic: /blob_detection/thresholded_image + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: Thresholded Image + Normalize Range: true + Queue Size: 2 + Transport Hint: raw + Unreliable: false + Value: true + - Class: rviz/Image + Enabled: true + Image Topic: /blob_detection/thresholded_image_with_mask + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: Thresholded Image With Mask + Normalize Range: true + Queue Size: 2 + Transport Hint: raw + Unreliable: false + Value: true + - Class: rviz/Image + Enabled: true + Image Topic: /blob_detection/morphology_ex_image + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: MorphologyEx Image + Normalize Range: true + Queue Size: 2 + Transport Hint: raw + Unreliable: false + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Default Light: true + Fixed Frame: map + Frame Rate: 30 + Name: root + Tools: + - Class: rviz/Interact + Hide Inactive Objects: true + - Class: rviz/MoveCamera + - Class: rviz/Select + - Class: rviz/FocusCamera + - Class: rviz/Measure + - Class: rviz/SetInitialPose + Theta std deviation: 0.2617993950843811 + Topic: /initialpose + X std deviation: 0.5 + Y std deviation: 0.5 + - Class: rviz/SetGoal + Topic: /move_base_simple/goal + - Class: rviz/PublishPoint + Single click: true + Topic: /clicked_point + Value: true + Views: + Current: + Class: rviz/Orbit + Distance: 10 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 0 + Y: 0 + Z: 0 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 0.8353979587554932 + Target Frame: + Value: Orbit (rviz) + Yaw: 0.7603980302810669 + Saved: ~ +Window Geometry: + Blob Detection Demo: + collapsed: false + Displays: + collapsed: false + Height: 1472 + Hide Left Dock: false + Hide Right Dock: true + MorphologyEx Image: + collapsed: false + QMainWindow State: 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 + Selection: + collapsed: false + Thresholded Image: + collapsed: false + Thresholded Image With Mask: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: true + Width: 2426 + X: 134 + Y: 68 diff --git a/config/blob_detection_config.yaml b/config/blob_detection_config.yaml new file mode 100644 index 00000000..89f1e707 --- /dev/null +++ b/config/blob_detection_config.yaml @@ -0,0 +1,86 @@ +blob_color: 255 +debug_view: true +filter_by_area: true +filter_by_circularity: false +filter_by_color: true +filter_by_convexity: false +filter_by_inertia: false +hue_lower_limit: 0 +hue_upper_limit: 179 +image: + compressed: + format: jpeg + jpeg_quality: 80 + png_level: 9 + compressedDepth: + depth_max: 10.0 + depth_quantization: 100.0 + png_level: 9 + theora: + keyframe_frequency: 64 + optimize_for: 1 + quality: 31 + target_bitrate: 800000 +max_area: 40000.0 +max_area_upper_limit: 40000 +max_circularity: 1.0 +max_convexity: 1.0 +max_inertia_ratio: 1.0 +min_area: 0.0 +min_area_upper_limit: 40000 +min_circularity: 0.0 +min_convexity: 0.0 +min_dist_between_blobs: 0.0 +min_dist_between_blobs_upper_limit: 200 +min_inertia_ratio: 0.0 +morphology_ex_image: + compressed: + format: jpeg + jpeg_quality: 80 + png_level: 9 + compressedDepth: + depth_max: 10.0 + depth_quantization: 100.0 + png_level: 9 + theora: + keyframe_frequency: 64 + optimize_for: 1 + quality: 31 + target_bitrate: 800000 +morphology_ex_kernel_size: 3 +morphology_ex_type: 'off' +num_worker_threads: 1 +queue_size: 3 +sat_lower_limit: 0 +sat_upper_limit: 255 +thresholded_image: + compressed: + format: jpeg + jpeg_quality: 80 + png_level: 9 + compressedDepth: + depth_max: 10.0 + depth_quantization: 100.0 + png_level: 9 + theora: + keyframe_frequency: 64 + optimize_for: 1 + quality: 31 + target_bitrate: 800000 +thresholded_image_with_mask: + compressed: + format: jpeg + jpeg_quality: 80 + png_level: 9 + compressedDepth: + depth_max: 10.0 + depth_quantization: 100.0 + png_level: 9 + theora: + keyframe_frequency: 64 + optimize_for: 1 + quality: 31 + target_bitrate: 800000 +use_camera_info: false +val_lower_limit: 0 +val_upper_limit: 255 diff --git a/launch/blob_detection.launch b/launch/blob_detection.launch new file mode 100644 index 00000000..b9002a17 --- /dev/null +++ b/launch/blob_detection.launch @@ -0,0 +1,192 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/msg/Blob.msg b/msg/Blob.msg new file mode 100644 index 00000000..dc4cc9f6 --- /dev/null +++ b/msg/Blob.msg @@ -0,0 +1,2 @@ +Point2D center +float64 radius \ No newline at end of file diff --git a/msg/BlobArray.msg b/msg/BlobArray.msg new file mode 100644 index 00000000..cb21d422 --- /dev/null +++ b/msg/BlobArray.msg @@ -0,0 +1 @@ +Blob[] blobs \ No newline at end of file diff --git a/msg/BlobArrayStamped.msg b/msg/BlobArrayStamped.msg new file mode 100644 index 00000000..b7d3979e --- /dev/null +++ b/msg/BlobArrayStamped.msg @@ -0,0 +1,2 @@ +Header header +Blob[] blobs \ No newline at end of file diff --git a/nodelet_plugins.xml b/nodelet_plugins.xml index 62f76e3d..f7d1ab30 100644 --- a/nodelet_plugins.xml +++ b/nodelet_plugins.xml @@ -127,6 +127,10 @@ Nodelet for histogram equalization + + Nodelet to find blobs + + @@ -159,4 +163,4 @@ - + \ No newline at end of file diff --git a/package.xml b/package.xml index a2154776..04af6279 100644 --- a/package.xml +++ b/package.xml @@ -39,6 +39,8 @@ sensor_msgs std_msgs std_srvs + rqt_reconfigure + rviz roslaunch rostest @@ -52,4 +54,4 @@ - + \ No newline at end of file diff --git a/src/nodelet/blob_detection_nodelet.cpp b/src/nodelet/blob_detection_nodelet.cpp new file mode 100644 index 00000000..180b0cf3 --- /dev/null +++ b/src/nodelet/blob_detection_nodelet.cpp @@ -0,0 +1,574 @@ +// -*- coding:utf-8-unix; mode: c++; indent-tabs-mode: nil; c-basic-offset: 2; +// -*- +/********************************************************************* +* Software License Agreement (BSD License) +* +* Copyright (c) 2022, Hui Shi, University of Tartu. +* All rights reserved. +* +* Redistribution and use in source and binary forms, with or without +* modification, are permitted provided that the following conditions +* are met: +* +* * Redistributions of source code must retain the above copyright +* notice, this list of conditions and the following disclaimer. +* * Redistributions in binary form must reproduce the above +* copyright notice, this list of conditions and the following +* disclaimer in the documentation and/or other materials provided +* with the distribution. +* * Neither the name of the Hui Shi, University of Tartu nor the names of its +* contributors may be used to endorse or promote products derived +* from this software without specific prior written permission. +* +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +* POSSIBILITY OF SUCH DAMAGE. +*********************************************************************/ + +// https://github.com/ros-perception/opencv_apps +/** + * @file The package + * @brief Opencv_apps Package + * @author Opencv_apps team + */ + +// https://github.com/spmallick/learnopencv/blob/master/BlobDetector/blob.cpp +/** + * @file blob.cpp + * @brief Demo code for Blob Detection + * @author LearnOpenCV team + */ + +#include "opencv_apps/nodelet.h" +#include +#include +#include +#include + +#include +#include +#include + +#include "opencv_apps/Blob.h" +#include "opencv_apps/BlobArray.h" +#include "opencv_apps/BlobArrayStamped.h" +#include "opencv_apps/BlobDetectionConfig.h" +#include + +namespace opencv_apps +{ +class BlobDetectionNodelet : public opencv_apps::Nodelet +{ + image_transport::Publisher img_pub_; + image_transport::Publisher thresholded_img_pub_; + image_transport::Publisher thresholded_img_with_mask_pub_; + image_transport::Publisher morphology_ex_img_pub_; + image_transport::Subscriber img_sub_; + image_transport::CameraSubscriber cam_sub_; + ros::Publisher msg_pub_; + + boost::shared_ptr it_; + + typedef opencv_apps::BlobDetectionConfig Config; + typedef dynamic_reconfigure::Server ReconfigureServer; + Config config_; + boost::shared_ptr reconfigure_server_; + + int queue_size_; + bool debug_view_; + ros::Time prev_stamp_; + + std::string window_name_, thresholded_image_name_, thresholded_image_with_mask_name_, morphology_ex_image_name_, + blob_detector_params_name_; + static bool need_config_update_; + + cv::SimpleBlobDetector::Params params_; + cv::SimpleBlobDetector::Params prev_params_; + cv::Ptr detector_; + + // initial and max values of the parameters of interests. + int hue_lower_limit_; + int sat_lower_limit_; + int val_lower_limit_; + int hue_upper_limit_; + int sat_upper_limit_; + int val_upper_limit_; + + std::string morphology_ex_type_; + std::string morphology_ex_type_default_value_; + std::string prev_morphology_ex_type_; + int morphology_ex_kernel_size_; + int morphology_ex_kernel_size_initial_value_; + + int filter_by_color_int; // for trackbar, trackbar requires int type. + int blob_color_int; // Filter by color. + int filter_by_area_int; // Filter by Area. + int min_area_int; + int max_area_int; + int min_area_upper_limit_; + int max_area_upper_limit_; + int min_dist_between_blobs_int; // min dist between blobs + int min_dist_between_blobs_upper_limit_; + int filter_by_circularity_int; // Filter by Circularity + int min_circularity_int; + int max_circularity_int; + int filter_by_inertia_int; // Filter by Inertia + int min_inertia_ratio_int; + int max_inertia_ratio_int; + int filter_by_convexity_int; // Filter by Convexity + int min_convexity_int; + int max_convexity_int; + + // for checking if the blob detector params changes + bool compareBlobDetectorParams(const cv::SimpleBlobDetector::Params& params_1, + const cv::SimpleBlobDetector::Params& params_2) + { + if (params_1.filterByColor != params_2.filterByColor) + return false; + else if (params_1.blobColor != params_2.blobColor) + return false; + + else if (params_1.filterByArea != params_2.filterByArea) + return false; + else if (params_1.minArea != params_2.minArea) + return false; + else if (params_1.maxArea != params_2.maxArea) + return false; + + else if (params_1.minDistBetweenBlobs != params_2.minDistBetweenBlobs) + return false; + + else if (params_1.filterByCircularity != params_2.filterByCircularity) + return false; + else if (params_1.minCircularity != params_2.minCircularity) + return false; + else if (params_1.maxCircularity != params_2.maxCircularity) + return false; + + else if (params_1.filterByInertia != params_2.filterByInertia) + return false; + else if (params_1.minInertiaRatio != params_2.minInertiaRatio) + return false; + else if (params_1.maxInertiaRatio != params_2.maxInertiaRatio) + return false; + + else if (params_1.filterByConvexity != params_2.filterByConvexity) + return false; + else if (params_1.minConvexity != params_2.minConvexity) + return false; + else if (params_1.maxConvexity != params_2.maxConvexity) + return false; + + return true; + } + + // ROS dynamic reconfigure call back + void reconfigureCallback(Config& new_config, uint32_t level) + { + config_ = new_config; + hue_lower_limit_ = config_.hue_lower_limit; + sat_lower_limit_ = config_.sat_lower_limit; + val_lower_limit_ = config_.val_lower_limit; + hue_upper_limit_ = config_.hue_upper_limit; + sat_upper_limit_ = config_.sat_upper_limit; + val_upper_limit_ = config_.val_upper_limit; + + morphology_ex_type_ = config_.morphology_ex_type; + morphology_ex_kernel_size_ = config_.morphology_ex_kernel_size; + + params_.filterByColor = config_.filter_by_color; + params_.blobColor = config_.blob_color; + params_.filterByArea = config_.filter_by_area; + params_.minArea = config_.min_area; + params_.maxArea = config_.max_area; + params_.minDistBetweenBlobs = config_.min_dist_between_blobs; + params_.filterByCircularity = config_.filter_by_circularity; + params_.minCircularity = config_.min_circularity; + params_.maxCircularity = config_.max_circularity; + params_.filterByInertia = config_.filter_by_inertia; + params_.minInertiaRatio = config_.min_inertia_ratio; + params_.maxInertiaRatio = config_.max_inertia_ratio; + params_.filterByConvexity = config_.filter_by_convexity; + params_.minConvexity = config_.min_convexity; + params_.maxConvexity = config_.max_convexity; + + filter_by_color_int = int(params_.filterByColor); + blob_color_int = int(params_.blobColor); + filter_by_area_int = int(params_.filterByArea); + min_area_int = int(params_.minArea); + max_area_int = int(params_.maxArea); + min_dist_between_blobs_int = int(params_.minDistBetweenBlobs); + filter_by_circularity_int = int(params_.filterByCircularity); + min_circularity_int = int(params_.minCircularity * 100); + max_circularity_int = int(params_.maxCircularity * 100); + filter_by_inertia_int = int(params_.filterByInertia); + min_inertia_ratio_int = int(params_.minInertiaRatio * 100); + max_inertia_ratio_int = int(params_.maxInertiaRatio * 100); + filter_by_convexity_int = int(params_.filterByConvexity); + min_convexity_int = int(params_.minConvexity * 100); + max_convexity_int = int(params_.maxConvexity * 100); + } + + const std::string& frameWithDefault(const std::string& frame, const std::string& image_frame) + { + if (frame.empty()) + return image_frame; + return frame; + } + + void imageCallbackWithInfo(const sensor_msgs::ImageConstPtr& msg, const sensor_msgs::CameraInfoConstPtr& cam_info) + { + doWork(msg, cam_info->header.frame_id); + } + + void imageCallback(const sensor_msgs::ImageConstPtr& msg) + { + doWork(msg, msg->header.frame_id); + } + + // opencv trackbar callback + static void trackbarCallback(int /*unused*/, void* /*unused*/) + { + need_config_update_ = true; + } + + void doWork(const sensor_msgs::ImageConstPtr& msg, const std::string& input_frame_from_msg) + { + // Work on the image. + try + { + // Convert the image into something opencv can handle. + cv::Mat frame_src = cv_bridge::toCvShare(msg, sensor_msgs::image_encodings::BGR8)->image; + + // Messages + opencv_apps::BlobArrayStamped blobs_msg; + blobs_msg.header = msg->header; + + // Do the work + // if the image is gray image, convert it to BGR image + cv::Mat frame; + if (frame_src.channels() > 1) + { + frame = frame_src; + } + else + { + cv::cvtColor(frame_src, frame, cv::COLOR_GRAY2BGR); + } + + // create the windows, and attach the trackbars + if (debug_view_) + { + cv::namedWindow(window_name_, cv::WINDOW_AUTOSIZE); + cv::namedWindow(thresholded_image_name_, cv::WINDOW_AUTOSIZE); + cv::namedWindow(thresholded_image_with_mask_name_, cv::WINDOW_AUTOSIZE); + cv::namedWindow(blob_detector_params_name_, cv::WINDOW_AUTOSIZE); + + if (morphology_ex_type_ != prev_morphology_ex_type_) + { + if (prev_morphology_ex_type_ == "opening" || prev_morphology_ex_type_ == "closing") + { + cv::destroyWindow(morphology_ex_image_name_); + } + morphology_ex_kernel_size_ = morphology_ex_kernel_size_initial_value_; + config_.morphology_ex_type = morphology_ex_type_; + config_.morphology_ex_kernel_size = morphology_ex_kernel_size_; + reconfigure_server_->updateConfig(config_); + } + + if (morphology_ex_type_ == "opening" || morphology_ex_type_ == "closing") + { + if (morphology_ex_type_ == "opening") + { + morphology_ex_image_name_ = "Opening Image"; + } + else if (morphology_ex_type_ == "closing") + { + morphology_ex_image_name_ = "Closing Image"; + } + + cv::namedWindow(morphology_ex_image_name_, cv::WINDOW_AUTOSIZE); + cv::createTrackbar("MorphologyEx Kernel Size", morphology_ex_image_name_, &morphology_ex_kernel_size_, 100, + trackbarCallback); + } + + prev_morphology_ex_type_ = morphology_ex_type_; + + cv::createTrackbar("Hue Lower Limit", thresholded_image_name_, &hue_lower_limit_, 179, trackbarCallback); + cv::createTrackbar("Hue Upper Limit", thresholded_image_name_, &hue_upper_limit_, 179, trackbarCallback); + cv::createTrackbar("Sat Lower Limit", thresholded_image_name_, &sat_lower_limit_, 255, trackbarCallback); + cv::createTrackbar("Sat Upper Limit", thresholded_image_name_, &sat_upper_limit_, 255, trackbarCallback); + cv::createTrackbar("Val Lower Limit", thresholded_image_name_, &val_lower_limit_, 255, trackbarCallback); + cv::createTrackbar("Val Upper Limit", thresholded_image_name_, &val_upper_limit_, 255, trackbarCallback); + + cv::createTrackbar("Filter By Color", blob_detector_params_name_, &filter_by_color_int, 1, trackbarCallback); + cv::createTrackbar("Blob Color", blob_detector_params_name_, &blob_color_int, 255, trackbarCallback); + cv::createTrackbar("Filter By Area", blob_detector_params_name_, &filter_by_area_int, 1, trackbarCallback); + cv::createTrackbar("Min Area", blob_detector_params_name_, &min_area_int, min_area_upper_limit_, + trackbarCallback); + cv::createTrackbar("Max Area", blob_detector_params_name_, &max_area_int, max_area_upper_limit_, + trackbarCallback); + cv::createTrackbar("Min Dist Between Blobs", blob_detector_params_name_, &min_dist_between_blobs_int, + min_dist_between_blobs_upper_limit_, trackbarCallback); + cv::createTrackbar("Filter By Circularity", blob_detector_params_name_, &filter_by_circularity_int, 1, + trackbarCallback); + cv::createTrackbar("Min Circularity", blob_detector_params_name_, &min_circularity_int, 100, trackbarCallback); + cv::createTrackbar("Max Circularity", blob_detector_params_name_, &max_circularity_int, 100, trackbarCallback); + cv::createTrackbar("Filter By Inertia", blob_detector_params_name_, &filter_by_inertia_int, 1, trackbarCallback); + cv::createTrackbar("Min Inertia Ratio", blob_detector_params_name_, &min_inertia_ratio_int, 100, + trackbarCallback); + cv::createTrackbar("Max Inertia Ratio", blob_detector_params_name_, &max_inertia_ratio_int, 100, + trackbarCallback); + cv::createTrackbar("Filter By Convexity", blob_detector_params_name_, &filter_by_convexity_int, 1, + trackbarCallback); + cv::createTrackbar("Min Convexity", blob_detector_params_name_, &min_convexity_int, 100, trackbarCallback); + cv::createTrackbar("Max Convexity", blob_detector_params_name_, &max_convexity_int, 100, trackbarCallback); + + if (need_config_update_) + { + config_.hue_lower_limit = hue_lower_limit_; + config_.sat_lower_limit = sat_lower_limit_; + config_.val_lower_limit = val_lower_limit_; + config_.hue_upper_limit = hue_upper_limit_; + config_.sat_upper_limit = sat_upper_limit_; + config_.val_upper_limit = val_upper_limit_; + + config_.morphology_ex_kernel_size = morphology_ex_kernel_size_; + + config_.filter_by_color = params_.filterByColor = (bool)filter_by_color_int; + config_.blob_color = params_.blobColor = blob_color_int; + config_.filter_by_area = params_.filterByArea = (bool)filter_by_area_int; + config_.min_area = params_.minArea = (float)min_area_int; + config_.max_area = params_.maxArea = (float)max_area_int; + config_.min_dist_between_blobs = params_.minDistBetweenBlobs = (float)min_dist_between_blobs_int; + config_.filter_by_circularity = params_.filterByCircularity = (bool)filter_by_circularity_int; + config_.min_circularity = params_.minCircularity = (float)min_circularity_int / 100; + config_.max_circularity = params_.maxCircularity = (float)max_circularity_int / 100; + config_.filter_by_inertia = params_.filterByInertia = (bool)filter_by_inertia_int; + config_.min_inertia_ratio = params_.minInertiaRatio = (float)min_inertia_ratio_int / 100; + config_.max_inertia_ratio = params_.maxInertiaRatio = (float)max_inertia_ratio_int / 100; + config_.filter_by_convexity = params_.filterByConvexity = (bool)filter_by_convexity_int; + config_.min_convexity = params_.minConvexity = (float)min_convexity_int / 100; + config_.max_convexity = params_.maxConvexity = (float)max_convexity_int / 100; + + reconfigure_server_->updateConfig(config_); + need_config_update_ = false; + } + } + + // convert the image to HSV image + cv::Mat hsv_image; + cv::cvtColor(frame, hsv_image, cv::COLOR_BGR2HSV); + // threshold the HSV image + cv::Mat thresholded_image; + cv::inRange(hsv_image, cv::Scalar(hue_lower_limit_, sat_lower_limit_, val_lower_limit_), + cv::Scalar(hue_upper_limit_, sat_upper_limit_, val_upper_limit_), thresholded_image); + // thresholded image with a color mask + cv::Mat thresholded_image_with_mask; + cv::bitwise_and(frame, frame, thresholded_image_with_mask, thresholded_image); + + // do morphological operation if required + cv::Mat morphology_ex_image; + if (morphology_ex_type_ == "opening" || morphology_ex_type_ == "closing") + { + cv::Mat kernel; + // morphology_ex_kernel_size_ must be odd number + if (morphology_ex_kernel_size_ % 2 != 1) + { + morphology_ex_kernel_size_ = morphology_ex_kernel_size_ + 1; + } + + kernel = cv::getStructuringElement( + cv::MORPH_RECT, cv::Size(morphology_ex_kernel_size_, morphology_ex_kernel_size_), cv::Point(-1, -1)); + + if (morphology_ex_type_ == "opening") + { + cv::morphologyEx(thresholded_image, morphology_ex_image, cv::MORPH_OPEN, kernel); + } + else if (morphology_ex_type_ == "closing") + { + cv::morphologyEx(thresholded_image, morphology_ex_image, cv::MORPH_CLOSE, kernel); + } + } + + // create a new blob detector if the detector params changes + if (!compareBlobDetectorParams(params_, prev_params_)) + { + detector_ = cv::SimpleBlobDetector::create(params_); + } + + // keypoints will hold the results of the detection + std::vector keypoints; + // runs the actual detection + if (morphology_ex_type_ == "opening" || morphology_ex_type_ == "closing") + { + detector_->detect(morphology_ex_image, keypoints); + } + else + { + detector_->detect(thresholded_image, keypoints); + } + + prev_params_ = params_; + + // convert the detection results to ROS messages + for (const cv::KeyPoint& i : keypoints) + { + opencv_apps::Blob blob_msg; + blob_msg.center.x = i.pt.x; + blob_msg.center.y = i.pt.y; + blob_msg.radius = i.size; + blobs_msg.blobs.push_back(blob_msg); + } + + // draw circles on the blobs detected, for displaying purposes + cv::Mat out_image; + drawKeypoints(frame, keypoints, out_image, cv::Scalar(0, 0, 255), cv::DrawMatchesFlags::DRAW_RICH_KEYPOINTS); + + // shows the results + if (debug_view_) + { + cv::imshow(window_name_, out_image); + cv::imshow(thresholded_image_name_, thresholded_image); + cv::imshow(thresholded_image_with_mask_name_, thresholded_image_with_mask); + + if (morphology_ex_type_ == "opening" || morphology_ex_type_ == "closing") + { + cv::imshow(morphology_ex_image_name_, morphology_ex_image); + } + + // a waitKey for switching between different types of morphological + // operations via keyboard input. + // 'o' for opening, 'c' for closing, 'n' for no morphological operation + char c = (char)cv::waitKey(1); + switch (c) + { + case 'o': + morphology_ex_type_ = "opening"; + break; + case 'c': + morphology_ex_type_ = "closing"; + break; + case 'n': + morphology_ex_type_ = "off"; + break; + } + } + + // Publish the debug images if there is a subscriber. + if (thresholded_img_pub_.getNumSubscribers() > 0) + { + cv::Mat out_thresholded_image; + cv::cvtColor(thresholded_image, out_thresholded_image, cv::COLOR_GRAY2BGR); + sensor_msgs::Image::Ptr out_thresholded_img = + cv_bridge::CvImage(msg->header, "bgr8", out_thresholded_image).toImageMsg(); + thresholded_img_pub_.publish(out_thresholded_img); + } + + if (thresholded_img_with_mask_pub_.getNumSubscribers() > 0) + { + sensor_msgs::Image::Ptr out_thresholded_img_with_mask = + cv_bridge::CvImage(msg->header, "bgr8", thresholded_image_with_mask).toImageMsg(); + thresholded_img_with_mask_pub_.publish(out_thresholded_img_with_mask); + } + + if (morphology_ex_img_pub_.getNumSubscribers() > 0) + { + if (morphology_ex_type_ == "opening" || morphology_ex_type_ == "closing") + { + cv::Mat out_morphology_ex_image; + cv::cvtColor(morphology_ex_image, out_morphology_ex_image, cv::COLOR_GRAY2BGR); + sensor_msgs::Image::Ptr out_morphology_ex_img = + cv_bridge::CvImage(msg->header, "bgr8", out_morphology_ex_image).toImageMsg(); + morphology_ex_img_pub_.publish(out_morphology_ex_img); + } + } + // convert the output image to ROS messages + sensor_msgs::Image::Ptr out_img = cv_bridge::CvImage(msg->header, "bgr8", out_image).toImageMsg(); + // Publish the output image and blobs messages. + img_pub_.publish(out_img); + msg_pub_.publish(blobs_msg); + } + catch (cv::Exception& e) + { + NODELET_ERROR("Image processing error: %s %s %s %i", e.err.c_str(), e.func.c_str(), e.file.c_str(), e.line); + } + + prev_stamp_ = msg->header.stamp; + } + + void subscribe() // NOLINT(modernize-use-override) + { + NODELET_DEBUG("Subscribing to image topic."); + if (config_.use_camera_info) + cam_sub_ = it_->subscribeCamera("image", queue_size_, &BlobDetectionNodelet::imageCallbackWithInfo, this); + else + img_sub_ = it_->subscribe("image", queue_size_, &BlobDetectionNodelet::imageCallback, this); + } + + void unsubscribe() // NOLINT(modernize-use-override) + { + NODELET_DEBUG("Unsubscribing from image topic."); + img_sub_.shutdown(); + cam_sub_.shutdown(); + } + +public: + virtual void onInit() // NOLINT(modernize-use-override) + { + // initialize the nodelet + Nodelet::onInit(); + it_ = boost::shared_ptr(new image_transport::ImageTransport(*nh_)); + + pnh_->param("queue_size", queue_size_, 3); + pnh_->param("debug_view", debug_view_, false); + pnh_->param("morphology_ex_type", morphology_ex_type_, morphology_ex_type_default_value_); + pnh_->param("min_area_upper_limit", min_area_upper_limit_, 40000); + pnh_->param("max_area_upper_limit", max_area_upper_limit_, 40000); + pnh_->param("min_dist_between_blobs_upper_limit", min_dist_between_blobs_upper_limit_, 200); + + if (debug_view_) + { + always_subscribe_ = true; + } + prev_stamp_ = ros::Time(0, 0); + + window_name_ = "Blob Detection Demo"; + thresholded_image_name_ = "Thresholded Image"; + thresholded_image_with_mask_name_ = "Thresholded Image With Mask"; + morphology_ex_image_name_ = "Opening Image"; + blob_detector_params_name_ = "Blob Detector Params"; + + morphology_ex_type_default_value_ = "off"; + prev_morphology_ex_type_ = "off"; + morphology_ex_kernel_size_initial_value_ = 3; + + reconfigure_server_ = boost::make_shared>(*pnh_); + dynamic_reconfigure::Server::CallbackType f = + boost::bind(&BlobDetectionNodelet::reconfigureCallback, this, boost::placeholders::_1, boost::placeholders::_2); + reconfigure_server_->setCallback(f); + + img_pub_ = advertiseImage(*pnh_, "image", 1); + msg_pub_ = advertise(*pnh_, "blobs", 1); + + thresholded_img_pub_ = advertiseImage(*pnh_, "thresholded_image", 1); + thresholded_img_with_mask_pub_ = advertiseImage(*pnh_, "thresholded_image_with_mask", 1); + morphology_ex_img_pub_ = advertiseImage(*pnh_, "morphology_ex_image", 1); + + onInitPostProcess(); + } +}; +bool BlobDetectionNodelet::need_config_update_ = false; +} // namespace opencv_apps + +#include +PLUGINLIB_EXPORT_CLASS(opencv_apps::BlobDetectionNodelet, nodelet::Nodelet);