Colony

/**

 * Robot Detection

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

 *

 * @author Rich Hong

 * @author Ryan Cahoon

 * @date 11/18/2007

 */

//TODO: Add camera stabilization

#include "vision.h"

#include <cv.h>

#include <highgui.h>

#include <stdio.h>

#include <stdlib.h>

#define MIN_PADDING 70

#define DEBUG 1 //Debug to find threshold level

int write_JPEG_to_mem (unsigned char *storage, int length, IplImage *img, int quality);

struct CenterP {

  CvPoint center;

  int count;

};

/*static char* filename;*/

static CvCapture *capture;

int vision_init(/*char* filename_*/) {

  /*filename = filename_;*/

  printf("Vision init. %s\n", DEBUG ? "Debugging on" : "");

  capture = cvCaptureFromCAM(0);

  if( !capture ) {

    fprintf( stderr, "ERROR: capture is NULL \n" );

    return -1;

  }

  if (DEBUG)

  {

    printf("Vision debugging start\n");

    cvNamedWindow("mywindow1", CV_WINDOW_AUTOSIZE);

  }

  return 0;

}

void vision_close()

{

  if (DEBUG)

    cvDestroyWindow("mywindow1");

  cvReleaseCapture(&capture);

  printf("Vision closed.\n");

}

int getImageBytes(unsigned char *buffer, int length)

{

  IplImage* im_capture;

  if(!cvGrabFrame(capture)){              // capture a frame 

    printf("Could not grab a frame\n\7");

    exit(0);

  }

  im_capture=cvRetrieveFrame(capture);           // retrieve the captured frame

  return write_JPEG_to_mem (buffer, length, im_capture, 30);

}

int vision_get_robot_positions(VisionPosition** positions) {

  IplImage *image03;

  if(!cvGrabFrame(capture)){              // capture a frame 

    printf("Could not grab a frame\n\7");

    return -1;

  }

  IplImage *im_capture=cvRetrieveFrame(capture);           // retrieve the captured frame

  image03 = cvCreateImage(cvGetSize(im_capture), im_capture->depth, 1);

  cvCvtColor(im_capture, image03, CV_RGB2GRAY);

  struct CenterP centers[100] = {0};

  int count = 0;

  cvSmooth( image03, image03, CV_GAUSSIAN, 9, 9 ); // smooth it, otherwise a lot of false circles may be detected

  CvMemStorage* storage = cvCreateMemStorage(0);

  CvSeq* circles = cvHoughCircles( image03, storage, CV_HOUGH_GRADIENT, 1, 1, 100, 43 );

  int i;

  for( i = 0; i < circles->total && count < 100; i++ )

  {

    //TODO: Augment with Harr cascade second level detecting orbs + identify robot with orb color

    float* p = (float*)cvGetSeqElem( circles, i );

    CvPoint center = cvPoint(cvRound(p[0]), cvRound(p[1]));

    int r = cvRound(p[2]);

    if (DEBUG) cvCircle( image03, center, 3, CV_RGB(255,255,255), -1, 8, 0 );

    int dist2 = MIN_PADDING;

    int loc = count;

    int j;

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

    {

      int dx = centers[j].center.x-center.x;

      int dy = centers[j].center.y-center.y;

      int d2 = dx*dx + dy*dy;

      if (d2 < dist2)

      {

        dist2 = d2;

        loc = j;

      }

    }

    int c = centers[loc].count;

    centers[loc].center.x = (centers[loc].center.x * c + center.x) / (c + 1);

    centers[loc].center.y = (centers[loc].center.y * c + center.y) / (c + 1);

    centers[loc].count++;

    if (loc == count) count++;

  }

  if (DEBUG)

  {

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

    {

      cvCircle( image03, centers[i].center, 60, CV_RGB(255,255,255), 3, 8, 0 );

    }

    cvShowImage( "mywindow1", image03 );

  }

  cvReleaseMemStorage(&storage);

  cvReleaseImage(&image03);

  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 < count; i++) {

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

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

    c++;

  }

  *positions = pos_array;

  return count;

}