View source for Cam shift tracker.cpp v1

<pre>

//*************************************************************************
//  CamShift Tracker for ROS
// 
//  Copyright (C) 2012-15, Ed C. Epp
//  http://zdome.net/wiki/index.php/ROS
//
//  Based on camshiftdemo.cpp found in:
//      github.com/Itseez/opencv/tree/2.4/samples/cpp//
//  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 <opencv2/video/tracking.hpp>
#include <image_transport/image_transport.h>
#include <cv_bridge/cv_bridge.h>
#include <sensor_msgs/image_encodings.h>
#include <opencv2/highgui/highgui.hpp>
#include <geometry_msgs/Point.h>

//****************************************************************************
//          on_mouse function
//****************************************************************************

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

//****************************************************************************
//          help function
//****************************************************************************

static void help()
{
    std::cout << "\nThis is a demo that shows mean-shift based tracking\n"
            "You select a color objects such as your face and it tracks it.\n"
            "This reads from video camera (0 by default, or the camera number "
            "the user enters\n"
            "Usage: \n"
            "   ./camshiftdemo [camera number]\n";

    std::cout << "\n\nHot keys: \n"
            "\tESC - quit the program\n"
            "\tc - stop the tracking\n"
            "\tb - switch to/from backprojection view\n"
            "\th - show/hide object histogram\n"
            "To initialize tracking, select the object with mouse\n";
}

//****************************************************************************
//          CamShaftTracker class
//****************************************************************************

class CamShaftTracker
{

protected:

    // ROS related
    ros::NodeHandle                 my_handler;
    image_transport::ImageTransport my_image_transport;
    image_transport::Subscriber     my_image_subscriber;
    cv_bridge::CvImagePtr           my_webcam_img;
    ros::Publisher                  my_tracker_publisher;

    // Image related
    cv::Mat         my_image;
    cv::Mat         my_hsv;
    cv::Mat         my_hue;
    cv::Mat         my_mask;
    cv::Mat         my_histogram;
    cv::Mat         my_histogram_img;
    cv::Mat         my_backproject;

    // Image range
    int             my_vmin;
    int             my_vmax;
    int             my_smin;
  
    // Histogram related
    int             my_hist_dims;
    float*          my_hist_ranges_array;

    // CamShift related
    cv::Rect        my_track_window;  // must be global or program faults
    CvBox2D         my_track_box;
    CvConnectedComp my_track_comp;

    // Execution state
    bool            my_back_project_mode;
    bool            my_show_histogram;

public:

    // ---------- CamShaftTracker ------------------------------------------

    CamShaftTracker (ros::NodeHandle & nh):my_handler (nh), 
                                           my_image_transport (my_handler)
    {
        // Image initialization
        my_image         = cv::Mat::zeros(200, 320, CV_8UC3);
        my_hsv           = cv::Mat::zeros(200, 320, CV_8UC3);
        my_hue           = cv::Mat::zeros(200, 320, CV_8UC3);
        my_mask          = cv::Mat::zeros(200, 320, CV_8UC3);
        my_histogram     = cv::Mat::zeros(200, 320, CV_8UC3);
        my_histogram_img = cv::Mat::zeros(200, 320, CV_8UC3);

        // Listen for raw image message topic messages 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 );
        cvMoveWindow("CamShiftTracking", 100, 100);
        cv::namedWindow ("Histogram", 1);
        cvMoveWindow("Histogram", 900, 100);

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

	// Initialize execution state
        my_back_project_mode = false;
	my_show_histogram = true;

        // Create publisher
        my_tracker_publisher = my_handler.advertise<geometry_msgs::Point>
                       ("tracker", 1000);
    }
 
    // ---------- imageCallback ----------------------------------------------

    void imageCallback (const sensor_msgs::ImageConstPtr & msg_ptr)
    {
        const float* hist_ranges = my_hist_ranges_array;

        // Copy in the webcam image
        try
        {
            my_webcam_img = 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 the image to cv::Mat
        my_image = cv::Mat (my_webcam_img->image).clone ();

        // Convert Input image from BGR to HSV 
        cv::cvtColor (my_image, my_hsv, cv::COLOR_BGR2HSV);

        my_backproject  = cv::Mat (my_webcam_img->image).clone ();

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

 	    inRange( my_hsv, cv::Scalar(0,my_smin,MIN(vmin,vmax)),
		     cv::Scalar(180,256,MAX(vmin,vmax)), my_mask );
	    int ch[] = {0, 0};
	    my_hue.create(my_hsv.size(), my_hsv.depth());
	    mixChannels(&my_hsv, 1, &my_hue, 1, ch, 1);

            // only do this when the tracking area changes
            if (our_new_object_selected)
            {
 		// Set up the histrogram matrix
		cv::Mat roi(my_hue, our_selection);
		cv::Mat mask_roi(my_mask, our_selection);
		cv::calcHist (&roi, 1, 0, mask_roi, my_histogram, 1, 
                              &my_hist_dims, &hist_ranges);
		cv::normalize(my_histogram, my_histogram, 0, 255, CV_MINMAX);

                our_new_object_selected = 0;
                my_track_window = our_selection;

		// Set up the histrogram image
		my_histogram_img = cv::Scalar::all(0);
		bin_w = my_histogram_img.cols / my_hist_dims;
		cv::Mat buf(1, my_hist_dims, CV_8UC3);
		for (int i = 0; i < my_hist_dims; i++)
		    buf.at<cv::Vec3b>(i) = 
                        cv::Vec3b(cv::saturate_cast<uchar>
                                 (i*180./my_hist_dims), 255, 255);
		cvtColor(buf, buf, CV_HSV2BGR);

		// Draw the historgram bars in the histogram image
                for( int i = 0; i < my_hist_dims; i++ )
                {
		    int val = cv::saturate_cast<int>
                        (my_histogram.at<float>(i) *my_histogram_img.rows/255);
                    rectangle( my_histogram_img, 
                           cv::Point(i*bin_w,my_histogram_img.rows),
                           cv::Point((i+1)*bin_w,my_histogram_img.rows - val),
                           cv::Scalar(buf.at<cv::Vec3b>(i)), -1, 8 );
                }
            }

            // do the CAM Shift computations
	    cv::calcBackProject( &my_hue, 1, 0, my_histogram, 
                                   my_backproject, &hist_ranges);
            my_backproject &= my_mask;

	    cv::RotatedRect my_track_box = cv::CamShift(
                              my_backproject, my_track_window,
			      cv::TermCriteria( 
				   CV_TERMCRIT_EPS | CV_TERMCRIT_ITER, 
                                   10, 1 ));
            if( my_track_window.area() <= 1 )
            {
                int cols = my_backproject.cols, 
                    rows = my_backproject.rows, 
                    r    = (MIN(cols, rows) + 5)/6;
                my_track_window = cv::Rect(my_track_window.x - r, 
                                           my_track_window.y - r,
                                           my_track_window.x + r, 
                                           my_track_window.y + r) &
		              cv::Rect(0, 0, cols, rows);
            }

            if(my_back_project_mode )
	        cvtColor( my_backproject, my_image, cv::COLOR_GRAY2BGR );

            ellipse( my_image, my_track_box, cv::Scalar(0,0,255), 3, CV_AA );
 
            // 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);

	    cv::Mat roi(my_image, cvRect(x, y, width, height));
            bitwise_not(roi, roi);
            //ROS_INFO("Our Dragging: x: %d  y: %d  width: %d  height: %d", 
            //         x, y, width, height);
        }

        // Display Input image and histogram
	cv::imshow ("CamShiftTracking", my_image);
	if (my_show_histogram)
	{
	    cv::imshow ("Histogram", my_histogram_img);
	}

	// Handle keyboard commands
        char c = (char)cv::waitKey(10);
        if( c == 27 )
	    exit(0);
        switch(c)
        {
        case 'b':
            my_back_project_mode =! my_back_project_mode;
            break;
        case 'c':
            our_object_selected = 0;
            my_histogram_img = cv::Scalar::all(0);
            break;
        case 'h':
            my_show_histogram = !my_show_histogram;
            if( !my_show_histogram )
	        cv::destroyWindow( "Histogram" );
            else
	    {
	        cv::namedWindow( "Histogram", 1 );
                cvMoveWindow("Histogram", 900, 100);
	    }
            break;
        default:
            ;
        }
     }

};

//****************************************************************************
//          main
//****************************************************************************

int main (int argc, char **argv)
{
    help();

    // Initialize ROS Node
    ros::init (argc, argv, "CAM_Shift_Tracking");

    // Start node and create a Node Handle
    ros::NodeHandle nHandle;

    // Instantiate CamShaftTracker Object
    CamShaftTracker camShaftTracker (nHandle);

    // Spin ...
    ros::spin ();
    // ... until done
    return 0;
}

</pre>