Hough tracker.cpp v1
From wikidb
/* * hough_tracker * Track an object of a given color. * * Copyright (C) 2012-2015, Ed C. Epp * http://zdome.net/wiki/index.php/ROS * * Based I Heart Robotics * iheart-ros-pkg/ihr_demos/ihr_opencv/src/ihr_demo_hough.cpp * I Heart Robotics <iheartrobotics@gmail.com> * http://www.iheartrobotics.com * * August 2015 Ported to ROS Jade and OpenCV 2 * * This program is free software: you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by * the Free Software Foundation, either version 3 of the License, or * (at your option) any later version. * * This program is distributed in the hope that it will be useful, * but WITHOUT ANY WARRANTY; without even the implied warranty of * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * GNU General Public License for more details. * * You should have received a copy of the GNU General Public License * along with this program. If not, see <http://www.gnu.org/licenses/>. */ #include <ros/ros.h> #include <image_transport/image_transport.h> #include <cv_bridge/cv_bridge.h> #include <sensor_msgs/image_encodings.h> #include <opencv2/imgproc/imgproc.hpp> #include <opencv2/highgui/highgui.hpp> #include <geometry_msgs/Point.h> // ----------------------------------------------------------------------------- // ---------- TrackIt class --------------------------------------------------- // ----------------------------------------------------------------------------- class TrackIt { protected: ros::NodeHandle nh_; image_transport::ImageTransport it_; image_transport::Subscriber image_sub_; cv_bridge::CvImagePtr bridge_; cv::Mat img_in_; cv::Mat img_hsv_; cv::Mat img_hue_; cv::Mat img_sat_; cv::Mat img_bin_; cv::Mat img_out_; ros::Publisher tracker_pub_; public: // ---------- TrackIt constructor ------------------------------------------- TrackIt (ros::NodeHandle & nh):nh_ (nh), it_ (nh_) { // Listen for image messages on a topic and setup callback image_sub_ = it_.subscribe ("/usb_cam/image_raw", 1, &TrackIt::imageCallback, this); // Open HighGUI Window cv::namedWindow ("hsv modified input", 1); cv::namedWindow ("detected output", 1); // Create publisher tracker_pub_ = nh_.advertise<geometry_msgs::Point> ("tracker", 1000); } // ---------- imageCallback -------------------------------------------------- void imageCallback (const sensor_msgs::ImageConstPtr & msg_ptr) { ROS_DEBUG ("."); // Convert ROS Imput Image Message to OpenCV Mat try { bridge_ = cv_bridge::toCvCopy(msg_ptr, sensor_msgs::image_encodings::BGR8); } catch (cv_bridge::Exception error) { ROS_ERROR ("cv_bridge Input Error: %s", error.what()); } // Convert IplImage to cv::Mat img_in_ = cv::Mat (bridge_->image).clone (); // output = input img_out_ = img_in_.clone (); // Convert Input image from BGR to HSV cv::cvtColor (img_in_, img_hsv_, CV_BGR2HSV); // Zero Matrices img_hue_ = cv::Mat::zeros(img_hsv_.rows, img_hsv_.cols, CV_8U); img_sat_ = cv::Mat::zeros(img_hsv_.rows, img_hsv_.cols, CV_8U); img_bin_ = cv::Mat::zeros(img_hsv_.rows, img_hsv_.cols, CV_8U); // HSV Channel 0 -> img_hue_ & HSV Channel 1 -> img_sat_ int from_to[] = { 0,0, 1,1}; cv::Mat img_split[] = { img_hue_, img_sat_}; cv::mixChannels(&img_hsv_, 3, img_split, 2, from_to, 2); // The output pixel is white if the input pixel // hue is orange and saturation is reasonable for(int i = 0; i < img_out_.rows; i++) { for(int j = 0; j < img_out_.cols; j++) { // Roughly a Baby or Sky Blue if(img_hue_.at<uchar>(i,j) > 95 && img_hue_.at<uchar>(i,j) < 105 && img_sat_.at<uchar>(i,j) > 100) { img_bin_.at<uchar>(i,j) = 255; } else { img_bin_.at<uchar>(i,j) = 0; } } } // strel_size is the size of the structuring element cv::Size strel_size; strel_size.width = 3; strel_size.height = 3; // Create an elliptical structuring element cv::Mat strel = cv::getStructuringElement(cv::MORPH_ELLIPSE,strel_size); // Apply an opening morphological operation using the structing element // to the image twice cv::morphologyEx(img_bin_, img_bin_, cv::MORPH_OPEN, strel, cv::Point(-1, -1), 2); // Convert White on Black to Black on White by inverting the image cv::bitwise_not(img_bin_, img_bin_); // Blur the image to improve detection cv::GaussianBlur(img_bin_, img_bin_, cv::Size(7, 7), 2, 2 ); // See http://opencv.willowgarage.com/documentation/cpp/ // feature_detection.html?highlight=hough#HoughCircles // The vector circles will hold the position and radius of the detected // circles cv::vector<cv::Vec3f> circles; // Detect circles That have a radius between 20 and 400 that are a minimum // of 70 pixels apart cv::HoughCircles(img_bin_, circles, CV_HOUGH_GRADIENT, 1, 70, 140, 15, 20, 400 ); // publish the location of all circles for( size_t i = 0; i < circles.size(); i++ ) { geometry_msgs::Point msg; // round the floats to an int cv::Point center(cvRound(circles[i][0]), cvRound(circles[i][1])); int radius = cvRound(circles[i][2]); // TODO OPTOMIZE // draw the circle center cv::circle( img_out_, center, 3, cv::Scalar(0,255,0), -1, 8, 0 ); // draw the circle outline cv::circle( img_out_, center, radius+1, cv::Scalar(0,0,255), 2, 8, 0 ); // Debugging Output ROS_INFO("x: %d y: %d r: %d",center.x,center.y, radius); // publish circle location msg.x = center.x; msg.y = center.y; msg.z = radius; tracker_pub_.publish(msg); } // Display morphed image // TODO OPTOMIZE //cv::imshow ("detected output", bridge_->image); // early test cv::imshow ("hsv modified input", img_bin_); cv::imshow ("detected output", img_out_); // Needed to keep the HighGUI window open cv::waitKey (3); } }; // ----------------------------------------------------------------------------- // ---------- main ------------------------------------------------------------- // ----------------------------------------------------------------------------- int main (int argc, char **argv) { // Initialize ROS Node ROS_INFO ("Hough Tracker Init"); ros::init (argc, argv, "ihr_demo1"); // Start node and create a Node Handle ros::NodeHandle nh; // Instaniate TrackIt Object TrackIt d (nh); // Spin ... ros::spin (); // ... until done return 0; }