*
******************************************************************************/
-extern "C" {
#include <inttypes.h>
#include <stdlib.h>
#include <stdio.h>
#include <getopt.h>
#include <unistd.h>
-#include <orte.h>
-//#include "roboorte.h"
-//#include "rozkorte.h"
-}
-
#include <opencv/cv.h>
#include <opencv/highgui.h>
#include "clr2float.h"
+#include "masks_globals.h"
+#include "t_decision_box.h"
+extern "C" {
+#include <roboorte_robottype.h>
+#include <robot.h>
+}
/******************************************************************************/
+// uncomment next line to "log" the output frames - save them as pnm to the working directory
+//#define PPC_DEBUG
+
// modes definitions
-#define MODE_QUIT 0x01
-#define MODE_REALTIME 0x02
-#define MODE_RECOGNIZE 0x04
+#define MODE_QUIT 0x01
+#define MODE_REALTIME 0x02
+#define MODE_RECOGNIZE 0x04
+#define MODE_WAIT 0x08
// mask of all modes / errors
-#define MASK_MODES 0x07
-#define MASK_ERRORS (~MASK_MODES)
+#define MASK_MODES 0x0F
+#define MASK_ERRORS (~MASK_MODES)
// errors
-#define ERR_MASK_READ_FAILURE 0x08
+#define ERR_MASK_READ_FAILURE 0x10
-// default mode in standard session
-#define START_MODE MODE_RECOGNIZE
+// default mode in standard session (i.e. on ppc)
+#define START_MODE MODE_WAIT
-#ifdef ROZKUK_DEBUG /************ DEBUG SESSION ONLY ************/
+#ifdef ROZKUK_DEBUG /************ DEBUG SESSION ONLY ************/
-// default mode in debug session
+// default mode in debug session (i.e. on pc)
#undef START_MODE
-#define START_MODE MODE_REALTIME
+#define START_MODE MODE_REALTIME
// highgui windows names
-#define WINDOW_ORIG "ROZKUK original scene"
-#define WINDOW_THRES "ROZKUK threshold"
-#define WINDOW_MASK "ROZKUK mask"
-#define WINDOW_PROD "ROZKUK product"
+#define WINDOW_ORIG "ROZKUK original scene"
+#define WINDOW_THRES "ROZKUK threshold"
+#define WINDOW_MASK "ROZKUK mask"
+#define WINDOW_PROD "ROZKUK product"
// values of keys
-#define KEY_ESC 0x1B
-#define KEY_SPACE 0x20
-#define KEY_P 0x50
-#define KEY_R 0x52
-#define KEY_S 0x53
-#define KEY_p 0x70
-#define KEY_r 0x72
-#define KEY_s 0x73
+#define KEY_ESC 0x1B
+#define KEY_SPACE 0x20
+#define KEY_P 0x50
+#define KEY_R 0x52
+#define KEY_S 0x53
+#define KEY_W 0x57
+#define KEY_p 0x70
+#define KEY_r 0x72
+#define KEY_s 0x73
+#define KEY_w 0x77
// number of frames passed in rozkuk mode before refresh
-#define PASS_FRAMES 0
+#define PASS_FRAMES 0
-#endif /*----------- DEBUG SESSION ONLY -----------*/
+#endif /*----------- DEBUG SESSION ONLY -----------*/
//default threshold and saturation
-#define THRESHOLD 70
-#define SATURATION 1.0
+#define THRESHOLD 70
+#define SATURATION 1.0
-//number of all masks
-#define MASKSCNT 5
//mask filename pattern
-#define MASKFILENAME "../rozkuk_masks/mask%02d.bin"
+#ifdef ROZKUK_DEBUG /************ DEBUG SESSION ONLY ************/
+#define MASKFILENAME "./mask%02d%c%c.pnm"
+#else /******************** PPC *******************/
+#define MASKFILENAME "./mask%02d%c%c.pnm"
+#endif /*------------------------------------------*/
//size of frames from camera
-#define FRAME_WIDTH 640
-#define FRAME_HEIGHT 480
-#define FRAME_SIZE (FRAME_WIDTH * FRAME_HEIGHT)
+#define FRAME_WIDTH 640
+#define FRAME_HEIGHT 480
+#define FRAME_SIZE (FRAME_WIDTH * FRAME_HEIGHT)
+
+//both (side and center) subresults will be returned in one int masked binary as 00ccssss
+#define MASK_SIDE 0x0F
+#define MASK_CENTER_SHIFT 4
+#define MASK_CENTER (0x03 << 4)
+
+//number of results to select the most supported one from
+#define DECISION_BOX_SIZE 5
+
+//camera control system (if true, rozkuk recognizes, else it waits)
+#ifdef ROZKUK_DEBUG /************ DEBUG SESSION ONLY ************/
+#define ORTE_CAMERA_CONTROL 1
+#else /******************** PPC *******************/
+#define ORTE_CAMERA_CONTROL orte.camera_control.on
+#endif /*------------------------------------------*/
/******************************************************************************/
// function declarations
-#ifdef ROZKUK_DEBUG /************ DEBUG SESSION ONLY ************/
+#ifdef ROZKUK_DEBUG /************ DEBUG SESSION ONLY ************/
+int saveFrame(const CvArr *img);
void drawPauseText(IplImage *img);
-int saveFrame(const IplImage *img);
-void displayProduct(const CvMat *floatMat, int recognizedLayout);
+void displayProduct(const CvMat *floatMat, int sideConfig, int centerConfig);
+void displayMasksSum(const CvMat *mat1, const CvMat *mat2, const char *windowName);
void displayFloatMat(const CvMat *floatMat, const char *windowName);
int modeRealtime(CvCapture* capture);
-#endif /*----------- DEBUG SESSION ONLY -----------*/
-int modeRecognize(CvCapture* capture);
+#endif /*----------- DEBUG SESSION ONLY -----------*/
+int modeRecognize(CvCapture* capture, CORBA_octet currentColor);
+int modeWait(CvCapture *capture);
int modeManager(int defaultMode);
-int recognize(const CvMat *frame);
+int countThreshold(const IplImage *frame);
+int recognize(const CvMat *frame, CvMat **masks, int masksCnt, double *resDist=NULL);
int loadMask(const char *filename, CvMat **mask);
-int loadMasks(void);
+int loadMasks(char color);
void freeMasks(void);
+void rcv_cmr_ctrl_cb(const ORTERecvInfo *info, void *vinstance, void *recvCallBackParam);
/******************************************************************************/
// variable declarations
-CvMat **masks=NULL; //float mask matrices
-#ifdef ROZKUK_DEBUG /************ DEBUG SESSION ONLY ************/
-CvFont font; //text font
+struct robottype_orte_data orte;
+int orteError=0;
+CvMat **sideMasks=NULL; //float mask matrices
+CvMat **centerMasks=NULL;
+#ifdef ROZKUK_DEBUG /************ DEBUG SESSION ONLY ************/
+CvFont font; //text font
+#endif /*----------- DEBUG SESSION ONLY -----------*/
/******************************************************************************/
-/** Writes text "pause" in the CAMERA window. */
-void drawPauseText(IplImage *img) {
- cvPutText(img,"pause", cvPoint(300,440), &font, cvScalar(255,255,255));
- cvShowImage(WINDOW_ORIG, img);
-}
-
-/******************************************************************************/
+#if defined (ROZKUK_DEBUG) || defined (PPC_DEBUG) /************ BOTH DEBUG SESSIONS ************/
/** Saves current image to a new file in the current directory. */
-int saveFrame(const IplImage *img) {
+int saveFrame(const CvArr *img) {
struct stat stFileInfo;
- char filename[30] = "snapshot00.png";
unsigned int i=0;
+
//find a non-existent filename
+#if defined (ROZKUK_DEBUG)
+ char filename[30] = "snapshot00.png";
while(!stat(filename, &stFileInfo)) sprintf(filename, "snapshot%02d.png", ++i);
+#else
+ char filename[30] = "snapshot00.pnm";
+ while(!stat(filename, &stFileInfo)) sprintf(filename, "snapshot%02d.pnm", ++i);
+#endif
+
+ fprintf(stdout, "Saving frame to %s...\n", filename);
//save image to file
return cvSaveImage(filename, img);
}
+#endif /*----------- BOTH DEBUG SESSIONS -----------*/
+
+/******************************************************************************/
+
+#ifdef ROZKUK_DEBUG /************ DEBUG SESSION ONLY ************/
+
+/** Writes text "pause" in the CAMERA window. */
+void drawPauseText(IplImage *img) {
+ cvPutText(img,"pause", cvPoint(300,440), &font, cvScalar(255,255,255));
+ cvShowImage(WINDOW_ORIG, img);
+}
+
/******************************************************************************/
/** Displays in separate window the result of multiplying camera frame (in float)
* and recognized mask.
* @param floatMat Pointer to the camera frame converted to float matrix.
- * @param recognizedLayout Index of recognized situation. */
-void displayProduct(const CvMat *floatMat, int recognizedLayout) {
- CvMat *product = cvCloneMat(floatMat);
+ * @param sideConfig Index of recognized side situation.
+ * @param centerConfig Index of recognized center situation. */
+void displayProduct(const CvMat *floatMat, int sideConfig, int centerConfig) {
+ CvMat *sideProduct = cvCloneMat(floatMat);
+ CvMat *centerProduct = cvCloneMat(floatMat);
+
+ cvMul(floatMat, sideMasks[sideConfig], sideProduct); //count product of frame and side mask
+ cvMul(floatMat, centerMasks[centerConfig], centerProduct); //count product of frame and center mask
+ displayMasksSum(sideProduct, centerProduct, WINDOW_PROD); //display sum
+
+ cvReleaseMat(&sideProduct);
+ cvReleaseMat(¢erProduct);
+}
- cvMul(floatMat, masks[recognizedLayout], product); // count product
- displayFloatMat(product, WINDOW_PROD);
+/******************************************************************************/
- cvReleaseMat(&product);
+/** Displays a sum of two matrices in specified window. */
+void displayMasksSum(const CvMat *mat1, const CvMat *mat2, const char *windowName) {
+ CvMat *sum = cvCloneMat(mat1);
+
+ cvAdd(mat1, mat2, sum);
+ displayFloatMat(sum, windowName);
+
+ cvReleaseMat(&sum);
}
/******************************************************************************/
double min=0.0, max=0.0;
CvMat *normalized = cvCloneMat(floatMat);
- cvMinMaxLoc(floatMat, &min, &max); // find extremes
- cvConvertScale(floatMat, normalized, 1/(max-min), -(min/(max-min))); // normalize
+ cvMinMaxLoc(floatMat, &min, &max); // find extremes
+ cvConvertScale(floatMat, normalized, 1/(max-min), -(min/(max-min))); // normalize
cvShowImage(windowName, normalized);
cvReleaseMat(&normalized);
* @param capture Pointer to a camera capture.
* @return Code of mode to switch to. */
int modeRealtime(CvCapture* capture) {
- IplImage* frame = NULL; // frame from camera
- int pause=0; // set pause=1 to pause the video
-
+ IplImage* frame = NULL; // frame from camera
+ int pause=0; // set pause=1 to pause the video
+
while(1) {
/* keyboard events handeling */
- switch(cvWaitKey(10) & 0xFF) { // wait 10ms for an event
- // stop capturing
- case KEY_ESC:
- return MODE_QUIT;
- // switch to recognize mode
- case KEY_R:
- case KEY_r:
- return MODE_RECOGNIZE;
- // (un)pause
- case KEY_P:
- case KEY_p:
- pause = !pause;
- drawPauseText(frame);
- break;
- // save frame to file
- case KEY_S:
- case KEY_s:
- saveFrame(frame);
- break;
- }
-
- frame = cvQueryFrame(capture); // Get one frame
- if(!frame) {
- perror("NULL frame");
- continue;
- }
-
- if(!pause) cvShowImage(WINDOW_ORIG, frame); // show image (! Do not release the frame !)
+ switch(cvWaitKey(10) & 0xFF) { // wait 10ms for an event
+ // stop capturing
+ case KEY_ESC:
+ return MODE_QUIT;
+ // switch to recognize mode
+ case KEY_R:
+ case KEY_r:
+ return MODE_RECOGNIZE;
+ // (un)pause
+ case KEY_P:
+ case KEY_p:
+ pause = !pause;
+ drawPauseText(frame);
+ break;
+ // save frame to file
+ case KEY_S:
+ case KEY_s:
+ saveFrame(frame);
+ break;
+ }
+
+ frame = cvQueryFrame(capture); // Get one frame
+ if(!frame) {
+ fprintf(stderr, "NULL frame\n");
+ continue;
+ }
+
+ if(!pause) cvShowImage(WINDOW_ORIG, frame); // show image (! Do not release the frame !)
}
return MODE_QUIT;
}
-#endif /*----------- DEBUG SESSION ONLY -----------*/
+#endif /*----------- DEBUG SESSION ONLY -----------*/
/******************************************************************************/
/** Implements the camera recognition of corns.
* In DEBUG session the results are also displayed on screen.
* @param capture Pointer to a camera capture.
+ * @param currentColor Number of starting color. If it changes in orte during
+ * recognition, this mode needs to restart (to load suitable masks).
* @return Code of mode to switch to. */
-int modeRecognize(CvCapture* capture) {
- IplImage *frame = NULL; // frame from camera
- IplImage *thresFrame = NULL; // thresholded frame
- CvMat *floatMat = NULL; // float <-1,1> image of the frame
- int layout; // number of recognized situation
-#ifdef ROZKUK_DEBUG /************ DEBUG SESSION ONLY ************/
- int framesPassed = PASS_FRAMES; // number of passed frames before refresh
- int pause=0; // set pause=1 to pause processing
-#endif /*----------- DEBUG SESSION ONLY -----------*/
- while(1) {
-#ifdef ROZKUK_DEBUG /************ DEBUG SESSION ONLY ************/
+int modeRecognize(CvCapture* capture, CORBA_octet currentColor) {
+ IplImage *frame = NULL; // frame from camera
+ IplImage *thresFrame = NULL; // thresholded frame
+ CvMat *floatMat = NULL; // float <-1,1> image of the frame
+ int sideConfig, centerConfig; // indices of recognized configurations
+ double sideDist, centerDist; // distances between the best and second best results
+ TDecisionBox sideBox(DECISION_BOX_SIZE, SIDEMASKSCNT); // side solutions "averaging" decision box
+ TDecisionBox centerBox(DECISION_BOX_SIZE, CENTERMASKSCNT); // center solutions "averaging" decision box
+#ifdef ROZKUK_DEBUG /************ DEBUG SESSION ONLY ************/
+ int framesPassed = PASS_FRAMES; // number of passed frames before refresh
+ int pause=0; // set pause=1 to pause processing
+#endif /*----------- DEBUG SESSION ONLY -----------*/
+ while(ORTE_CAMERA_CONTROL) {
+
+ //if color changed during recognition, restart the mode to load appropriate masks
+ if(orte.camera_control.game_color != currentColor) return MODE_RECOGNIZE;
+
+#ifdef ROZKUK_DEBUG /************ DEBUG SESSION ONLY ************/
/* keyboard events handeling */
- switch(cvWaitKey(10) & 0xFF) { // wait 10ms for an event
- // stop capturing
- case KEY_ESC:
- return MODE_QUIT;
- // skip the frame
- case KEY_SPACE:
- framesPassed = PASS_FRAMES;
- break;
- // switch to recognize mode
- case KEY_R:
- case KEY_r:
- return MODE_REALTIME;
- // (un)pause
- case KEY_P:
- case KEY_p:
- pause = !pause;
- drawPauseText(frame);
- break;
- // save frame to file
- case KEY_S:
- case KEY_s:
- saveFrame(frame);
- break;
- }
-#endif /*----------- DEBUG SESSION ONLY -----------*/
+ switch(cvWaitKey(10) & 0xFF) { // wait 10ms for an event
+ // stop capturing
+ case KEY_ESC:
+ return MODE_QUIT;
+ // skip the frame
+ case KEY_SPACE:
+ framesPassed = PASS_FRAMES;
+ break;
+ // switch to recognize mode
+ case KEY_R:
+ case KEY_r:
+ return MODE_REALTIME;
+ // (un)pause
+ case KEY_P:
+ case KEY_p:
+ pause = !pause;
+ drawPauseText(frame);
+ break;
+ // save frame to file
+ case KEY_S:
+ case KEY_s:
+ saveFrame(frame);
+ break;
+ // wait
+ case KEY_W:
+ case KEY_w:
+ return MODE_WAIT;
+ }
+#endif /*----------- DEBUG SESSION ONLY -----------*/
// Get one frame
-#ifdef ROZKUK_DEBUG /************ DEBUG SESSION ONLY ************/
+#ifdef ROZKUK_DEBUG /************ DEBUG SESSION ONLY ************/
if(pause) cvQueryFrame(capture);
else
-#endif /*----------- DEBUG SESSION ONLY -----------*/
+#endif /*----------- DEBUG SESSION ONLY -----------*/
frame = cvQueryFrame(capture);
- if(!frame) {
- perror("NULL frame");
- continue;
- }
-
-#ifdef ROZKUK_DEBUG /************ DEBUG SESSION ONLY ************/
+ if(!frame) {
+ fprintf(stderr, "NULL frame\n");
+ // save error code to orte and publish it
+ orte.camera_result.error |= camera_ERR_NO_FRAME;
+ ORTEPublicationSend(orte.publication_camera_result);
+ usleep(100*1000);
+ continue;
+ } else if(orte.camera_result.error & camera_ERR_NO_FRAME) {
+ orte.camera_result.error &= ~camera_ERR_NO_FRAME;
+ ORTEPublicationSend(orte.publication_camera_result);
+ }
+
+#ifdef ROZKUK_DEBUG /************ DEBUG SESSION ONLY ************/
if(pause) continue;
- // transform colorful image to its float <-1,1> representation
- cvReleaseImage(&thresFrame);
- thresFrame = cvCreateImage(cvSize(640,480),IPL_DEPTH_8U,1);
-#endif /*----------- DEBUG SESSION ONLY -----------*/
+ // transform colorful image to its float <-1,1> representation
+ cvReleaseImage(&thresFrame);
+ thresFrame = cvCreateImage(cvSize(FRAME_WIDTH,FRAME_HEIGHT),IPL_DEPTH_8U,1);
+#endif /*----------- DEBUG SESSION ONLY -----------*/
// convert to float
- clr2float(frame, &floatMat, THRESHOLD, SATURATION, thresFrame); // get float representation (in DEBUG session, thresholded image is stored to thresFrame)
- layout = recognize(floatMat);
- printf("varianta: %d\n", layout);
-#ifdef ROZKUK_DEBUG /************ DEBUG SESSION ONLY ************/
- displayProduct(floatMat, layout);
-#endif /*----------- DEBUG SESSION ONLY -----------*/
+ int threshold = countThreshold(frame);
+ clr2float(frame, &floatMat, threshold, SATURATION, thresFrame); // get float representation (in DEBUG session, thresholded image is stored to thresFrame)
+ //recognize side and center configurations
+ sideConfig = recognize(floatMat, sideMasks, SIDEMASKSCNT, &sideDist);
+ centerConfig = recognize(floatMat, centerMasks, CENTERMASKSCNT, ¢erDist);
+ fprintf(stderr, "SINGLE: thr: %3d, side: %d (%.2f), center: %d (%.2f)\n", threshold, sideConfig, sideDist, centerConfig, centerDist);
+ //push the results to the decision box and get "averaged" solutions from it
+ sideBox.insertItem(sideConfig, sideDist);
+ centerBox.insertItem(centerConfig, centerDist);
+ sideBox.decide(&sideConfig, &sideDist);
+ centerBox.decide(¢erConfig, ¢erDist);
+ //publish results
+ orte.camera_result.side = sideConfig;
+ orte.camera_result.center = centerConfig;
+ orte.camera_result.sideDist = sideDist;
+ orte.camera_result.centerDist = centerDist;
+ ORTEPublicationSend(orte.publication_camera_result);
+ fprintf(stderr, "ORTE: side: %d (%.2f), center: %d (%.2f)\n", sideConfig, sideDist, centerConfig, centerDist);
+
+#ifdef ROZKUK_DEBUG /************ DEBUG SESSION ONLY ************/
+ displayProduct(floatMat, sideConfig, centerConfig);
+#elif defined (PPC_DEBUG) ///////////// PPC DEBUG SESSION //////////////
+ saveFrame(frame);
+#endif /*----------- DEBUG SESSION ONLY -----------*/
cvReleaseMat(&floatMat);
-#ifdef ROZKUK_DEBUG /************ DEBUG SESSION ONLY ************/
+#ifdef ROZKUK_DEBUG /************ DEBUG SESSION ONLY ************/
if(framesPassed++ > PASS_FRAMES) {
- framesPassed=0;
- cvShowImage(WINDOW_ORIG, frame); // show image (! Do not release the frame !)
- cvShowImage(WINDOW_THRES, thresFrame);
- displayFloatMat(masks[layout], WINDOW_MASK); // display selected mask
- cvReleaseImage(&thresFrame);
+ framesPassed=0;
+ cvShowImage(WINDOW_ORIG, frame); // show image (! Do not release the frame !)
+ cvShowImage(WINDOW_THRES, thresFrame);
+ displayMasksSum(sideMasks[sideConfig], centerMasks[centerConfig], WINDOW_MASK); // display selected mask
+ cvReleaseImage(&thresFrame);
}
-#endif /*----------- DEBUG SESSION ONLY -----------*/
+#endif /*----------- DEBUG SESSION ONLY -----------*/
}
- return MODE_QUIT;
+ return MODE_WAIT;
+}
+
+/******************************************************************************/
+
+/** Waits while no action is wanted. To stop waiting set orte.camera_control.on=TRUE
+ * or in DEBUG session press W key.
+ * @param capture Pointer to a camera capture.
+ * @return Code of mode to switch to. */
+int modeWait(CvCapture *capture) {
+ while(!ORTE_CAMERA_CONTROL) {
+#ifdef ROZKUK_DEBUG /************ DEBUG SESSION ONLY ************/
+ //process keyboard events
+ switch(cvWaitKey(10) & 0xFF) { // wait 10ms for an event
+ // exit program
+ case KEY_ESC:
+ return MODE_QUIT;
+
+ // stop waiting
+ case KEY_W:
+ case KEY_w:
+ return MODE_RECOGNIZE;
+
+ }
+#endif /*----------- DEBUG SESSION ONLY -----------*/
+ fprintf(stderr, "waiting...\n");
+ sleep(1);
+ }
+ return MODE_RECOGNIZE;
}
/******************************************************************************/
* @param defaultMode The first mode to run. */
int modeManager(int defaultMode) {
int mode=defaultMode;
- CvCapture* capture;
-
+ int ret;
+ CvCapture* capture;
+ CORBA_octet color;
+
// connect to a camera
- while(!(capture=cvCaptureFromCAM(CV_CAP_ANY))) {
- perror("NULL capture");
- usleep(500*1000);
- }
-
-#ifdef ROZKUK_DEBUG /************ DEBUG SESSION ONLY ************/
+ while(!((capture=cvCaptureFromCAM(0)) || (capture=cvCaptureFromCAM(1)))) {
+ fprintf(stderr, "NULL capture (is camera connected?)\n");
+ orte.camera_result.error |= camera_ERR_NO_VIDEO;
+ ORTEPublicationSend(orte.publication_camera_result);
+ usleep(500*1000);
+ }
+ orte.camera_result.error &= ~camera_ERR_NO_VIDEO;
+ ORTEPublicationSend(orte.publication_camera_result);
+ fprintf(stderr, "rozkuk started, camera connected successfully\n");
+
+#ifdef ROZKUK_DEBUG /************ DEBUG SESSION ONLY ************/
// create gui windows and init font
- cvNamedWindow(WINDOW_ORIG, CV_WINDOW_AUTOSIZE); // real camera video / one frame viewer
- cvInitFont(&font, CV_FONT_HERSHEY_SIMPLEX, 1.0, 1.0, 0, 1); //init text font
-#endif /*----------- DEBUG SESSION ONLY -----------*/
-
- while(!(mode & MODE_QUIT)) {
- switch(mode & MASK_MODES) {
-
-#ifdef ROZKUK_DEBUG /************ DEBUG SESSION ONLY ************/
- case MODE_REALTIME:
- cvDestroyWindow(WINDOW_THRES);
- mode = modeRealtime(capture);
- break;
-#endif /*----------- DEBUG SESSION ONLY -----------*/
-
- case MODE_RECOGNIZE:
-#ifdef ROZKUK_DEBUG /************ DEBUG SESSION ONLY ************/
- cvNamedWindow(WINDOW_THRES, CV_WINDOW_AUTOSIZE);
- cvMoveWindow(WINDOW_THRES, cvQueryFrame(capture)->width, 0);
- cvNamedWindow(WINDOW_MASK, CV_WINDOW_AUTOSIZE);
- cvMoveWindow(WINDOW_MASK, 0, cvQueryFrame(capture)->height+50);
- cvNamedWindow(WINDOW_PROD, CV_WINDOW_AUTOSIZE);
- cvMoveWindow(WINDOW_PROD, cvQueryFrame(capture)->width, cvQueryFrame(capture)->height+50);
-#endif /*----------- DEBUG SESSION ONLY -----------*/
- mode = modeRecognize(capture);
- break;
-
- default:
- goto end; // jump out of the loop
- }
- }
+ cvNamedWindow(WINDOW_ORIG, CV_WINDOW_AUTOSIZE); // real camera video / one frame viewer
+ cvInitFont(&font, CV_FONT_HERSHEY_SIMPLEX, 1.0, 1.0, 0, 1); //init text font
+#endif /*----------- DEBUG SESSION ONLY -----------*/
+
+ while(!(mode & MODE_QUIT)) {
+ switch(mode & MASK_MODES) {
+
+#ifdef ROZKUK_DEBUG /************ DEBUG SESSION ONLY ************/
+ case MODE_REALTIME:
+ cvDestroyWindow(WINDOW_THRES);
+ mode = modeRealtime(capture);
+ break;
+#endif /*----------- DEBUG SESSION ONLY -----------*/
+
+ case MODE_RECOGNIZE:
+#ifdef ROZKUK_DEBUG /************ DEBUG SESSION ONLY ************/
+ cvNamedWindow(WINDOW_THRES, CV_WINDOW_AUTOSIZE);
+ cvMoveWindow(WINDOW_THRES, cvQueryFrame(capture)->width, 0);
+ cvNamedWindow(WINDOW_MASK, CV_WINDOW_AUTOSIZE);
+ cvMoveWindow(WINDOW_MASK, 0, cvQueryFrame(capture)->height+50);
+ cvNamedWindow(WINDOW_PROD, CV_WINDOW_AUTOSIZE);
+ cvMoveWindow(WINDOW_PROD, cvQueryFrame(capture)->width, cvQueryFrame(capture)->height+50);
+#endif /*----------- DEBUG SESSION ONLY -----------*/
+ //load masks and run recognition mode, then free masks (they are in memory only while in recognition mode)
+ color = orte.camera_control.game_color;
+ if((ret=loadMasks((color==BLUE ? clBLUE : clYELLOW)))) return ret;
+ mode = modeRecognize(capture, color);
+ freeMasks();
+ break;
+
+ case MODE_WAIT:
+ mode = modeWait(capture);
+ break;
+
+ default:
+ goto end; // jump out of the loop
+ }
+ }
end:
- cvReleaseCapture(&capture);
-#ifdef ROZKUK_DEBUG /************ DEBUG SESSION ONLY ************/
- cvDestroyWindow(WINDOW_ORIG);
- cvDestroyWindow(WINDOW_THRES);
- cvDestroyWindow(WINDOW_MASK);
- cvDestroyWindow(WINDOW_PROD);
-#endif /*----------- DEBUG SESSION ONLY -----------*/
- return mode & MASK_ERRORS;
+ cvReleaseCapture(&capture);
+#ifdef ROZKUK_DEBUG /************ DEBUG SESSION ONLY ************/
+ cvDestroyWindow(WINDOW_ORIG);
+ cvDestroyWindow(WINDOW_THRES);
+ cvDestroyWindow(WINDOW_MASK);
+ cvDestroyWindow(WINDOW_PROD);
+#endif /*----------- DEBUG SESSION ONLY -----------*/
+ return mode & MASK_ERRORS;
+}
+
+/******************************************************************************/
+
+/** Returns a threshold computed from the frame. */
+int countThreshold(const IplImage *frame) {
+ return cvAvg(frame).val[0];
}
/******************************************************************************/
/** Returns an ordinary number of recognized layout.
- * @param frame Float representation of frame. */
-int recognize(const CvMat *frame) {
+ * @param frame Float representation of frame.
+ * @param masks Array of masks to compare with.
+ * @param masksCnt Size of the array.
+ * @param resDist If not NULL, a distance between the best and second best result is stored here.*/
+int recognize(const CvMat *frame, CvMat **masks, int masksCnt, double *resDist) {
double max = cvDotProduct(frame, masks[0]);
+ double max2 = -(FRAME_SIZE); // the least possible value
int maxInd = 0;
- for(int i=1; i<MASKSCNT; i++) {
+ for(int i=1; i<masksCnt; i++) {
double prod = cvDotProduct(frame, masks[i]);
if(prod > max) {
+ max2 = max;
max = prod;
maxInd = i;
- }
+ } else if(prod > max2) max2 = prod;
}
- return maxInd;
+ if(resDist) *resDist = max-max2;
+ return maxInd;
}
/******************************************************************************/
* @param mask Address of array, where to alloc memory and store the data.
* @return Length of read data or zero in case of error. */
int loadMask(const char *filename, CvMat **mask) {
- float *data;
int len;
- FILE *pFile = fopen(filename, "rb"); //open binary file for reading
- if(!pFile) {
- char msg[200];
- sprintf(msg, "Mask %s cannot be loaded.", filename);
- perror(msg);
+
+ IplImage *img = cvLoadImage(filename, CV_LOAD_IMAGE_GRAYSCALE);
+ if(!img) {
+ fprintf(stderr, "Mask %s cannot be loaded.\n", filename);
+ fprintf(stderr, "Run me from _build/user/camera/rozkuk\n");
return 0;
}
- *mask = cvCreateMat(FRAME_HEIGHT, FRAME_WIDTH, CV_32FC1);
- data = (*mask)->data.fl;
- len = fread((float *)data, sizeof(float), FRAME_SIZE, pFile); //read data
-
- fclose(pFile);
+ *mask = NULL;
+ if(!(len=clr2float(img, mask, CL_NEUTRAL, 1.0, NULL)))
+ fprintf(stderr, "A problem while converting mask to float occured.\n");
+ else
+ fprintf(stderr, "Mask %s successfully loaded.\n", filename);
+
+ cvReleaseImage(&img);
return len;
}
/** Loads all the float masks from files.
* @return Zero if ok, else non-zero. */
-int loadMasks(void) {
- masks = (CvMat **)malloc(MASKSCNT*sizeof(CvMat *)); //alloc memory for array of masks
- for(int i=0; i<MASKSCNT; i++) {
- char filename[100];
- sprintf(filename, MASKFILENAME, i);
- if(!loadMask(filename, masks+i)) return ERR_MASK_READ_FAILURE;
+int loadMasks(char color) {
+ char filename[100];
+ //alloc memory for arrays of masks
+ sideMasks = (CvMat **)malloc(SIDEMASKSCNT*sizeof(CvMat *));
+ centerMasks = (CvMat **)malloc(CENTERMASKSCNT*sizeof(CvMat *));
+ //load masks
+ for(int i=0; i<SIDEMASKSCNT; i++) {
+ sprintf(filename, MASKFILENAME, i, orSIDE, color);
+ if(!loadMask(filename, sideMasks+i)) return ERR_MASK_READ_FAILURE;
+ }
+ for(int i=0; i<CENTERMASKSCNT; i++) {
+ sprintf(filename, MASKFILENAME, i, orCENTER, color);
+ if(!loadMask(filename, centerMasks+i)) return ERR_MASK_READ_FAILURE;
}
return 0;
}
/** Frees the memory alloced for masks. */
void freeMasks(void) {
- for(int i=0; i<MASKSCNT; i++) {
- cvReleaseMat(masks+i);
+ for(int i=0; i<SIDEMASKSCNT; i++) cvReleaseMat(sideMasks+i);
+ for(int i=0; i<CENTERMASKSCNT; i++) cvReleaseMat(centerMasks+i);
+ free(sideMasks);
+ free(centerMasks);
+}
+
+/******************************************************************************/
+
+/** Orte camera control callback function. */
+void rcv_cmr_ctrl_cb(const ORTERecvInfo *info, void *vinstance, void *recvCallBackParam) {
+#ifdef ORTE_DEBUG
+ struct robottype_orte_data *orte_data = (struct robottype_orte_data *)recvCallBackParam;
+#endif /*----------- ORTE_DEBUG -----------*/
+
+ switch (info->status) {
+ case NEW_DATA:
+ // nothing to do - changes will be processed in the recognition loop
+#ifdef ORTE_DEBUG
+ printf("orte: New data: ctrl %d, clr %d\n", orte_data->camera_control.on, orte_data->camera_control.game_color);
+#endif /*----------- ORTE_DEBUG -----------*/
+ break;
+ case DEADLINE:
+ printf("ORTE deadline occurred - CMR_CTRL receive\n");
+ break;
}
- free(masks);
}
/******************************************************************************/
int main(int argc, char *argv[]) {
int ret;
-/* while (orte_init()) {
- perror("orte_init");
- usleep(500*1000);
- }
-*/
- if((ret=loadMasks())) return ret;
- ret = modeManager(START_MODE);
- freeMasks();
+
+ ret = robottype_roboorte_init(&orte);
+ if(ret < 0) {
+ fprintf(stderr, "robottype_roboorte_init failed\n");
+ return ret;
+ }
+ robottype_publisher_camera_result_create(&orte, NULL, NULL);
+ robottype_subscriber_camera_control_create(&orte, rcv_cmr_ctrl_cb, &orte);
+
+ modeManager(START_MODE);
+
+ ret = robottype_roboorte_destroy(&orte);
return ret;
}
projects / eurobot / public.git / blobdiff