From c09258c84b8ff70ed837d236af43daf8d6f6ca0a Mon Sep 17 00:00:00 2001 From: HuiShiSerenaShi Date: Fri, 28 Jan 2022 17:16:45 +0200 Subject: [PATCH 01/17] test if the fork works --- src/nodelet/blob_detection_nodelet.cpp | 651 +++++++++++++++++++++++++ 1 file changed, 651 insertions(+) create mode 100644 src/nodelet/blob_detection_nodelet.cpp diff --git a/src/nodelet/blob_detection_nodelet.cpp b/src/nodelet/blob_detection_nodelet.cpp new file mode 100644 index 00000000..bc470174 --- /dev/null +++ b/src/nodelet/blob_detection_nodelet.cpp @@ -0,0 +1,651 @@ +// -*- coding:utf-8-unix; mode: c++; indent-tabs-mode: nil; c-basic-offset: 2; -*- +/********************************************************************* +* Software License Agreement (BSD License) +* +* Copyright (c) 2014, Kei Okada. +* 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 Kei Okada 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/Itseez/opencv/blob/2.4/samples/cpp/tutorial_code/ImgTrans/HoughCircle_Demo.cpp +/** + * @file HoughCircle_Demo.cpp + * @brief Demo code for Hough Transform + * @author OpenCV team + */ + +#include +#include "opencv_apps/nodelet.h" +#include +#include +#include + +#include +#include +#include + +#include +#include "opencv_apps/BlobDetectionConfig.h" +#include "opencv_apps/Blob.h" +#include "opencv_apps/BlobArray.h" +#include "opencv_apps/BlobArrayStamped.h" + +// 1. opencv trackbar debug +//2. ros dynamic setup mode, launch file with dynamic and rviz with img topic attached. +//3. nothing, only launch file load the yaml file loading, 2 and 3 use the same launch file, arg-mode, default-simple run which is the 3rd case +// another mode is 1st, then 2nd. +// Q1. setup mode, there is no thresholded image publish in the node. +// Q2. in launch file, how to switch between the launch of different group of nodes. +namespace opencv_apps +{ +class BlobDetectionNodelet : public opencv_apps::Nodelet +{ + image_transport::Publisher 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_, blob_detector_params_name_, opening_image_name_, closing_image_name_; // reference to camshift_nodelet + static bool need_config_update_; + + cv::SimpleBlobDetector::Params params_; // + cv::SimpleBlobDetector::Params prev_params_; + + //this is for opencv version 2: SimpleBlobDetector detector(params); check the following link for more info + // https://learnopencv.com/blob-detection-using-opencv-python-c/ + //cv::Ptr detector = cv::SimpleBlobDetector::create(params); // this line was without the 3 cv:: + // where should we create the detector and params ? https://ip.festo-didactic.com/DigitalEducation/EITManufacturing/UNICO/FDRenderer/index.html?LearningPath=0a4e3fe94887400cad125a990fd2d518&Language=EN&FDEP=true&NoPHP=true + cv::Ptr detector; + + // initial and max values of the parameters of interests. + int lowHue; + int lowSat; + int lowVal; + int highHue; // h_limit_max or high_hue_ ? + int highSat; + int highVal; + + std::string morphology_ex_type_; + int morphology_ex_kernel_size_; + int morphology_ex_kernel_size_initial_value_; + + int filterByColor_int; // for trackbar, because trackbar only accept int type. + int blobColor_int; + + int filterByArea_int; // ??? + int minArea_int; + int maxArea_int; + int max_minArea_; + int max_maxArea_; + + int minDistBetweenBlobs_int; + int max_minDistBetweenBlobs_; + // Filter by Area. + + // // Filter by Circularity + int filterByCircularity_int; + int minCircularity_int; + int maxCircularity_int; + + // Filter by Inertia + int filterByInertia_int; + int minInertiaRatio_int; + int maxInertiaRatio_int; + + // Filter by Convexity + int filterByConvexity_int; + int minConvexity_int; + int maxConvexity_int; + + bool compareBlobDetectorParams(cv::SimpleBlobDetector::Params params_1, cv::SimpleBlobDetector::Params params_2)// contour_moments line 61 counter1 counter2, & + { + 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; + } + + + void reconfigureCallback(Config& new_config, uint32_t level) + { + config_ = new_config; + + lowHue = config_.lowHue; + lowSat = config_.lowSat; + lowVal = config_.lowVal; + highHue = config_.highHue; + highSat = config_.highSat; + highVal = config_.highVal; + + morphology_ex_kernel_size_ = config_.morphology_ex_kernel_size; + + params_.filterByColor = config_.filterByColor; + params_.blobColor = config_.blobColor; + + // // Filter by Area. + params_.filterByArea = config_.filterByArea; + params_.minArea = config_.minArea; + params_.maxArea = config_.maxArea; + + params_.minDistBetweenBlobs = config_.minDistBetweenBlobs; + + // // Filter by Circularity + params_.filterByCircularity = config_.filterByCircularity; + params_.minCircularity = config_.minCircularity; + params_.maxCircularity = config_.maxCircularity; + + // Filter by Inertia + params_.filterByInertia = config_.filterByInertia; + params_.minInertiaRatio = config_.minInertiaRatio; + params_.maxInertiaRatio = config_.maxInertiaRatio; + // Filter by Convexity + params_.filterByConvexity = config_.filterByConvexity; + params_.minConvexity = config_.minConvexity; + params_.maxConvexity = config_.maxConvexity; + + + filterByColor_int = int(params_.filterByColor); + blobColor_int = int(params_.blobColor); + + filterByArea_int = int(params_.filterByArea); + minArea_int = int(params_.minArea); + maxArea_int = int(params_.maxArea); + + minDistBetweenBlobs_int = int(params_.minDistBetweenBlobs); + + filterByCircularity_int = int(params_.filterByCircularity); + minCircularity_int = int(params_.minCircularity*100); + std::cout << "test reconfigure callback convert int(params_.minCircularity*100) to : " << minCircularity_int << std::endl; + maxCircularity_int = int(params_.maxCircularity*100); + std::cout << "test reconfigure callback convert int(params_.maxCircularity*100) to : " << maxCircularity_int << std::endl; + + filterByInertia_int = int(params_.filterByInertia); + minInertiaRatio_int = int(params_.minInertiaRatio*100); + std::cout << "test reconfigure callback convert int(params_.minInertiaRatio*100) to : " << minInertiaRatio_int << std::endl; + maxInertiaRatio_int = int(params_.maxInertiaRatio*100); + std::cout << "test reconfigure callback convert int(params_.maxInertiaRatio*100) to : " << maxInertiaRatio_int << std::endl; + + filterByConvexity_int = int(params_.filterByConvexity); + minConvexity_int = int(params_.minConvexity*100); + std::cout << "test reconfigure callback convert int(params_.minConvexity*100) to : " << minConvexity_int << std::endl; + maxConvexity_int = int(params_.maxConvexity*100); + std::cout << "test reconfigure callback convert int(params_.maxConvexity*100) to : " << maxConvexity_int << std::endl; + + ROS_INFO("test for reconfigure call back."); + } + + 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); + } + + // static void trackbarCallback(int /*unused*/, void* /*unused*/) + static void trackbarCallback(int value, void* userdata) + { + need_config_update_ = true; + ROS_INFO("test for trackbar call back."); + } + + 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 = cv_bridge::toCvShare(msg, sensor_msgs::image_encodings::BGR8)->image; + + // Messages + opencv_apps::BlobArrayStamped blobs_msg; + blobs_msg.header = msg->header; + + // Do the work + // do we need to check whether the frame is BGR ? since we convert it from BGR to HSV +// Serena : change to ours, do thresholding here + // if (frame.channels() > 1) + // { + // cv::cvtColor(frame, src_gray, cv::COLOR_BGR2GRAY); + // } + // else + // { + // src_gray = frame; + // } + + // create the main window, 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_ == "opening") + { + cv::namedWindow(opening_image_name_, cv::WINDOW_AUTOSIZE); + cv::createTrackbar("morphology_ex_kernel_size", opening_image_name_, &morphology_ex_kernel_size_, 100, trackbarCallback); + } + + else if (morphology_ex_type_ == "closing") + { + cv::namedWindow(closing_image_name_, cv::WINDOW_AUTOSIZE); + cv::createTrackbar("morphology_ex_kernel_size", closing_image_name_, &morphology_ex_kernel_size_, 100, trackbarCallback); + } + + cv::createTrackbar("lowHue", thresholded_image_name_, &lowHue, 179, trackbarCallback);// should we increase the range? const int max_value_H = 360/2; https://docs.opencv.org/3.4/da/d97/tutorial_threshold_inRange.html + cv::createTrackbar("lowSat", thresholded_image_name_, &lowSat, 255, trackbarCallback); + cv::createTrackbar("lowVal", thresholded_image_name_, &lowVal, 255, trackbarCallback); + + cv::createTrackbar("highHue", thresholded_image_name_, &highHue, 179, trackbarCallback); + cv::createTrackbar("highSat", thresholded_image_name_, &highSat, 255, trackbarCallback); + cv::createTrackbar("highVal", thresholded_image_name_, &highVal, 255, trackbarCallback); + + cv::createTrackbar("filterByColor", blob_detector_params_name_, &filterByColor_int, 1, trackbarCallback); + cv::createTrackbar("blobColor", blob_detector_params_name_, &blobColor_int, 255, trackbarCallback); + + cv::createTrackbar("filterByArea", blob_detector_params_name_, &filterByArea_int, 1, trackbarCallback); + cv::createTrackbar("minArea", blob_detector_params_name_, &minArea_int, max_minArea_, trackbarCallback); + cv::createTrackbar("maxArea", blob_detector_params_name_, &maxArea_int, max_maxArea_, trackbarCallback);// ?? + + cv::createTrackbar("minDistBetweenBlobs", blob_detector_params_name_, &minDistBetweenBlobs_int, max_minDistBetweenBlobs_, trackbarCallback);// ?? + + cv::createTrackbar("filterByCircularity", blob_detector_params_name_, &filterByCircularity_int, 1, trackbarCallback); + cv::createTrackbar("minCircularity", blob_detector_params_name_, &minCircularity_int, 100, trackbarCallback); + cv::createTrackbar("maxCircularity", blob_detector_params_name_, &maxCircularity_int, 100, trackbarCallback); + + cv::createTrackbar("filterByInertia", blob_detector_params_name_, &filterByInertia_int, 1, trackbarCallback); + cv::createTrackbar("minInertiaRatio", blob_detector_params_name_, &minInertiaRatio_int, 100, trackbarCallback); + cv::createTrackbar("maxInertiaRatio", blob_detector_params_name_, &maxInertiaRatio_int, 100, trackbarCallback); + + cv::createTrackbar("filterByConvexity", blob_detector_params_name_, &filterByConvexity_int, 1, trackbarCallback); + cv::createTrackbar("minConvexity", blob_detector_params_name_, &minConvexity_int, 100, trackbarCallback); + cv::createTrackbar("maxConvexity", blob_detector_params_name_, &maxConvexity_int, 100, trackbarCallback); + + if (need_config_update_) + { + config_.lowHue = lowHue; + config_.lowSat = lowSat; + config_.lowVal = lowVal; + config_.highHue = highHue; + config_.highSat = highSat; + config_.highVal = highVal; + + config_.morphology_ex_kernel_size = morphology_ex_kernel_size_; + + + config_.filterByColor = params_.filterByColor = (bool)filterByColor_int; + config_.blobColor = params_.blobColor = blobColor_int; + + // // Filter by Area. + config_.filterByArea = params_.filterByArea = (bool)filterByArea_int; + config_.minArea = params_.minArea = (double)minArea_int;//should be the same after printing the 2 types + std::cout << "test trackbar changes params_.minArea to : " << params_.minArea << std::endl; + config_.maxArea = params_.maxArea = (double)maxArea_int;// use float + std::cout << "test trackbar changes params_.maxArea to : " << params_.maxArea << std::endl; + + config_.minDistBetweenBlobs = params_.minDistBetweenBlobs = (double)minDistBetweenBlobs_int; + std::cout << "test trackbar changes params_.minDistBetweenBlobs to : " << params_.minDistBetweenBlobs << std::endl; + + + // // Filter by Circularity + config_.filterByCircularity = params_.filterByCircularity = (bool)filterByCircularity_int; + config_.minCircularity = params_.minCircularity = (double)minCircularity_int/100.00; + std::cout << "test trackbar changes params_.minCircularity to : " << params_.minCircularity << std::endl; + config_.maxCircularity = params_.maxCircularity = (double)maxCircularity_int/100.00; + std::cout << "test trackbar changes params_.maxCircularity to : " << params_.maxCircularity << std::endl; + + // Filter by Inertia + config_.filterByInertia = params_.filterByInertia = (bool)filterByInertia_int; + config_.minInertiaRatio = params_.minInertiaRatio = (double)minInertiaRatio_int/100.00; + std::cout << "test trackbar changes params_.minInertiaRatio to : " << params_.minInertiaRatio << std::endl; + config_.maxInertiaRatio = params_.maxInertiaRatio = (double)maxInertiaRatio_int/100.00; + std::cout << "test trackbar changes params_.maxInertiaRatio to : " << params_.maxInertiaRatio << std::endl; + + // Filter by Convexity + config_.filterByConvexity = params_.filterByConvexity = (bool)filterByConvexity_int; + config_.minConvexity = params_.minConvexity = (double)minConvexity_int/100.00; + std::cout << "test trackbar changes params_.minConvexity to : " << params_.minConvexity << std::endl; + config_.maxConvexity = params_.maxConvexity = (double)maxConvexity_int/100.00; + std::cout << "test trackbar changes params_.maxConvexity to : " << params_.maxConvexity << std::endl; + + ROS_INFO("test for before updating config. when the trackbar moves, the params server is updated."); + reconfigure_server_->updateConfig(config_); + need_config_update_ = false; + } + } + + cv::Mat HSV_image; + cv::cvtColor(frame, HSV_image, cv::COLOR_BGR2HSV); // hsv hue range is 180 and hsv full is 360 + cv::Mat thresholded_image; + cv::inRange(HSV_image, cv::Scalar(lowHue, lowSat, lowVal), cv::Scalar(highHue, highSat, highVal), thresholded_image); // there were not the 2 cv:: + + cv::Mat thresholded_image_with_mask; + cv::bitwise_and(frame,frame,thresholded_image_with_mask,thresholded_image); //bitwise_and(img,mask,m_out); + //Mat kernel = getStructuringElement(MORPH_RECT, Size(5,5), Point(-1, -1)); + + //Mat element = getStructuringElement(MORPH_RECT, + //Size(2 * morph_size + 1,2 * morph_size + 1), + //Point(morph_size, morph_size)); + + cv::Mat kernel; + cv::Mat opening_image; + cv::Mat closing_image; + + if (morphology_ex_type_ == "opening") + { + 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)); + + + cv::morphologyEx(thresholded_image, opening_image, cv::MORPH_OPEN, kernel); + //morphologyEx(img, open, CV_MOP_OPEN, kernel); + + //morphologyEx( src, dst, MORPH_TOPHAT, element, Point(-1,-1), i ); + //morphologyEx( src, dst, MORPH_TOPHAT, element ); // here iteration=1 +//MORPH_OPEN + } + + else if (morphology_ex_type_ == "closing") + { + 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)); + + //morphologyEx(img, close, CV_MOP_CLOSE, kernel); + //morphologyEx(image, output, MORPH_CLOSE, element,Point(-1, -1), 2); + cv::morphologyEx(thresholded_image, closing_image, cv::MORPH_CLOSE, kernel); + } + + // Serena : not related to us, or later we use the same way to check on something if needed + // those paramaters cannot be =0 + // so we must check here + //canny_threshold_ = std::max(canny_threshold_, 1.0); + //accumulator_threshold_ = std::max(accumulator_threshold_, 1.0); + + + if (!compareBlobDetectorParams(params_, prev_params_)) + { + detector = cv::SimpleBlobDetector::create(params_); + ROS_INFO("test for create new detector."); + } + + // runs the detection, and update the display + // will hold the results of the detection + std::vector keypoints; + // runs the actual detection + if (morphology_ex_type_ == "opening") + { + detector->detect(opening_image, keypoints); + } + + else if (morphology_ex_type_ == "closing") + { + detector->detect(closing_image, keypoints); + } + + else + { + detector->detect(thresholded_image, keypoints); + } + + prev_params_ = params_; + + if (keypoints.size() > 0) + { + std::cout << "test if there is keypoint detected : " << keypoints[0].size << std::endl; + } + + + + // cv::Mat out_image; + // will we encounter this situation ?? if (frame.channels() == 1) + // { + // cv::cvtColor(frame, out_image, cv::COLOR_GRAY2BGR); + // } + // else + // { + // out_image = frame; + // } + + // clone the colour, input image for displaying purposes + + for (const cv::KeyPoint& i : keypoints) + { + // cv::Point center(cvRound(i.pt[0]), cvRound(i.pt[1])); // should we round or not ? + // int radius = cvRound(i.size); // does not need to re-define. +// Serena : change to ours, generate the blob messages here + opencv_apps::Blob blob_msg; + // blob_msg.center.x = center.x; + // blob_msg.center.y = center.y; + // blob_msg.radius = radius; + 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); // why do not put in blobs_msg directly ? + } + + cv::Mat out_image; + drawKeypoints(frame, keypoints, out_image, cv::Scalar(0,0,255), cv::DrawMatchesFlags::DRAW_RICH_KEYPOINTS ); // there were not the 2 cv:: + + // shows the results + + if (debug_view_) + { + cv::imshow(window_name_, out_image);// show debug image or 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") + { + cv::imshow(opening_image_name_, opening_image); + } + + else if (morphology_ex_type_ == "closing") + { + cv::imshow(closing_image_name_, closing_image); + } + + char c = (char)cv::waitKey(1); + + // if( c == 27 ) + // break; + switch (c) + { + case 'o': + morphology_ex_type_ = "opening"; + cv::destroyWindow(closing_image_name_); + morphology_ex_kernel_size_ = morphology_ex_kernel_size_initial_value_; + break; + case 'c': + morphology_ex_type_ = "closing"; + cv::destroyWindow(opening_image_name_); + morphology_ex_kernel_size_ = morphology_ex_kernel_size_initial_value_; + break; + case 'n': + morphology_ex_type_ = "off"; + cv::destroyWindow(opening_image_name_); + cv::destroyWindow(closing_image_name_); + morphology_ex_kernel_size_ = morphology_ex_kernel_size_initial_value_; + break; + } + } + + // Publish the image. + // Publish the image. + sensor_msgs::Image::Ptr out_img = cv_bridge::CvImage(msg->header, "bgr8", out_image).toImageMsg(); + img_pub_.publish(out_img); + msg_pub_.publish(blobs_msg); //s + } + 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) + { + 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); + + if (debug_view_) + { + always_subscribe_ = debug_view_; // or 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"; + opening_image_name_ = "Opening Image"; + closing_image_name_ = "Closing Image"; + blob_detector_params_name_ = "Blob Detector Params"; + // delete output screen in launch file. + ROS_INFO("test for oninit."); + + morphology_ex_kernel_size_initial_value_ = 3; + max_minArea_ = 10000; // if 2560*1600 = 4096000 + max_maxArea_ = 10000; // if 2560*1600 = 4096000 + max_minDistBetweenBlobs_ = 1000000; // if 2560*1600 = 4096000 + + reconfigure_server_ = boost::make_shared >(*pnh_); + dynamic_reconfigure::Server::CallbackType f = + boost::bind(&BlobDetectionNodelet::reconfigureCallback, this, _1, _2); + reconfigure_server_->setCallback(f); + + img_pub_ = advertiseImage(*pnh_, "image", 1); + msg_pub_ = advertise(*pnh_, "blobs", 1); + + onInitPostProcess(); + } +}; +bool BlobDetectionNodelet::need_config_update_ = false; +} // namespace opencv_apps + +// the following block (namespace) is not needed in our case +namespace blob_detection +{ +class BlobDetectionNodelet : public opencv_apps::BlobDetectionNodelet +{ +public: + virtual void onInit() // NOLINT(modernize-use-override) + { + //ROS_WARN("DeprecationWarning: Nodelet hough_circles/hough_circles is deprecated, " + //"and renamed to opencv_apps/hough_circles."); + opencv_apps::BlobDetectionNodelet::onInit(); + } +}; +} // namespace hough_circles + +#include +PLUGINLIB_EXPORT_CLASS(opencv_apps::BlobDetectionNodelet, nodelet::Nodelet); +PLUGINLIB_EXPORT_CLASS(blob_detection::BlobDetectionNodelet, nodelet::Nodelet); + From cfedd290528db2b746726b95451d668728debc05 Mon Sep 17 00:00:00 2001 From: HuiShiSerenaShi Date: Sat, 29 Jan 2022 13:36:50 +0200 Subject: [PATCH 02/17] add the basic solution files to the fork(under branch - blob_detection_nodelet). the solution is under improving --- CMakeLists.txt | 6 ++ cfg/BlobDetection.cfg | 91 ++++++++++++++++++++++++++ launch/blob_detection.launch | 41 ++++++++++++ msg/Blob.msg | 2 + msg/BlobArray.msg | 1 + msg/BlobArrayStamped.msg | 2 + nodelet_plugins.xml | 5 ++ src/nodelet/blob_detection_nodelet.cpp | 1 - 8 files changed, 148 insertions(+), 1 deletion(-) create mode 100755 cfg/BlobDetection.cfg create mode 100644 launch/blob_detection.launch create mode 100644 msg/Blob.msg create mode 100644 msg/BlobArray.msg create mode 100644 msg/BlobArrayStamped.msg diff --git a/CMakeLists.txt b/CMakeLists.txt index 3b2dbb50..8788535e 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -65,6 +65,7 @@ generate_dynamic_reconfigure_options( cfg/HLSColorFilter.cfg cfg/HSVColorFilter.cfg cfg/WatershedSegmentation.cfg + cfg/BlobDetection.cfg ) ## Generate messages in the 'msg' folder @@ -101,6 +102,9 @@ add_message_files( Contour.msg ContourArray.msg ContourArrayStamped.msg + Blob.msg + BlobArray.msg + BlobArrayStamped.msg ) add_service_files( @@ -325,6 +329,8 @@ 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 +opencv_apps_add_nodelet(blob_detection src/nodelet/blob_detection_nodelet.cpp) + # 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) diff --git a/cfg/BlobDetection.cfg b/cfg/BlobDetection.cfg new file mode 100755 index 00000000..22089f45 --- /dev/null +++ b/cfg/BlobDetection.cfg @@ -0,0 +1,91 @@ +#! /usr/bin/env python +# Software License Agreement (BSD License) +# +# Copyright (c) 2014, Kei Okada. +# 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 Kei Okada 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("lowHue", double_t, 0, "lowHue", 0, 0, 179) +gen.add("highHue", double_t, 0, "highHue", 179, 0, 179) +gen.add("lowSat", double_t, 0, "lowSat", 0, 0, 255) +gen.add("highSat", double_t, 0, "highSat", 255, 0, 255) +gen.add("lowVal", double_t, 0, "lowVal", 0, 0, 255) +gen.add("highVal", double_t, 0, "highVal", 255, 0, 255) + +gen.add("morphology_ex_kernel_size", int_t, 0, "morphology_ex_kernel_size", 3, 0, 100) + +gen.add("minDistBetweenBlobs", int_t, 0, "minDistBetweenBlobs", 100, 0, 1000000) # if 2560*1600 = 4096000 + +gen.add("filterByColor", bool_t, 0, "filterByColor", True) +gen.add("blobColor", int_t, 0, "blobColor", 255, 0, 255) + +gen.add("filterByArea", bool_t, 0, "filterByArea", True) +gen.add("minArea", int_t, 0, "minArea", 5000, 0, 10000000) +gen.add("maxArea", int_t, 0, "maxArea", 9000000, 0, 10000000) +# gen.add("minArea", int_t, 0, "minArea", 0, 0, 10000000) +# gen.add("maxArea", int_t, 0, "maxArea", 10000000, 0, 10000000) + +gen.add("filterByCircularity", bool_t, 0, "filterByCircularity", True) +gen.add("minCircularity", double_t, 0, "minCircularity", 0, 0, 1) +gen.add("maxCircularity", double_t, 0, "maxCircularity", 1, 0, 1) + +gen.add("filterByConvexity", bool_t, 0, "filterByConvexity", True) +gen.add("minConvexity", double_t, 0, "minConvexity", 0, 0, 1) +gen.add("maxConvexity", double_t, 0, "maxConvexity", 1, 0, 1) + +gen.add("filterByInertia", bool_t, 0, "filterByInertia", True) +gen.add("minInertiaRatio", double_t, 0, "minInertiaRatio", 0, 0, 1) +gen.add("maxInertiaRatio", double_t, 0, "maxInertiaRatio", 1, 0, 1) + + +blobMsgType = gen.enum([ gen.const("Blob", str_t, "Blob", "Blob"), + gen.const("BlobArray", str_t, "BlobArray", "BlobArray"), + gen.const("BlobArrayStamped", str_t, "BlobArrayStamped", "BlobArrayStamped")], + "An enum to set blobMsgType") + +gen.add("blobMsgType", str_t, 0, "blobMsgType edited via an enum", "Blob", edit_method=blobMsgType) + +# debug_view_type_enum = gen.enum([ gen.const("Input", int_t, 0, "Input image"), +# gen.const("Blur", int_t, 1, "GaussianBlur image"), +# gen.const("Canny", int_t, 2, "Canny edge image"), +# ], +# "An enum for debug view") + +# gen.add("debug_image_type", int_t, 0, "Select image type for debug output", 0, 0, 2, edit_method=debug_view_type_enum) + + +exit(gen.generate(PACKAGE, "blob_detection", "BlobDetection")) diff --git a/launch/blob_detection.launch b/launch/blob_detection.launch new file mode 100644 index 00000000..4dcb72c3 --- /dev/null +++ b/launch/blob_detection.launch @@ -0,0 +1,41 @@ + + + + + + + + + + + + + + + + + + + + + + + + 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 8c473265..4c9b1c96 100644 --- a/nodelet_plugins.xml +++ b/nodelet_plugins.xml @@ -115,6 +115,10 @@ Nodelet to demonstrate the famous watershed segmentation algorithm in OpenCV: watershed() + + Nodelet to find blobs + + @@ -147,4 +151,5 @@ + diff --git a/src/nodelet/blob_detection_nodelet.cpp b/src/nodelet/blob_detection_nodelet.cpp index bc470174..acacaff6 100644 --- a/src/nodelet/blob_detection_nodelet.cpp +++ b/src/nodelet/blob_detection_nodelet.cpp @@ -648,4 +648,3 @@ class BlobDetectionNodelet : public opencv_apps::BlobDetectionNodelet #include PLUGINLIB_EXPORT_CLASS(opencv_apps::BlobDetectionNodelet, nodelet::Nodelet); PLUGINLIB_EXPORT_CLASS(blob_detection::BlobDetectionNodelet, nodelet::Nodelet); - From ba308a41baa9db4c21d1733e0b03e1c8d489c26e Mon Sep 17 00:00:00 2001 From: robotont-10 Date: Sat, 29 Jan 2022 18:52:33 +0200 Subject: [PATCH 03/17] update --- src/nodelet/blob_detection_nodelet.cpp | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/src/nodelet/blob_detection_nodelet.cpp b/src/nodelet/blob_detection_nodelet.cpp index acacaff6..f452e10b 100644 --- a/src/nodelet/blob_detection_nodelet.cpp +++ b/src/nodelet/blob_detection_nodelet.cpp @@ -67,6 +67,7 @@ namespace opencv_apps class BlobDetectionNodelet : public opencv_apps::Nodelet { image_transport::Publisher img_pub_; + image_transport::Publisher thresholded_img_pub_; image_transport::Subscriber img_sub_; image_transport::CameraSubscriber cam_sub_; ros::Publisher msg_pub_; @@ -559,8 +560,13 @@ class BlobDetectionNodelet : public opencv_apps::Nodelet // Publish the image. // Publish the image. + cv::Mat out_thresholded_image; + cv::cvtColor(thresholded_image, out_thresholded_image, cv::COLOR_GRAY2BGR); sensor_msgs::Image::Ptr out_img = cv_bridge::CvImage(msg->header, "bgr8", out_image).toImageMsg(); + sensor_msgs::Image::Ptr out_thresholded_img = cv_bridge::CvImage(msg->header, "bgr8", out_thresholded_image).toImageMsg(); + img_pub_.publish(out_img); + thresholded_img_pub_.publish(out_thresholded_img); msg_pub_.publish(blobs_msg); //s } catch (cv::Exception& e) @@ -622,6 +628,7 @@ class BlobDetectionNodelet : public opencv_apps::Nodelet reconfigure_server_->setCallback(f); img_pub_ = advertiseImage(*pnh_, "image", 1); + thresholded_img_pub_ = advertiseImage(*pnh_, "thresholded_image", 1); msg_pub_ = advertise(*pnh_, "blobs", 1); onInitPostProcess(); From aac32d230f5bed7a01e6544ccfe585b4f712078e Mon Sep 17 00:00:00 2001 From: HuiShiSerenaShi Date: Thu, 3 Feb 2022 15:26:51 +0200 Subject: [PATCH 04/17] update --- cfg/BlobDetection.cfg | 10 +- config/blob_detection.rviz | 161 +++++++++++++++++++++++++ config/blob_detection_config.yaml | 69 +++++++++++ launch/blob_detection.launch | 4 + launch/blob_detection_ims.launch | 104 ++++++++++++++++ param/blob_detectio_testn.yaml | 55 +++++++++ src/nodelet/blob_detection_nodelet.cpp | 144 +++++++++++++--------- 7 files changed, 484 insertions(+), 63 deletions(-) create mode 100644 config/blob_detection.rviz create mode 100644 config/blob_detection_config.yaml create mode 100644 launch/blob_detection_ims.launch create mode 100644 param/blob_detectio_testn.yaml diff --git a/cfg/BlobDetection.cfg b/cfg/BlobDetection.cfg index 22089f45..b1601308 100755 --- a/cfg/BlobDetection.cfg +++ b/cfg/BlobDetection.cfg @@ -72,12 +72,12 @@ gen.add("minInertiaRatio", double_t, 0, "minInertiaRatio", 0, 0, 1) gen.add("maxInertiaRatio", double_t, 0, "maxInertiaRatio", 1, 0, 1) -blobMsgType = gen.enum([ gen.const("Blob", str_t, "Blob", "Blob"), - gen.const("BlobArray", str_t, "BlobArray", "BlobArray"), - gen.const("BlobArrayStamped", str_t, "BlobArrayStamped", "BlobArrayStamped")], - "An enum to set blobMsgType") +morphology_ex_type = gen.enum([ gen.const("off", str_t, "off", "off"), + gen.const("opening", str_t, "opening", "opening"), + gen.const("closing", str_t, "closing", "closing")], + "An enum to set morphology_ex_type") -gen.add("blobMsgType", str_t, 0, "blobMsgType edited via an enum", "Blob", edit_method=blobMsgType) +gen.add("morphology_ex_type", str_t, 0, "morphology_ex_type edited via an enum", "off", edit_method=morphology_ex_type) # debug_view_type_enum = gen.enum([ gen.const("Input", int_t, 0, "Input image"), # gen.const("Blur", int_t, 1, "GaussianBlur image"), diff --git a/config/blob_detection.rviz b/config/blob_detection.rviz new file mode 100644 index 00000000..e69564c2 --- /dev/null +++ b/config/blob_detection.rviz @@ -0,0 +1,161 @@ +Panels: + - Class: rviz/Displays + Help Height: 172 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /Status1 + - /Image1 + - /Image2 + - /Image3 + 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: Image +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: 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 + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: Image + 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: 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.785398006439209 + Target Frame: + Value: Orbit (rviz) + Yaw: 0.785398006439209 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 1472 + Hide Left Dock: false + Hide Right Dock: false + Image: + collapsed: false + QMainWindow State: 000000ff00000000fd00000004000000000000026b00000482fc020000000afb0000001200530065006c0065006300740069006f006e00000001e10000009b000000da00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000dafb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000008600000482000001de00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d00610067006503000004b7000003880000026b000001d6fb0000000a0049006d00610067006503000005cf000002400000026b000000e200000001000001b400000482fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730100000086000004820000017800fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b20000000000000000000000020000097a000000a9fc0100000002fb0000000a0049006d00610067006503000003ce0000016600000280000001e0fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000097a00000073fc0100000002fb0000000800540069006d006501000000000000097a000006de00fffffffb0000000800540069006d006501000000000000045000000000000000000000053d0000048200000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: false + 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..1658d60f --- /dev/null +++ b/config/blob_detection_config.yaml @@ -0,0 +1,69 @@ +blobColor: 255 +debug_view: false +filterByArea: true +filterByCircularity: true +filterByColor: true +filterByConvexity: true +filterByInertia: true +highHue: 179.0 +highSat: 255.0 +highVal: 255.0 +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 +lowHue: 80.55000000000001 +lowSat: 0.0 +lowVal: 0.0 +maxArea: 9000000 +maxCircularity: 1.0 +maxConvexity: 1.0 +maxInertiaRatio: 1.0 +minArea: 5000 +minCircularity: 0.0 +minConvexity: 0.0 +minDistBetweenBlobs: 100 +minInertiaRatio: 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: 16 +morphology_ex_type: opening +num_worker_threads: 1 +queue_size: 3 +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 +use_camera_info: false diff --git a/launch/blob_detection.launch b/launch/blob_detection.launch index 4dcb72c3..3de8a60e 100644 --- a/launch/blob_detection.launch +++ b/launch/blob_detection.launch @@ -7,6 +7,8 @@ + + @@ -27,6 +29,8 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/param/blob_detectio_testn.yaml b/param/blob_detectio_testn.yaml new file mode 100644 index 00000000..3ee90761 --- /dev/null +++ b/param/blob_detectio_testn.yaml @@ -0,0 +1,55 @@ +blobColor: 255 +blobMsgType: Blob +debug_view: false +filterByArea: true +filterByCircularity: true +filterByColor: true +filterByConvexity: true +filterByInertia: true +highHue: 179.0 +highSat: 255.0 +highVal: 255.0 +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 +lowHue: 161.10000000000002 +lowSat: 0.0 +lowVal: 0.0 +maxArea: 9000000 +maxCircularity: 1.0 +maxConvexity: 1.0 +maxInertiaRatio: 1.0 +minArea: 5000 +minCircularity: 0.0 +minConvexity: 0.0 +minDistBetweenBlobs: 100 +minInertiaRatio: 0.0 +morphology_ex_kernel_size: 3 +num_worker_threads: 1 +queue_size: 3 +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 +use_camera_info: false diff --git a/src/nodelet/blob_detection_nodelet.cpp b/src/nodelet/blob_detection_nodelet.cpp index f452e10b..89e889bc 100644 --- a/src/nodelet/blob_detection_nodelet.cpp +++ b/src/nodelet/blob_detection_nodelet.cpp @@ -68,6 +68,9 @@ 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_; @@ -83,7 +86,7 @@ class BlobDetectionNodelet : public opencv_apps::Nodelet bool debug_view_; ros::Time prev_stamp_; - std::string window_name_, thresholded_image_name_, thresholded_image_with_mask_name_, blob_detector_params_name_, opening_image_name_, closing_image_name_; // reference to camshift_nodelet + std::string window_name_, thresholded_image_name_, thresholded_image_with_mask_name_, blob_detector_params_name_, morphology_ex_image_name_; // reference to camshift_nodelet static bool need_config_update_; cv::SimpleBlobDetector::Params params_; // @@ -104,6 +107,9 @@ class BlobDetectionNodelet : public opencv_apps::Nodelet int highVal; std::string morphology_ex_type_; + std::string prev_morphology_ex_type_; + std::string morphology_ex_type_default_value_; + //char morphology_ex_type_; int morphology_ex_kernel_size_; int morphology_ex_kernel_size_initial_value_; @@ -176,7 +182,6 @@ class BlobDetectionNodelet : public opencv_apps::Nodelet return true; } - void reconfigureCallback(Config& new_config, uint32_t level) { config_ = new_config; @@ -189,7 +194,7 @@ class BlobDetectionNodelet : public opencv_apps::Nodelet highVal = config_.highVal; morphology_ex_kernel_size_ = config_.morphology_ex_kernel_size; - + morphology_ex_type_ = config_.morphology_ex_type; params_.filterByColor = config_.filterByColor; params_.blobColor = config_.blobColor; @@ -300,19 +305,34 @@ class BlobDetectionNodelet : public opencv_apps::Nodelet 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_ == "opening") + + if (morphology_ex_type_ != prev_morphology_ex_type_) { - cv::namedWindow(opening_image_name_, cv::WINDOW_AUTOSIZE); - cv::createTrackbar("morphology_ex_kernel_size", opening_image_name_, &morphology_ex_kernel_size_, 100, trackbarCallback); + 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_); } - else if (morphology_ex_type_ == "closing") - { - cv::namedWindow(closing_image_name_, cv::WINDOW_AUTOSIZE); - cv::createTrackbar("morphology_ex_kernel_size", closing_image_name_, &morphology_ex_kernel_size_, 100, trackbarCallback); + 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("morphology_ex_kernel_size", morphology_ex_image_name_, &morphology_ex_kernel_size_, 100, trackbarCallback); } + prev_morphology_ex_type_ = morphology_ex_type_; + cv::createTrackbar("lowHue", thresholded_image_name_, &lowHue, 179, trackbarCallback);// should we increase the range? const int max_value_H = 360/2; https://docs.opencv.org/3.4/da/d97/tutorial_threshold_inRange.html cv::createTrackbar("lowSat", thresholded_image_name_, &lowSat, 255, trackbarCallback); cv::createTrackbar("lowVal", thresholded_image_name_, &lowVal, 255, trackbarCallback); @@ -409,11 +429,10 @@ class BlobDetectionNodelet : public opencv_apps::Nodelet //Point(morph_size, morph_size)); cv::Mat kernel; - cv::Mat opening_image; - cv::Mat closing_image; + cv::Mat morphology_ex_image; - if (morphology_ex_type_ == "opening") - { + if (morphology_ex_type_ == "opening" || morphology_ex_type_ == "closing") + { if (morphology_ex_kernel_size_ % 2 != 1) { morphology_ex_kernel_size_ = morphology_ex_kernel_size_ + 1; @@ -421,28 +440,25 @@ class BlobDetectionNodelet : public opencv_apps::Nodelet 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); + } + } - cv::morphologyEx(thresholded_image, opening_image, cv::MORPH_OPEN, kernel); //morphologyEx(img, open, CV_MOP_OPEN, kernel); //morphologyEx( src, dst, MORPH_TOPHAT, element, Point(-1,-1), i ); //morphologyEx( src, dst, MORPH_TOPHAT, element ); // here iteration=1 //MORPH_OPEN - } - - else if (morphology_ex_type_ == "closing") - { - 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)); //morphologyEx(img, close, CV_MOP_CLOSE, kernel); //morphologyEx(image, output, MORPH_CLOSE, element,Point(-1, -1), 2); - cv::morphologyEx(thresholded_image, closing_image, cv::MORPH_CLOSE, kernel); - } // Serena : not related to us, or later we use the same way to check on something if needed // those paramaters cannot be =0 @@ -461,14 +477,9 @@ class BlobDetectionNodelet : public opencv_apps::Nodelet // will hold the results of the detection std::vector keypoints; // runs the actual detection - if (morphology_ex_type_ == "opening") - { - detector->detect(opening_image, keypoints); - } - - else if (morphology_ex_type_ == "closing") + if (morphology_ex_type_ == "opening" || morphology_ex_type_ == "closing") { - detector->detect(closing_image, keypoints); + detector->detect(morphology_ex_image, keypoints); } else @@ -483,8 +494,6 @@ class BlobDetectionNodelet : public opencv_apps::Nodelet std::cout << "test if there is keypoint detected : " << keypoints[0].size << std::endl; } - - // cv::Mat out_image; // will we encounter this situation ?? if (frame.channels() == 1) // { @@ -523,14 +532,9 @@ class BlobDetectionNodelet : public opencv_apps::Nodelet cv::imshow(thresholded_image_name_, thresholded_image); cv::imshow(thresholded_image_with_mask_name_, thresholded_image_with_mask); - if (morphology_ex_type_ == "opening") - { - cv::imshow(opening_image_name_, opening_image); - } - - else if (morphology_ex_type_ == "closing") + if (morphology_ex_type_ == "opening" || morphology_ex_type_ == "closing") { - cv::imshow(closing_image_name_, closing_image); + cv::imshow(morphology_ex_image_name_, morphology_ex_image); } char c = (char)cv::waitKey(1); @@ -541,32 +545,51 @@ class BlobDetectionNodelet : public opencv_apps::Nodelet { case 'o': morphology_ex_type_ = "opening"; - cv::destroyWindow(closing_image_name_); - morphology_ex_kernel_size_ = morphology_ex_kernel_size_initial_value_; + //morphology_ex_kernel_size_ = morphology_ex_kernel_size_initial_value_; break; case 'c': morphology_ex_type_ = "closing"; - cv::destroyWindow(opening_image_name_); - morphology_ex_kernel_size_ = morphology_ex_kernel_size_initial_value_; + //morphology_ex_kernel_size_ = morphology_ex_kernel_size_initial_value_; break; case 'n': morphology_ex_type_ = "off"; - cv::destroyWindow(opening_image_name_); - cv::destroyWindow(closing_image_name_); - morphology_ex_kernel_size_ = morphology_ex_kernel_size_initial_value_; + //morphology_ex_kernel_size_ = morphology_ex_kernel_size_initial_value_; break; } } + 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); + } // Publish the image. // Publish the image. - cv::Mat out_thresholded_image; - cv::cvtColor(thresholded_image, out_thresholded_image, cv::COLOR_GRAY2BGR); + + if (thresholded_img_with_mask_pub_.getNumSubscribers() > 0) + { + //cv::Mat out_thresholded_image_with_mask; + //cv::cvtColor(thresholded_image_with_mask, out_thresholded_image_with_mask, cv::COLOR_GRAY2BGR); + 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); + } + } + sensor_msgs::Image::Ptr out_img = cv_bridge::CvImage(msg->header, "bgr8", out_image).toImageMsg(); - sensor_msgs::Image::Ptr out_thresholded_img = cv_bridge::CvImage(msg->header, "bgr8", out_thresholded_image).toImageMsg(); - + img_pub_.publish(out_img); - thresholded_img_pub_.publish(out_thresholded_img); msg_pub_.publish(blobs_msg); //s } catch (cv::Exception& e) @@ -601,6 +624,7 @@ class BlobDetectionNodelet : public opencv_apps::Nodelet 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_); if (debug_view_) { @@ -611,12 +635,13 @@ class BlobDetectionNodelet : public opencv_apps::Nodelet window_name_ = "Blob Detection Demo"; thresholded_image_name_ = "Thresholded Image"; thresholded_image_with_mask_name_ = "Thresholded Image With Mask"; - opening_image_name_ = "Opening Image"; - closing_image_name_ = "Closing Image"; + morphology_ex_image_name_ = "Opening Image"; blob_detector_params_name_ = "Blob Detector Params"; // delete output screen in launch file. ROS_INFO("test for oninit."); + prev_morphology_ex_type_ = "off"; + morphology_ex_type_default_value_ = "off"; morphology_ex_kernel_size_initial_value_ = 3; max_minArea_ = 10000; // if 2560*1600 = 4096000 max_maxArea_ = 10000; // if 2560*1600 = 4096000 @@ -629,6 +654,9 @@ class BlobDetectionNodelet : public opencv_apps::Nodelet img_pub_ = advertiseImage(*pnh_, "image", 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); + msg_pub_ = advertise(*pnh_, "blobs", 1); onInitPostProcess(); From 2f21fafb32b8895062b540ad3a89019a02361199 Mon Sep 17 00:00:00 2001 From: HuiShiSerenaShi Date: Thu, 3 Feb 2022 15:28:39 +0200 Subject: [PATCH 05/17] update --- param/blob_detectio_testn.yaml | 55 ---------------------------------- 1 file changed, 55 deletions(-) delete mode 100644 param/blob_detectio_testn.yaml diff --git a/param/blob_detectio_testn.yaml b/param/blob_detectio_testn.yaml deleted file mode 100644 index 3ee90761..00000000 --- a/param/blob_detectio_testn.yaml +++ /dev/null @@ -1,55 +0,0 @@ -blobColor: 255 -blobMsgType: Blob -debug_view: false -filterByArea: true -filterByCircularity: true -filterByColor: true -filterByConvexity: true -filterByInertia: true -highHue: 179.0 -highSat: 255.0 -highVal: 255.0 -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 -lowHue: 161.10000000000002 -lowSat: 0.0 -lowVal: 0.0 -maxArea: 9000000 -maxCircularity: 1.0 -maxConvexity: 1.0 -maxInertiaRatio: 1.0 -minArea: 5000 -minCircularity: 0.0 -minConvexity: 0.0 -minDistBetweenBlobs: 100 -minInertiaRatio: 0.0 -morphology_ex_kernel_size: 3 -num_worker_threads: 1 -queue_size: 3 -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 -use_camera_info: false From 6e3461240d47e06217e189eec638b17ba44ac9c8 Mon Sep 17 00:00:00 2001 From: HuiShiSerenaShi Date: Fri, 4 Feb 2022 16:18:41 +0200 Subject: [PATCH 06/17] update --- cfg/BlobDetection.cfg | 57 ++--- launch/blob_detection.launch | 38 ++- src/nodelet/blob_detection_nodelet.cpp | 323 +++++++++++-------------- 3 files changed, 178 insertions(+), 240 deletions(-) diff --git a/cfg/BlobDetection.cfg b/cfg/BlobDetection.cfg index b1601308..57e0f0ce 100755 --- a/cfg/BlobDetection.cfg +++ b/cfg/BlobDetection.cfg @@ -39,53 +39,44 @@ 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("lowHue", double_t, 0, "lowHue", 0, 0, 179) -gen.add("highHue", double_t, 0, "highHue", 179, 0, 179) -gen.add("lowSat", double_t, 0, "lowSat", 0, 0, 255) -gen.add("highSat", double_t, 0, "highSat", 255, 0, 255) -gen.add("lowVal", double_t, 0, "lowVal", 0, 0, 255) -gen.add("highVal", double_t, 0, "highVal", 255, 0, 255) -gen.add("morphology_ex_kernel_size", int_t, 0, "morphology_ex_kernel_size", 3, 0, 100) +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", "off"), + gen.const("opening", str_t, "opening", "opening"), + gen.const("closing", str_t, "closing", "closing")], + "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", 3, 0, 100) -gen.add("minDistBetweenBlobs", int_t, 0, "minDistBetweenBlobs", 100, 0, 1000000) # if 2560*1600 = 4096000 gen.add("filterByColor", bool_t, 0, "filterByColor", True) gen.add("blobColor", int_t, 0, "blobColor", 255, 0, 255) gen.add("filterByArea", bool_t, 0, "filterByArea", True) -gen.add("minArea", int_t, 0, "minArea", 5000, 0, 10000000) -gen.add("maxArea", int_t, 0, "maxArea", 9000000, 0, 10000000) -# gen.add("minArea", int_t, 0, "minArea", 0, 0, 10000000) -# gen.add("maxArea", int_t, 0, "maxArea", 10000000, 0, 10000000) +gen.add("minArea", double_t, 0, "minArea", 5000, 0, 10000000) +gen.add("maxArea", double_t, 0, "maxArea", 9000000, 0, 10000000) + +gen.add("minDistBetweenBlobs", double_t, 0, "minDistBetweenBlobs", 100, 0, 1000000) # if 2560*1600 = 4096000 -gen.add("filterByCircularity", bool_t, 0, "filterByCircularity", True) +gen.add("filterByCircularity", bool_t, 0, "filterByCircularity", False) gen.add("minCircularity", double_t, 0, "minCircularity", 0, 0, 1) gen.add("maxCircularity", double_t, 0, "maxCircularity", 1, 0, 1) -gen.add("filterByConvexity", bool_t, 0, "filterByConvexity", True) -gen.add("minConvexity", double_t, 0, "minConvexity", 0, 0, 1) -gen.add("maxConvexity", double_t, 0, "maxConvexity", 1, 0, 1) - -gen.add("filterByInertia", bool_t, 0, "filterByInertia", True) +gen.add("filterByInertia", bool_t, 0, "filterByInertia", False) gen.add("minInertiaRatio", double_t, 0, "minInertiaRatio", 0, 0, 1) gen.add("maxInertiaRatio", double_t, 0, "maxInertiaRatio", 1, 0, 1) - -morphology_ex_type = gen.enum([ gen.const("off", str_t, "off", "off"), - gen.const("opening", str_t, "opening", "opening"), - gen.const("closing", str_t, "closing", "closing")], - "An enum to set morphology_ex_type") - -gen.add("morphology_ex_type", str_t, 0, "morphology_ex_type edited via an enum", "off", edit_method=morphology_ex_type) - -# debug_view_type_enum = gen.enum([ gen.const("Input", int_t, 0, "Input image"), -# gen.const("Blur", int_t, 1, "GaussianBlur image"), -# gen.const("Canny", int_t, 2, "Canny edge image"), -# ], -# "An enum for debug view") - -# gen.add("debug_image_type", int_t, 0, "Select image type for debug output", 0, 0, 2, edit_method=debug_view_type_enum) +gen.add("filterByConvexity", bool_t, 0, "filterByConvexity", False) +gen.add("minConvexity", double_t, 0, "minConvexity", 0, 0, 1) +gen.add("maxConvexity", double_t, 0, "maxConvexity", 1, 0, 1) exit(gen.generate(PACKAGE, "blob_detection", "BlobDetection")) diff --git a/launch/blob_detection.launch b/launch/blob_detection.launch index 3de8a60e..aef38ee2 100644 --- a/launch/blob_detection.launch +++ b/launch/blob_detection.launch @@ -7,21 +7,16 @@ - - - - - + + + + + + + + @@ -29,17 +24,14 @@ + + + + + + + - - diff --git a/src/nodelet/blob_detection_nodelet.cpp b/src/nodelet/blob_detection_nodelet.cpp index 89e889bc..84fdb77d 100644 --- a/src/nodelet/blob_detection_nodelet.cpp +++ b/src/nodelet/blob_detection_nodelet.cpp @@ -56,12 +56,7 @@ #include "opencv_apps/BlobArray.h" #include "opencv_apps/BlobArrayStamped.h" -// 1. opencv trackbar debug -//2. ros dynamic setup mode, launch file with dynamic and rviz with img topic attached. -//3. nothing, only launch file load the yaml file loading, 2 and 3 use the same launch file, arg-mode, default-simple run which is the 3rd case -// another mode is 1st, then 2nd. -// Q1. setup mode, there is no thresholded image publish in the node. -// Q2. in launch file, how to switch between the launch of different group of nodes. + namespace opencv_apps { class BlobDetectionNodelet : public opencv_apps::Nodelet @@ -86,62 +81,62 @@ class BlobDetectionNodelet : public opencv_apps::Nodelet bool debug_view_; ros::Time prev_stamp_; - std::string window_name_, thresholded_image_name_, thresholded_image_with_mask_name_, blob_detector_params_name_, morphology_ex_image_name_; // reference to camshift_nodelet + std::string window_name_, thresholded_image_name_, thresholded_image_with_mask_name_, morphology_ex_image_name_, blob_detector_params_name_; // reference to camshift_nodelet static bool need_config_update_; - cv::SimpleBlobDetector::Params params_; // + cv::SimpleBlobDetector::Params params_; cv::SimpleBlobDetector::Params prev_params_; //this is for opencv version 2: SimpleBlobDetector detector(params); check the following link for more info // https://learnopencv.com/blob-detection-using-opencv-python-c/ //cv::Ptr detector = cv::SimpleBlobDetector::create(params); // this line was without the 3 cv:: // where should we create the detector and params ? https://ip.festo-didactic.com/DigitalEducation/EITManufacturing/UNICO/FDRenderer/index.html?LearningPath=0a4e3fe94887400cad125a990fd2d518&Language=EN&FDEP=true&NoPHP=true - cv::Ptr detector; + cv::Ptr detector_; // initial and max values of the parameters of interests. - int lowHue; - int lowSat; - int lowVal; - int highHue; // h_limit_max or high_hue_ ? - int highSat; - int highVal; + 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 prev_morphology_ex_type_; std::string morphology_ex_type_default_value_; - //char morphology_ex_type_; + std::string prev_morphology_ex_type_; + int morphology_ex_kernel_size_; int morphology_ex_kernel_size_initial_value_; - int filterByColor_int; // for trackbar, because trackbar only accept int type. - int blobColor_int; + int filter_by_color_int; // for trackbar, because trackbar only accept int type. + int blob_color_int; - int filterByArea_int; // ??? - int minArea_int; - int maxArea_int; - int max_minArea_; - int max_maxArea_; + int filter_by_area_int; + int min_area_int; + int max_area_int; + int min_area_upper_limit_; + int max_area_upper_limit_; - int minDistBetweenBlobs_int; - int max_minDistBetweenBlobs_; + int min_dist_between_blobs_int; + int min_dist_between_blobs_upper_limit_; // Filter by Area. // // Filter by Circularity - int filterByCircularity_int; - int minCircularity_int; - int maxCircularity_int; + int filter_by_circularity_int; + int min_circularity_int; + int max_circularity_int; // Filter by Inertia - int filterByInertia_int; - int minInertiaRatio_int; - int maxInertiaRatio_int; + int filter_by_inertia_int; + int min_inertia_ratio_int; + int max_inertia_ratio_int; // Filter by Convexity - int filterByConvexity_int; - int minConvexity_int; - int maxConvexity_int; + int filter_by_convexity_int; + int min_convexity_int; + int max_convexity_int; - bool compareBlobDetectorParams(cv::SimpleBlobDetector::Params params_1, cv::SimpleBlobDetector::Params params_2)// contour_moments line 61 counter1 counter2, & + bool compareBlobDetectorParams(const cv::SimpleBlobDetector::Params& params_1, const cv::SimpleBlobDetector::Params& params_2)// contour_moments line 61 counter1 counter2, & { if (params_1.filterByColor != params_2.filterByColor) return false; @@ -186,15 +181,16 @@ class BlobDetectionNodelet : public opencv_apps::Nodelet { config_ = new_config; - lowHue = config_.lowHue; - lowSat = config_.lowSat; - lowVal = config_.lowVal; - highHue = config_.highHue; - highSat = config_.highSat; - highVal = config_.highVal; + 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_kernel_size_ = config_.morphology_ex_kernel_size; morphology_ex_type_ = config_.morphology_ex_type; + morphology_ex_kernel_size_ = config_.morphology_ex_kernel_size; + params_.filterByColor = config_.filterByColor; params_.blobColor = config_.blobColor; @@ -220,32 +216,32 @@ class BlobDetectionNodelet : public opencv_apps::Nodelet params_.maxConvexity = config_.maxConvexity; - filterByColor_int = int(params_.filterByColor); - blobColor_int = int(params_.blobColor); + filter_by_color_int = int(params_.filterByColor); + blob_color_int = int(params_.blobColor); - filterByArea_int = int(params_.filterByArea); - minArea_int = int(params_.minArea); - maxArea_int = int(params_.maxArea); + filter_by_area_int = int(params_.filterByArea); + min_area_int = int(params_.minArea); + max_area_int = int(params_.maxArea); - minDistBetweenBlobs_int = int(params_.minDistBetweenBlobs); + min_dist_between_blobs_int = int(params_.minDistBetweenBlobs); - filterByCircularity_int = int(params_.filterByCircularity); - minCircularity_int = int(params_.minCircularity*100); - std::cout << "test reconfigure callback convert int(params_.minCircularity*100) to : " << minCircularity_int << std::endl; - maxCircularity_int = int(params_.maxCircularity*100); - std::cout << "test reconfigure callback convert int(params_.maxCircularity*100) to : " << maxCircularity_int << std::endl; + filter_by_circularity_int = int(params_.filterByCircularity); + min_circularity_int = int(params_.minCircularity*100); + std::cout << "test reconfigure callback convert int(params_.minCircularity*100) to : " << min_circularity_int << std::endl; + max_circularity_int = int(params_.maxCircularity*100); + std::cout << "test reconfigure callback convert int(params_.maxCircularity*100) to : " << max_circularity_int << std::endl; - filterByInertia_int = int(params_.filterByInertia); - minInertiaRatio_int = int(params_.minInertiaRatio*100); - std::cout << "test reconfigure callback convert int(params_.minInertiaRatio*100) to : " << minInertiaRatio_int << std::endl; - maxInertiaRatio_int = int(params_.maxInertiaRatio*100); - std::cout << "test reconfigure callback convert int(params_.maxInertiaRatio*100) to : " << maxInertiaRatio_int << std::endl; + filter_by_inertia_int = int(params_.filterByInertia); + min_inertia_ratio_int = int(params_.minInertiaRatio*100); + std::cout << "test reconfigure callback convert int(params_.minInertiaRatio*100) to : " << min_inertia_ratio_int << std::endl; + max_inertia_ratio_int = int(params_.maxInertiaRatio*100); + std::cout << "test reconfigure callback convert int(params_.maxInertiaRatio*100) to : " << max_inertia_ratio_int << std::endl; - filterByConvexity_int = int(params_.filterByConvexity); - minConvexity_int = int(params_.minConvexity*100); - std::cout << "test reconfigure callback convert int(params_.minConvexity*100) to : " << minConvexity_int << std::endl; - maxConvexity_int = int(params_.maxConvexity*100); - std::cout << "test reconfigure callback convert int(params_.maxConvexity*100) to : " << maxConvexity_int << std::endl; + filter_by_convexity_int = int(params_.filterByConvexity); + min_convexity_int = int(params_.minConvexity*100); + std::cout << "test reconfigure callback convert int(params_.minConvexity*100) to : " << min_convexity_int << std::endl; + max_convexity_int = int(params_.maxConvexity*100); + std::cout << "test reconfigure callback convert int(params_.maxConvexity*100) to : " << max_convexity_int << std::endl; ROS_INFO("test for reconfigure call back."); } @@ -267,8 +263,7 @@ class BlobDetectionNodelet : public opencv_apps::Nodelet doWork(msg, msg->header.frame_id); } - // static void trackbarCallback(int /*unused*/, void* /*unused*/) - static void trackbarCallback(int value, void* userdata) + static void trackbarCallback(int /*unused*/, void* /*unused*/) { need_config_update_ = true; ROS_INFO("test for trackbar call back."); @@ -280,23 +275,24 @@ class BlobDetectionNodelet : public opencv_apps::Nodelet try { // Convert the image into something opencv can handle. - cv::Mat frame = cv_bridge::toCvShare(msg, sensor_msgs::image_encodings::BGR8)->image; + 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 - // do we need to check whether the frame is BGR ? since we convert it from BGR to HSV -// Serena : change to ours, do thresholding here - // if (frame.channels() > 1) - // { - // cv::cvtColor(frame, src_gray, cv::COLOR_BGR2GRAY); - // } - // else - // { - // src_gray = frame; - // } + + /// Convert it to 3 channel + cv::Mat frame; + if (frame_src.channels() > 1) + { + frame = frame_src; + } + else + { + cv::cvtColor(frame_src, frame, cv::COLOR_GRAY2BGR); + } // create the main window, and attach the trackbars if (debug_view_) @@ -308,7 +304,12 @@ class BlobDetectionNodelet : public opencv_apps::Nodelet if (morphology_ex_type_ != prev_morphology_ex_type_) { - cv::destroyWindow(morphology_ex_image_name_); + + 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_; @@ -328,85 +329,85 @@ class BlobDetectionNodelet : public opencv_apps::Nodelet } cv::namedWindow(morphology_ex_image_name_, cv::WINDOW_AUTOSIZE); - cv::createTrackbar("morphology_ex_kernel_size", morphology_ex_image_name_, &morphology_ex_kernel_size_, 100, trackbarCallback); + cv::createTrackbar("MorphologyEx Kernel Size", morphology_ex_image_name_, &morphology_ex_kernel_size_, 100, trackbarCallback); } prev_morphology_ex_type_ = morphology_ex_type_; - cv::createTrackbar("lowHue", thresholded_image_name_, &lowHue, 179, trackbarCallback);// should we increase the range? const int max_value_H = 360/2; https://docs.opencv.org/3.4/da/d97/tutorial_threshold_inRange.html - cv::createTrackbar("lowSat", thresholded_image_name_, &lowSat, 255, trackbarCallback); - cv::createTrackbar("lowVal", thresholded_image_name_, &lowVal, 255, trackbarCallback); + cv::createTrackbar("Hue Lower Limit", thresholded_image_name_, &hue_lower_limit_, 179, trackbarCallback);// should we increase the range? const int max_value_H = 360/2; https://docs.opencv.org/3.4/da/d97/tutorial_threshold_inRange.html + 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("highHue", thresholded_image_name_, &highHue, 179, trackbarCallback); - cv::createTrackbar("highSat", thresholded_image_name_, &highSat, 255, trackbarCallback); - cv::createTrackbar("highVal", thresholded_image_name_, &highVal, 255, trackbarCallback); - cv::createTrackbar("filterByColor", blob_detector_params_name_, &filterByColor_int, 1, trackbarCallback); - cv::createTrackbar("blobColor", blob_detector_params_name_, &blobColor_int, 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("filterByArea", blob_detector_params_name_, &filterByArea_int, 1, trackbarCallback); - cv::createTrackbar("minArea", blob_detector_params_name_, &minArea_int, max_minArea_, trackbarCallback); - cv::createTrackbar("maxArea", blob_detector_params_name_, &maxArea_int, max_maxArea_, 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("minDistBetweenBlobs", blob_detector_params_name_, &minDistBetweenBlobs_int, max_minDistBetweenBlobs_, 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("filterByCircularity", blob_detector_params_name_, &filterByCircularity_int, 1, trackbarCallback); - cv::createTrackbar("minCircularity", blob_detector_params_name_, &minCircularity_int, 100, trackbarCallback); - cv::createTrackbar("maxCircularity", blob_detector_params_name_, &maxCircularity_int, 100, 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("filterByInertia", blob_detector_params_name_, &filterByInertia_int, 1, trackbarCallback); - cv::createTrackbar("minInertiaRatio", blob_detector_params_name_, &minInertiaRatio_int, 100, trackbarCallback); - cv::createTrackbar("maxInertiaRatio", blob_detector_params_name_, &maxInertiaRatio_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("filterByConvexity", blob_detector_params_name_, &filterByConvexity_int, 1, trackbarCallback); - cv::createTrackbar("minConvexity", blob_detector_params_name_, &minConvexity_int, 100, trackbarCallback); - cv::createTrackbar("maxConvexity", blob_detector_params_name_, &maxConvexity_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_.lowHue = lowHue; - config_.lowSat = lowSat; - config_.lowVal = lowVal; - config_.highHue = highHue; - config_.highSat = highSat; - config_.highVal = highVal; + 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_.filterByColor = params_.filterByColor = (bool)filterByColor_int; - config_.blobColor = params_.blobColor = blobColor_int; + config_.filterByColor = params_.filterByColor = (bool)filter_by_color_int; + config_.blobColor = params_.blobColor = blob_color_int; // // Filter by Area. - config_.filterByArea = params_.filterByArea = (bool)filterByArea_int; - config_.minArea = params_.minArea = (double)minArea_int;//should be the same after printing the 2 types + config_.filterByArea = params_.filterByArea = (bool)filter_by_area_int; + config_.minArea = params_.minArea = (float)min_area_int;//should be the same after printing the 2 types std::cout << "test trackbar changes params_.minArea to : " << params_.minArea << std::endl; - config_.maxArea = params_.maxArea = (double)maxArea_int;// use float + config_.maxArea = params_.maxArea = (float)max_area_int;// use float std::cout << "test trackbar changes params_.maxArea to : " << params_.maxArea << std::endl; - config_.minDistBetweenBlobs = params_.minDistBetweenBlobs = (double)minDistBetweenBlobs_int; + config_.minDistBetweenBlobs = params_.minDistBetweenBlobs = (float)min_dist_between_blobs_int; std::cout << "test trackbar changes params_.minDistBetweenBlobs to : " << params_.minDistBetweenBlobs << std::endl; // // Filter by Circularity - config_.filterByCircularity = params_.filterByCircularity = (bool)filterByCircularity_int; - config_.minCircularity = params_.minCircularity = (double)minCircularity_int/100.00; + config_.filterByCircularity = params_.filterByCircularity = (bool)filter_by_circularity_int; + config_.minCircularity = params_.minCircularity = (float)min_circularity_int/100; std::cout << "test trackbar changes params_.minCircularity to : " << params_.minCircularity << std::endl; - config_.maxCircularity = params_.maxCircularity = (double)maxCircularity_int/100.00; + config_.maxCircularity = params_.maxCircularity = (float)max_circularity_int/100; std::cout << "test trackbar changes params_.maxCircularity to : " << params_.maxCircularity << std::endl; // Filter by Inertia - config_.filterByInertia = params_.filterByInertia = (bool)filterByInertia_int; - config_.minInertiaRatio = params_.minInertiaRatio = (double)minInertiaRatio_int/100.00; + config_.filterByInertia = params_.filterByInertia = (bool)filter_by_inertia_int; + config_.minInertiaRatio = params_.minInertiaRatio = (float)min_inertia_ratio_int/100; std::cout << "test trackbar changes params_.minInertiaRatio to : " << params_.minInertiaRatio << std::endl; - config_.maxInertiaRatio = params_.maxInertiaRatio = (double)maxInertiaRatio_int/100.00; + config_.maxInertiaRatio = params_.maxInertiaRatio = (float)max_inertia_ratio_int/100; std::cout << "test trackbar changes params_.maxInertiaRatio to : " << params_.maxInertiaRatio << std::endl; // Filter by Convexity - config_.filterByConvexity = params_.filterByConvexity = (bool)filterByConvexity_int; - config_.minConvexity = params_.minConvexity = (double)minConvexity_int/100.00; + config_.filterByConvexity = params_.filterByConvexity = (bool)filter_by_convexity_int; + config_.minConvexity = params_.minConvexity = (float)min_convexity_int/100; std::cout << "test trackbar changes params_.minConvexity to : " << params_.minConvexity << std::endl; - config_.maxConvexity = params_.maxConvexity = (double)maxConvexity_int/100.00; + config_.maxConvexity = params_.maxConvexity = (float)max_convexity_int/100; std::cout << "test trackbar changes params_.maxConvexity to : " << params_.maxConvexity << std::endl; ROS_INFO("test for before updating config. when the trackbar moves, the params server is updated."); @@ -418,21 +419,17 @@ class BlobDetectionNodelet : public opencv_apps::Nodelet cv::Mat HSV_image; cv::cvtColor(frame, HSV_image, cv::COLOR_BGR2HSV); // hsv hue range is 180 and hsv full is 360 cv::Mat thresholded_image; - cv::inRange(HSV_image, cv::Scalar(lowHue, lowSat, lowVal), cv::Scalar(highHue, highSat, highVal), thresholded_image); // there were not the 2 cv:: + 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); cv::Mat thresholded_image_with_mask; cv::bitwise_and(frame,frame,thresholded_image_with_mask,thresholded_image); //bitwise_and(img,mask,m_out); - //Mat kernel = getStructuringElement(MORPH_RECT, Size(5,5), Point(-1, -1)); - //Mat element = getStructuringElement(MORPH_RECT, - //Size(2 * morph_size + 1,2 * morph_size + 1), - //Point(morph_size, morph_size)); - - cv::Mat kernel; cv::Mat morphology_ex_image; if (morphology_ex_type_ == "opening" || morphology_ex_type_ == "closing") { + cv::Mat kernel; + if (morphology_ex_kernel_size_ % 2 != 1) { morphology_ex_kernel_size_ = morphology_ex_kernel_size_ + 1; @@ -451,25 +448,9 @@ class BlobDetectionNodelet : public opencv_apps::Nodelet } } - //morphologyEx(img, open, CV_MOP_OPEN, kernel); - - //morphologyEx( src, dst, MORPH_TOPHAT, element, Point(-1,-1), i ); - //morphologyEx( src, dst, MORPH_TOPHAT, element ); // here iteration=1 -//MORPH_OPEN - - //morphologyEx(img, close, CV_MOP_CLOSE, kernel); - //morphologyEx(image, output, MORPH_CLOSE, element,Point(-1, -1), 2); - - // Serena : not related to us, or later we use the same way to check on something if needed - // those paramaters cannot be =0 - // so we must check here - //canny_threshold_ = std::max(canny_threshold_, 1.0); - //accumulator_threshold_ = std::max(accumulator_threshold_, 1.0); - - if (!compareBlobDetectorParams(params_, prev_params_)) { - detector = cv::SimpleBlobDetector::create(params_); + detector_ = cv::SimpleBlobDetector::create(params_); ROS_INFO("test for create new detector."); } @@ -479,12 +460,12 @@ class BlobDetectionNodelet : public opencv_apps::Nodelet // runs the actual detection if (morphology_ex_type_ == "opening" || morphology_ex_type_ == "closing") { - detector->detect(morphology_ex_image, keypoints); + detector_->detect(morphology_ex_image, keypoints); } else { - detector->detect(thresholded_image, keypoints); + detector_->detect(thresholded_image, keypoints); } prev_params_ = params_; @@ -494,41 +475,25 @@ class BlobDetectionNodelet : public opencv_apps::Nodelet std::cout << "test if there is keypoint detected : " << keypoints[0].size << std::endl; } - // cv::Mat out_image; - // will we encounter this situation ?? if (frame.channels() == 1) - // { - // cv::cvtColor(frame, out_image, cv::COLOR_GRAY2BGR); - // } - // else - // { - // out_image = frame; - // } - // clone the colour, input image for displaying purposes for (const cv::KeyPoint& i : keypoints) { - // cv::Point center(cvRound(i.pt[0]), cvRound(i.pt[1])); // should we round or not ? - // int radius = cvRound(i.size); // does not need to re-define. -// Serena : change to ours, generate the blob messages here opencv_apps::Blob blob_msg; - // blob_msg.center.x = center.x; - // blob_msg.center.y = center.y; - // blob_msg.radius = radius; 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); // why do not put in blobs_msg directly ? + blobs_msg.blobs.push_back(blob_msg); } cv::Mat out_image; - drawKeypoints(frame, keypoints, out_image, cv::Scalar(0,0,255), cv::DrawMatchesFlags::DRAW_RICH_KEYPOINTS ); // there were not the 2 cv:: + 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);// show debug image or out image? + cv::imshow(window_name_, out_image); cv::imshow(thresholded_image_name_, thresholded_image); cv::imshow(thresholded_image_with_mask_name_, thresholded_image_with_mask); @@ -538,26 +503,21 @@ class BlobDetectionNodelet : public opencv_apps::Nodelet } char c = (char)cv::waitKey(1); - - // if( c == 27 ) - // break; switch (c) { case 'o': morphology_ex_type_ = "opening"; - //morphology_ex_kernel_size_ = morphology_ex_kernel_size_initial_value_; break; case 'c': morphology_ex_type_ = "closing"; - //morphology_ex_kernel_size_ = morphology_ex_kernel_size_initial_value_; break; case 'n': morphology_ex_type_ = "off"; - //morphology_ex_kernel_size_ = morphology_ex_kernel_size_initial_value_; break; } } + // Publish the image. if (thresholded_img_pub_.getNumSubscribers() > 0) { cv::Mat out_thresholded_image; @@ -565,13 +525,9 @@ class BlobDetectionNodelet : public opencv_apps::Nodelet sensor_msgs::Image::Ptr out_thresholded_img = cv_bridge::CvImage(msg->header, "bgr8", out_thresholded_image).toImageMsg(); thresholded_img_pub_.publish(out_thresholded_img); } - // Publish the image. - // Publish the image. if (thresholded_img_with_mask_pub_.getNumSubscribers() > 0) { - //cv::Mat out_thresholded_image_with_mask; - //cv::cvtColor(thresholded_image_with_mask, out_thresholded_image_with_mask, cv::COLOR_GRAY2BGR); 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); } @@ -590,7 +546,7 @@ class BlobDetectionNodelet : public opencv_apps::Nodelet sensor_msgs::Image::Ptr out_img = cv_bridge::CvImage(msg->header, "bgr8", out_image).toImageMsg(); img_pub_.publish(out_img); - msg_pub_.publish(blobs_msg); //s + msg_pub_.publish(blobs_msg); } catch (cv::Exception& e) { @@ -628,7 +584,7 @@ class BlobDetectionNodelet : public opencv_apps::Nodelet if (debug_view_) { - always_subscribe_ = debug_view_; // or true + always_subscribe_ = true; } prev_stamp_ = ros::Time(0, 0); @@ -639,13 +595,13 @@ class BlobDetectionNodelet : public opencv_apps::Nodelet blob_detector_params_name_ = "Blob Detector Params"; // delete output screen in launch file. ROS_INFO("test for oninit."); - - prev_morphology_ex_type_ = "off"; + morphology_ex_type_default_value_ = "off"; + prev_morphology_ex_type_ = "off"; morphology_ex_kernel_size_initial_value_ = 3; - max_minArea_ = 10000; // if 2560*1600 = 4096000 - max_maxArea_ = 10000; // if 2560*1600 = 4096000 - max_minDistBetweenBlobs_ = 1000000; // if 2560*1600 = 4096000 + min_area_upper_limit_ = 10000; // if 2560*1600 = 4096000 + max_area_upper_limit_ = 10000; // if 2560*1600 = 4096000 + min_dist_between_blobs_upper_limit_ = 1000000; // if 2560*1600 = 4096000 reconfigure_server_ = boost::make_shared >(*pnh_); dynamic_reconfigure::Server::CallbackType f = @@ -656,7 +612,6 @@ class BlobDetectionNodelet : public opencv_apps::Nodelet 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); - msg_pub_ = advertise(*pnh_, "blobs", 1); onInitPostProcess(); From d6a8532d344a7807460f64098fe435661121b436 Mon Sep 17 00:00:00 2001 From: HuiShiSerenaShi Date: Sat, 5 Feb 2022 20:15:38 +0200 Subject: [PATCH 07/17] update --- CMakeLists.txt | 2 +- cfg/BlobDetection.cfg | 58 +++--- config/blob_detection_config.yaml | 61 +++--- launch/blob_detection.launch | 56 +++++- launch/blob_detection_ims.launch | 228 +++++++++++++--------- nodelet_plugins.xml | 3 +- package.xml | 2 +- src/nodelet/blob_detection_nodelet.cpp | 250 +++++++++---------------- 8 files changed, 338 insertions(+), 322 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 8788535e..7ec6da68 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -384,4 +384,4 @@ if(CATKIN_ENABLE_TESTING) endforeach() endif() add_subdirectory(test) -endif() +endif() \ No newline at end of file diff --git a/cfg/BlobDetection.cfg b/cfg/BlobDetection.cfg index 57e0f0ce..192d2497 100755 --- a/cfg/BlobDetection.cfg +++ b/cfg/BlobDetection.cfg @@ -1,7 +1,7 @@ #! /usr/bin/env python # Software License Agreement (BSD License) # -# Copyright (c) 2014, Kei Okada. +# Copyright (c) 2022, Hui Shi, University of Tartu. # All rights reserved. # # Redistribution and use in source and binary forms, with or without @@ -14,7 +14,7 @@ # 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 Kei Okada nor the names of its +# * 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. # @@ -39,7 +39,6 @@ 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) @@ -47,36 +46,27 @@ gen.add("sat_upper_limit", int_t, 0, "The maximum allowed field value Saturation 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", "off"), - gen.const("opening", str_t, "opening", "opening"), - gen.const("closing", str_t, "closing", "closing")], +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", 3, 0, 100) - - -gen.add("filterByColor", bool_t, 0, "filterByColor", True) -gen.add("blobColor", int_t, 0, "blobColor", 255, 0, 255) - -gen.add("filterByArea", bool_t, 0, "filterByArea", True) -gen.add("minArea", double_t, 0, "minArea", 5000, 0, 10000000) -gen.add("maxArea", double_t, 0, "maxArea", 9000000, 0, 10000000) - -gen.add("minDistBetweenBlobs", double_t, 0, "minDistBetweenBlobs", 100, 0, 1000000) # if 2560*1600 = 4096000 - -gen.add("filterByCircularity", bool_t, 0, "filterByCircularity", False) -gen.add("minCircularity", double_t, 0, "minCircularity", 0, 0, 1) -gen.add("maxCircularity", double_t, 0, "maxCircularity", 1, 0, 1) - -gen.add("filterByInertia", bool_t, 0, "filterByInertia", False) -gen.add("minInertiaRatio", double_t, 0, "minInertiaRatio", 0, 0, 1) -gen.add("maxInertiaRatio", double_t, 0, "maxInertiaRatio", 1, 0, 1) - -gen.add("filterByConvexity", bool_t, 0, "filterByConvexity", False) -gen.add("minConvexity", double_t, 0, "minConvexity", 0, 0, 1) -gen.add("maxConvexity", double_t, 0, "maxConvexity", 1, 0, 1) - - -exit(gen.generate(PACKAGE, "blob_detection", "BlobDetection")) +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, 300000) +gen.add("max_area", double_t, 0, "Extracted blobs have an area between minArea (inclusive) and maxArea (exclusive).", 300000, 0, 300000) +gen.add("min_dist_between_blobs", double_t, 0, "The blobs located closer than minDistBetweenBlobs are merged.", 0, 0, 800) +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_config.yaml b/config/blob_detection_config.yaml index 1658d60f..ddf99e2a 100644 --- a/config/blob_detection_config.yaml +++ b/config/blob_detection_config.yaml @@ -1,13 +1,12 @@ -blobColor: 255 +blob_color: 255 debug_view: false -filterByArea: true -filterByCircularity: true -filterByColor: true -filterByConvexity: true -filterByInertia: true -highHue: 179.0 -highSat: 255.0 -highVal: 255.0 +filter_by_area: true +filter_by_circularity: false +filter_by_color: true +filter_by_convexity: false +filter_by_inertia: false +hue_lower_limit: 1 +hue_upper_limit: 179 image: compressed: format: jpeg @@ -22,18 +21,18 @@ image: optimize_for: 1 quality: 31 target_bitrate: 800000 -lowHue: 80.55000000000001 -lowSat: 0.0 -lowVal: 0.0 -maxArea: 9000000 -maxCircularity: 1.0 -maxConvexity: 1.0 -maxInertiaRatio: 1.0 -minArea: 5000 -minCircularity: 0.0 -minConvexity: 0.0 -minDistBetweenBlobs: 100 -minInertiaRatio: 0.0 +max_area: 300000.0 +max_area_upper_limit: 300000 +max_circularity: 1.0 +max_convexity: 1.0 +max_inertia_ratio: 1.0 +min_area: 0.0 +min_area_upper_limit: 300000 +min_circularity: 0.0 +min_convexity: 0.0 +min_dist_between_blobs: 800.0 +min_dist_between_blobs_upper_limit: 800 +min_inertia_ratio: 0.0 morphology_ex_image: compressed: format: jpeg @@ -48,10 +47,12 @@ morphology_ex_image: optimize_for: 1 quality: 31 target_bitrate: 800000 -morphology_ex_kernel_size: 16 +morphology_ex_kernel_size: 17 morphology_ex_type: opening num_worker_threads: 1 queue_size: 3 +sat_lower_limit: 0 +sat_upper_limit: 255 thresholded_image: compressed: format: jpeg @@ -66,4 +67,20 @@ thresholded_image: 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 index aef38ee2..0094a636 100644 --- a/launch/blob_detection.launch +++ b/launch/blob_detection.launch @@ -4,9 +4,12 @@ - + + + + @@ -15,8 +18,30 @@ - - + + + + + + + + + + + + + + + + + + + + + + + + @@ -33,5 +58,30 @@ + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/launch/blob_detection_ims.launch b/launch/blob_detection_ims.launch index ec4eb3db..a7643935 100644 --- a/launch/blob_detection_ims.launch +++ b/launch/blob_detection_ims.launch @@ -1,104 +1,146 @@ + + - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/nodelet_plugins.xml b/nodelet_plugins.xml index 4c9b1c96..0816abe9 100644 --- a/nodelet_plugins.xml +++ b/nodelet_plugins.xml @@ -151,5 +151,4 @@ - - + \ No newline at end of file diff --git a/package.xml b/package.xml index a2154776..425e43ce 100644 --- a/package.xml +++ b/package.xml @@ -52,4 +52,4 @@ - + \ No newline at end of file diff --git a/src/nodelet/blob_detection_nodelet.cpp b/src/nodelet/blob_detection_nodelet.cpp index 84fdb77d..4c713c6f 100644 --- a/src/nodelet/blob_detection_nodelet.cpp +++ b/src/nodelet/blob_detection_nodelet.cpp @@ -2,7 +2,7 @@ /********************************************************************* * Software License Agreement (BSD License) * -* Copyright (c) 2014, Kei Okada. +* Copyright (c) 2022, Hui Shi, University of Tartu. * All rights reserved. * * Redistribution and use in source and binary forms, with or without @@ -15,7 +15,7 @@ * 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 Kei Okada nor the names of its +* * 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. * @@ -33,11 +33,18 @@ * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ -// https://github.com/Itseez/opencv/blob/2.4/samples/cpp/tutorial_code/ImgTrans/HoughCircle_Demo.cpp +// https://github.com/ros-perception/opencv_apps /** - * @file HoughCircle_Demo.cpp - * @brief Demo code for Hough Transform - * @author OpenCV team + * @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 @@ -65,7 +72,6 @@ class BlobDetectionNodelet : public opencv_apps::Nodelet 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_; @@ -81,16 +87,11 @@ class BlobDetectionNodelet : public opencv_apps::Nodelet 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_; // reference to camshift_nodelet + 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_; - - //this is for opencv version 2: SimpleBlobDetector detector(params); check the following link for more info - // https://learnopencv.com/blob-detection-using-opencv-python-c/ - //cv::Ptr detector = cv::SimpleBlobDetector::create(params); // this line was without the 3 cv:: - // where should we create the detector and params ? https://ip.festo-didactic.com/DigitalEducation/EITManufacturing/UNICO/FDRenderer/index.html?LearningPath=0a4e3fe94887400cad125a990fd2d518&Language=EN&FDEP=true&NoPHP=true cv::Ptr detector_; // initial and max values of the parameters of interests. @@ -104,39 +105,30 @@ class BlobDetectionNodelet : public opencv_apps::Nodelet 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, because trackbar only accept int type. - int blob_color_int; - - int filter_by_area_int; + 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; + int min_dist_between_blobs_int; // min dist between blobs int min_dist_between_blobs_upper_limit_; - // Filter by Area. - - // // Filter by Circularity - int filter_by_circularity_int; + int filter_by_circularity_int; // Filter by Circularity int min_circularity_int; int max_circularity_int; - - // Filter by Inertia - int filter_by_inertia_int; + int filter_by_inertia_int; // Filter by Inertia int min_inertia_ratio_int; int max_inertia_ratio_int; - - // Filter by Convexity - int filter_by_convexity_int; + int filter_by_convexity_int; // Filter by Convexity int min_convexity_int; int max_convexity_int; - bool compareBlobDetectorParams(const cv::SimpleBlobDetector::Params& params_1, const cv::SimpleBlobDetector::Params& params_2)// contour_moments line 61 counter1 counter2, & + // 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; @@ -177,10 +169,10 @@ class BlobDetectionNodelet : public opencv_apps::Nodelet 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; @@ -191,59 +183,37 @@ class BlobDetectionNodelet : public opencv_apps::Nodelet morphology_ex_type_ = config_.morphology_ex_type; morphology_ex_kernel_size_ = config_.morphology_ex_kernel_size; - params_.filterByColor = config_.filterByColor; - params_.blobColor = config_.blobColor; - - // // Filter by Area. - params_.filterByArea = config_.filterByArea; - params_.minArea = config_.minArea; - params_.maxArea = config_.maxArea; - - params_.minDistBetweenBlobs = config_.minDistBetweenBlobs; - - // // Filter by Circularity - params_.filterByCircularity = config_.filterByCircularity; - params_.minCircularity = config_.minCircularity; - params_.maxCircularity = config_.maxCircularity; - - // Filter by Inertia - params_.filterByInertia = config_.filterByInertia; - params_.minInertiaRatio = config_.minInertiaRatio; - params_.maxInertiaRatio = config_.maxInertiaRatio; - // Filter by Convexity - params_.filterByConvexity = config_.filterByConvexity; - params_.minConvexity = config_.minConvexity; - params_.maxConvexity = config_.maxConvexity; - + 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); - std::cout << "test reconfigure callback convert int(params_.minCircularity*100) to : " << min_circularity_int << std::endl; max_circularity_int = int(params_.maxCircularity*100); - std::cout << "test reconfigure callback convert int(params_.maxCircularity*100) to : " << max_circularity_int << std::endl; - filter_by_inertia_int = int(params_.filterByInertia); min_inertia_ratio_int = int(params_.minInertiaRatio*100); - std::cout << "test reconfigure callback convert int(params_.minInertiaRatio*100) to : " << min_inertia_ratio_int << std::endl; max_inertia_ratio_int = int(params_.maxInertiaRatio*100); - std::cout << "test reconfigure callback convert int(params_.maxInertiaRatio*100) to : " << max_inertia_ratio_int << std::endl; - filter_by_convexity_int = int(params_.filterByConvexity); min_convexity_int = int(params_.minConvexity*100); - std::cout << "test reconfigure callback convert int(params_.minConvexity*100) to : " << min_convexity_int << std::endl; max_convexity_int = int(params_.maxConvexity*100); - std::cout << "test reconfigure callback convert int(params_.maxConvexity*100) to : " << max_convexity_int << std::endl; - - ROS_INFO("test for reconfigure call back."); } const std::string& frameWithDefault(const std::string& frame, const std::string& image_frame) @@ -263,10 +233,10 @@ class BlobDetectionNodelet : public opencv_apps::Nodelet doWork(msg, msg->header.frame_id); } + // opencv trackbar callback static void trackbarCallback(int /*unused*/, void* /*unused*/) { need_config_update_ = true; - ROS_INFO("test for trackbar call back."); } void doWork(const sensor_msgs::ImageConstPtr& msg, const std::string& input_frame_from_msg) @@ -282,8 +252,7 @@ class BlobDetectionNodelet : public opencv_apps::Nodelet blobs_msg.header = msg->header; // Do the work - - /// Convert it to 3 channel + // if the image is gray image, convert it to BGR image cv::Mat frame; if (frame_src.channels() > 1) { @@ -294,7 +263,7 @@ class BlobDetectionNodelet : public opencv_apps::Nodelet cv::cvtColor(frame_src, frame, cv::COLOR_GRAY2BGR); } - // create the main window, and attach the trackbars + // create the windows, and attach the trackbars if (debug_view_) { cv::namedWindow(window_name_, cv::WINDOW_AUTOSIZE); @@ -304,12 +273,10 @@ class BlobDetectionNodelet : public opencv_apps::Nodelet 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_; @@ -322,7 +289,6 @@ class BlobDetectionNodelet : public opencv_apps::Nodelet { morphology_ex_image_name_ = "Opening Image"; } - else if (morphology_ex_type_ == "closing") { morphology_ex_image_name_ = "Closing Image"; @@ -334,31 +300,25 @@ class BlobDetectionNodelet : public opencv_apps::Nodelet prev_morphology_ex_type_ = morphology_ex_type_; - cv::createTrackbar("Hue Lower Limit", thresholded_image_name_, &hue_lower_limit_, 179, trackbarCallback);// should we increase the range? const int max_value_H = 360/2; https://docs.opencv.org/3.4/da/d97/tutorial_threshold_inRange.html + 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("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); @@ -374,62 +334,43 @@ class BlobDetectionNodelet : public opencv_apps::Nodelet 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; - config_.filterByColor = params_.filterByColor = (bool)filter_by_color_int; - config_.blobColor = params_.blobColor = blob_color_int; - - // // Filter by Area. - config_.filterByArea = params_.filterByArea = (bool)filter_by_area_int; - config_.minArea = params_.minArea = (float)min_area_int;//should be the same after printing the 2 types - std::cout << "test trackbar changes params_.minArea to : " << params_.minArea << std::endl; - config_.maxArea = params_.maxArea = (float)max_area_int;// use float - std::cout << "test trackbar changes params_.maxArea to : " << params_.maxArea << std::endl; - - config_.minDistBetweenBlobs = params_.minDistBetweenBlobs = (float)min_dist_between_blobs_int; - std::cout << "test trackbar changes params_.minDistBetweenBlobs to : " << params_.minDistBetweenBlobs << std::endl; - - - // // Filter by Circularity - config_.filterByCircularity = params_.filterByCircularity = (bool)filter_by_circularity_int; - config_.minCircularity = params_.minCircularity = (float)min_circularity_int/100; - std::cout << "test trackbar changes params_.minCircularity to : " << params_.minCircularity << std::endl; - config_.maxCircularity = params_.maxCircularity = (float)max_circularity_int/100; - std::cout << "test trackbar changes params_.maxCircularity to : " << params_.maxCircularity << std::endl; - - // Filter by Inertia - config_.filterByInertia = params_.filterByInertia = (bool)filter_by_inertia_int; - config_.minInertiaRatio = params_.minInertiaRatio = (float)min_inertia_ratio_int/100; - std::cout << "test trackbar changes params_.minInertiaRatio to : " << params_.minInertiaRatio << std::endl; - config_.maxInertiaRatio = params_.maxInertiaRatio = (float)max_inertia_ratio_int/100; - std::cout << "test trackbar changes params_.maxInertiaRatio to : " << params_.maxInertiaRatio << std::endl; - - // Filter by Convexity - config_.filterByConvexity = params_.filterByConvexity = (bool)filter_by_convexity_int; - config_.minConvexity = params_.minConvexity = (float)min_convexity_int/100; - std::cout << "test trackbar changes params_.minConvexity to : " << params_.minConvexity << std::endl; - config_.maxConvexity = params_.maxConvexity = (float)max_convexity_int/100; - std::cout << "test trackbar changes params_.maxConvexity to : " << params_.maxConvexity << std::endl; - - ROS_INFO("test for before updating config. when the trackbar moves, the params server is updated."); 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); // hsv hue range is 180 and hsv full is 360 + 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); //bitwise_and(img,mask,m_out); + 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; @@ -441,28 +382,25 @@ class BlobDetectionNodelet : public opencv_apps::Nodelet { 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_); - ROS_INFO("test for create new detector."); } - // runs the detection, and update the display - // will hold the results of the detection + // 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); @@ -470,13 +408,7 @@ class BlobDetectionNodelet : public opencv_apps::Nodelet prev_params_ = params_; - if (keypoints.size() > 0) - { - std::cout << "test if there is keypoint detected : " << keypoints[0].size << std::endl; - } - - // clone the colour, input image for displaying purposes - + // convert the detection results to ROS messages for (const cv::KeyPoint& i : keypoints) { opencv_apps::Blob blob_msg; @@ -486,11 +418,11 @@ class BlobDetectionNodelet : public opencv_apps::Nodelet 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 ); + 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); @@ -501,7 +433,9 @@ class BlobDetectionNodelet : public opencv_apps::Nodelet { 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) { @@ -517,7 +451,7 @@ class BlobDetectionNodelet : public opencv_apps::Nodelet } } - // Publish the image. + // Publish the debug images if there is a subscriber. if (thresholded_img_pub_.getNumSubscribers() > 0) { cv::Mat out_thresholded_image; @@ -542,9 +476,9 @@ class BlobDetectionNodelet : public opencv_apps::Nodelet 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); } @@ -575,12 +509,16 @@ class BlobDetectionNodelet : public opencv_apps::Nodelet 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_, 300000); + pnh_->param("max_area_upper_limit", max_area_upper_limit_, 300000); + pnh_->param("min_dist_between_blobs_upper_limit", min_dist_between_blobs_upper_limit_, 800); if (debug_view_) { @@ -593,15 +531,10 @@ class BlobDetectionNodelet : public opencv_apps::Nodelet thresholded_image_with_mask_name_ = "Thresholded Image With Mask"; morphology_ex_image_name_ = "Opening Image"; blob_detector_params_name_ = "Blob Detector Params"; - // delete output screen in launch file. - ROS_INFO("test for oninit."); morphology_ex_type_default_value_ = "off"; prev_morphology_ex_type_ = "off"; morphology_ex_kernel_size_initial_value_ = 3; - min_area_upper_limit_ = 10000; // if 2560*1600 = 4096000 - max_area_upper_limit_ = 10000; // if 2560*1600 = 4096000 - min_dist_between_blobs_upper_limit_ = 1000000; // if 2560*1600 = 4096000 reconfigure_server_ = boost::make_shared >(*pnh_); dynamic_reconfigure::Server::CallbackType f = @@ -609,10 +542,11 @@ class BlobDetectionNodelet : public opencv_apps::Nodelet 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); - msg_pub_ = advertise(*pnh_, "blobs", 1); onInitPostProcess(); } @@ -620,21 +554,5 @@ class BlobDetectionNodelet : public opencv_apps::Nodelet bool BlobDetectionNodelet::need_config_update_ = false; } // namespace opencv_apps -// the following block (namespace) is not needed in our case -namespace blob_detection -{ -class BlobDetectionNodelet : public opencv_apps::BlobDetectionNodelet -{ -public: - virtual void onInit() // NOLINT(modernize-use-override) - { - //ROS_WARN("DeprecationWarning: Nodelet hough_circles/hough_circles is deprecated, " - //"and renamed to opencv_apps/hough_circles."); - opencv_apps::BlobDetectionNodelet::onInit(); - } -}; -} // namespace hough_circles - #include PLUGINLIB_EXPORT_CLASS(opencv_apps::BlobDetectionNodelet, nodelet::Nodelet); -PLUGINLIB_EXPORT_CLASS(blob_detection::BlobDetectionNodelet, nodelet::Nodelet); From 6b3fdab50f71fca91d042beaa1c6da71c850053d Mon Sep 17 00:00:00 2001 From: HuiShiSerenaShi Date: Mon, 21 Feb 2022 15:57:33 +0200 Subject: [PATCH 08/17] changed the debug_mode names in the launch file, and changed the cmakelists.txt & nodelet_plugins.xml for solving the merge conflits --- CMakeLists.txt | 30 +++++++++++++++++++----------- launch/blob_detection_ims.launch | 9 ++++----- nodelet_plugins.xml | 12 ++++++++++-- 3 files changed, 33 insertions(+), 18 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 7ec6da68..93ce2b37 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -2,7 +2,7 @@ cmake_minimum_required(VERSION 2.8.3) project(opencv_apps) ## https://stackoverflow.com/questions/10984442/how-to-detect-c11-support-of-a-compiler-with-cmake -if(CMAKE_COMPILER_IS_GNUCXX) +if(CMAKE_COMPILER_IS_GNUCXX AND CMAKE_CXX_COMPILER_VERSION VERSION_LESS "7") execute_process(COMMAND ${CMAKE_C_COMPILER} -dumpversion OUTPUT_VARIABLE GCC_VERSION) if (GCC_VERSION VERSION_GREATER 4.7 OR GCC_VERSION VERSION_EQUAL 4.7) message(STATUS "C++11 activated.") @@ -13,8 +13,6 @@ if(CMAKE_COMPILER_IS_GNUCXX) else () message(FATAL_ERROR "C++11 needed. Therefore a gcc compiler with a version higher than 4.3 is needed.") endif() -else(CMAKE_COMPILER_IS_GNUCXX) - add_definitions("-std=c++0x") endif(CMAKE_COMPILER_IS_GNUCXX) find_package(catkin REQUIRED COMPONENTS cv_bridge dynamic_reconfigure message_generation image_transport nodelet roscpp sensor_msgs std_msgs std_srvs) @@ -45,6 +43,7 @@ generate_dynamic_reconfigure_options( cfg/DiscreteFourierTransform.cfg cfg/AddingImages.cfg cfg/Smoothing.cfg + cfg/Morphology.cfg cfg/Pyramids.cfg cfg/EdgeDetection.cfg cfg/HoughLines.cfg cfg/HoughCircles.cfg cfg/FindContours.cfg cfg/ConvexHull.cfg cfg/GeneralContours.cfg cfg/ContourMoments.cfg @@ -53,6 +52,7 @@ generate_dynamic_reconfigure_options( cfg/GoodfeatureTrack.cfg cfg/CornerHarris.cfg # + cfg/EqualizeHistogram.cfg cfg/CamShift.cfg cfg/FBackFlow.cfg cfg/LKFlow.cfg @@ -64,8 +64,8 @@ generate_dynamic_reconfigure_options( cfg/RGBColorFilter.cfg cfg/HLSColorFilter.cfg cfg/HSVColorFilter.cfg + cfg/LabColorFilter.cfg cfg/WatershedSegmentation.cfg - cfg/BlobDetection.cfg ) ## Generate messages in the 'msg' folder @@ -102,9 +102,6 @@ add_message_files( Contour.msg ContourArray.msg ContourArrayStamped.msg - Blob.msg - BlobArray.msg - BlobArrayStamped.msg ) add_service_files( @@ -173,7 +170,7 @@ opencv_apps_add_nodelet(discrete_fourier_transform src/nodelet/discrete_fourier_ # ./tutorial_code/Histograms_Matching/calcBackProject_Demo2.cpp # ./tutorial_code/Histograms_Matching/calcHist_Demo.cpp # ./tutorial_code/Histograms_Matching/compareHist_Demo.cpp - # ./tutorial_code/Histograms_Matching/EqualizeHist_Demo.cpp + opencv_apps_add_nodelet(equalize_histogram src/nodelet/equalize_histogram_nodelet.cpp) # ./tutorial_code/Histograms_Matching/EqualizeHist_Demo.cpp # ./tutorial_code/Histograms_Matching/MatchTemplate_Demo.cpp # imagecodecs @@ -183,13 +180,14 @@ opencv_apps_add_nodelet(discrete_fourier_transform src/nodelet/discrete_fourier_ # ./tutorial_code/ImgProc/BasicLinearTransforms.cpp # ./tutorial_code/ImgProc/Morphology_1.cpp # ./tutorial_code/ImgProc/Morphology_2.cpp - # ./tutorial_code/ImgProc/Morphology_3.cpp +opencv_apps_add_nodelet(morphology src/nodelet/morphology_nodelet.cpp) # ./tutorial_code/ImgProc/Morphology_3.cpp opencv_apps_add_nodelet(pyramids src/nodelet/pyramids_nodelet.cpp) # ./tutorial_code/ImgProc/Pyramids.cpp opencv_apps_add_nodelet(smoothing src/nodelet/smoothing_nodelet.cpp) # ./tutorial_code/ImgProc/Smoothing.cpp opencv_apps_add_nodelet(threshold src/nodelet/threshold_nodelet.cpp) # ./tutorial_code/ImgProc/Threshold.cpp opencv_apps_add_nodelet(rgb_color_filter src/nodelet/color_filter_nodelet.cpp) # ./tutorial_code/ImgProc/Threshold_inRange.cpp opencv_apps_add_nodelet(hls_color_filter src/nodelet/color_filter_nodelet.cpp) # ./tutorial_code/ImgProc/Threshold_inRange.cpp opencv_apps_add_nodelet(hsv_color_filter src/nodelet/color_filter_nodelet.cpp) # ./tutorial_code/ImgProc/Threshold_inRange.cpp +opencv_apps_add_nodelet(lab_color_filter src/nodelet/color_filter_nodelet.cpp) # ImgTrans opencv_apps_add_nodelet(edge_detection src/nodelet/edge_detection_nodelet.cpp) # ./tutorial_code/ImgTrans/CannyDetector_Demo.cpp @@ -329,12 +327,22 @@ 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 -opencv_apps_add_nodelet(blob_detection src/nodelet/blob_detection_nodelet.cpp) - # 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) + +# TApi +# ./tapi/bgfg_segm.cpp +# ./tapi/camshift.cpp +# opencv_apps_add_nodelet(equalize_histogram src/nodelet/equalize_histogram_nodelet.cpp) ./tapi/clahe.cpp +# ./tapi/dense_optical_flow.cpp +# ./tapi/hog.cpp +# ./tapi/opencl_custom_kernel.cpp +# ./tapi/pyrlk_optical_flow.cpp +# ./tapi/squares.cpp +# ./tapi/ufacedetect.cpp + # https://github.com/Itseez/opencv/blob/2.4/samples/cpp/simpleflow_demo.cpp # simple flow requires opencv-contrib https://github.com/ros-perception/vision_opencv/issues/108 if(OPENCV_HAVE_OPTFLOW) diff --git a/launch/blob_detection_ims.launch b/launch/blob_detection_ims.launch index a7643935..3a043834 100644 --- a/launch/blob_detection_ims.launch +++ b/launch/blob_detection_ims.launch @@ -1,12 +1,11 @@ - + - @@ -44,7 +43,7 @@ - + @@ -88,7 +87,7 @@ - + @@ -134,7 +133,7 @@ - + diff --git a/nodelet_plugins.xml b/nodelet_plugins.xml index 0816abe9..ad4b7de2 100644 --- a/nodelet_plugins.xml +++ b/nodelet_plugins.xml @@ -7,6 +7,10 @@ Nodelet to fourier transform + + Nodelet to apply morphological transformations + + Nodelet to smoothing by homogeneous filter, bilateral filter, gaussian filter, median filter @@ -111,12 +115,16 @@ Nodelet of hsv color filter + + Nodelet of lab color filter + + Nodelet to demonstrate the famous watershed segmentation algorithm in OpenCV: watershed() - - Nodelet to find blobs + + Nodelet for histogram equalization diff --git a/src/nodelet/blob_detection_nodelet.cpp b/src/nodelet/blob_detection_nodelet.cpp index 4c713c6f..4c055867 100644 --- a/src/nodelet/blob_detection_nodelet.cpp +++ b/src/nodelet/blob_detection_nodelet.cpp @@ -538,7 +538,7 @@ class BlobDetectionNodelet : public opencv_apps::Nodelet reconfigure_server_ = boost::make_shared >(*pnh_); dynamic_reconfigure::Server::CallbackType f = - boost::bind(&BlobDetectionNodelet::reconfigureCallback, this, _1, _2); + boost::bind(&BlobDetectionNodelet::reconfigureCallback, this, boost::placeholders::_1, boost::placeholders::_2); reconfigure_server_->setCallback(f); img_pub_ = advertiseImage(*pnh_, "image", 1); From c7d53f0014a2956157b175a7d9c7480af6848734 Mon Sep 17 00:00:00 2001 From: HuiShiSerenaShi Date: Mon, 21 Feb 2022 17:59:59 +0200 Subject: [PATCH 10/17] changed the launch file name to blob_detection_debug_mode.launch --- ...blob_detection_ims.launch => blob_detection_debug_mode.launch} | 0 1 file changed, 0 insertions(+), 0 deletions(-) rename launch/{blob_detection_ims.launch => blob_detection_debug_mode.launch} (100%) diff --git a/launch/blob_detection_ims.launch b/launch/blob_detection_debug_mode.launch similarity index 100% rename from launch/blob_detection_ims.launch rename to launch/blob_detection_debug_mode.launch From 5147492cde017468e94383e6affea9042b53aa5f Mon Sep 17 00:00:00 2001 From: HuiShiSerenaShi Date: Mon, 7 Mar 2022 13:55:05 +0200 Subject: [PATCH 11/17] updated launch file --- cfg/BlobDetection.cfg | 6 +- config/blob_detection_config.yaml | 14 +- launch/blob_detection.launch | 197 ++++++++++++++++++------ launch/blob_detection_debug_mode.launch | 145 ----------------- src/nodelet/blob_detection_nodelet.cpp | 6 +- 5 files changed, 164 insertions(+), 204 deletions(-) delete mode 100644 launch/blob_detection_debug_mode.launch diff --git a/cfg/BlobDetection.cfg b/cfg/BlobDetection.cfg index 192d2497..9138e35e 100755 --- a/cfg/BlobDetection.cfg +++ b/cfg/BlobDetection.cfg @@ -56,9 +56,9 @@ gen.add("morphology_ex_kernel_size", int_t, 0, "morphological operation kernel s 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, 300000) -gen.add("max_area", double_t, 0, "Extracted blobs have an area between minArea (inclusive) and maxArea (exclusive).", 300000, 0, 300000) -gen.add("min_dist_between_blobs", double_t, 0, "The blobs located closer than minDistBetweenBlobs are merged.", 0, 0, 800) +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) diff --git a/config/blob_detection_config.yaml b/config/blob_detection_config.yaml index 13568a3c..7156efd9 100644 --- a/config/blob_detection_config.yaml +++ b/config/blob_detection_config.yaml @@ -1,4 +1,4 @@ -blob_color: 0 +blob_color: 255 debug_view: false filter_by_area: true filter_by_circularity: false @@ -21,17 +21,17 @@ image: optimize_for: 1 quality: 31 target_bitrate: 800000 -max_area: 300000.0 -max_area_upper_limit: 300000 +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: 300000 -min_circularity: 1.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: 800 +min_dist_between_blobs_upper_limit: 200 min_inertia_ratio: 0.0 morphology_ex_image: compressed: @@ -82,5 +82,5 @@ thresholded_image_with_mask: quality: 31 target_bitrate: 800000 use_camera_info: false -val_lower_limit: 113 +val_lower_limit: 0 val_upper_limit: 255 diff --git a/launch/blob_detection.launch b/launch/blob_detection.launch index 0094a636..064db281 100644 --- a/launch/blob_detection.launch +++ b/launch/blob_detection.launch @@ -5,11 +5,14 @@ + - - - + + + @@ -26,9 +29,9 @@ - + - + @@ -43,45 +46,147 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/launch/blob_detection_debug_mode.launch b/launch/blob_detection_debug_mode.launch deleted file mode 100644 index d477c10b..00000000 --- a/launch/blob_detection_debug_mode.launch +++ /dev/null @@ -1,145 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/src/nodelet/blob_detection_nodelet.cpp b/src/nodelet/blob_detection_nodelet.cpp index 4c055867..e0bb8478 100644 --- a/src/nodelet/blob_detection_nodelet.cpp +++ b/src/nodelet/blob_detection_nodelet.cpp @@ -516,9 +516,9 @@ class BlobDetectionNodelet : public opencv_apps::Nodelet 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_, 300000); - pnh_->param("max_area_upper_limit", max_area_upper_limit_, 300000); - pnh_->param("min_dist_between_blobs_upper_limit", min_dist_between_blobs_upper_limit_, 800); + 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_) { From ff54342f6063f0d8ba69a4dd63b851e425d522f7 Mon Sep 17 00:00:00 2001 From: HuiShiSerenaShi Date: Mon, 7 Mar 2022 14:09:13 +0200 Subject: [PATCH 12/17] changed the default mode in the launch file --- launch/blob_detection.launch | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/launch/blob_detection.launch b/launch/blob_detection.launch index 064db281..b9002a17 100644 --- a/launch/blob_detection.launch +++ b/launch/blob_detection.launch @@ -5,7 +5,7 @@ - From 9d2dd808f355f7cc84519911acd966f720f3edf1 Mon Sep 17 00:00:00 2001 From: HuiShiSerenaShi Date: Tue, 31 May 2022 12:20:05 +0300 Subject: [PATCH 13/17] fix clang-format --- config/blob_detection_config.yaml | 38 ++++----- launch/blob_detection.launch | 2 +- src/nodelet/blob_detection_nodelet.cpp | 102 ++++++++++++++----------- 3 files changed, 79 insertions(+), 63 deletions(-) diff --git a/config/blob_detection_config.yaml b/config/blob_detection_config.yaml index 7156efd9..376c3889 100644 --- a/config/blob_detection_config.yaml +++ b/config/blob_detection_config.yaml @@ -1,12 +1,12 @@ blob_color: 255 debug_view: false filter_by_area: true -filter_by_circularity: false +filter_by_circularity: true filter_by_color: true -filter_by_convexity: false -filter_by_inertia: false -hue_lower_limit: 0 -hue_upper_limit: 179 +filter_by_convexity: true +filter_by_inertia: true +hue_lower_limit: 114 +hue_upper_limit: 121 image: compressed: format: jpeg @@ -21,18 +21,18 @@ image: optimize_for: 1 quality: 31 target_bitrate: 800000 -max_area: 40000.0 +max_area: 39600.0 max_area_upper_limit: 40000 -max_circularity: 1.0 +max_circularity: 0.77 max_convexity: 1.0 -max_inertia_ratio: 1.0 -min_area: 0.0 +max_inertia_ratio: 0.66 +min_area: 13200.0 min_area_upper_limit: 40000 -min_circularity: 0.0 -min_convexity: 0.0 -min_dist_between_blobs: 0.0 +min_circularity: 0.53 +min_convexity: 0.88 +min_dist_between_blobs: 200.0 min_dist_between_blobs_upper_limit: 200 -min_inertia_ratio: 0.0 +min_inertia_ratio: 0.42 morphology_ex_image: compressed: format: jpeg @@ -47,12 +47,12 @@ morphology_ex_image: optimize_for: 1 quality: 31 target_bitrate: 800000 -morphology_ex_kernel_size: 3 -morphology_ex_type: 'off' +morphology_ex_kernel_size: 11 +morphology_ex_type: opening num_worker_threads: 1 queue_size: 3 -sat_lower_limit: 0 -sat_upper_limit: 255 +sat_lower_limit: 131 +sat_upper_limit: 181 thresholded_image: compressed: format: jpeg @@ -82,5 +82,5 @@ thresholded_image_with_mask: quality: 31 target_bitrate: 800000 use_camera_info: false -val_lower_limit: 0 -val_upper_limit: 255 +val_lower_limit: 131 +val_upper_limit: 169 diff --git a/launch/blob_detection.launch b/launch/blob_detection.launch index b9002a17..3c083352 100644 --- a/launch/blob_detection.launch +++ b/launch/blob_detection.launch @@ -5,7 +5,7 @@ - diff --git a/src/nodelet/blob_detection_nodelet.cpp b/src/nodelet/blob_detection_nodelet.cpp index e0bb8478..5c400c11 100644 --- a/src/nodelet/blob_detection_nodelet.cpp +++ b/src/nodelet/blob_detection_nodelet.cpp @@ -1,4 +1,5 @@ -// -*- coding:utf-8-unix; mode: c++; indent-tabs-mode: nil; c-basic-offset: 2; -*- +// -*- coding:utf-8-unix; mode: c++; indent-tabs-mode: nil; c-basic-offset: 2; +// -*- /********************************************************************* * Software License Agreement (BSD License) * @@ -47,22 +48,21 @@ * @author LearnOpenCV team */ -#include #include "opencv_apps/nodelet.h" -#include #include +#include +#include #include #include #include #include -#include -#include "opencv_apps/BlobDetectionConfig.h" #include "opencv_apps/Blob.h" #include "opencv_apps/BlobArray.h" #include "opencv_apps/BlobArrayStamped.h" - +#include "opencv_apps/BlobDetectionConfig.h" +#include namespace opencv_apps { @@ -87,10 +87,11 @@ class BlobDetectionNodelet : public opencv_apps::Nodelet 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_; + 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 params_; cv::SimpleBlobDetector::Params prev_params_; cv::Ptr detector_; @@ -109,8 +110,8 @@ class BlobDetectionNodelet : public opencv_apps::Nodelet 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 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_; @@ -128,7 +129,8 @@ class BlobDetectionNodelet : public opencv_apps::Nodelet 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) + bool compareBlobDetectorParams(const cv::SimpleBlobDetector::Params& params_1, + const cv::SimpleBlobDetector::Params& params_2) { if (params_1.filterByColor != params_2.filterByColor) return false; @@ -206,14 +208,14 @@ class BlobDetectionNodelet : public opencv_apps::Nodelet 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); + 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); + 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); + 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) @@ -284,7 +286,7 @@ class BlobDetectionNodelet : public opencv_apps::Nodelet } if (morphology_ex_type_ == "opening" || morphology_ex_type_ == "closing") - { + { if (morphology_ex_type_ == "opening") { morphology_ex_image_name_ = "Opening Image"; @@ -295,7 +297,8 @@ class BlobDetectionNodelet : public opencv_apps::Nodelet } cv::namedWindow(morphology_ex_image_name_, cv::WINDOW_AUTOSIZE); - cv::createTrackbar("MorphologyEx Kernel Size", morphology_ex_image_name_, &morphology_ex_kernel_size_, 100, trackbarCallback); + cv::createTrackbar("MorphologyEx Kernel Size", morphology_ex_image_name_, &morphology_ex_kernel_size_, 100, + trackbarCallback); } prev_morphology_ex_type_ = morphology_ex_type_; @@ -310,16 +313,23 @@ class BlobDetectionNodelet : public opencv_apps::Nodelet 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 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 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); @@ -341,14 +351,14 @@ class BlobDetectionNodelet : public opencv_apps::Nodelet 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_.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_.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; + 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; @@ -360,23 +370,25 @@ class BlobDetectionNodelet : public opencv_apps::Nodelet 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); + 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); + 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; + { + 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)); + 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") { @@ -420,7 +432,7 @@ class BlobDetectionNodelet : public opencv_apps::Nodelet // 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 ); + drawKeypoints(frame, keypoints, out_image, cv::Scalar(0, 0, 255), cv::DrawMatchesFlags::DRAW_RICH_KEYPOINTS); // shows the results if (debug_view_) @@ -434,7 +446,8 @@ class BlobDetectionNodelet : public opencv_apps::Nodelet cv::imshow(morphology_ex_image_name_, morphology_ex_image); } - // a waitKey for switching between different types of morphological operations via keyboard input. + // 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) @@ -456,13 +469,15 @@ class BlobDetectionNodelet : public opencv_apps::Nodelet { 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(); + 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(); + 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); } @@ -472,7 +487,8 @@ class BlobDetectionNodelet : public opencv_apps::Nodelet { 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(); + 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); } } @@ -536,7 +552,7 @@ class BlobDetectionNodelet : public opencv_apps::Nodelet prev_morphology_ex_type_ = "off"; morphology_ex_kernel_size_initial_value_ = 3; - reconfigure_server_ = boost::make_shared >(*pnh_); + 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); From 3b70d5f48f3f618fdfae5ba805a844196f149a4d Mon Sep 17 00:00:00 2001 From: HuiShiSerenaShi Date: Tue, 31 May 2022 13:21:55 +0300 Subject: [PATCH 14/17] fix catkin_lint --- package.xml | 2 ++ 1 file changed, 2 insertions(+) diff --git a/package.xml b/package.xml index 425e43ce..04af6279 100644 --- a/package.xml +++ b/package.xml @@ -39,6 +39,8 @@ sensor_msgs std_msgs std_srvs + rqt_reconfigure + rviz roslaunch rostest From 7e0b96e5ddd8a8603d6381af12d655f826d6cc89 Mon Sep 17 00:00:00 2001 From: HuiShiSerenaShi Date: Tue, 31 May 2022 15:41:13 +0300 Subject: [PATCH 15/17] check OpenCV version in CMakeLists.txt --- CMakeLists.txt | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index fd81b895..6c696d6c 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -331,7 +331,9 @@ 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 -opencv_apps_add_nodelet(blob_detection src/nodelet/blob_detection_nodelet.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) @@ -393,6 +395,9 @@ 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() From 0be6d9fa7be4ac79e290be01359c718f328ef439 Mon Sep 17 00:00:00 2001 From: HuiShiSerenaShi Date: Tue, 31 May 2022 16:12:48 +0300 Subject: [PATCH 16/17] fix clang-tidy --- src/nodelet/blob_detection_nodelet.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/nodelet/blob_detection_nodelet.cpp b/src/nodelet/blob_detection_nodelet.cpp index 5c400c11..180b0cf3 100644 --- a/src/nodelet/blob_detection_nodelet.cpp +++ b/src/nodelet/blob_detection_nodelet.cpp @@ -366,11 +366,11 @@ class BlobDetectionNodelet : public opencv_apps::Nodelet } // convert the image to HSV image - cv::Mat HSV_image; - cv::cvtColor(frame, HSV_image, cv::COLOR_BGR2HSV); + 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::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; From e71db83ec84c472b78a0fecff595b3f0f9457b5f Mon Sep 17 00:00:00 2001 From: HuiShiSerenaShi Date: Wed, 1 Jun 2022 12:29:55 +0300 Subject: [PATCH 17/17] changed the default debug_mode in launch file --- config/blob_detection_config.yaml | 40 +++++++++++++++---------------- launch/blob_detection.launch | 2 +- 2 files changed, 21 insertions(+), 21 deletions(-) diff --git a/config/blob_detection_config.yaml b/config/blob_detection_config.yaml index 376c3889..89f1e707 100644 --- a/config/blob_detection_config.yaml +++ b/config/blob_detection_config.yaml @@ -1,12 +1,12 @@ blob_color: 255 -debug_view: false +debug_view: true filter_by_area: true -filter_by_circularity: true +filter_by_circularity: false filter_by_color: true -filter_by_convexity: true -filter_by_inertia: true -hue_lower_limit: 114 -hue_upper_limit: 121 +filter_by_convexity: false +filter_by_inertia: false +hue_lower_limit: 0 +hue_upper_limit: 179 image: compressed: format: jpeg @@ -21,18 +21,18 @@ image: optimize_for: 1 quality: 31 target_bitrate: 800000 -max_area: 39600.0 +max_area: 40000.0 max_area_upper_limit: 40000 -max_circularity: 0.77 +max_circularity: 1.0 max_convexity: 1.0 -max_inertia_ratio: 0.66 -min_area: 13200.0 +max_inertia_ratio: 1.0 +min_area: 0.0 min_area_upper_limit: 40000 -min_circularity: 0.53 -min_convexity: 0.88 -min_dist_between_blobs: 200.0 +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.42 +min_inertia_ratio: 0.0 morphology_ex_image: compressed: format: jpeg @@ -47,12 +47,12 @@ morphology_ex_image: optimize_for: 1 quality: 31 target_bitrate: 800000 -morphology_ex_kernel_size: 11 -morphology_ex_type: opening +morphology_ex_kernel_size: 3 +morphology_ex_type: 'off' num_worker_threads: 1 queue_size: 3 -sat_lower_limit: 131 -sat_upper_limit: 181 +sat_lower_limit: 0 +sat_upper_limit: 255 thresholded_image: compressed: format: jpeg @@ -82,5 +82,5 @@ thresholded_image_with_mask: quality: 31 target_bitrate: 800000 use_camera_info: false -val_lower_limit: 131 -val_upper_limit: 169 +val_lower_limit: 0 +val_upper_limit: 255 diff --git a/launch/blob_detection.launch b/launch/blob_detection.launch index 3c083352..b9002a17 100644 --- a/launch/blob_detection.launch +++ b/launch/blob_detection.launch @@ -5,7 +5,7 @@ -