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;
}
