Colony

#!/bin/sh

gcc -ggdb `pkg-config opencv --cflags --libs` vision.c -o vision

#!/bin/sh

wget http://roboclub9.frc.ri.cmu.edu/colonet.jpg

./vision colonet.jpg > colonet.txt

/**

 * find the locations of each robot

 * based on opencv's sample program fitellipse.c by Denis Burenkov

 *

 * @author Rich Hong

 * @date 11/10/2007

 */

#ifdef _CH_

#pragma package <opencv>

#endif

#ifndef _EiC

#include "cv.h"

#include "highgui.h"

#endif

#include <stdio.h>

#include <stdlib.h>

int slider_pos = 200;

#define MINH 100 //min threshold level

#define MAXH 150 //max threshold level

#define DEBUG 0 //Debug to find threshold level

// Load the source image. HighGUI use.

IplImage *image02 = 0, *image03 = 0, *image04 = 0;

void process_image(int h);

void loop_image();

struct CenterP {

	CvPoint center;

	int count;

};

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

    const char* filename = argc == 2 ? argv[1] : (char*)"colonet.jpg";

    // load image and force it to be grayscale

    if( (image03 = cvLoadImage(filename, 0)) == 0 ) return -1;

    // Create the destination images

    image02 = cvCloneImage( image03 );

    image04 = cvCloneImage( image03 );

	if (DEBUG){

    	// Create windows.

    	cvNamedWindow("Source", 1);

    	// Show the image.

    	cvShowImage("Source", image03);

	}

    cvNamedWindow("Result", 1);

    if (DEBUG){

		// Create toolbars. HighGUI use.

    	cvCreateTrackbar( "Threshold", "Result", &slider_pos, 255, process_image );

	}

	if (!DEBUG) loop_image();

    if (DEBUG) process_image(0);

    // Wait for a key stroke; the same function arranges events processing

    cvWaitKey(0);

    cvReleaseImage(&image02);

    cvReleaseImage(&image03);

    if (DEBUG) cvDestroyWindow("Source");

    cvDestroyWindow("Result");

    return 0;

}

void loop_image(){

	CvMemStorage* stor;

    CvSeq* cont;

    CvBox2D32f* box;

    CvPoint* PointArray;

    CvPoint2D32f* PointArray2D32f;

    // Create dynamic structure and sequence.

    stor = cvCreateMemStorage(0);

    cont = cvCreateSeq(CV_SEQ_ELTYPE_POINT, sizeof(CvSeq), sizeof(CvPoint) , stor);

	struct CenterP bestc[100];

	int index=0;

	int h;

	for (h=MINH;h<MAXH;h++){

		// Threshold the source image. This needful for cvFindContours().

    	cvThreshold( image03, image02, h, 255, CV_THRESH_BINARY );

    	// Find all contours.

    	cvFindContours( image02, stor, &cont, sizeof(CvContour), CV_RETR_LIST, CV_CHAIN_APPROX_NONE, cvPoint(0,0));

    	// Clear images. IPL use.

    	cvZero(image02);

    	cvZero(image04);

    	// This cycle draw all contours and approximate it by ellipses.

    	for(;cont;cont = cont->h_next) {   

        	int i; // Indicator of cycle.

        	int count = cont->total; // This is number point in contour

        	CvPoint center;

        	CvSize size;

        	// Number point must be more than or equal to 6 (for cvFitEllipse_32f).

        	if( count < 6 ) continue;

        	// Alloc memory for contour point set.    

        	PointArray = (CvPoint*)malloc( count*sizeof(CvPoint) );

        	PointArray2D32f= (CvPoint2D32f*)malloc( count*sizeof(CvPoint2D32f) );

        	// Alloc memory for ellipse data.

        	box = (CvBox2D32f*)malloc(sizeof(CvBox2D32f));

        	// Get contour point set.

        	cvCvtSeqToArray(cont, PointArray, CV_WHOLE_SEQ);

        	// Convert CvPoint set to CvBox2D32f set.

        	for(i=0; i<count; i++) {

            	PointArray2D32f[i].x = (float)PointArray[i].x;

            	PointArray2D32f[i].y = (float)PointArray[i].y;

        	}

        	// Fits ellipse to current contour.

        	cvFitEllipse(PointArray2D32f, count, box);

        	// Convert ellipse data from float to integer representation.

        	center.x = cvRound(box->center.x);

        	center.y = cvRound(box->center.y);

        	size.width = cvRound(box->size.width*0.5);

        	size.height = cvRound(box->size.height*0.5);

        	box->angle = -box->angle;

			if (size.width>10&&size.height>10&&size.width<20&&size.height<20){

				//printf("%d %d %d %d\n",center.x,center.y,size.width,size.height);

				int found=0;

				int j;

				for (j=0;j<index;j++)

					if (abs(bestc[j].center.x-center.x)<9&&abs(bestc[j].center.y-center.y)<9){

						bestc[j].count++;

						found=1;

						break;

					}

				if (!found){

					struct CenterP c;

					c.center=center;

					c.count=1;

					bestc[index]=c;

					index++;

				}

			}

        	// Free memory.          

        	free(PointArray);

        	free(PointArray2D32f);

      		free(box);

		}

	}

	image04 = cvCloneImage( image03 );

	int i;

	for (i=0;i<index;i++){

		if (bestc[i].count>7){

			printf("%d,%d\n",bestc[i].center.x,bestc[i].center.y);

			cvCircle(image04,bestc[i].center, 20, CV_RGB(0,0,0),5,8,0);

		}

	}

    // Show image. HighGUI use.

    cvShowImage( "Result", image04 );

}

// Define trackbar callback functon. This function find contours,

// draw it and approximate it by ellipses.

void process_image(int h)

{

    CvMemStorage* stor;

    CvSeq* cont;

    CvBox2D32f* box;

    CvPoint* PointArray;

    CvPoint2D32f* PointArray2D32f;

    // Create dynamic structure and sequence.

    stor = cvCreateMemStorage(0);

    cont = cvCreateSeq(CV_SEQ_ELTYPE_POINT, sizeof(CvSeq), sizeof(CvPoint) , stor);

    // Threshold the source image. This needful for cvFindContours().

    cvThreshold( image03, image02, slider_pos, 255, CV_THRESH_BINARY );

    // Find all contours.

    cvFindContours( image02, stor, &cont, sizeof(CvContour), 

                    CV_RETR_LIST, CV_CHAIN_APPROX_NONE, cvPoint(0,0));

    // Clear images. IPL use.

    cvZero(image02);

    cvZero(image04);

    // This cycle draw all contours and approximate it by ellipses.

    for(;cont;cont = cont->h_next)

    {   

        int i; // Indicator of cycle.

        int count = cont->total; // This is number point in contour

        CvPoint center;

        CvSize size;

        // Number point must be more than or equal to 6 (for cvFitEllipse_32f).        

        if( count < 6 )

            continue;

        // Alloc memory for contour point set.    

        PointArray = (CvPoint*)malloc( count*sizeof(CvPoint) );

        PointArray2D32f= (CvPoint2D32f*)malloc( count*sizeof(CvPoint2D32f) );

        // Alloc memory for ellipse data.

        box = (CvBox2D32f*)malloc(sizeof(CvBox2D32f));

        // Get contour point set.

        cvCvtSeqToArray(cont, PointArray, CV_WHOLE_SEQ);

        // Convert CvPoint set to CvBox2D32f set.

        for(i=0; i<count; i++)

        {

            PointArray2D32f[i].x = (float)PointArray[i].x;

            PointArray2D32f[i].y = (float)PointArray[i].y;

        }

        // Fits ellipse to current contour.

        cvFitEllipse(PointArray2D32f, count, box);

        // Draw current contour.

        cvDrawContours(image04,cont,CV_RGB(255,255,255),CV_RGB(255,255,255),0,1,8,cvPoint(0,0));

        // Convert ellipse data from float to integer representation.

        center.x = cvRound(box->center.x);

        center.y = cvRound(box->center.y);

        size.width = cvRound(box->size.width*0.5);

        size.height = cvRound(box->size.height*0.5);

        box->angle = -box->angle;

		if (size.width>10&&size.height>10&&size.width<20&&size.height<20){

		printf("%d %d %d %d\n",center.x,center.y,size.width,size.height);

        // Draw ellipse.

        cvEllipse(image04, center, size,

                  box->angle, 0, 360,

                  CV_RGB(0,0,255), 1, CV_AA, 0);

		}

        // Free memory.          

        free(PointArray);

        free(PointArray2D32f);

        free(box);

    }

    // Show image. HighGUI use.

    cvShowImage( "Result", image04 );

}

#ifdef _EiC

main(1,"vision.c");

#endif

You need to have OpenCV installed and correct path configuration.