hough_circles.cpp
Go to the documentation of this file.
00001 // -*- mode: C++ -*-
00002 /*********************************************************************
00003  * Software License Agreement (BSD License)
00004  *
00005  *  Copyright (c) 2014, JSK Lab
00006  *  All rights reserved.
00007  *
00008  *  Redistribution and use in source and binary forms, with or without
00009  *  modification, are permitted provided that the following conditions
00010  *  are met:
00011  *
00012  *   * Redistributions of source code must retain the above copyright
00013  *     notice, this list of conditions and the following disclaimer.
00014  *   * Redistributions in binary form must reproduce the above
00015  *     copyright notice, this list of conditions and the following
00016  *     disclaimer in the documentation and/o2r other materials provided
00017  *     with the distribution.
00018  *   * Neither the name of the Willow Garage nor the names of its
00019  *     contributors may be used to endorse or promote products derived
00020  *     from this software without specific prior written permission.
00021  *
00022  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00023  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00024  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00025  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00026  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00027  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00028  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00029  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00030  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00031  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00032  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00033  *  POSSIBILITY OF SUCH DAMAGE.
00034  *********************************************************************/
00035 
00036 #include <ros/ros.h>
00037 #include <nodelet/nodelet.h>
00038 #include <image_transport/image_transport.h>
00039 #include "opencv2/highgui/highgui.hpp"
00040 #include "opencv2/imgproc/imgproc.hpp"
00041 #include <jsk_perception/Circle2DArray.h>
00042 #include <cv_bridge/cv_bridge.h>
00043 #include <sensor_msgs/image_encodings.h>
00044 #include <image_view2/ImageMarker2.h>
00045 #include <dynamic_reconfigure/server.h>
00046 #include "jsk_perception/HoughCirclesConfig.h"
00047 
00048 namespace jsk_perception
00049 {
00050   class HoughCircleDetector: public nodelet::Nodelet
00051   {
00052   public:
00053     typedef jsk_perception::HoughCirclesConfig Config;
00054     Config config_;
00055     boost::shared_ptr<dynamic_reconfigure::Server<Config> > srv_;
00056     ros::NodeHandle nh_, pnh_;
00057     boost::shared_ptr<image_transport::ImageTransport> it_;
00058     ros::Publisher circle_pub_, marker_pub_;
00059     image_transport::Subscriber image_sub_;
00060     int subscriber_count_;
00061     int previous_marker_num_;
00062 
00063     int gaussian_blur_size_;
00064     double gaussian_sigma_x_;
00065     double gaussian_sigma_y_;
00066     double edge_threshold_;
00067     double voting_threshold_;
00068     double dp_;
00069     int min_circle_radius_;
00070     int max_circle_radius_;
00071 
00072     boost::mutex mutex_;
00073     
00074     void imageCallback(const sensor_msgs::Image::ConstPtr& image)
00075     {
00076       boost::mutex::scoped_lock lock(mutex_);
00077       cv_bridge::CvImagePtr cv_ptr;
00078       cv_ptr = cv_bridge::toCvCopy(image, sensor_msgs::image_encodings::BGR8);
00079       cv::Mat bgr_image = cv_ptr->image;
00080       cv::Mat gray_image;
00081       cv::cvtColor(bgr_image, gray_image, CV_BGR2GRAY);
00082       cv::GaussianBlur(gray_image, gray_image,
00083                        cv::Size(gaussian_blur_size_, gaussian_blur_size_),
00084                        gaussian_sigma_x_, gaussian_sigma_y_);
00085       std::vector<cv::Vec3f> circles;
00086       cv::HoughCircles(gray_image, circles, CV_HOUGH_GRADIENT,
00087                        dp_, gray_image.rows / 4, edge_threshold_, voting_threshold_,
00088                        min_circle_radius_, max_circle_radius_);
00089       jsk_perception::Circle2DArray circles_msg;
00090       circles_msg.header = image->header;
00091       for (size_t i = 0; i < circles.size(); i++) {
00092         jsk_perception::Circle2D circle;
00093         circle.header = image->header;
00094         circle.radius = circles[i][2];
00095         circle.x = circles[i][0];
00096         circle.y = circles[i][1];
00097         circles_msg.circles.push_back(circle);
00098         // marker
00099         image_view2::ImageMarker2 marker;
00100         marker.header = image->header;
00101         marker.type = image_view2::ImageMarker2::CIRCLE;
00102         marker.id = i;
00103         marker.action = image_view2::ImageMarker2::ADD;
00104         marker.position.x = circles[i][0];
00105         marker.position.y = circles[i][1];
00106         marker.scale = circles[i][2];
00107         marker_pub_.publish(marker);
00108       }
00109       circle_pub_.publish(circles_msg);
00110       if (circles.size() < previous_marker_num_) {
00111         for (size_t i = circles.size(); i < previous_marker_num_; i++) {
00112           image_view2::ImageMarker2 marker;
00113           marker.header = image->header;
00114           marker.id = i;
00115           marker.action = image_view2::ImageMarker2::REMOVE;
00116           marker_pub_.publish(marker);
00117         }
00118       }
00119       previous_marker_num_ = circles.size();
00120     }
00121 
00122     void configCallback(Config &new_config, uint32_t level)
00123     {
00124       boost::mutex::scoped_lock lock(mutex_);
00125       gaussian_blur_size_ = new_config.gaussian_blur_size;
00126       gaussian_sigma_x_ = new_config.gaussian_sigma_x;
00127       gaussian_sigma_y_ = new_config.gaussian_sigma_y;
00128       edge_threshold_ = new_config.edge_threshold;
00129       voting_threshold_ = new_config.voting_threshold;
00130       dp_ = new_config.dp;
00131       min_circle_radius_ = new_config.min_circle_radius;
00132       max_circle_radius_ = new_config.max_circle_radius;
00133     }
00134     
00135     virtual void onInit()
00136     {
00137       subscriber_count_ = 0;
00138       previous_marker_num_ = 0;
00139       nh_ = ros::NodeHandle(getNodeHandle(), "image");
00140       pnh_ = getPrivateNodeHandle();
00141       srv_ = boost::make_shared <dynamic_reconfigure::Server<Config> > (pnh_);
00142       dynamic_reconfigure::Server<Config>::CallbackType f =
00143         boost::bind (&HoughCircleDetector::configCallback, this, _1, _2);
00144       srv_->setCallback (f);
00145       it_.reset(new image_transport::ImageTransport(nh_));
00146       circle_pub_ = pnh_.advertise<jsk_perception::Circle2DArray>("output", 1);
00147       marker_pub_ = pnh_.advertise<image_view2::ImageMarker2>("image_marker", 1);
00148       image_sub_ = it_->subscribe("", 1, &HoughCircleDetector::imageCallback, this);
00149     }
00150   };
00151 }
00152 
00153 #include <pluginlib/class_list_macros.h>
00154 typedef jsk_perception::HoughCircleDetector HoughCircleDetector;
00155 PLUGINLIB_DECLARE_CLASS (jsk_perception, HoughCircleDetector, HoughCircleDetector, nodelet::Nodelet);


jsk_perception
Author(s): Manabu Saito
autogenerated on Mon Oct 6 2014 01:16:59