Colony

/**

 * Robot Detection

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

 *

 * @author Rich Hong

 * @date 11/18/2007

 */

#include <vision.h>

#include <cv.h>

#include <highgui.h>

#include <stdio.h>

#include <stdlib.h>

#define MINH 100 //min threshold level

#define MAXH 220 //max threshold level

#define DEBUG 0 //Debug to find threshold level

struct CenterP {

  CvPoint center;

  int count;

};

static char* filename;

int vision_init(char* filename_) {

  filename = filename_;

  return 0;

}

int vision_get_robot_positions(VisionPosition** positions) {

  IplImage* image03;

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

  IplImage* image02 = cvCloneImage(image03);

  IplImage* image04 = cvCloneImage(image03);

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

  // Create dynamic structure and sequence.

  CvMemStorage* stor = cvCreateMemStorage(0);

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

  struct CenterP bestc[100];

  int index=0;

  for (int 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.

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

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

      // Alloc memory for ellipse data.

      CvBox2D32f* 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, 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 count = 0;

  int i;

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

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

      count++;

    }

  }

  VisionPosition* pos_array = (VisionPosition*)malloc(sizeof(VisionPosition) * count);

  if (pos_array == NULL) {

    fprintf(stderr, "malloc failed\n");

    return -1;

  }

  int c = 0;

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

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

      pos_array[c].x = bestc[i].center.x;

      pos_array[c].y = bestc[i].center.y;

      c++;

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

    }

  }

  if (DEBUG) cvWaitKey(0);

  cvReleaseImage(&image02);

  cvReleaseImage(&image03);

  if (DEBUG) cvDestroyWindow("Result");

  // Show image. HighGUI use.

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

  cvReleaseImage(&image04);

  cvReleaseMemStorage(&stor);

  *positions = pos_array;

  return count;

}