source: liacs/mss/project/colour-tracking.c@ 13

/* OpenCV Hello world, with filtering of various filters 
 * Based on http://home.iitk.ac.in/~swagatk/faq/opencv.html#Colar_Based_Tracking 
 * Rick van der Zwet <info@rickvanderzwet.nl>
 */

#include <cv.h>
#include <cxcore.h>
#include <cvaux.h>
#include <highgui.h>
#include <stdio.h>

#define DEBUG 1

#define RVALUE(x,y,step) (x*step+y*3+2)
#define GVALUE(x,y,step) (x*step+y*3+1)
#define BVALUE(x,y,step) (x*step+y*3+0)

/* Used for playing with bounderies, returned max value of the pair */
int max(const int x, const int y) {
  return ((x > y) ? x : y);
}

/* Used for playing with bounderies, returned min value of the pair */
int min(const int x, const int y) {
  return ((x < y) ? x : y);

}

int cvInitSystem(int argc, char *argv[]) {
    // Don't care which camera to use. Use specified number elsewise
    int cam_number = atoi(argv[1]) > 0 ? atoi(argv[1]) : -1;
    CvCapture *capture = cvCaptureFromCAM(cam_number);
    int width = cvGetCaptureProperty(capture, CV_CAP_PROP_FRAME_WIDTH);
    int height = cvGetCaptureProperty(capture, CV_CAP_PROP_FRAME_HEIGHT);


    uchar *filterdata, *outdata, *routdata, *goutdata, *boutdata;

    
    //Create a window
    cvNamedWindow("RGB_Image", CV_WINDOW_AUTOSIZE);
    cvMoveWindow("RGB_Image",0,0);
    cvNamedWindow("R_Image", CV_WINDOW_AUTOSIZE);    
    cvMoveWindow("R_Image",0,height);
    cvNamedWindow("G_Image", CV_WINDOW_AUTOSIZE);    
    cvMoveWindow("G_Image",width,0);
    cvNamedWindow("B_Image", CV_WINDOW_AUTOSIZE);    
    cvMoveWindow("B_Image",width,height);
    cvNamedWindow("F_Image", CV_WINDOW_AUTOSIZE);


    //Create Image Skeletons
    IplImage *image   = NULL;
    IplImage *output  = cvCreateImage( cvSize(width,height), 8, 3);
    IplImage *boutput = cvCreateImage( cvSize(width,height), 8, 3);
    IplImage *routput = cvCreateImage( cvSize(width,height), 8, 3);
    IplImage *goutput = cvCreateImage( cvSize(width,height), 8, 3);
    IplImage *filter  = cvCreateImage( cvSize(width,height), 8, 3);
    IplImage *target  = cvCreateImage( cvSize(width,height), 8, 3);


    // Some temp pointers
    int i, j, k, m, n, o, p;
    int r, g, b;
    int q;
        
    
    // Video run starts here ...
    while (1)
    {                 
        image = cvQueryFrame(capture);
        
        // We need some input before running any further
        if (image == NULL) {
          printf("Ehm no input in the image\n");
          continue;
        }

        //This is needed for accessing pixel elements
        int step = image->widthStep/sizeof(uchar);

        cvCopy(image, output, NULL);
        cvCopy(image, routput, NULL);
        cvCopy(image, goutput, NULL);
        cvCopy(image, boutput, NULL);
        

        outdata = (uchar *) output->imageData;
        routdata = (uchar *) routput->imageData;
        goutdata = (uchar *) goutput->imageData;
        boutdata = (uchar *) boutput->imageData;

        k = 0, p = 0;
        for( m = 0; m < image->height; m++) //row
        {
            for( n = 0; n < image->width; n++) //column
            {
                routdata[BVALUE(m,n,step)] = 0; // clear blue part
                routdata[GVALUE(m,n,step)] = 0; // clear green part

                goutdata[BVALUE(m,n,step)] = 0; // clear blue part
                goutdata[RVALUE(m,n,step)] = 0; // clear red part

                boutdata[GVALUE(m,n,step)] = 0; // clear green part
                boutdata[RVALUE(m,n,step)] = 0; // clear red part
            }
        } 

        // Clear filter for new use
        // cvSetZero(filter);
        // ... or clone orginal image for mapping over it
        //cvCopy(goutput, filter, NULL);

        // Make it yellow
        //cvOr( routput, goutput, filter, NULL);

        // XXX: Use stuff like cvMat, cvGetRawData instead of internal hacking
        // Make green 'fade' out by the presence of many red as well
        filterdata = (uchar *) filter->imageData;
        for( m = 0; m < image->height; m++) //row
        {
            for( n = 0; n < image->width; n++) //column
            {
                r = outdata[RVALUE(m,n,step)];
                g = outdata[GVALUE(m,n,step)];
                b = outdata[BVALUE(m,n,step)];
                // Many red, less blue and green. Good for laser pointer tracing
                p = (r < 220) ? 0 : r;
                p = (b < 180 || g < 180) ? 0 : p;
                //p = (abs(p - filterdata[GVALUE(m,n,step)]) > 20) ? p : 0;
                filterdata[GVALUE(m,n,step)] = p;
            }
        }


        int xmin = 0, xmax = 0;
        int ymin = 0, ymax = 0;
        int value = 0, cmax = 0;

        // Find the 'very' green point and mark it's surroundings
        const int stepSize = 5;
        for(m = 0; m < image->height; m = m+stepSize)
        {
            for(n = 0; n < image->width; n = n+stepSize)
            {
                // Make it stop processing all points, pick the ones with high value
                value = filterdata[GVALUE(m,n,step)];
                if (value > cmax) 
                {
                    // detect 'square'
                    p = 0;
                    int SIZE = 3;
                    for (i = m; i  < min(m+SIZE,height); i++) 
                    {
                        for (j = n; j < min(n+SIZE,width); j++) 
                        {
                            value = filterdata[GVALUE(i,j,step)];
                            if (value > cmax) 
                            {
                                p++;
                            }
                        }
                    }

                        
                    SIZE = 6;
                    /* Make sure envirionment is 'low' */
                    for (i = max(m-SIZE,0); i  < m; i++) 
                    {
                        for (j = max(n-SIZE,0); j < n; j++) 
                        {
                            q = filterdata[GVALUE(i,j,step)];
                            if (q > value) 
                            {
                                p--;
                            }
                        }
                    }
                    for (i = min(m+SIZE,height); i  < min(m+(2*SIZE),height); i++) 
                    {
                        for (j= min(m+SIZE,width); j < min(m+(2*SIZE),width); j++) 
                        {
                            q = filterdata[GVALUE(i,j,step)];
                            if (q > value) 
                            {
                                p--;
                            }
                        }
                    }

                    if (p > SIZE) 
                    {
                            //printf("%ix%i=%i\n",n,m,value);
                            cmax = value;
                            xmax = n;
                            ymax = m;
                    }
                }
            }
        }
        
        // Prepare combined output
        // Some way of making the picture more pretty
        //cvSmooth(filter,target, CV_MEDIAN, 3, 3, 3, 3);
        cvCopy(filter,target, NULL);
        cvCircle(target, cvPoint(xmax, ymax), 10, CV_RGB(255,0,0),4, 8, 0);
                        

        // Show all the images
        cvShowImage("RGB_Image", output);
        cvShowImage("R_Image", routput);
        cvShowImage("B_Image", boutput);       
        cvShowImage("G_Image", goutput);       
        cvShowImage("F_Image", target);       
                    
                
        // wait for any to press to quit
        // Eeks! also HACK to make it refresh properly
        if (cvWaitKey(20) != -1) {
                break;
        }
    } //for each frame ...


    cvReleaseCapture(&capture);
    cvDestroyWindow("RGB_Image");
    cvDestroyWindow("R_Image");
    cvDestroyWindow("G_Image");
    cvDestroyWindow("B_Image");
    cvDestroyWindow("F_Image");

    return 0;

}

// Get ourself into the highGUI main stuff
int main(int argc, char *argv[]) {
   return cvInitSystem(argc, argv);
}