hough_circles_nodelet.cpp
Go to the documentation of this file.
1 // -*- coding:utf-8-unix; mode: c++; indent-tabs-mode: nil; c-basic-offset: 2; -*-
2 /*********************************************************************
3 * Software License Agreement (BSD License)
4 *
5 * Copyright (c) 2014, Kei Okada.
6 * All rights reserved.
7 *
8 * Redistribution and use in source and binary forms, with or without
9 * modification, are permitted provided that the following conditions
10 * are met:
11 *
12 * * Redistributions of source code must retain the above copyright
13 * notice, this list of conditions and the following disclaimer.
14 * * Redistributions in binary form must reproduce the above
15 * copyright notice, this list of conditions and the following
16 * disclaimer in the documentation and/or other materials provided
17 * with the distribution.
18 * * Neither the name of the Kei Okada nor the names of its
19 * contributors may be used to endorse or promote products derived
20 * from this software without specific prior written permission.
21 *
22 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
23 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
24 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
25 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
26 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
27 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
28 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
29 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
30 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
31 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
32 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
33 * POSSIBILITY OF SUCH DAMAGE.
34 *********************************************************************/
35 
36 // https://github.com/Itseez/opencv/blob/2.4/samples/cpp/tutorial_code/ImgTrans/HoughCircle_Demo.cpp
43 #include <ros/ros.h>
44 #include "opencv_apps/nodelet.h"
46 #include <cv_bridge/cv_bridge.h>
48 
49 #include <opencv2/highgui/highgui.hpp>
50 #include <opencv2/imgproc/imgproc.hpp>
51 
52 #include <dynamic_reconfigure/server.h>
53 #include "opencv_apps/HoughCirclesConfig.h"
54 #include "opencv_apps/Circle.h"
55 #include "opencv_apps/CircleArray.h"
56 #include "opencv_apps/CircleArrayStamped.h"
57 
58 namespace opencv_apps
59 {
61 {
66 
68 
69  typedef opencv_apps::HoughCirclesConfig Config;
70  typedef dynamic_reconfigure::Server<Config> ReconfigureServer;
71  Config config_;
73 
77 
78  std::string window_name_;
79  static bool need_config_update_;
80 
81  // initial and max values of the parameters of interests.
87  int canny_threshold_int; // for trackbar
98  double dp_;
99  int dp_int;
102 
105 
106  void reconfigureCallback(Config& new_config, uint32_t level)
107  {
108  config_ = new_config;
109  canny_threshold_ = config_.canny_threshold;
110  accumulator_threshold_ = config_.accumulator_threshold;
111  gaussian_blur_size_ = config_.gaussian_blur_size;
112  gaussian_sigma_x_ = config_.gaussian_sigma_x;
113  gaussian_sigma_y_ = config_.gaussian_sigma_y;
114 
115  dp_ = config_.dp;
116  min_circle_radius_ = config_.min_circle_radius;
117  max_circle_radius_ = config_.max_circle_radius;
118  debug_image_type_ = config_.debug_image_type;
119  min_distance_between_circles_ = config_.min_distance_between_circles;
120  canny_threshold_int = int(canny_threshold_);
121  accumulator_threshold_int = int(accumulator_threshold_);
122  gaussian_sigma_x_int = int(gaussian_sigma_x_);
123  gaussian_sigma_y_int = int(gaussian_sigma_y_);
124  min_distance_between_circles_int = int(min_distance_between_circles_);
125  dp_int = int(dp_);
126  }
127 
128  const std::string& frameWithDefault(const std::string& frame, const std::string& image_frame)
129  {
130  if (frame.empty())
131  return image_frame;
132  return frame;
133  }
134 
135  void imageCallbackWithInfo(const sensor_msgs::ImageConstPtr& msg, const sensor_msgs::CameraInfoConstPtr& cam_info)
136  {
137  doWork(msg, cam_info->header.frame_id);
138  }
139 
140  void imageCallback(const sensor_msgs::ImageConstPtr& msg)
141  {
142  doWork(msg, msg->header.frame_id);
143  }
144 
145  static void trackbarCallback(int value, void* userdata)
146  {
147  need_config_update_ = true;
148  }
149 
150  void doWork(const sensor_msgs::ImageConstPtr& msg, const std::string& input_frame_from_msg)
151  {
152  // Work on the image.
153  try
154  {
155  // Convert the image into something opencv can handle.
156  cv::Mat frame = cv_bridge::toCvShare(msg, sensor_msgs::image_encodings::BGR8)->image;
157 
158  // Messages
159  opencv_apps::CircleArrayStamped circles_msg;
160  circles_msg.header = msg->header;
161 
162  // Do the work
163  std::vector<cv::Rect> faces;
164  cv::Mat src_gray, edges;
165 
166  if (frame.channels() > 1)
167  {
168  cv::cvtColor(frame, src_gray, cv::COLOR_BGR2GRAY);
169  }
170  else
171  {
172  src_gray = frame;
173  }
174 
175  // create the main window, and attach the trackbars
176  if (debug_view_)
177  {
178  cv::namedWindow(window_name_, cv::WINDOW_AUTOSIZE);
179 
180  cv::createTrackbar("Canny Threshold", window_name_, &canny_threshold_int, max_canny_threshold_,
182  cv::createTrackbar("Accumulator Threshold", window_name_, &accumulator_threshold_int,
183  max_accumulator_threshold_, trackbarCallback);
184  cv::createTrackbar("Gaussian Blur Size", window_name_, &gaussian_blur_size_, 30, trackbarCallback);
185  cv::createTrackbar("Gaussian Sigam X", window_name_, &gaussian_sigma_x_int, 10, trackbarCallback);
186  cv::createTrackbar("Gaussian Sigma Y", window_name_, &gaussian_sigma_y_int, 10, trackbarCallback);
187  cv::createTrackbar("Min Distance between Circles", window_name_, &min_distance_between_circles_int, 100,
189  cv::createTrackbar("Dp", window_name_, &dp_int, 100, trackbarCallback);
190  cv::createTrackbar("Min Circle Radius", window_name_, &min_circle_radius_, 500, trackbarCallback);
191  cv::createTrackbar("Max Circle Radius", window_name_, &max_circle_radius_, 2000, trackbarCallback);
192 
193  if (need_config_update_)
194  {
195  config_.canny_threshold = canny_threshold_ = (double)canny_threshold_int;
196  config_.accumulator_threshold = accumulator_threshold_ = (double)accumulator_threshold_int;
197  config_.gaussian_blur_size = gaussian_blur_size_;
198  config_.gaussian_sigma_x = gaussian_sigma_x_ = (double)gaussian_sigma_x_int;
199  config_.gaussian_sigma_y = gaussian_sigma_y_ = (double)gaussian_sigma_y_int;
200  config_.min_distance_between_circles = min_distance_between_circles_ =
201  (double)min_distance_between_circles_int;
202  config_.dp = dp_ = (double)dp_int;
203  config_.min_circle_radius = min_circle_radius_;
204  config_.max_circle_radius = max_circle_radius_;
205  reconfigure_server_->updateConfig(config_);
206  need_config_update_ = false;
207  }
208  }
209 
210  // Reduce the noise so we avoid false circle detection
211  // gaussian_blur_size_ must be odd number
212  if (gaussian_blur_size_ % 2 != 1)
213  {
214  gaussian_blur_size_ = gaussian_blur_size_ + 1;
215  }
216  cv::GaussianBlur(src_gray, src_gray, cv::Size(gaussian_blur_size_, gaussian_blur_size_), gaussian_sigma_x_,
217  gaussian_sigma_y_);
218 
219  // those paramaters cannot be =0
220  // so we must check here
221  canny_threshold_ = std::max(canny_threshold_, 1.0);
222  accumulator_threshold_ = std::max(accumulator_threshold_, 1.0);
223 
224  if (debug_view_)
225  {
226  // https://github.com/Itseez/opencv/blob/2.4.8/modules/imgproc/src/hough.cpp#L817
227  cv::Canny(frame, edges, MAX(canny_threshold_ / 2, 1), canny_threshold_, 3);
228  }
229  if (min_distance_between_circles_ == 0)
230  { // set inital value
231  min_distance_between_circles_ = src_gray.rows / 8;
232  config_.min_distance_between_circles = min_distance_between_circles_;
233  reconfigure_server_->updateConfig(config_);
234  }
235  // runs the detection, and update the display
236  // will hold the results of the detection
237  std::vector<cv::Vec3f> circles;
238  // runs the actual detection
239  cv::HoughCircles(src_gray, circles, CV_HOUGH_GRADIENT, dp_, min_distance_between_circles_, canny_threshold_,
240  accumulator_threshold_, min_circle_radius_, max_circle_radius_);
241 
242  cv::Mat out_image;
243  if (frame.channels() == 1)
244  {
245  cv::cvtColor(frame, out_image, cv::COLOR_GRAY2BGR);
246  }
247  else
248  {
249  out_image = frame;
250  }
251 
252  // clone the colour, input image for displaying purposes
253  for (const cv::Vec3f& i : circles)
254  {
255  cv::Point center(cvRound(i[0]), cvRound(i[1]));
256  int radius = cvRound(i[2]);
257  // circle center
258  circle(out_image, center, 3, cv::Scalar(0, 255, 0), -1, 8, 0);
259  // circle outline
260  circle(out_image, center, radius, cv::Scalar(0, 0, 255), 3, 8, 0);
261 
262  opencv_apps::Circle circle_msg;
263  circle_msg.center.x = center.x;
264  circle_msg.center.y = center.y;
265  circle_msg.radius = radius;
266  circles_msg.circles.push_back(circle_msg);
267  }
268 
269  // shows the results
270  if (debug_view_ || debug_image_pub_.getNumSubscribers() > 0)
271  {
272  cv::Mat debug_image;
273  switch (debug_image_type_)
274  {
275  case 1:
276  debug_image = src_gray;
277  break;
278  case 2:
279  debug_image = edges;
280  break;
281  default:
282  debug_image = frame;
283  break;
284  }
285  if (debug_view_)
286  {
287  cv::imshow(window_name_, debug_image);
288  int c = cv::waitKey(1);
289  if (c == 's')
290  {
291  debug_image_type_ = (++debug_image_type_) % 3;
292  config_.debug_image_type = debug_image_type_;
293  reconfigure_server_->updateConfig(config_);
294  }
295  }
296  if (debug_image_pub_.getNumSubscribers() > 0)
297  {
298  sensor_msgs::Image::Ptr out_debug_img =
299  cv_bridge::CvImage(msg->header, msg->encoding, debug_image).toImageMsg();
300  debug_image_pub_.publish(out_debug_img);
301  }
302  }
303 
304  // Publish the image.
305  sensor_msgs::Image::Ptr out_img = cv_bridge::CvImage(msg->header, "bgr8", out_image).toImageMsg();
306  img_pub_.publish(out_img);
307  msg_pub_.publish(circles_msg);
308  }
309  catch (cv::Exception& e)
310  {
311  NODELET_ERROR("Image processing error: %s %s %s %i", e.err.c_str(), e.func.c_str(), e.file.c_str(), e.line);
312  }
313 
314  prev_stamp_ = msg->header.stamp;
315  }
316 
317  void subscribe() // NOLINT(modernize-use-override)
318  {
319  NODELET_DEBUG("Subscribing to image topic.");
320  if (config_.use_camera_info)
321  cam_sub_ = it_->subscribeCamera("image", queue_size_, &HoughCirclesNodelet::imageCallbackWithInfo, this);
322  else
323  img_sub_ = it_->subscribe("image", queue_size_, &HoughCirclesNodelet::imageCallback, this);
324  }
325 
326  void unsubscribe() // NOLINT(modernize-use-override)
327  {
328  NODELET_DEBUG("Unsubscribing from image topic.");
329  img_sub_.shutdown();
330  cam_sub_.shutdown();
331  }
332 
333 public:
334  virtual void onInit() // NOLINT(modernize-use-override)
335  {
336  Nodelet::onInit();
338 
339  debug_image_type_ = 0;
340  pnh_->param("queue_size", queue_size_, 3);
341  pnh_->param("debug_view", debug_view_, false);
342  if (debug_view_)
343  {
345  }
346  prev_stamp_ = ros::Time(0, 0);
347 
348  window_name_ = "Hough Circle Detection Demo";
349  canny_threshold_initial_value_ = 200;
350  accumulator_threshold_initial_value_ = 50;
351  max_accumulator_threshold_ = 200;
352  max_canny_threshold_ = 255;
353  min_distance_between_circles_ = 0;
354 
355  // declare and initialize both parameters that are subjects to change
356  canny_threshold_ = canny_threshold_initial_value_;
357  accumulator_threshold_ = accumulator_threshold_initial_value_;
358 
359  reconfigure_server_ = boost::make_shared<dynamic_reconfigure::Server<Config> >(*pnh_);
360  dynamic_reconfigure::Server<Config>::CallbackType f =
361  boost::bind(&HoughCirclesNodelet::reconfigureCallback, this, _1, _2);
362  reconfigure_server_->setCallback(f);
363 
364  img_pub_ = advertiseImage(*pnh_, "image", 1);
365  msg_pub_ = advertise<opencv_apps::CircleArrayStamped>(*pnh_, "circles", 1);
366 
367  debug_image_type_ = 0;
368  debug_image_pub_ = advertiseImage(*pnh_, "debug_image", 1);
369 
371  }
372 };
374 } // namespace opencv_apps
375 
376 namespace hough_circles
377 {
379 {
380 public:
381  virtual void onInit() // NOLINT(modernize-use-override)
382  {
383  ROS_WARN("DeprecationWarning: Nodelet hough_circles/hough_circles is deprecated, "
384  "and renamed to opencv_apps/hough_circles.");
386  }
387 };
388 } // namespace hough_circles
389 
CvImageConstPtr toCvShare(const sensor_msgs::ImageConstPtr &source, const std::string &encoding=std::string())
image_transport::Publisher debug_image_pub_
void subscribe()
This method is called when publisher is subscribed by other nodes. Set up subscribers in this method...
#define NODELET_ERROR(...)
void publish(const boost::shared_ptr< M > &message) const
void reconfigureCallback(Config &new_config, uint32_t level)
void imageCallbackWithInfo(const sensor_msgs::ImageConstPtr &msg, const sensor_msgs::CameraInfoConstPtr &cam_info)
f
Nodelet to automatically subscribe/unsubscribe topics according to subscription of advertised topics...
Definition: nodelet.h:70
image_transport::Publisher img_pub_
Demo code to calculate moments.
Definition: nodelet.h:48
boost::shared_ptr< ros::NodeHandle > pnh_
Shared pointer to private nodehandle.
Definition: nodelet.h:250
void doWork(const sensor_msgs::ImageConstPtr &msg, const std::string &input_frame_from_msg)
#define ROS_WARN(...)
uint32_t getNumSubscribers() const
dynamic_reconfigure::Server< Config > ReconfigureServer
void unsubscribe()
This method is called when publisher is unsubscribed by other nodes. Shut down subscribers in this me...
bool always_subscribe_
A flag to disable watching mechanism and always subscribe input topics. It can be specified via ~alwa...
Definition: nodelet.h:273
image_transport::Subscriber img_sub_
virtual void onInitPostProcess()
Post processing of initialization of nodelet. You need to call this method in order to use always_sub...
Definition: nodelet.cpp:57
virtual void onInit()
Initialize nodehandles nh_ and pnh_. Subclass should call this method in its onInit method...
Definition: nodelet.cpp:40
virtual void onInit()
Initialize nodehandles nh_ and pnh_. Subclass should call this method in its onInit method...
void publish(const sensor_msgs::Image &message) const
image_transport::CameraSubscriber cam_sub_
void imageCallback(const sensor_msgs::ImageConstPtr &msg)
boost::shared_ptr< ros::NodeHandle > nh_
Shared pointer to nodehandle.
Definition: nodelet.h:245
PLUGINLIB_EXPORT_CLASS(opencv_apps::HoughCirclesNodelet, nodelet::Nodelet)
boost::shared_ptr< ReconfigureServer > reconfigure_server_
image_transport::Publisher advertiseImage(ros::NodeHandle &nh, const std::string &topic, int queue_size)
Advertise an image topic and watch the publisher. Publishers which are created by this method...
Definition: nodelet.h:180
static void trackbarCallback(int value, void *userdata)
opencv_apps::HoughCirclesConfig Config
const std::string & frameWithDefault(const std::string &frame, const std::string &image_frame)
#define NODELET_DEBUG(...)
sensor_msgs::ImagePtr toImageMsg() const
virtual void onInit()
Initialize nodehandles nh_ and pnh_. Subclass should call this method in its onInit method...
boost::shared_ptr< image_transport::ImageTransport > it_


opencv_apps
Author(s): Kei Okada
autogenerated on Wed Apr 24 2019 03:00:17