CamShift - ICK4

#ifdef _CH_
#pragma package <opencv>
#endif

#define CV_NO_BACKWARD_COMPATIBILITY

#ifndef _EiC
#include "cv.h"
#include "highgui.h"
#include <stdio.h>
#include <ctype.h>
#endif

IplImage* image = 0, * hsv = 0, * hue = 0, * mask = 0, * backproject = 0, * histimg = 0, * tracker;
CvHistogram* hist = 0;

int backproject_mode = 0;
int select_object = 0;
int track_object = 0;
int show_hist = 1;
CvPoint origin;
CvRect selection;
CvRect track_window;
CvBox2D track_box;
CvConnectedComp track_comp;
int hdims = 16;
float hranges_arr[] = { 0,180 };
float* hranges = hranges_arr;
int vmin = 10, vmax = 256, smin = 30;

void on_mouse(int event, int x, int y, int flags, void* param)
{
    if (!image)
        return;

    if (image->origin)
        y = image->height - y;

    if (select_object)
    {
        selection.x = MIN(x, origin.x);
        selection.y = MIN(y, origin.y);
        selection.width = selection.x + CV_IABS(x - origin.x);
        selection.height = selection.y + CV_IABS(y - origin.y);

        selection.x = MAX(selection.x, 0);
        selection.y = MAX(selection.y, 0);
        selection.width = MIN(selection.width, image->width);
        selection.height = MIN(selection.height, image->height);
        selection.width -= selection.x;
        selection.height -= selection.y;
    }

    switch (event)
    {
    case CV_EVENT_LBUTTONDOWN:
        origin = cvPoint(x, y);
        selection = cvRect(x, y, 0, 0);
        select_object = 1;
        break;
    case CV_EVENT_LBUTTONUP:
        select_object = 0;
        if (selection.width > 0 && selection.height > 0)
            track_object = -1;
        break;
    }
}


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

int main(int argc, char** argv)
{
    CvCapture* capture = 0;
    CvPoint* points = malloc(1);
    int size = 1;
    CvScalar track_color;

    if (argc == 1 || (argc == 2 && strlen(argv[1]) == 1 && isdigit(argv[1][0])))
        capture = cvCaptureFromCAM(argc == 2 ? argv[1][0] - '0' : 0);
    else if (argc == 2)
        capture = cvCaptureFromAVI(argv[1]);

    if (!capture)
    {
        fprintf(stderr, "Could not initialize capturing...\n");
        return -1;
    }

    printf("Hot 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");

    cvNamedWindow("Histogram", 1);
    cvNamedWindow("CamShiftDemo", 1);
    cvNamedWindow("Tracker", 1);
    cvSetMouseCallback("CamShiftDemo", on_mouse, 0);
    cvCreateTrackbar("Vmin", "CamShiftDemo", &vmin, 256, 0);
    cvCreateTrackbar("Vmax", "CamShiftDemo", &vmax, 256, 0);
    cvCreateTrackbar("Smin", "CamShiftDemo", &smin, 256, 0);

    for (;;)
    {
        IplImage* frame = 0;
        int i, bin_w, c;

        frame = cvQueryFrame(capture);
        if (!frame)
            break;

        if (!image)
        {
            /* allocate all the buffers */
            image = cvCreateImage(cvGetSize(frame), 8, 3);
            image->origin = frame->origin;
            hsv = cvCreateImage(cvGetSize(frame), 8, 3);
            hue = cvCreateImage(cvGetSize(frame), 8, 1);
            mask = cvCreateImage(cvGetSize(frame), 8, 1);
            backproject = cvCreateImage(cvGetSize(frame), 8, 1);
            hist = cvCreateHist(1, &hdims, CV_HIST_ARRAY, &hranges, 1);
            histimg = cvCreateImage(cvSize(320, 200), 8, 3);
            cvZero(histimg);
            tracker = cvCreateImage(cvGetSize(frame), 8, 3);
        }

        cvCopy(frame, image, 0);
        cvCvtColor(image, hsv, CV_BGR2HSV);

        if (track_object)
        {
            int _vmin = vmin, _vmax = vmax;

            cvInRangeS(hsv, cvScalar(0, smin, MIN(_vmin, _vmax), 0),
                cvScalar(180, 256, MAX(_vmin, _vmax), 0), mask);
            cvSplit(hsv, hue, 0, 0, 0);

            if (track_object < 0)
            {
                float max_val = 0.f;
                cvSetImageROI(hue, selection);
                cvSetImageROI(mask, selection);
                cvCalcHist(&hue, hist, 0, mask);
                cvGetMinMaxHistValue(hist, 0, &max_val, 0, 0);
                cvConvertScale(hist->bins, hist->bins, max_val ? 255. / max_val : 0., 0);
                cvResetImageROI(hue);
                cvResetImageROI(mask);
                track_window = selection;
                track_object = 1;

                cvZero(histimg);
                bin_w = histimg->width / hdims;
                for (i = 0; i < hdims; i++)
                {
                    int val = cvRound(cvGetReal1D(hist->bins, i) * histimg->height / 255);
                    CvScalar color = hsv2rgb(i * 180.f / hdims);
                    if (val == 200) {
                        track_color = color;
                    }
                    cvRectangle(histimg, cvPoint(i * bin_w, histimg->height),
                        cvPoint((i + 1) * bin_w, histimg->height - val),
                        color, -1, 8, 0);
                }
            }

            cvCalcBackProject(&hue, backproject, hist);
            cvAnd(backproject, mask, backproject, 0);
            cvMeanShift(backproject, track_window,
                cvTermCriteria(CV_TERMCRIT_EPS | CV_TERMCRIT_ITER, 10, 1),
                &track_comp, &track_box);

            double x = track_window.width / 2;
            double y = track_window.height / 2;

            points[size - 1] = cvPoint(track_window.x + x, track_window.y + y);

            size++;
            points = realloc(points, size * sizeof(CvPoint));

            int j;
            for (j = 1; j < size; j++) {
                cvLine(tracker, points[j], points[j], track_color, 5, 4, 0);
            }

            track_window = track_comp.rect;

            if (backproject_mode)
                track_box.angle = -track_box.angle;
            cvEllipseBox(image, track_box, CV_RGB(255, 0, 0), 3, CV_AA, 0);
            //cvRectangle(image, cvPoint(track_window.x, track_window.y),
            //    cvPoint(track_window.x + track_window.width, track_window.y + track_window.height), CV_RGB(0, 0, 255), 3, CV_AA, 0);
        }

        if (select_object && selection.width > 0 && selection.height > 0)
        {
            cvSetImageROI(image, selection);
            cvXorS(image, cvScalarAll(255), image, 0);
            cvResetImageROI(image);
        }

        cvShowImage("CamShiftDemo", image);
        cvShowImage("Histogram", histimg);
        cvShowImage("Tracker", tracker);

        c = cvWaitKey(10);
        if ((char)c == 27)
            break;
        switch ((char)c)
        {
        case 'b':
            backproject_mode ^= 1;
            break;
        case 'c':
            track_object = 0;
            cvZero(histimg);
            break;
        case 'h':
            show_hist ^= 1;
            if (!show_hist)
                cvDestroyWindow("Histogram");
            else
                cvNamedWindow("Histogram", 1);
            break;
        default:
            ;
        }
    }

    cvReleaseCapture(&capture);
    cvDestroyWindow("CamShiftDemo");
    cvDestroyWindow("Tracker");

    return 0;
}

#ifdef _EiC
main(1, "camshiftdemo.c");
#endif