/trunk/code/projects/colonet/server/vision/vision.c - Diff - Colony - Robotics Club

Revision 937

New vision algorithm utilitizing Hough Transform and edge detection
instead of the older one using ellipse fitting and thresholding

     #include <stdio.h>
     #include <stdlib.h>
     #define MINH 50 //min threshold level
     #define MAXH 75 //max threshold level
     #define MIN_PADDING 70
     #define MIN_BOX_WIDTH 10
     #define MAX_BOX_WIDTH 20
     #define MIN_BOX_HEIGHT 10
     #define MAX_BOX_HEIGHT 20
     #define BEST_CENTER_PADDING_X 9
     #define BEST_CENTER_PADDING_Y 9
     #define ELLIPSE_COUNT_TO_BE_OBJECT 7
     #define DEBUG 1 //Debug to find threshold level
     int write_JPEG_to_mem (unsigned char *storage, int length, IplImage *img, int quality);
-...
     int vision_init(/*char* filename_*/) {
       /*filename = filename_;*/
       if (DEBUG)
         cvNamedWindow("Result", CV_WINDOW_AUTOSIZE);
       //TODO: replace with 1394 capture so we don't have to use Coriander
       capture = cvCreateCameraCapture(-1);
       if( !capture ) {
         fprintf( stderr, "ERROR: capture is NULL \n" );
         return -1;
+      }
       if (DEBUG)
         cvNamedWindow("Result", CV_WINDOW_AUTOSIZE);
       return 0;
+    }
-...
+    }
     int vision_get_robot_positions(VisionPosition** positions) {
       IplImage* image03;
       IplImage* im_capture;
       IplImage *image03;
       if(!cvGrabFrame(capture)){              // capture a frame
         printf("Could not grab a frame\n\7");
         exit(0);
         return -1;
+      }
       im_capture=cvRetrieveFrame(capture);           // retrieve the captured frame
       IplImage *im_capture=cvRetrieveFrame(capture);           // retrieve the captured frame
       image03 = cvCreateImage(cvGetSize(im_capture), im_capture->depth, 1);
       cvCvtColor(im_capture, image03, CV_RGB2GRAY);
       /*/ load image and force it to be grayscale
       if ((image03 = cvLoadImage(filename, 0)) == 0) {
         fprintf(stderr, "Failed to load image.\n");
         return -1;
       }*/
       struct CenterP centers[100] = {0};
       int count = 0;
       // Create the destination images
       IplImage* image02 = cvCloneImage(image03);
       IplImage* image04 = cvCloneImage(image03);
       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 );
       // Create dynamic structure and sequence.
       CvMemStorage* stor = cvCreateMemStorage(0);
       CvSeq* cont = cvCreateSeq(CV_SEQ_ELTYPE_POINT, sizeof(CvSeq), sizeof(CvPoint) , stor);
       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
       struct CenterP bestc[100];
       int index=0;
       int h;
         float* p = (float*)cvGetSeqElem( circles, i );
         CvPoint center = cvPoint(cvRound(p[0]), cvRound(p[1]));
         int r = cvRound(p[2]);
       for (h = MINH; h < MAXH; h++) {
         // Threshold the source image. This needful for cvFindContours().
         cvThreshold(image03, image02, h, 255, CV_THRESH_BINARY);
         if (DEBUG) cvCircle( image03, center, 3, CV_RGB(255,255,255), -1, 8, 0 );
         // Find all contours.
         cvFindContours(image02, stor, &cont, sizeof(CvContour), CV_RETR_LIST, CV_CHAIN_APPROX_NONE, cvPoint(0,0));
         float dist = MIN_PADDING;
         int loc = count;
         // Clear images. IPL use.
         cvZero(image02);
         cvZero(image04);
         int j;
         for (j = 0; j < count; j++)
+        {
           int dx = centers[j].center.x-center.x;
           int dy = centers[j].center.y-center.y;
           float d = cvSqrt(dx*dx + dy*dy);
         // 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;
           if (d < dist)
+          {
             dist = d;
             loc = j;
+          }
+        }
           // Fits ellipse to current contour.
           cvFitEllipse(PointArray2D32f, count, box);
         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++;
+      }
           // 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 (DEBUG)
+      {
         for (i = 0; i < count; i++)
+        {
           cvCircle( image03, centers[i].center, 60, CV_RGB(255,255,255), 3, 8, 0 );
+        }
           if (size.width > MIN_BOX_WIDTH && size.height > MIN_BOX_HEIGHT
               && size.width < MAX_BOX_WIDTH && size.height < MAX_BOX_HEIGHT){
             //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) < BEST_CENTER_PADDING_X
                   && abs(bestc[j].center.y-center.y) < BEST_CENTER_PADDING_Y) {
                 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);
+        }
         cvShowImage( "Result", image03 );
+      }
       image04 = cvCloneImage(image03);
       cvReleaseMemStorage(&storage);
       int count = 0;
       int i;
       for (i = 0; i < index; i++) {
         if (bestc[i].count > ELLIPSE_COUNT_TO_BE_OBJECT){
           count++;
+        }
+      }
       cvReleaseImage(&image03);
       VisionPosition* pos_array = (VisionPosition*)malloc(sizeof(VisionPosition) * count);
       if (pos_array == NULL) {
-...
+      }
       int c = 0;
       for (i = 0; i < index; i++) {
         if (bestc[i].count > ELLIPSE_COUNT_TO_BE_OBJECT){
           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);
+        }
       for (i = 0; i < count; i++) {
         pos_array[c].x = centers[i].center.x;
         pos_array[c].y = centers[i].center.y;
         c++;
+      }
       cvReleaseImage(&image02);
       cvReleaseImage(&image03);
       // Show image. HighGUI use.
       if (DEBUG) cvShowImage( "Result", image04 );
       cvReleaseImage(&image04);
       cvReleaseMemStorage(&stor);
       *positions = pos_array;
       return count;
+    }

Also available in: Unified diff

Project

General

Profile

Colony

Revision 937