Track It using CamShift
From wikidb
//*************************************************************************
// CamShift Tracker for ROS
//
// Copyright (C) 2012-13, Ed C. Epp
// http://zdome.net/wiki/index.php/ROS
//
// Based on camshiftdemo.c found in:
// /usr/share/doc/opencv-doc/examples/c
//
// 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 "sensor_msgs/Image.h"
#include "image_transport/image_transport.h"
#include "cv_bridge/CvBridge.h"
#include <opencv/cv.h>
#include <opencv/highgui.h>
#include <math.h>
#include "geometry_msgs/Point.h"
//****************************************************************************
// hsv2rgb
//****************************************************************************
CvScalar hsv2rgb( float hue )
{
int rgb[3], p, sector;
static const int sector_data[][3]=
{{0,2,1}, {1,2,0}, {1,0,2}, {2,0,1}, {2,1,0}, {0,1,2}};
hue *= 0.033333333333333333333333333333333f;
sector = cvFloor(hue);
p = cvRound(255*(hue - sector));
p ^= sector & 1 ? 255 : 0;
rgb[sector_data[sector][0]] = 255;
rgb[sector_data[sector][1]] = 0;
rgb[sector_data[sector][2]] = p;
return cvScalar(rgb[2], rgb[1], rgb[0],0);
}
//****************************************************************************
// on_mouse
//****************************************************************************
CvRect our_selection;
int our_object_selected = 0;
int our_new_object_selected = 0;
int our_dragging = 0;
CvPoint our_drag_location;
void on_mouse (int event, int x, int y, int flags, void* param)
{
our_dragging = 0;
switch (event)
{
case CV_EVENT_LBUTTONDOWN:
our_selection = cvRect(x,y,0,0);
break;
case CV_EVENT_MOUSEMOVE:
if (flags == CV_EVENT_FLAG_LBUTTON)
{
our_dragging = 1;
our_drag_location = cvPoint(x,y);
}
break;
case CV_EVENT_LBUTTONUP:
our_selection.width = abs (x - our_selection.x);
our_selection.height = abs (y - our_selection.y);
our_selection.x = MIN (x, our_selection.x);
our_selection.y = MIN (y, our_selection.y);
our_object_selected = our_new_object_selected = 1;
//ROS_INFO("Mouse up: x: %d y: %d width: %d height: %d",
// our_selection.x, our_selection.y,
// our_selection.width, our_selection.height);
break;
}
}
//****************************************************************************
// CamShaftTracker
//****************************************************************************
class CamShaftTracker
{
protected:
// ROS related
ros::NodeHandle my_handler;
image_transport::ImageTransport my_image_transport;
image_transport::Subscriber my_image_subscriber;
sensor_msgs::CvBridge my_bridge;
ros::Publisher my_tracker_publisher;
// Image related
IplImage *my_webcam_input;
IplImage *my_image;
IplImage *my_hsv;
IplImage *my_hue;
IplImage *my_mask;
IplImage *my_histogram_img;
IplImage *my_backproject;
CvHistogram *my_histogram;
// Image range
int my_vmin;
int my_vmax;
int my_smin;
// Histogram related
int my_hist_dims;
float* my_hist_ranges_array;
float* my_hist_ranges;
// CamShift related
CvRect my_track_window;
CvBox2D my_track_box;
CvConnectedComp my_track_comp;
public:
// ---------- CamShaftTracker ------------------------------------------
CamShaftTracker (ros::NodeHandle & nh):my_handler (nh),
my_image_transport (my_handler)
{
// Listen for image messages on a topic and setup callback
my_image_subscriber = my_image_transport.subscribe (
"/usb_cam/image_raw", 1,
&CamShaftTracker::imageCallback, this);
// Open HighGUI Windows
cv::namedWindow ("CamShiftTracking", 1);
cvSetMouseCallback( "CamShiftTracking", on_mouse, 0 );
cv::namedWindow ("Histogram", 1);
cvMoveWindow("Histogram", 0, 400);
// Initialize images
my_webcam_input = NULL;
my_image = NULL;
my_hsv = NULL;
my_hue = NULL;
my_mask = NULL;
my_histogram_img = NULL;
my_backproject = NULL;
my_histogram = NULL;
// Initialize ranges
my_vmin = 10;
my_vmax = 256;
my_smin = 30;
// Initialize histogram
my_hist_dims = 16;
my_hist_ranges_array = new float[2];
my_hist_ranges_array[0] = 0;
my_hist_ranges_array[1] = 180;
my_hist_ranges = my_hist_ranges_array;
// Create publisher TODO
my_tracker_publisher = my_handler.advertise<geometry_msgs::Point>
("tracker", 1000);
}
// ---------- imageCallback ----------------------------------------------
void imageCallback (const sensor_msgs::ImageConstPtr & msg_ptr)
{
// Convert ROS Input Image Message to IplImage
try
{
my_webcam_input = my_bridge.imgMsgToCv (msg_ptr, "bgr8");
}
catch (sensor_msgs::CvBridgeException error)
{
ROS_ERROR ("CvBridge Input Error");
}
// Create all the images on the first call
if (!my_image)
{
my_image = cvCreateImage (cvGetSize(my_webcam_input), 8, 3);
my_image->origin = my_webcam_input->origin;
my_hsv = cvCreateImage (cvGetSize(my_webcam_input), 8, 3);
my_hue = cvCreateImage (cvGetSize(my_webcam_input), 8, 1);
my_mask = cvCreateImage (cvGetSize(my_webcam_input), 8, 1);
my_backproject = cvCreateImage (cvGetSize(my_webcam_input), 8, 1);
my_histogram = cvCreateHist (1, &my_hist_dims, CV_HIST_ARRAY,
&my_hist_ranges, 1);
my_histogram_img = cvCreateImage (cvSize(320,200), 8, 3);
cvZero( my_histogram_img );
//ROS_INFO("-- Images created --\n");
}
cvCopy( my_webcam_input, my_image, 0);
cvCvtColor( my_image, my_hsv, CV_BGR2HSV);
// only do this is we have selected an tracking area
if (our_object_selected)
{
int bin_w;
int vmin = my_vmin;
int vmax = my_vmax;
geometry_msgs::Point msg;
cvInRangeS( my_hsv, cvScalar(0,my_smin,MIN(vmin,vmax),0),
cvScalar(180,256,MAX(vmin,vmax),0), my_mask );
cvSplit (my_hsv, my_hue, 0, 0, 0);
// only do this when the tracking area changes
if (our_new_object_selected)
{
float max_val = 0.f;
our_new_object_selected = 0;
cvSetImageROI (my_hue, our_selection);
cvSetImageROI (my_mask, our_selection);
cvCalcHist (&my_hue, my_histogram, 0, my_mask);
cvGetMinMaxHistValue( my_histogram, 0, &max_val, 0, 0 );
cvConvertScale( my_histogram->bins, my_histogram->bins,
max_val ? 255. / max_val : 0., 0 );
cvResetImageROI (my_hue); // without this there is an assertion
cvResetImageROI (my_mask);
cvZero( my_histogram_img );
bin_w = my_histogram_img->width / my_hist_dims;
for( int i = 0; i < my_hist_dims; i++ )
{
int val = cvRound(cvGetReal1D(my_histogram->bins,i)
*my_histogram_img->height/255 );
CvScalar color = hsv2rgb(i*180.f/my_hist_dims);
cvRectangle( my_histogram_img,
cvPoint(i*bin_w,my_histogram_img->height),
cvPoint((i+1)*
bin_w,my_histogram_img->height - val),
color, -1, 8, 0 );
}
my_track_window = our_selection;
}
// do the CAM Shift computations
cvCalcBackProject( &my_hue, my_backproject, my_histogram );
cvAnd( my_backproject, my_mask, my_backproject, 0 );
cvCamShift( my_backproject, my_track_window,
cvTermCriteria( CV_TERMCRIT_EPS | CV_TERMCRIT_ITER,
10, 1 ),
&my_track_comp, &my_track_box );
my_track_window = my_track_comp.rect;
cvEllipseBox(my_image, my_track_box, CV_RGB(255,0,0), 3, CV_AA, 0);
// publish ellipse location
msg.x = my_track_box.center.x;
msg.y = my_track_box.center.y;
msg.z = my_track_box.size.width;
//ROS_INFO ("--- x: %f y: %f width: %f ---\n",
// msg.x, msg.y, msg.z);
my_tracker_publisher.publish(msg);
}
// Display a negative region when mouse dragging
if( our_dragging )
{
int x = MIN (our_drag_location.x, our_selection.x);
int y = MIN (our_drag_location.y, our_selection.y);
int width = abs (our_drag_location.x - our_selection.x);
int height = abs (our_drag_location.y - our_selection.y);
cvSetImageROI( my_image, cvRect(x, y, width, height) );
cvXorS( my_image, cvScalarAll(255), my_image, 0 );
cvResetImageROI( my_image );
}
// Display Input image and histogram
cvShowImage ("CamShiftTracking", my_image);
cvShowImage ("Histogram", my_histogram_img);
// Needed to keep the HighGUI window open
cv::waitKey (3);
}
};
//****************************************************************************
// main
//****************************************************************************
int main (int argc, char **argv)
{
// Initialize ROS Node
ros::init (argc, argv, "CAM Shift Tracking");
// Start node and create a Node Handle
ros::NodeHandle nHandle;
// Instaniate CamShaftTracker Object
CamShaftTracker camShaftTracker (nHandle);
// Spin ...
ros::spin ();
// ... until done
return 0;
}
