1 /*******************************************************************************
3 * rozkuk - OpenCV based corns camera recognition program
5 * Created for Eurobot 2010 competition.
7 * Petr Kubizňák (kubiznak.petr@gmail.com), 2010
9 ******************************************************************************/
15 #include <semaphore.h>
19 #include <opencv/cv.h>
20 #include <opencv/highgui.h>
22 #include "clr2float.h"
23 #include "masks_globals.h"
26 #include <roboorte_robottype.h>
29 /******************************************************************************/
31 // uncomment next line to "log" the output frames - save them as pnm to the working directory
35 #define MODE_QUIT 0x01
36 #define MODE_REALTIME 0x02
37 #define MODE_RECOGNIZE 0x04
38 #define MODE_WAIT 0x08
40 // mask of all modes / errors
41 #define MASK_MODES 0x0F
42 #define MASK_ERRORS (~MASK_MODES)
45 #define ERR_MASK_READ_FAILURE 0x10
47 // default mode in standard session
48 #define START_MODE MODE_RECOGNIZE
50 #ifdef ROZKUK_DEBUG /************ DEBUG SESSION ONLY ************/
52 // default mode in debug session
54 #define START_MODE MODE_REALTIME
56 // highgui windows names
57 #define WINDOW_ORIG "ROZKUK original scene"
58 #define WINDOW_THRES "ROZKUK threshold"
59 #define WINDOW_MASK "ROZKUK mask"
60 #define WINDOW_PROD "ROZKUK product"
64 #define KEY_SPACE 0x20
74 // number of frames passed in rozkuk mode before refresh
77 #endif /*----------- DEBUG SESSION ONLY -----------*/
79 //default threshold and saturation
81 #define SATURATION 1.0
83 //mask filename pattern
84 #ifdef ROZKUK_DEBUG /************ DEBUG SESSION ONLY ************/
85 #define MASKFILENAME "./mask%02d%c%c.pnm"
86 #else /******************** PPC *******************/
87 #define MASKFILENAME "./mask%02d%c%c.pnm"
88 #endif /*------------------------------------------*/
90 //size of frames from camera
91 #define FRAME_WIDTH 640
92 #define FRAME_HEIGHT 480
93 #define FRAME_SIZE (FRAME_WIDTH * FRAME_HEIGHT)
95 //both (side and center) subresults will be returned in one int masked binary as 00ccssss
96 #define MASK_SIDE 0x0F
97 #define MASK_CENTER_SHIFT 4
98 #define MASK_CENTER (0x03 << 4)
100 struct robottype_orte_data orte;
102 /******************************************************************************/
104 // function declarations
105 #ifdef ROZKUK_DEBUG /************ DEBUG SESSION ONLY ************/
106 int saveFrame(const CvArr *img);
107 void drawPauseText(IplImage *img);
108 void displayProduct(const CvMat *floatMat, int sideConfig, int centerConfig);
109 void displayMasksSum(const CvMat *mat1, const CvMat *mat2, const char *windowName);
110 void displayFloatMat(const CvMat *floatMat, const char *windowName);
111 int modeRealtime(CvCapture* capture);
112 #endif /*----------- DEBUG SESSION ONLY -----------*/
113 int modeRecognize(CvCapture* capture);
114 int modeWait(CvCapture *capture);
115 int modeManager(int defaultMode);
116 int countThreshold(const IplImage *frame);
117 int recognize(const CvMat *frame, CvMat **masks, int masksCnt, double *resDist=NULL);
118 int loadMask(const char *filename, CvMat **mask);
119 int loadMasks(char color);
120 void freeMasks(void);
122 /******************************************************************************/
124 // variable declarations
125 CvMat **sideMasks=NULL; //float mask matrices
126 CvMat **centerMasks=NULL;
127 #ifdef ROZKUK_DEBUG /************ DEBUG SESSION ONLY ************/
128 CvFont font; //text font
129 #endif /*----------- DEBUG SESSION ONLY -----------*/
131 /******************************************************************************/
133 #if defined (ROZKUK_DEBUG) || defined (PPC_DEBUG) /************ BOTH DEBUG SESSIONS ************/
135 /** Saves current image to a new file in the current directory. */
136 int saveFrame(const CvArr *img) {
137 struct stat stFileInfo;
140 //find a non-existent filename
141 #if defined (ROZKUK_DEBUG)
142 char filename[30] = "snapshot00.png";
143 while(!stat(filename, &stFileInfo)) sprintf(filename, "snapshot%02d.png", ++i);
145 char filename[30] = "snapshot00.pnm";
146 while(!stat(filename, &stFileInfo)) sprintf(filename, "snapshot%02d.pnm", ++i);
149 fprintf(stdout, "Saving frame to %s...\n", filename);
151 return cvSaveImage(filename, img);
154 #endif /*----------- BOTH DEBUG SESSIONS -----------*/
156 /******************************************************************************/
158 #ifdef ROZKUK_DEBUG /************ DEBUG SESSION ONLY ************/
160 /** Writes text "pause" in the CAMERA window. */
161 void drawPauseText(IplImage *img) {
162 cvPutText(img,"pause", cvPoint(300,440), &font, cvScalar(255,255,255));
163 cvShowImage(WINDOW_ORIG, img);
166 /******************************************************************************/
168 /** Displays in separate window the result of multiplying camera frame (in float)
169 * and recognized mask.
170 * @param floatMat Pointer to the camera frame converted to float matrix.
171 * @param sideConfig Index of recognized side situation.
172 * @param centerConfig Index of recognized center situation. */
173 void displayProduct(const CvMat *floatMat, int sideConfig, int centerConfig) {
174 CvMat *sideProduct = cvCloneMat(floatMat);
175 CvMat *centerProduct = cvCloneMat(floatMat);
177 cvMul(floatMat, sideMasks[sideConfig], sideProduct); //count product of frame and side mask
178 cvMul(floatMat, centerMasks[centerConfig], centerProduct); //count product of frame and center mask
179 displayMasksSum(sideProduct, centerProduct, WINDOW_PROD); //display sum
181 cvReleaseMat(&sideProduct);
182 cvReleaseMat(¢erProduct);
185 /******************************************************************************/
187 /** Displays a sum of two matrices in specified window. */
188 void displayMasksSum(const CvMat *mat1, const CvMat *mat2, const char *windowName) {
189 CvMat *sum = cvCloneMat(mat1);
191 cvAdd(mat1, mat2, sum);
192 displayFloatMat(sum, windowName);
197 /******************************************************************************/
199 /** Normalizes the matrix values and displays them in window specified by name.
200 * @param floatMat Pointer to matrix data to display.
201 * @param windowName Name of window in which to display the image. */
202 void displayFloatMat(const CvMat *floatMat, const char *windowName) {
203 double min=0.0, max=0.0;
204 CvMat *normalized = cvCloneMat(floatMat);
206 cvMinMaxLoc(floatMat, &min, &max); // find extremes
207 cvConvertScale(floatMat, normalized, 1/(max-min), -(min/(max-min))); // normalize
208 cvShowImage(windowName, normalized);
210 cvReleaseMat(&normalized);
213 /******************************************************************************/
215 /** Displays a real video in a window (only for DEBUG session).
216 * @param capture Pointer to a camera capture.
217 * @return Code of mode to switch to. */
218 int modeRealtime(CvCapture* capture) {
219 IplImage* frame = NULL; // frame from camera
220 int pause=0; // set pause=1 to pause the video
223 /* keyboard events handeling */
224 switch(cvWaitKey(10) & 0xFF) { // wait 10ms for an event
228 // switch to recognize mode
231 return MODE_RECOGNIZE;
236 drawPauseText(frame);
238 // save frame to file
245 frame = cvQueryFrame(capture); // Get one frame
247 fprintf(stdout, "NULL frame\n");
251 if(!pause) cvShowImage(WINDOW_ORIG, frame); // show image (! Do not release the frame !)
255 #endif /*----------- DEBUG SESSION ONLY -----------*/
257 /******************************************************************************/
259 /** Implements the camera recognition of corns.
260 * In DEBUG session the results are also displayed on screen.
261 * @param capture Pointer to a camera capture.
262 * @return Code of mode to switch to. */
263 int modeRecognize(CvCapture* capture) {
264 IplImage *frame = NULL; // frame from camera
265 IplImage *thresFrame = NULL; // thresholded frame
266 CvMat *floatMat = NULL; // float <-1,1> image of the frame
267 int sideConfig, centerConfig; // indices of recognized configurations
268 double sideDist, centerDist; // distances between the best and second best results
269 #ifdef ROZKUK_DEBUG /************ DEBUG SESSION ONLY ************/
270 int framesPassed = PASS_FRAMES; // number of passed frames before refresh
271 int pause=0; // set pause=1 to pause processing
272 #endif /*----------- DEBUG SESSION ONLY -----------*/
273 while(1) { // TODO: test on ~ camera.running.on
275 #ifdef ROZKUK_DEBUG /************ DEBUG SESSION ONLY ************/
276 /* keyboard events handeling */
277 switch(cvWaitKey(10) & 0xFF) { // wait 10ms for an event
283 framesPassed = PASS_FRAMES;
285 // switch to recognize mode
288 return MODE_REALTIME;
293 drawPauseText(frame);
295 // save frame to file
305 #endif /*----------- DEBUG SESSION ONLY -----------*/
308 #ifdef ROZKUK_DEBUG /************ DEBUG SESSION ONLY ************/
309 if(pause) cvQueryFrame(capture);
311 #endif /*----------- DEBUG SESSION ONLY -----------*/
312 frame = cvQueryFrame(capture);
314 fprintf(stdout, "NULL frame\n");
318 #ifdef ROZKUK_DEBUG /************ DEBUG SESSION ONLY ************/
321 // transform colorful image to its float <-1,1> representation
322 cvReleaseImage(&thresFrame);
323 thresFrame = cvCreateImage(cvSize(640,480),IPL_DEPTH_8U,1);
324 #endif /*----------- DEBUG SESSION ONLY -----------*/
326 int threshold = countThreshold(frame);
327 clr2float(frame, &floatMat, threshold, SATURATION, thresFrame); // get float representation (in DEBUG session, thresholded image is stored to thresFrame)
328 //recognize side and center layouts
329 sideConfig = recognize(floatMat, sideMasks, SIDEMASKSCNT, &sideDist);
330 centerConfig = recognize(floatMat, centerMasks, CENTERMASKSCNT, ¢erDist);
332 fprintf(stdout, "thr: %d, side: %d (%f), center: %d (%f)\n", threshold, sideConfig, sideDist, centerConfig, centerDist);
333 orte.camera_result.side = sideConfig;
334 orte.camera_result.center = centerConfig;
335 // TODO: do propagate also the distances
336 ORTEPublicationSend(orte.publication_camera_result);
338 #ifdef ROZKUK_DEBUG /************ DEBUG SESSION ONLY ************/
339 displayProduct(floatMat, sideConfig, centerConfig);
340 #elif defined (PPC_DEBUG) ///////////// PPC DEBUG SESSION //////////////
342 #endif /*----------- DEBUG SESSION ONLY -----------*/
343 cvReleaseMat(&floatMat);
345 #ifdef ROZKUK_DEBUG /************ DEBUG SESSION ONLY ************/
346 if(framesPassed++ > PASS_FRAMES) {
348 cvShowImage(WINDOW_ORIG, frame); // show image (! Do not release the frame !)
349 cvShowImage(WINDOW_THRES, thresFrame);
350 displayMasksSum(sideMasks[sideConfig], centerMasks[centerConfig], WINDOW_MASK); // display selected mask
351 cvReleaseImage(&thresFrame);
353 #endif /*----------- DEBUG SESSION ONLY -----------*/
359 /******************************************************************************/
361 /** Waits while no action is wanted. To stop waiting set orte.camera_control.on=TRUE
362 * or in DEBUG session press W key.
363 * @param capture Pointer to a camera capture.
364 * @return Code of mode to switch to. */
365 int modeWait(CvCapture *capture) {
366 while(!orte.camera_control.on) {
367 #ifdef ROZKUK_DEBUG /************ DEBUG SESSION ONLY ************/
368 //process keyboard events
369 switch(cvWaitKey(10) & 0xFF) { // wait 10ms for an event
377 return MODE_RECOGNIZE;
380 #endif /*----------- DEBUG SESSION ONLY -----------*/
381 fprintf(stdout, "waiting...\n");
384 return MODE_RECOGNIZE;
387 /******************************************************************************/
389 /** Switches between modes of the program.
390 * @param defaultMode The first mode to run. */
391 int modeManager(int defaultMode) {
392 int mode=defaultMode;
396 // connect to a camera
397 while(!((capture=cvCaptureFromCAM(0)) || (capture=cvCaptureFromCAM(1)))) {
398 fprintf(stdout, "NULL capture - no /dev/video# found\n");
402 #ifdef ROZKUK_DEBUG /************ DEBUG SESSION ONLY ************/
403 // create gui windows and init font
404 cvNamedWindow(WINDOW_ORIG, CV_WINDOW_AUTOSIZE); // real camera video / one frame viewer
405 cvInitFont(&font, CV_FONT_HERSHEY_SIMPLEX, 1.0, 1.0, 0, 1); //init text font
406 #endif /*----------- DEBUG SESSION ONLY -----------*/
408 while(!(mode & MODE_QUIT)) {
409 switch(mode & MASK_MODES) {
411 #ifdef ROZKUK_DEBUG /************ DEBUG SESSION ONLY ************/
413 cvDestroyWindow(WINDOW_THRES);
414 mode = modeRealtime(capture);
416 #endif /*----------- DEBUG SESSION ONLY -----------*/
419 #ifdef ROZKUK_DEBUG /************ DEBUG SESSION ONLY ************/
420 cvNamedWindow(WINDOW_THRES, CV_WINDOW_AUTOSIZE);
421 cvMoveWindow(WINDOW_THRES, cvQueryFrame(capture)->width, 0);
422 cvNamedWindow(WINDOW_MASK, CV_WINDOW_AUTOSIZE);
423 cvMoveWindow(WINDOW_MASK, 0, cvQueryFrame(capture)->height+50);
424 cvNamedWindow(WINDOW_PROD, CV_WINDOW_AUTOSIZE);
425 cvMoveWindow(WINDOW_PROD, cvQueryFrame(capture)->width, cvQueryFrame(capture)->height+50);
426 #endif /*----------- DEBUG SESSION ONLY -----------*/
427 //load masks and run recognition mode, then free masks (they are in memory only while in recognition mode)
428 if((ret=loadMasks((orte.camera_control.game_color==BLUE ? clBLUE : clYELLOW)))) return ret;
429 mode = modeRecognize(capture);
434 mode = modeWait(capture);
438 goto end; // jump out of the loop
443 cvReleaseCapture(&capture);
444 #ifdef ROZKUK_DEBUG /************ DEBUG SESSION ONLY ************/
445 cvDestroyWindow(WINDOW_ORIG);
446 cvDestroyWindow(WINDOW_THRES);
447 cvDestroyWindow(WINDOW_MASK);
448 cvDestroyWindow(WINDOW_PROD);
449 #endif /*----------- DEBUG SESSION ONLY -----------*/
450 return mode & MASK_ERRORS;
453 /******************************************************************************/
455 /** Returns a threshold computed from the frame. */
456 int countThreshold(const IplImage *frame) {
457 return cvAvg(frame).val[0];
460 /******************************************************************************/
462 /** Returns an ordinary number of recognized layout.
463 * @param frame Float representation of frame.
464 * @param masks Array of masks to compare with.
465 * @param masksCnt Size of the array.
466 * @param resDist If not NULL, a distance between the best and second best result is stored here.*/
467 int recognize(const CvMat *frame, CvMat **masks, int masksCnt, double *resDist) {
468 double max = cvDotProduct(frame, masks[0]);
469 double max2 = -(FRAME_SIZE); // the least possible value
471 for(int i=1; i<masksCnt; i++) {
472 double prod = cvDotProduct(frame, masks[i]);
477 } else if(prod > max2) max2 = prod;
479 if(resDist) *resDist = max-max2;
483 /******************************************************************************/
485 /** Loads binary (float) data to *mask from file.
486 * @param filename Path to file to read.
487 * @param mask Address of array, where to alloc memory and store the data.
488 * @return Length of read data or zero in case of error. */
489 int loadMask(const char *filename, CvMat **mask) {
492 IplImage *img = cvLoadImage(filename, CV_LOAD_IMAGE_GRAYSCALE);
494 fprintf(stdout, "Mask %s cannot be loaded.\n", filename);
495 fprintf(stderr, "Run me from _build/user/camera/rozkuk\n");
500 if(!(len=clr2float(img, mask, CL_NEUTRAL, 1.0, NULL))) {
501 fprintf(stderr, "A problem while converting mask to float occured.\n");
503 fprintf(stdout, "Mask %s successfully loaded.\n", filename);
505 cvReleaseImage(&img);
509 /******************************************************************************/
511 /** Loads all the float masks from files.
512 * @return Zero if ok, else non-zero. */
513 int loadMasks(char color) {
515 //alloc memory for arrays of masks
516 sideMasks = (CvMat **)malloc(SIDEMASKSCNT*sizeof(CvMat *));
517 centerMasks = (CvMat **)malloc(CENTERMASKSCNT*sizeof(CvMat *));
519 for(int i=0; i<SIDEMASKSCNT; i++) {
520 sprintf(filename, MASKFILENAME, i, orSIDE, color);
521 if(!loadMask(filename, sideMasks+i)) return ERR_MASK_READ_FAILURE;
523 for(int i=0; i<CENTERMASKSCNT; i++) {
524 sprintf(filename, MASKFILENAME, i, orCENTER, color);
525 if(!loadMask(filename, centerMasks+i)) return ERR_MASK_READ_FAILURE;
530 /******************************************************************************/
532 /** Frees the memory alloced for masks. */
533 void freeMasks(void) {
534 for(int i=0; i<SIDEMASKSCNT; i++) cvReleaseMat(sideMasks+i);
535 for(int i=0; i<CENTERMASKSCNT; i++) cvReleaseMat(centerMasks+i);
540 /******************************************************************************/
542 int main(int argc, char *argv[]) {
545 ret = robottype_roboorte_init(&orte);
547 fprintf(stderr, "robottype_roboorte_init failed\n");
550 robottype_publisher_camera_result_create(&orte, NULL, NULL);
551 robottype_subscriber_camera_control_create(&orte, NULL, NULL);
553 modeManager(START_MODE);
555 ret = robottype_roboorte_destroy(&orte);