src/camera/rozkuk/rozkuk.cxx

   1 /*******************************************************************************
   2  *
   3  * rozkuk - OpenCV based corns camera recognition program
   4  *
   5  * Created for Eurobot 2010 competition.
   6  *
   7  * Petr Kubizňák (kubiznak.petr@gmail.com), 2010
   8  *
   9  ******************************************************************************/
  10
  11 #include <inttypes.h>
  12 #include <stdlib.h>
  13 #include <stdio.h>
  14 #include <sys/stat.h>
  15 #include <semaphore.h>
  16 #include <getopt.h>
  17 #include <unistd.h>
  18
  19 #include <opencv/cv.h>
  20 #include <opencv/highgui.h>
  21
  22 #include "clr2float.h"
  23 #include "masks_globals.h"
  24
  25 extern "C" {
  26 #include <roboorte_robottype.h>
  27 #include <robot.h>
  28 }
  29 /******************************************************************************/
  30
  31 // modes definitions
  32 #define MODE_QUIT                                       0x01
  33 #define MODE_REALTIME                   0x02
  34 #define MODE_RECOGNIZE          0x04
  35 #define MODE_WAIT                                       0x08
  36
  37 // mask of all modes / errors
  38 #define MASK_MODES                              0x0F
  39 #define MASK_ERRORS                             (~MASK_MODES)
  40
  41 // errors
  42 #define ERR_MASK_READ_FAILURE   0x10
  43
  44 // default mode in standard session
  45 #define START_MODE                              MODE_RECOGNIZE
  46
  47 #ifdef ROZKUK_DEBUG                             /************ DEBUG SESSION ONLY ************/
  48
  49 // default mode in debug session
  50 #undef START_MODE
  51 #define START_MODE                              MODE_REALTIME
  52
  53 // highgui windows names
  54 #define WINDOW_ORIG                             "ROZKUK original scene"
  55 #define WINDOW_THRES                    "ROZKUK threshold"
  56 #define WINDOW_MASK                             "ROZKUK mask"
  57 #define WINDOW_PROD                             "ROZKUK product"
  58
  59 // values of keys
  60 #define KEY_ESC                                         0x1B
  61 #define KEY_SPACE                                       0x20
  62 #define KEY_P                                                   0x50
  63 #define KEY_R                                                   0x52
  64 #define KEY_S                                                   0x53
  65 #define KEY_W                                                   0x57
  66 #define KEY_p                                                   0x70
  67 #define KEY_r                                                   0x72
  68 #define KEY_s                                                   0x73
  69 #define KEY_w                                                   0x77
  70
  71 // number of frames passed in rozkuk mode before refresh
  72 #define PASS_FRAMES                             0
  73
  74 #endif                                                                          /*----------- DEBUG SESSION ONLY -----------*/
  75
  76 //default threshold and saturation
  77 #define THRESHOLD                                       70
  78 #define SATURATION                              1.0
  79
  80 //mask filename pattern
  81 #ifdef ROZKUK_DEBUG                             /************ DEBUG SESSION ONLY ************/
  82 #define MASKFILENAME                    "./mask%02d%c%c.bin"
  83 #else                                                                                   /******************** PPC *******************/
  84 #define MASKFILENAME                    "./mask%02d%c%c.bin"
  85 #endif                                                                          /*------------------------------------------*/
  86
  87 //size of frames from camera
  88 #define FRAME_WIDTH                             640
  89 #define FRAME_HEIGHT                    480
  90 #define FRAME_SIZE                              (FRAME_WIDTH * FRAME_HEIGHT)
  91
  92 //both (side and center) subresults will be returned in one int masked binary as 00ccssss
  93 #define MASK_SIDE         0x0F
  94 #define MASK_CENTER_SHIFT 4
  95 #define MASK_CENTER       (0x03 << 4)
  96
  97 struct robottype_orte_data orte;
  98
  99 /******************************************************************************/
 100
 101 // function declarations
 102 #ifdef ROZKUK_DEBUG                             /************ DEBUG SESSION ONLY ************/
 103 void drawPauseText(IplImage *img);
 104 int saveFrame(const IplImage *img);
 105 void displayProduct(const CvMat *floatMat, int sideLayout, int centerLayout);
 106 void displayMasksSum(const CvMat *mat1, const CvMat *mat2, const char *windowName);
 107 void displayFloatMat(const CvMat *floatMat, const char *windowName);
 108 int modeRealtime(CvCapture* capture);
 109 #endif                                                                          /*----------- DEBUG SESSION ONLY -----------*/
 110 int modeRecognize(CvCapture* capture);
 111 int modeWait(CvCapture *capture);
 112 int modeManager(int defaultMode);
 113 int countThreshold(const IplImage *frame);
 114 int recognize(const CvMat *frame, CvMat **masks, int masksCnt);
 115 int loadMask(const char *filename, CvMat **mask);
 116 int loadMasks(char color);
 117 void freeMasks(void);
 118
 119 /******************************************************************************/
 120
 121 // variable declarations
 122 CvMat **sideMasks=NULL;     //float mask matrices
 123 CvMat **centerMasks=NULL;
 124 #ifdef ROZKUK_DEBUG                             /************ DEBUG SESSION ONLY ************/
 125 CvFont font;                //text font
 126
 127 /******************************************************************************/
 128
 129 /** Writes text "pause" in the CAMERA window. */
 130 void drawPauseText(IplImage *img) {
 131         cvPutText(img,"pause", cvPoint(300,440), &font, cvScalar(255,255,255));
 132         cvShowImage(WINDOW_ORIG, img);
 133 }
 134
 135 /******************************************************************************/
 136
 137 /** Saves current image to a new file in the current directory. */
 138 int saveFrame(const IplImage *img) {
 139         struct stat stFileInfo;
 140         char filename[30] = "snapshot00.png";
 141         unsigned int i=0;
 142         //find a non-existent filename
 143         while(!stat(filename, &stFileInfo)) sprintf(filename, "snapshot%02d.png", ++i);
 144         //save image to file
 145         return cvSaveImage(filename, img);
 146 }
 147
 148 /******************************************************************************/
 149
 150 /** Displays in separate window the result of multiplying camera frame (in float)
 151   * and recognized mask.
 152   * @param floatMat Pointer to the camera frame converted to float matrix.
 153   * @param sideLayout Index of recognized side situation.
 154   * @param centerLayout Index of recognized center situation. */
 155 void displayProduct(const CvMat *floatMat, int sideLayout, int centerLayout) {
 156         CvMat *sideProduct = cvCloneMat(floatMat);
 157         CvMat *centerProduct = cvCloneMat(floatMat);
 158
 159         cvMul(floatMat, sideMasks[sideLayout], sideProduct);            //count product of frame and side mask
 160         cvMul(floatMat, centerMasks[centerLayout], centerProduct);      //count product of frame and center mask
 161         displayMasksSum(sideProduct, centerProduct, WINDOW_PROD);       //display sum
 162
 163         cvReleaseMat(&sideProduct);
 164         cvReleaseMat(&centerProduct);
 165 }
 166
 167 /******************************************************************************/
 168
 169 /** Displays a sum of two matrices in specified window. */
 170 void displayMasksSum(const CvMat *mat1, const CvMat *mat2, const char *windowName) {
 171         CvMat *sum = cvCloneMat(mat1);
 172
 173         cvAdd(mat1, mat2, sum);
 174         displayFloatMat(sum, windowName);
 175
 176         cvReleaseMat(&sum);
 177 }
 178
 179 /******************************************************************************/
 180
 181 /** Normalizes the matrix values and displays them in window specified by name.
 182   * @param floatMat Pointer to matrix data to display.
 183   * @param windowName Name of window in which to display the image. */
 184 void displayFloatMat(const CvMat *floatMat, const char *windowName) {
 185         double min=0.0, max=0.0;
 186         CvMat *normalized = cvCloneMat(floatMat);
 187
 188         cvMinMaxLoc(floatMat, &min, &max);                                                                                      // find extremes
 189         cvConvertScale(floatMat, normalized, 1/(max-min), -(min/(max-min)));            // normalize
 190         cvShowImage(windowName, normalized);
 191
 192         cvReleaseMat(&normalized);
 193 }
 194
 195 /******************************************************************************/
 196
 197 /** Displays a real video in a window (only for DEBUG session).
 198  * @param capture Pointer to a camera capture.
 199  * @return Code of mode to switch to. */
 200 int modeRealtime(CvCapture* capture) {
 201         IplImage* frame = NULL;                                                                                                                         // frame from camera
 202         int pause=0;                                                                                                                                                                    // set pause=1 to pause the video
 203
 204         while(1) {
 205                 /* keyboard events handeling */
 206                 switch(cvWaitKey(10) & 0xFF) {                                                                                  // wait 10ms for an event
 207                 // stop capturing
 208                 case KEY_ESC:
 209                         return MODE_QUIT;
 210                 // switch to recognize mode
 211                 case KEY_R:
 212                 case KEY_r:
 213                         return MODE_RECOGNIZE;
 214                 // (un)pause
 215                 case KEY_P:
 216                 case KEY_p:
 217                         pause = !pause;
 218                         drawPauseText(frame);
 219                         break;
 220                 // save frame to file
 221                 case KEY_S:
 222                 case KEY_s:
 223                         saveFrame(frame);
 224                         break;
 225                 }
 226
 227                 frame = cvQueryFrame(capture);                                                                                  // Get one frame
 228 //              printf("cvQueryFrame; cvGetErrorStatus=%d\n", cvGetErrorStatus());cvGetErrorStatus
 229                 if(!frame) {
 230                         fprintf(stdout, "NULL frame\n");
 231                         continue;
 232                 }
 233
 234                 if(!pause) cvShowImage(WINDOW_ORIG, frame);                                     // show image (! Do not release the frame !)
 235 //              printf("cvShowImage; cvGetErrorStatus=%d\n", cvGetErrorStatus());
 236         }
 237         return MODE_QUIT;
 238 }
 239 #endif                                                                          /*----------- DEBUG SESSION ONLY -----------*/
 240
 241 /******************************************************************************/
 242
 243 /** Implements the camera recognition of corns.
 244  * In DEBUG session the results are also displayed on screen.
 245  * @param capture Pointer to a camera capture.
 246  * @return Code of mode to switch to. */
 247 int modeRecognize(CvCapture* capture) {
 248         IplImage *frame = NULL;                                                                                                                         // frame from camera
 249         IplImage *thresFrame = NULL;                                                                                                    // thresholded frame
 250         CvMat *floatMat = NULL;                                                                                                                         // float <-1,1> image of the frame
 251         int sideLayout, centerLayout;                                                                                                   // indices of recognized situation
 252 #ifdef ROZKUK_DEBUG                             /************ DEBUG SESSION ONLY ************/
 253         int framesPassed = PASS_FRAMES;                                                                                         // number of passed frames before refresh
 254         int pause=0;                                                                                                                                                                    // set pause=1 to pause processing
 255 #endif                                                                          /*----------- DEBUG SESSION ONLY -----------*/
 256         while(1) {
 257
 258 #ifdef ROZKUK_DEBUG                             /************ DEBUG SESSION ONLY ************/
 259                 /* keyboard events handeling */
 260                 switch(cvWaitKey(10) & 0xFF) {                                                                                  // wait 10ms for an event
 261                 // stop capturing
 262                 case KEY_ESC:
 263                         return MODE_QUIT;
 264                 // skip the frame
 265                 case KEY_SPACE:
 266                         framesPassed = PASS_FRAMES;
 267                         break;
 268                 // switch to recognize mode
 269                 case KEY_R:
 270                 case KEY_r:
 271                         return MODE_REALTIME;
 272                 // (un)pause
 273                 case KEY_P:
 274                 case KEY_p:
 275                         pause = !pause;
 276                         drawPauseText(frame);
 277                         break;
 278                 // save frame to file
 279                 case KEY_S:
 280                 case KEY_s:
 281                         saveFrame(frame);
 282                         break;
 283                 // wait
 284                 case KEY_W:
 285                 case KEY_w:
 286                         return MODE_WAIT;
 287                 }
 288 #endif                                                                          /*----------- DEBUG SESSION ONLY -----------*/
 289
 290                 // Get one frame
 291 #ifdef ROZKUK_DEBUG                             /************ DEBUG SESSION ONLY ************/
 292                 if(pause) cvQueryFrame(capture);
 293                 else
 294 #endif                                                                          /*----------- DEBUG SESSION ONLY -----------*/
 295                         frame = cvQueryFrame(capture);
 296                 if(!frame) {
 297                         fprintf(stdout, "NULL frame\n");
 298                         continue;
 299                 }
 300
 301 #ifdef ROZKUK_DEBUG                             /************ DEBUG SESSION ONLY ************/
 302                 if(pause) continue;
 303
 304                 // transform colorful image to its float <-1,1> representation
 305                 cvReleaseImage(&thresFrame);
 306                 thresFrame = cvCreateImage(cvSize(640,480),IPL_DEPTH_8U,1);
 307 #endif                                                                          /*----------- DEBUG SESSION ONLY -----------*/
 308                 // convert to float
 309                 int threshold = countThreshold(frame);
 310                 clr2float(frame, &floatMat, threshold, SATURATION, thresFrame); // get float representation (in DEBUG session, thresholded image is stored to thresFrame)
 311                 //recognize side and center layouts
 312                 sideLayout = recognize(floatMat, sideMasks, SIDEMASKSCNT);
 313                 centerLayout = recognize(floatMat, centerMasks, CENTERMASKSCNT);
 314                 //publish results
 315                 fprintf(stdout, "thr: %d, side: %d, center: %d\n", threshold, sideLayout, centerLayout);
 316                 orte.camera_result.side = sideLayout;
 317                 orte.camera_result.center = centerLayout;
 318                 ORTEPublicationSend(orte.publication_camera_result);
 319
 320 #ifdef ROZKUK_DEBUG                             /************ DEBUG SESSION ONLY ************/
 321                 displayProduct(floatMat, sideLayout, centerLayout);
 322 #endif                                                                          /*----------- DEBUG SESSION ONLY -----------*/
 323                 cvReleaseMat(&floatMat);
 324
 325 #ifdef ROZKUK_DEBUG                             /************ DEBUG SESSION ONLY ************/
 326                 if(framesPassed++ > PASS_FRAMES) {
 327                         framesPassed=0;
 328                         cvShowImage(WINDOW_ORIG, frame);                                                                        // show image (! Do not release the frame !)
 329                         cvShowImage(WINDOW_THRES, thresFrame);
 330                         displayMasksSum(sideMasks[sideLayout], centerMasks[centerLayout], WINDOW_MASK);                 // display selected mask
 331                         cvReleaseImage(&thresFrame);
 332                 }
 333 #endif                                                                          /*----------- DEBUG SESSION ONLY -----------*/
 334
 335         }
 336         return MODE_QUIT;
 337 }
 338
 339 /******************************************************************************/
 340
 341 /** Waits while no action is wanted. To stop waiting set orte.camera_control.on=TRUE
 342   * or in DEBUG session press W key.
 343   * @param capture Pointer to a camera capture.
 344   * @return Code of mode to switch to. */
 345 int modeWait(CvCapture *capture) {
 346         while(!orte.camera_control.on) {
 347 #ifdef ROZKUK_DEBUG                             /************ DEBUG SESSION ONLY ************/
 348                 //process keyboard events
 349                 switch(cvWaitKey(10) & 0xFF) {                                                                                  // wait 10ms for an event
 350                 // exit program
 351                 case KEY_ESC:
 352                         return MODE_QUIT;
 353
 354                 // stop waiting
 355                 case KEY_W:
 356                 case KEY_w:
 357                         return MODE_RECOGNIZE;
 358
 359                 }
 360 #endif                                                                          /*----------- DEBUG SESSION ONLY -----------*/
 361                 printf("waiting...\n");
 362                 sleep(1);
 363         }
 364         return MODE_RECOGNIZE;
 365 }
 366
 367 /******************************************************************************/
 368
 369 /** Switches between modes of the program.
 370  * @param defaultMode The first mode to run. */
 371 int modeManager(int defaultMode) {
 372         int mode=defaultMode;
 373         int ret;
 374         CvCapture* capture;
 375
 376         // connect to a camera
 377         while(!((capture=cvCaptureFromCAM(0)) || (capture=cvCaptureFromCAM(1)))) {
 378                 fprintf(stdout, "NULL capture\n");
 379                 usleep(500*1000);
 380         }
 381
 382 #ifdef ROZKUK_DEBUG                             /************ DEBUG SESSION ONLY ************/
 383         // create gui windows and init font
 384         cvNamedWindow(WINDOW_ORIG, CV_WINDOW_AUTOSIZE);                         // real camera video / one frame viewer
 385         cvInitFont(&font, CV_FONT_HERSHEY_SIMPLEX, 1.0, 1.0, 0, 1);             //init text font
 386 #endif                                                                          /*----------- DEBUG SESSION ONLY -----------*/
 387
 388         while(!(mode & MODE_QUIT)) {
 389                 switch(mode & MASK_MODES) {
 390
 391 #ifdef ROZKUK_DEBUG                             /************ DEBUG SESSION ONLY ************/
 392                 case MODE_REALTIME:
 393                         cvDestroyWindow(WINDOW_THRES);
 394                         mode = modeRealtime(capture);
 395                         break;
 396 #endif                                                                          /*----------- DEBUG SESSION ONLY -----------*/
 397
 398                 case MODE_RECOGNIZE:
 399 #ifdef ROZKUK_DEBUG                             /************ DEBUG SESSION ONLY ************/
 400                         cvNamedWindow(WINDOW_THRES, CV_WINDOW_AUTOSIZE);
 401                         cvMoveWindow(WINDOW_THRES, cvQueryFrame(capture)->width, 0);
 402                         cvNamedWindow(WINDOW_MASK, CV_WINDOW_AUTOSIZE);
 403                         cvMoveWindow(WINDOW_MASK, 0, cvQueryFrame(capture)->height+50);
 404                         cvNamedWindow(WINDOW_PROD, CV_WINDOW_AUTOSIZE);
 405                         cvMoveWindow(WINDOW_PROD, cvQueryFrame(capture)->width, cvQueryFrame(capture)->height+50);
 406 #endif                                                                          /*----------- DEBUG SESSION ONLY -----------*/
 407                         //load masks and run recognition mode, then free masks (they are in memory only while in recognition mode)
 408                         if((ret=loadMasks((orte.camera_control.game_color==BLUE ? clBLUE : clYELLOW)))) return ret;
 409                         mode = modeRecognize(capture);
 410                         freeMasks();
 411                         break;
 412
 413                 case MODE_WAIT:
 414                         mode = modeWait(capture);
 415                         break;
 416
 417                 default:
 418                         goto end;                       // jump out of the loop
 419                 }
 420         }
 421
 422 end:
 423         cvReleaseCapture(&capture);
 424 #ifdef ROZKUK_DEBUG                             /************ DEBUG SESSION ONLY ************/
 425         cvDestroyWindow(WINDOW_ORIG);
 426         cvDestroyWindow(WINDOW_THRES);
 427         cvDestroyWindow(WINDOW_MASK);
 428         cvDestroyWindow(WINDOW_PROD);
 429 #endif                                                                          /*----------- DEBUG SESSION ONLY -----------*/
 430         return mode & MASK_ERRORS;
 431 }
 432
 433 /******************************************************************************/
 434
 435 /** Returns a threshold computed from the frame. */
 436 int countThreshold(const IplImage *frame) {
 437         return cvAvg(frame).val[0];
 438 }
 439
 440 /******************************************************************************/
 441
 442 /** Returns an ordinary number of recognized layout.
 443         * @param frame Float representation of frame. */
 444 int recognize(const CvMat *frame, CvMat **masks, int masksCnt) {
 445         double max = cvDotProduct(frame, masks[0]);
 446         int maxInd = 0;
 447         for(int i=1; i<masksCnt; i++)   {
 448                 double prod = cvDotProduct(frame, masks[i]);
 449                 if(prod > max) {
 450                         max = prod;
 451                         maxInd = i;
 452                 }
 453         }
 454         return maxInd;
 455 }
 456
 457 /******************************************************************************/
 458
 459 /** Loads binary (float) data to *mask from file.
 460  * @param filename Path to file to read.
 461  * @param mask Address of array, where to alloc memory and store the data.
 462  * @return Length of read data or zero in case of error. */
 463 int loadMask(const char *filename, CvMat **mask) {
 464         float *data;
 465         int len;
 466         FILE *pFile = fopen(filename, "rb");                                                                    //open binary file for reading
 467         if(!pFile) {
 468                 fprintf(stdout, "Mask %s cannot be loaded.\n", filename);
 469                 fprintf(stderr, "Run me from _build/user/camera/rozkuk\n");
 470                 return 0;
 471         }
 472
 473         *mask = cvCreateMat(FRAME_HEIGHT, FRAME_WIDTH, CV_32FC1);
 474         data = (*mask)->data.fl;
 475         len = fread((float *)data, sizeof(float), FRAME_SIZE, pFile);   //read data
 476
 477         fclose(pFile);
 478         return len;
 479 }
 480
 481 /******************************************************************************/
 482
 483 /** Loads all the float masks from files.
 484  * @return Zero if ok, else non-zero. */
 485 int loadMasks(char color) {
 486         char filename[100];
 487         //alloc memory for arrays of masks
 488         sideMasks = (CvMat **)malloc(SIDEMASKSCNT*sizeof(CvMat *));
 489         centerMasks = (CvMat **)malloc(CENTERMASKSCNT*sizeof(CvMat *));
 490         //load masks
 491         for(int i=0; i<SIDEMASKSCNT; i++) {
 492                 sprintf(filename, MASKFILENAME, i, orSIDE, color);
 493                 if(!loadMask(filename, sideMasks+i)) return ERR_MASK_READ_FAILURE;
 494         }
 495         for(int i=0; i<CENTERMASKSCNT; i++) {
 496                 sprintf(filename, MASKFILENAME, i, orCENTER, color);
 497                 if(!loadMask(filename, centerMasks+i)) return ERR_MASK_READ_FAILURE;
 498         }
 499         return 0;
 500 }
 501
 502 /******************************************************************************/
 503
 504 /** Frees the memory alloced for masks. */
 505 void freeMasks(void) {
 506         for(int i=0; i<SIDEMASKSCNT; i++) cvReleaseMat(sideMasks+i);
 507         for(int i=0; i<CENTERMASKSCNT; i++) cvReleaseMat(centerMasks+i);
 508         free(sideMasks);
 509         free(centerMasks);
 510 }
 511
 512 /******************************************************************************/
 513
 514 int main(int argc, char *argv[]) {
 515         int ret;
 516
 517         ret = robottype_roboorte_init(&orte);
 518         if(ret < 0) {
 519                 fprintf(stderr, "robottype_roboorte_init failed\n");
 520                 return ret;
 521         }
 522         robottype_publisher_camera_result_create(&orte, NULL, NULL);
 523         robottype_subscriber_camera_control_create(&orte, NULL, NULL);
 524
 525         modeManager(START_MODE);
 526
 527         ret = robottype_roboorte_destroy(&orte);
 528         return ret;
 529 }
 530