Colony

sleep .1s

void loop_image(void);

  CvPoint center;

  int count;

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

  // load image and force it to be grayscale

  if ((image03 = cvLoadImage(filename, 0)) == 0) {

    fprintf(stderr, "Failed to load image.\n");

    return -1;

  }

  // Create the destination images

  image02 = cvCloneImage( image03 );

  image04 = cvCloneImage( image03 );

  if (DEBUG) cvNamedWindow("Result", 1);

  loop_image();

  if (DEBUG) cvWaitKey(0);

  cvReleaseImage(&image02);

  cvReleaseImage(&image03);

  if (DEBUG) {

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

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

    }

  }

  // Show image. HighGUI use.

  if (DEBUG) cvShowImage( "Result", image04 );

/*

*/

Run "make" to compile.

Run "./vision.sh" to get robot positions.

# Makefile for vision.

default: vision fitellipse

vision: vision.c

	mkdir -p bin

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

fitellipse: fitellipse.c

	mkdir -p bin

	gcc -ggdb `pkg-config opencv --cflags --libs` fitellipse.c -o bin/fitellipse

clean:

	rm -rf bin *~