View source for Track It using CamShift

<pre>

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

</pre>