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 extern "C" {
  12 #include <inttypes.h>
  13 #include <stdlib.h>
  14 #include <stdio.h>
  15 #include <sys/stat.h>
  16 #include <semaphore.h>
  17 #include <getopt.h>
  18 #include <unistd.h>
  19
  20 #include <orte.h>
  21 //#include "roboorte.h"
  22 //#include "rozkorte.h"
  23 }
  24
  25 #include <opencv/cv.h>
  26 #include <opencv/highgui.h>
  27
  28 #include "clr2float.h"
  29
  30 /******************************************************************************/
  31
  32 // modes definitions
  33 #define MODE_QUIT                                       0x01
  34 #define MODE_REALTIME                   0x02
  35 #define MODE_RECOGNIZE          0x04
  36
  37 // mask of all modes / errors
  38 #define MASK_MODES                              0x07
  39 #define MASK_ERRORS                             (~MASK_MODES)
  40
  41 // errors
  42 #define ERR_MASK_READ_FAILURE   0x08
  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_p                                                   0x70
  66 #define KEY_r                                                   0x72
  67 #define KEY_s                                                   0x73
  68
  69 // number of frames passed in rozkuk mode before refresh
  70 #define PASS_FRAMES                             0
  71
  72 #endif                                                                          /*----------- DEBUG SESSION ONLY -----------*/
  73
  74 //default threshold and saturation
  75 #define THRESHOLD                                       70
  76 #define SATURATION                              1.0
  77
  78 //number of all masks
  79 #define MASKSCNT                                        5
  80 //mask filename pattern
  81 #define MASKFILENAME                    "../rozkuk_masks/mask%02d.bin"
  82
  83 //size of frames from camera
  84 #define FRAME_WIDTH                             640
  85 #define FRAME_HEIGHT                    480
  86 #define FRAME_SIZE                              (FRAME_WIDTH * FRAME_HEIGHT)
  87
  88 /******************************************************************************/
  89
  90 // function declarations
  91 #ifdef ROZKUK_DEBUG                             /************ DEBUG SESSION ONLY ************/
  92 void drawPauseText(IplImage *img);
  93 int saveFrame(const IplImage *img);
  94 void displayProduct(const CvMat *floatMat, int recognizedLayout);
  95 void displayFloatMat(const CvMat *floatMat, const char *windowName);
  96 int modeRealtime(CvCapture* capture);
  97 #endif                                                                          /*----------- DEBUG SESSION ONLY -----------*/
  98 int modeRecognize(CvCapture* capture);
  99 int modeManager(int defaultMode);
 100 int recognize(const CvMat *frame);
 101 int loadMask(const char *filename, CvMat **mask);
 102 int loadMasks(void);
 103 void freeMasks(void);
 104
 105 /******************************************************************************/
 106
 107 // variable declarations
 108 CvMat **masks=NULL;                     //float mask matrices
 109 #ifdef ROZKUK_DEBUG                             /************ DEBUG SESSION ONLY ************/
 110 CvFont font;                                            //text font
 111
 112 /******************************************************************************/
 113
 114 /** Writes text "pause" in the CAMERA window. */
 115 void drawPauseText(IplImage *img) {
 116         cvPutText(img,"pause", cvPoint(300,440), &font, cvScalar(255,255,255));
 117         cvShowImage(WINDOW_ORIG, img);
 118 }
 119
 120 /******************************************************************************/
 121
 122 /** Saves current image to a new file in the current directory. */
 123 int saveFrame(const IplImage *img) {
 124         struct stat stFileInfo;
 125         char filename[30] = "snapshot00.png";
 126         unsigned int i=0;
 127         //find a non-existent filename
 128         while(!stat(filename, &stFileInfo)) sprintf(filename, "snapshot%02d.png", ++i);
 129         //save image to file
 130         return cvSaveImage(filename, img);
 131 }
 132
 133 /******************************************************************************/
 134
 135 /** Displays in separate window the result of multiplying camera frame (in float)
 136   * and recognized mask.
 137   * @param floatMat Pointer to the camera frame converted to float matrix.
 138   * @param recognizedLayout Index of recognized situation. */
 139 void displayProduct(const CvMat *floatMat, int recognizedLayout) {
 140         CvMat *product = cvCloneMat(floatMat);
 141
 142         cvMul(floatMat, masks[recognizedLayout], product);              // count product
 143         displayFloatMat(product, WINDOW_PROD);
 144
 145         cvReleaseMat(&product);
 146 }
 147
 148 /******************************************************************************/
 149
 150 /** Normalizes the matrix values and displays them in window specified by name.
 151   * @param floatMat Pointer to matrix data to display.
 152   * @param windowName Name of window in which to display the image. */
 153 void displayFloatMat(const CvMat *floatMat, const char *windowName) {
 154         double min=0.0, max=0.0;
 155         CvMat *normalized = cvCloneMat(floatMat);
 156
 157         cvMinMaxLoc(floatMat, &min, &max);                                                                                      // find extremes
 158         cvConvertScale(floatMat, normalized, 1/(max-min), -(min/(max-min)));            // normalize
 159         cvShowImage(windowName, normalized);
 160
 161         cvReleaseMat(&normalized);
 162 }
 163
 164 /******************************************************************************/
 165
 166 /** Displays a real video in a window (only for DEBUG session).
 167  * @param capture Pointer to a camera capture.
 168  * @return Code of mode to switch to. */
 169 int modeRealtime(CvCapture* capture) {
 170   IplImage* frame = NULL;                                                                                                                               // frame from camera
 171   int pause=0;                                                                                                                                                                  // set pause=1 to pause the video
 172
 173         while(1) {
 174                 /* keyboard events handeling */
 175     switch(cvWaitKey(10) & 0xFF) {                                                                                      // wait 10ms for an event
 176     // stop capturing
 177     case KEY_ESC:
 178         return MODE_QUIT;
 179     // switch to recognize mode
 180     case KEY_R:
 181     case KEY_r:
 182         return MODE_RECOGNIZE;
 183     // (un)pause
 184     case KEY_P:
 185     case KEY_p:
 186         pause = !pause;
 187         drawPauseText(frame);
 188         break;
 189     // save frame to file
 190     case KEY_S:
 191     case KEY_s:
 192         saveFrame(frame);
 193         break;
 194     }
 195
 196     frame = cvQueryFrame(capture);                                                                                      // Get one frame
 197     if(!frame) {
 198       perror("NULL frame");
 199       continue;
 200     }
 201
 202     if(!pause) cvShowImage(WINDOW_ORIG, frame);                                 // show image (! Do not release the frame !)
 203         }
 204         return MODE_QUIT;
 205 }
 206 #endif                                                                          /*----------- DEBUG SESSION ONLY -----------*/
 207
 208 /******************************************************************************/
 209
 210 /** Implements the camera recognition of corns.
 211  * In DEBUG session the results are also displayed on screen.
 212  * @param capture Pointer to a camera capture.
 213  * @return Code of mode to switch to. */
 214 int modeRecognize(CvCapture* capture) {
 215         IplImage *frame = NULL;                                                                                                                         // frame from camera
 216   IplImage *thresFrame = NULL;                                                                                                  // thresholded frame
 217         CvMat *floatMat = NULL;                                                                                                                         // float <-1,1> image of the frame
 218   int layout;                                                                                                                                                                           // number of recognized situation
 219 #ifdef ROZKUK_DEBUG                             /************ DEBUG SESSION ONLY ************/
 220   int framesPassed = PASS_FRAMES;                                                                                               // number of passed frames before refresh
 221         int pause=0;                                                                                                                                                                    // set pause=1 to pause processing
 222 #endif                                                                          /*----------- DEBUG SESSION ONLY -----------*/
 223         while(1) {
 224 #ifdef ROZKUK_DEBUG                             /************ DEBUG SESSION ONLY ************/
 225                 /* keyboard events handeling */
 226     switch(cvWaitKey(10) & 0xFF) {                                                                                      // wait 10ms for an event
 227     // stop capturing
 228     case KEY_ESC:
 229         return MODE_QUIT;
 230     // skip the frame
 231     case KEY_SPACE:
 232         framesPassed = PASS_FRAMES;
 233         break;
 234     // switch to recognize mode
 235     case KEY_R:
 236     case KEY_r:
 237         return MODE_REALTIME;
 238     // (un)pause
 239     case KEY_P:
 240     case KEY_p:
 241         pause = !pause;
 242         drawPauseText(frame);
 243         break;
 244     // save frame to file
 245     case KEY_S:
 246     case KEY_s:
 247         saveFrame(frame);
 248         break;
 249     }
 250 #endif                                                                          /*----------- DEBUG SESSION ONLY -----------*/
 251
 252                 // Get one frame
 253 #ifdef ROZKUK_DEBUG                             /************ DEBUG SESSION ONLY ************/
 254                 if(pause) cvQueryFrame(capture);
 255                 else
 256 #endif                                                                          /*----------- DEBUG SESSION ONLY -----------*/
 257                         frame = cvQueryFrame(capture);
 258     if(!frame) {
 259       perror("NULL frame");
 260       continue;
 261     }
 262
 263 #ifdef ROZKUK_DEBUG                             /************ DEBUG SESSION ONLY ************/
 264                 if(pause) continue;
 265
 266     // transform colorful image to its float <-1,1> representation
 267     cvReleaseImage(&thresFrame);
 268         thresFrame = cvCreateImage(cvSize(640,480),IPL_DEPTH_8U,1);
 269 #endif                                                                          /*----------- DEBUG SESSION ONLY -----------*/
 270                 // convert to float
 271         clr2float(frame, &floatMat, THRESHOLD, SATURATION, thresFrame); // get float representation (in DEBUG session, thresholded image is stored to thresFrame)
 272         layout = recognize(floatMat);
 273                 printf("varianta: %d\n", layout);
 274 #ifdef ROZKUK_DEBUG                             /************ DEBUG SESSION ONLY ************/
 275         displayProduct(floatMat, layout);
 276 #endif                                                                          /*----------- DEBUG SESSION ONLY -----------*/
 277                 cvReleaseMat(&floatMat);
 278
 279 #ifdef ROZKUK_DEBUG                             /************ DEBUG SESSION ONLY ************/
 280                 if(framesPassed++ > PASS_FRAMES) {
 281             framesPassed=0;
 282             cvShowImage(WINDOW_ORIG, frame);                                                                    // show image (! Do not release the frame !)
 283         cvShowImage(WINDOW_THRES, thresFrame);
 284             displayFloatMat(masks[layout], WINDOW_MASK);                        // display selected mask
 285         cvReleaseImage(&thresFrame);
 286                 }
 287 #endif                                                                          /*----------- DEBUG SESSION ONLY -----------*/
 288
 289         }
 290         return MODE_QUIT;
 291 }
 292
 293 /******************************************************************************/
 294
 295 /** Switches between modes of the program.
 296  * @param defaultMode The first mode to run. */
 297 int modeManager(int defaultMode) {
 298         int mode=defaultMode;
 299   CvCapture* capture;
 300
 301         // connect to a camera
 302   while(!(capture=cvCaptureFromCAM(CV_CAP_ANY))) {
 303           perror("NULL capture");
 304     usleep(500*1000);
 305   }
 306
 307 #ifdef ROZKUK_DEBUG                             /************ DEBUG SESSION ONLY ************/
 308         // create gui windows and init font
 309   cvNamedWindow(WINDOW_ORIG, CV_WINDOW_AUTOSIZE);                               // real camera video / one frame viewer
 310         cvInitFont(&font, CV_FONT_HERSHEY_SIMPLEX, 1.0, 1.0, 0, 1);             //init text font
 311 #endif                                                                          /*----------- DEBUG SESSION ONLY -----------*/
 312
 313   while(!(mode & MODE_QUIT)) {
 314         switch(mode & MASK_MODES) {
 315
 316 #ifdef ROZKUK_DEBUG                             /************ DEBUG SESSION ONLY ************/
 317         case MODE_REALTIME:
 318                 cvDestroyWindow(WINDOW_THRES);
 319                 mode = modeRealtime(capture);
 320                 break;
 321 #endif                                                                          /*----------- DEBUG SESSION ONLY -----------*/
 322
 323         case MODE_RECOGNIZE:
 324 #ifdef ROZKUK_DEBUG                             /************ DEBUG SESSION ONLY ************/
 325                 cvNamedWindow(WINDOW_THRES, CV_WINDOW_AUTOSIZE);
 326                 cvMoveWindow(WINDOW_THRES, cvQueryFrame(capture)->width, 0);
 327                 cvNamedWindow(WINDOW_MASK, CV_WINDOW_AUTOSIZE);
 328                 cvMoveWindow(WINDOW_MASK, 0, cvQueryFrame(capture)->height+50);
 329                 cvNamedWindow(WINDOW_PROD, CV_WINDOW_AUTOSIZE);
 330                 cvMoveWindow(WINDOW_PROD, cvQueryFrame(capture)->width, cvQueryFrame(capture)->height+50);
 331 #endif                                                                          /*----------- DEBUG SESSION ONLY -----------*/
 332                 mode = modeRecognize(capture);
 333                 break;
 334
 335         default:
 336                 goto end;                       // jump out of the loop
 337         }
 338   }
 339
 340 end:
 341   cvReleaseCapture(&capture);
 342 #ifdef ROZKUK_DEBUG                             /************ DEBUG SESSION ONLY ************/
 343   cvDestroyWindow(WINDOW_ORIG);
 344   cvDestroyWindow(WINDOW_THRES);
 345   cvDestroyWindow(WINDOW_MASK);
 346   cvDestroyWindow(WINDOW_PROD);
 347 #endif                                                                          /*----------- DEBUG SESSION ONLY -----------*/
 348   return mode & MASK_ERRORS;
 349 }
 350
 351 /******************************************************************************/
 352
 353 /** Returns an ordinary number of recognized layout.
 354         * @param frame Float representation of frame. */
 355 int recognize(const CvMat *frame) {
 356         double max = cvDotProduct(frame, masks[0]);
 357         int maxInd = 0;
 358         for(int i=1; i<MASKSCNT; i++)   {
 359                 double prod = cvDotProduct(frame, masks[i]);
 360                 if(prod > max) {
 361                         max = prod;
 362                         maxInd = i;
 363                 }
 364         }
 365         return maxInd;
 366 }
 367
 368 /******************************************************************************/
 369
 370 /** Loads binary (float) data to *mask from file.
 371  * @param filename Path to file to read.
 372  * @param mask Address of array, where to alloc memory and store the data.
 373  * @return Length of read data or zero in case of error. */
 374 int loadMask(const char *filename, CvMat **mask) {
 375         float *data;
 376         int len;
 377         FILE *pFile = fopen(filename, "rb");                                                                    //open binary file for reading
 378         if(!pFile) {
 379                 char msg[200];
 380                 sprintf(msg, "Mask %s cannot be loaded.", filename);
 381                 perror(msg);
 382                 return 0;
 383         }
 384
 385         *mask = cvCreateMat(FRAME_HEIGHT, FRAME_WIDTH, CV_32FC1);
 386         data = (*mask)->data.fl;
 387         len = fread((float *)data, sizeof(float), FRAME_SIZE, pFile);   //read data
 388
 389         fclose(pFile);
 390         return len;
 391 }
 392
 393 /******************************************************************************/
 394
 395 /** Loads all the float masks from files.
 396  * @return Zero if ok, else non-zero. */
 397 int loadMasks(void) {
 398         masks = (CvMat **)malloc(MASKSCNT*sizeof(CvMat *));             //alloc memory for array of masks
 399         for(int i=0; i<MASKSCNT; i++) {
 400                 char filename[100];
 401                 sprintf(filename, MASKFILENAME, i);
 402                 if(!loadMask(filename, masks+i)) return ERR_MASK_READ_FAILURE;
 403         }
 404         return 0;
 405 }
 406
 407 /******************************************************************************/
 408
 409 /** Frees the memory alloced for masks. */
 410 void freeMasks(void) {
 411         for(int i=0; i<MASKSCNT; i++) {
 412                 cvReleaseMat(masks+i);
 413         }
 414         free(masks);
 415 }
 416
 417 /******************************************************************************/
 418
 419 int main(int argc, char *argv[]) {
 420         int ret;
 421 /*  while (orte_init()) {
 422     perror("orte_init");
 423     usleep(500*1000);
 424   }
 425 */
 426         if((ret=loadMasks())) return ret;
 427         ret = modeManager(START_MODE);
 428         freeMasks();
 429         return ret;
 430 }
 431