#include "clr2float.h"
#include "masks_globals.h"
+#include "t_decision_box.h"
extern "C" {
#include <roboorte_robottype.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_WAIT 0x08
+#define MODE_QUIT 0x01
+#define MODE_REALTIME 0x02
+#define MODE_RECOGNIZE 0x04
+#define MODE_WAIT 0x08
// mask of all modes / errors
-#define MASK_MODES 0x0F
-#define MASK_ERRORS (~MASK_MODES)
+#define MASK_MODES 0x0F
+#define MASK_ERRORS (~MASK_MODES)
// errors
-#define ERR_MASK_READ_FAILURE 0x10
+#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_W 0x57
-#define KEY_p 0x70
-#define KEY_r 0x72
-#define KEY_s 0x73
-#define KEY_w 0x77
+#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
//mask filename pattern
-#ifdef ROZKUK_DEBUG /************ DEBUG SESSION ONLY ************/
-#define MASKFILENAME "./mask%02d%c%c.bin"
-#else /******************** PPC *******************/
-#define MASKFILENAME "/dev/flash/mask%02d%c%c.bin" // FIXME: insert correct filepath mask
-#endif /*------------------------------------------*/
+#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)
-struct robottype_orte_data orte;
+//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 sideLayout, int centerLayout);
+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 countThreshold(const IplImage *frame);
-int recognize(const CvMat *frame, CvMat **masks, int masksCnt);
+int recognize(const CvMat *frame, CvMat **masks, int masksCnt, double *resDist=NULL);
int loadMask(const char *filename, CvMat **mask);
int loadMasks(char color);
void freeMasks(void);
+void rcv_cmr_ctrl_cb(const ORTERecvInfo *info, void *vinstance, void *recvCallBackParam);
/******************************************************************************/
// variable declarations
+struct robottype_orte_data orte;
+int orteError=0;
CvMat **sideMasks=NULL; //float mask matrices
CvMat **centerMasks=NULL;
-#ifdef ROZKUK_DEBUG /************ DEBUG SESSION ONLY ************/
+#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 sideLayout Index of recognized side situation.
- * @param centerLayout Index of recognized center situation. */
-void displayProduct(const CvMat *floatMat, int sideLayout, int centerLayout) {
+ * @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[sideLayout], sideProduct); //count product of frame and side mask
- cvMul(floatMat, centerMasks[centerLayout], centerProduct); //count product of frame and center mask
+ 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);
void displayMasksSum(const CvMat *mat1, const CvMat *mat2, const char *windowName) {
CvMat *sum = cvCloneMat(mat1);
- cvAdd(mat1, mat2, sum);
- displayFloatMat(sum, windowName);
-
- cvReleaseMat(&sum);
+ 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
+ switch(cvWaitKey(10) & 0xFF) { // wait 10ms for an event
// stop capturing
case KEY_ESC:
return MODE_QUIT;
break;
}
- frame = cvQueryFrame(capture); // Get one frame
+ frame = cvQueryFrame(capture); // Get one frame
if(!frame) {
- perror("NULL frame");
+ fprintf(stderr, "NULL frame\n");
continue;
}
- if(!pause) cvShowImage(WINDOW_ORIG, frame); // show image (! Do not release the frame !)
+ 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 sideLayout, centerLayout; // indices 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) {
+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 ************/
+#ifdef ROZKUK_DEBUG /************ DEBUG SESSION ONLY ************/
/* keyboard events handeling */
- switch(cvWaitKey(10) & 0xFF) { // wait 10ms for an event
+ switch(cvWaitKey(10) & 0xFF) { // wait 10ms for an event
// stop capturing
case KEY_ESC:
return MODE_QUIT;
case KEY_w:
return MODE_WAIT;
}
-#endif /*----------- DEBUG SESSION ONLY -----------*/
+#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");
+ 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 ************/
+#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 -----------*/
+ thresFrame = cvCreateImage(cvSize(FRAME_WIDTH,FRAME_HEIGHT),IPL_DEPTH_8U,1);
+#endif /*----------- DEBUG SESSION ONLY -----------*/
// convert to float
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 layouts
- sideLayout = recognize(floatMat, sideMasks, SIDEMASKSCNT);
- centerLayout = recognize(floatMat, centerMasks, CENTERMASKSCNT);
+ 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
- printf("thr: %d, side: %d, center: %d\n", threshold, sideLayout, centerLayout);
- orte.camera_result.side = sideLayout;
- orte.camera_result.center = centerLayout;
+ 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, sideLayout, centerLayout);
-#endif /*----------- DEBUG SESSION ONLY -----------*/
+#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_ORIG, frame); // show image (! Do not release the frame !)
cvShowImage(WINDOW_THRES, thresFrame);
- displayMasksSum(sideMasks[sideLayout], centerMasks[centerLayout], WINDOW_MASK); // display selected mask
+ 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;
}
/******************************************************************************/
* @param capture Pointer to a camera capture.
* @return Code of mode to switch to. */
int modeWait(CvCapture *capture) {
- while(!orte.camera_control.on) {
-#ifdef ROZKUK_DEBUG /************ DEBUG SESSION ONLY ************/
+ while(!ORTE_CAMERA_CONTROL) {
+#ifdef ROZKUK_DEBUG /************ DEBUG SESSION ONLY ************/
//process keyboard events
- switch(cvWaitKey(10) & 0xFF) { // wait 10ms for an event
+ switch(cvWaitKey(10) & 0xFF) { // wait 10ms for an event
// exit program
case KEY_ESC:
return MODE_QUIT;
return MODE_RECOGNIZE;
}
-#endif /*----------- DEBUG SESSION ONLY -----------*/
- printf("waiting...\n");
+#endif /*----------- DEBUG SESSION ONLY -----------*/
+ fprintf(stderr, "waiting...\n");
sleep(1);
}
return MODE_RECOGNIZE;
int mode=defaultMode;
int ret;
CvCapture* capture;
+ CORBA_octet color;
// connect to a camera
- while(!(capture=cvCaptureFromCAM(CV_CAP_ANY))) {
- perror("NULL capture");
+ 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 ************/
+#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 -----------*/
+ 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 ************/
+#ifdef ROZKUK_DEBUG /************ DEBUG SESSION ONLY ************/
case MODE_REALTIME:
cvDestroyWindow(WINDOW_THRES);
mode = modeRealtime(capture);
break;
-#endif /*----------- DEBUG SESSION ONLY -----------*/
+#endif /*----------- DEBUG SESSION ONLY -----------*/
case MODE_RECOGNIZE:
-#ifdef ROZKUK_DEBUG /************ DEBUG SESSION ONLY ************/
+#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);
+ 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 -----------*/
+ 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)
- if((ret=loadMasks((orte.camera_control.game_color==BLUE ? clBLUE : clYELLOW)))) return ret;
- mode = modeRecognize(capture);
+ color = orte.camera_control.game_color;
+ if((ret=loadMasks((color==BLUE ? clBLUE : clYELLOW)))) return ret;
+ mode = modeRecognize(capture, color);
freeMasks();
break;
break;
default:
- goto end; // jump out of the loop
+ goto end; // jump out of the loop
}
}
end:
cvReleaseCapture(&capture);
-#ifdef ROZKUK_DEBUG /************ DEBUG SESSION ONLY ************/
+#ifdef ROZKUK_DEBUG /************ DEBUG SESSION ONLY ************/
cvDestroyWindow(WINDOW_ORIG);
cvDestroyWindow(WINDOW_THRES);
cvDestroyWindow(WINDOW_MASK);
cvDestroyWindow(WINDOW_PROD);
-#endif /*----------- DEBUG SESSION ONLY -----------*/
+#endif /*----------- DEBUG SESSION ONLY -----------*/
return mode & MASK_ERRORS;
}
/******************************************************************************/
/** Returns an ordinary number of recognized layout.
- * @param frame Float representation of frame. */
-int recognize(const CvMat *frame, CvMat **masks, int masksCnt) {
+ * @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;
}
/******************************************************************************/
+/** 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;
+ }
+}
+
+/******************************************************************************/
+
int main(int argc, char *argv[]) {
int ret;
return ret;
}
robottype_publisher_camera_result_create(&orte, NULL, NULL);
- robottype_subscriber_camera_control_create(&orte, NULL, NULL);
-
+ robottype_subscriber_camera_control_create(&orte, rcv_cmr_ctrl_cb, &orte);
+
modeManager(START_MODE);
- ret = robottype_roboorte_destroy(&orte);
+ ret = robottype_roboorte_destroy(&orte);
return ret;
}
projects / eurobot / public.git / blobdiff