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 /******************************************************************************/
32 #define MODE_QUIT 0x01
33 #define MODE_REALTIME 0x02
34 #define MODE_RECOGNIZE 0x04
35 #define MODE_WAIT 0x08
37 // mask of all modes / errors
38 #define MASK_MODES 0x0F
39 #define MASK_ERRORS (~MASK_MODES)
42 #define ERR_MASK_READ_FAILURE 0x10
44 // default mode in standard session
45 #define START_MODE MODE_RECOGNIZE
47 #ifdef ROZKUK_DEBUG /************ DEBUG SESSION ONLY ************/
49 // default mode in debug session
51 #define START_MODE MODE_REALTIME
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"
61 #define KEY_SPACE 0x20
71 // number of frames passed in rozkuk mode before refresh
74 #endif /*----------- DEBUG SESSION ONLY -----------*/
76 //default threshold and saturation
78 #define SATURATION 1.0
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 "/dev/flash/mask%02d%c%c.bin" // FIXME: insert correct filepath mask
85 #endif /*------------------------------------------*/
87 //size of frames from camera
88 #define FRAME_WIDTH 640
89 #define FRAME_HEIGHT 480
90 #define FRAME_SIZE (FRAME_WIDTH * FRAME_HEIGHT)
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)
97 struct robottype_orte_data orte;
99 /******************************************************************************/
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);
119 /******************************************************************************/
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
127 /******************************************************************************/
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);
135 /******************************************************************************/
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";
142 //find a non-existent filename
143 while(!stat(filename, &stFileInfo)) sprintf(filename, "snapshot%02d.png", ++i);
145 return cvSaveImage(filename, img);
148 /******************************************************************************/
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);
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
163 cvReleaseMat(&sideProduct);
164 cvReleaseMat(¢erProduct);
167 /******************************************************************************/
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);
173 cvAdd(mat1, mat2, sum);
174 displayFloatMat(sum, windowName);
179 /******************************************************************************/
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);
188 cvMinMaxLoc(floatMat, &min, &max); // find extremes
189 cvConvertScale(floatMat, normalized, 1/(max-min), -(min/(max-min))); // normalize
190 cvShowImage(windowName, normalized);
192 cvReleaseMat(&normalized);
195 /******************************************************************************/
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
205 /* keyboard events handeling */
206 switch(cvWaitKey(10) & 0xFF) { // wait 10ms for an event
210 // switch to recognize mode
213 return MODE_RECOGNIZE;
218 drawPauseText(frame);
220 // save frame to file
227 frame = cvQueryFrame(capture); // Get one frame
229 perror("NULL frame");
233 if(!pause) cvShowImage(WINDOW_ORIG, frame); // show image (! Do not release the frame !)
237 #endif /*----------- DEBUG SESSION ONLY -----------*/
239 /******************************************************************************/
241 /** Implements the camera recognition of corns.
242 * In DEBUG session the results are also displayed on screen.
243 * @param capture Pointer to a camera capture.
244 * @return Code of mode to switch to. */
245 int modeRecognize(CvCapture* capture) {
246 IplImage *frame = NULL; // frame from camera
247 IplImage *thresFrame = NULL; // thresholded frame
248 CvMat *floatMat = NULL; // float <-1,1> image of the frame
249 int sideLayout, centerLayout; // indices of recognized situation
250 #ifdef ROZKUK_DEBUG /************ DEBUG SESSION ONLY ************/
251 int framesPassed = PASS_FRAMES; // number of passed frames before refresh
252 int pause=0; // set pause=1 to pause processing
253 #endif /*----------- DEBUG SESSION ONLY -----------*/
256 #ifdef ROZKUK_DEBUG /************ DEBUG SESSION ONLY ************/
257 /* keyboard events handeling */
258 switch(cvWaitKey(10) & 0xFF) { // wait 10ms for an event
264 framesPassed = PASS_FRAMES;
266 // switch to recognize mode
269 return MODE_REALTIME;
274 drawPauseText(frame);
276 // save frame to file
286 #endif /*----------- DEBUG SESSION ONLY -----------*/
289 #ifdef ROZKUK_DEBUG /************ DEBUG SESSION ONLY ************/
290 if(pause) cvQueryFrame(capture);
292 #endif /*----------- DEBUG SESSION ONLY -----------*/
293 frame = cvQueryFrame(capture);
295 perror("NULL frame");
299 #ifdef ROZKUK_DEBUG /************ DEBUG SESSION ONLY ************/
302 // transform colorful image to its float <-1,1> representation
303 cvReleaseImage(&thresFrame);
304 thresFrame = cvCreateImage(cvSize(640,480),IPL_DEPTH_8U,1);
305 #endif /*----------- DEBUG SESSION ONLY -----------*/
307 int threshold = countThreshold(frame);
308 clr2float(frame, &floatMat, threshold, SATURATION, thresFrame); // get float representation (in DEBUG session, thresholded image is stored to thresFrame)
309 //recognize side and center layouts
310 sideLayout = recognize(floatMat, sideMasks, SIDEMASKSCNT);
311 centerLayout = recognize(floatMat, centerMasks, CENTERMASKSCNT);
313 printf("thr: %d, side: %d, center: %d\n", threshold, sideLayout, centerLayout);
314 orte.camera_result.side = sideLayout;
315 orte.camera_result.center = centerLayout;
316 ORTEPublicationSend(orte.publication_camera_result);
318 #ifdef ROZKUK_DEBUG /************ DEBUG SESSION ONLY ************/
319 displayProduct(floatMat, sideLayout, centerLayout);
320 #endif /*----------- DEBUG SESSION ONLY -----------*/
321 cvReleaseMat(&floatMat);
323 #ifdef ROZKUK_DEBUG /************ DEBUG SESSION ONLY ************/
324 if(framesPassed++ > PASS_FRAMES) {
326 cvShowImage(WINDOW_ORIG, frame); // show image (! Do not release the frame !)
327 cvShowImage(WINDOW_THRES, thresFrame);
328 displayMasksSum(sideMasks[sideLayout], centerMasks[centerLayout], WINDOW_MASK); // display selected mask
329 cvReleaseImage(&thresFrame);
331 #endif /*----------- DEBUG SESSION ONLY -----------*/
337 /******************************************************************************/
339 /** Waits while no action is wanted. To stop waiting set orte.camera_control.on=TRUE
340 * or in DEBUG session press W key.
341 * @param capture Pointer to a camera capture.
342 * @return Code of mode to switch to. */
343 int modeWait(CvCapture *capture) {
344 while(!orte.camera_control.on) {
345 #ifdef ROZKUK_DEBUG /************ DEBUG SESSION ONLY ************/
346 //process keyboard events
347 switch(cvWaitKey(10) & 0xFF) { // wait 10ms for an event
355 return MODE_RECOGNIZE;
358 #endif /*----------- DEBUG SESSION ONLY -----------*/
359 printf("waiting...\n");
362 return MODE_RECOGNIZE;
365 /******************************************************************************/
367 /** Switches between modes of the program.
368 * @param defaultMode The first mode to run. */
369 int modeManager(int defaultMode) {
370 int mode=defaultMode;
374 // connect to a camera
375 while(!(capture=cvCaptureFromCAM(CV_CAP_ANY))) {
376 perror("NULL capture");
380 #ifdef ROZKUK_DEBUG /************ DEBUG SESSION ONLY ************/
381 // create gui windows and init font
382 cvNamedWindow(WINDOW_ORIG, CV_WINDOW_AUTOSIZE); // real camera video / one frame viewer
383 cvInitFont(&font, CV_FONT_HERSHEY_SIMPLEX, 1.0, 1.0, 0, 1); //init text font
384 #endif /*----------- DEBUG SESSION ONLY -----------*/
386 while(!(mode & MODE_QUIT)) {
387 switch(mode & MASK_MODES) {
389 #ifdef ROZKUK_DEBUG /************ DEBUG SESSION ONLY ************/
391 cvDestroyWindow(WINDOW_THRES);
392 mode = modeRealtime(capture);
394 #endif /*----------- DEBUG SESSION ONLY -----------*/
397 #ifdef ROZKUK_DEBUG /************ DEBUG SESSION ONLY ************/
398 cvNamedWindow(WINDOW_THRES, CV_WINDOW_AUTOSIZE);
399 cvMoveWindow(WINDOW_THRES, cvQueryFrame(capture)->width, 0);
400 cvNamedWindow(WINDOW_MASK, CV_WINDOW_AUTOSIZE);
401 cvMoveWindow(WINDOW_MASK, 0, cvQueryFrame(capture)->height+50);
402 cvNamedWindow(WINDOW_PROD, CV_WINDOW_AUTOSIZE);
403 cvMoveWindow(WINDOW_PROD, cvQueryFrame(capture)->width, cvQueryFrame(capture)->height+50);
404 #endif /*----------- DEBUG SESSION ONLY -----------*/
405 //load masks and run recognition mode, then free masks (they are in memory only while in recognition mode)
406 if((ret=loadMasks((orte.camera_control.game_color==BLUE ? clBLUE : clYELLOW)))) return ret;
407 mode = modeRecognize(capture);
412 mode = modeWait(capture);
416 goto end; // jump out of the loop
421 cvReleaseCapture(&capture);
422 #ifdef ROZKUK_DEBUG /************ DEBUG SESSION ONLY ************/
423 cvDestroyWindow(WINDOW_ORIG);
424 cvDestroyWindow(WINDOW_THRES);
425 cvDestroyWindow(WINDOW_MASK);
426 cvDestroyWindow(WINDOW_PROD);
427 #endif /*----------- DEBUG SESSION ONLY -----------*/
428 return mode & MASK_ERRORS;
431 /******************************************************************************/
433 /** Returns a threshold computed from the frame. */
434 int countThreshold(const IplImage *frame) {
435 return cvAvg(frame).val[0];
438 /******************************************************************************/
440 /** Returns an ordinary number of recognized layout.
441 * @param frame Float representation of frame. */
442 int recognize(const CvMat *frame, CvMat **masks, int masksCnt) {
443 double max = cvDotProduct(frame, masks[0]);
445 for(int i=1; i<masksCnt; i++) {
446 double prod = cvDotProduct(frame, masks[i]);
455 /******************************************************************************/
457 /** Loads binary (float) data to *mask from file.
458 * @param filename Path to file to read.
459 * @param mask Address of array, where to alloc memory and store the data.
460 * @return Length of read data or zero in case of error. */
461 int loadMask(const char *filename, CvMat **mask) {
464 FILE *pFile = fopen(filename, "rb"); //open binary file for reading
467 sprintf(msg, "Mask %s cannot be loaded.", filename);
469 fprintf(stderr, "Run me from _build/user/camera/rozkuk\n");
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
481 /******************************************************************************/
483 /** Loads all the float masks from files.
484 * @return Zero if ok, else non-zero. */
485 int loadMasks(char color) {
487 //alloc memory for arrays of masks
488 sideMasks = (CvMat **)malloc(SIDEMASKSCNT*sizeof(CvMat *));
489 centerMasks = (CvMat **)malloc(CENTERMASKSCNT*sizeof(CvMat *));
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;
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;
502 /******************************************************************************/
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);
512 /******************************************************************************/
514 int main(int argc, char *argv[]) {
517 ret = robottype_roboorte_init(&orte);
519 fprintf(stderr, "robottype_roboorte_init failed\n");
522 robottype_publisher_camera_result_create(&orte, NULL, NULL);
523 robottype_subscriber_camera_control_create(&orte, NULL, NULL);
525 modeManager(START_MODE);
527 ret = robottype_roboorte_destroy(&orte);