#include <inttypes.h>
#include <stdlib.h>
#include <stdio.h>
+#include <sys/stat.h>
#include <semaphore.h>
#include <getopt.h>
#include <unistd.h>
+
#include <orte.h>
//#include "roboorte.h"
-
//#include "rozkorte.h"
}
// mask of all modes / errors
#define MASK_MODES 0x07
-#define MASK_ERRORS ~MASK_MODES
+#define MASK_ERRORS (~MASK_MODES)
+
+// errors
+#define ERR_MASK_READ_FAILURE 0x08
// default mode in standard session
#define START_MODE MODE_RECOGNIZE
// highgui windows names
#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
// number of frames passed in rozkuk mode before refresh
#define PASS_FRAMES 0
#endif /*----------- DEBUG SESSION ONLY -----------*/
-#define THRESHOLD 50
+//default threshold and saturation
+#define THRESHOLD 70
+#define SATURATION 1.0
+
+//number of all masks
+#define MASKSCNT 5
+//mask filename pattern
+#define MASKFILENAME "../rozkuk_masks/mask%02d.bin"
+//size of frames from camera
+#define FRAME_WIDTH 640
+#define FRAME_HEIGHT 480
+#define FRAME_SIZE (FRAME_WIDTH * FRAME_HEIGHT)
+/******************************************************************************/
+
+// function declarations
#ifdef ROZKUK_DEBUG /************ DEBUG SESSION ONLY ************/
+void drawPauseText(IplImage *img);
+int saveFrame(const IplImage *img);
+void displayProduct(const CvMat *floatMat, int recognizedLayout);
+void displayFloatMat(const CvMat *floatMat, const char *windowName);
+int modeRealtime(CvCapture* capture);
+#endif /*----------- DEBUG SESSION ONLY -----------*/
+int modeRecognize(CvCapture* capture);
+int modeManager(int defaultMode);
+int recognize(const CvMat *frame);
+int loadMask(const char *filename, CvMat **mask);
+int loadMasks(void);
+void freeMasks(void);
/******************************************************************************/
+// variable declarations
+CvMat **masks=NULL; //float mask matrices
+#ifdef ROZKUK_DEBUG /************ DEBUG SESSION ONLY ************/
CvFont font; //text font
/******************************************************************************/
/******************************************************************************/
+/** Saves current image to a new file in the current directory. */
+int saveFrame(const IplImage *img) {
+ struct stat stFileInfo;
+ char filename[30] = "snapshot00.png";
+ unsigned int i=0;
+ //find a non-existent filename
+ while(!stat(filename, &stFileInfo)) sprintf(filename, "snapshot%02d.png", ++i);
+ //save image to file
+ return cvSaveImage(filename, 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);
+
+ cvMul(floatMat, masks[recognizedLayout], product); // count product
+ displayFloatMat(product, WINDOW_PROD);
+
+ cvReleaseMat(&product);
+}
+
+/******************************************************************************/
+
+/** Normalizes the matrix values and displays them in window specified by name.
+ * @param floatMat Pointer to matrix data to display.
+ * @param windowName Name of window in which to display the image. */
+void displayFloatMat(const CvMat *floatMat, const char *windowName) {
+ 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
+ cvShowImage(windowName, normalized);
+
+ cvReleaseMat(&normalized);
+}
+
+/******************************************************************************/
+
/** Displays a real video in a window (only for DEBUG session).
* @param capture Pointer to a camera capture.
* @return Code of mode to switch to. */
pause = !pause;
drawPauseText(frame);
break;
+ // save frame to file
+ case KEY_S:
+ case KEY_s:
+ saveFrame(frame);
+ break;
}
frame = cvQueryFrame(capture); // Get one frame
int modeRecognize(CvCapture* capture) {
IplImage *frame = NULL; // frame from camera
IplImage *thresFrame = NULL; // thresholded frame
- float *floatMap = NULL; // float <-1,1> image of the 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 ************/
/* keyboard events handeling */
pause = !pause;
drawPauseText(frame);
break;
+ // save frame to file
+ case KEY_S:
+ case KEY_s:
+ saveFrame(frame);
+ break;
}
#endif /*----------- DEBUG SESSION ONLY -----------*/
cvReleaseImage(&thresFrame);
thresFrame = cvCreateImage(cvSize(640,480),IPL_DEPTH_8U,1);
#endif /*----------- DEBUG SESSION ONLY -----------*/
- clr2float(frame, &floatMap, THRESHOLD, thresFrame); // get float representation (in DEBUG session, thresholded image is stored to thresFrame)
+ // 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 -----------*/
+ cvReleaseMat(&floatMat);
#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);
}
#endif /*----------- 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);
+ cvNamedWindow(WINDOW_PROD, CV_WINDOW_AUTOSIZE);
+ cvMoveWindow(WINDOW_PROD, cvQueryFrame(capture)->width, cvQueryFrame(capture)->height+50);
#endif /*----------- DEBUG SESSION ONLY -----------*/
mode = modeRecognize(capture);
break;
#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 an ordinary number of recognized layout.
+ * @param frame Float representation of frame. */
+int recognize(const CvMat *frame) {
+ double max = cvDotProduct(frame, masks[0]);
+ int maxInd = 0;
+ for(int i=1; i<MASKSCNT; i++) {
+ double prod = cvDotProduct(frame, masks[i]);
+ if(prod > max) {
+ max = prod;
+ maxInd = i;
+ }
+ }
+ return maxInd;
+}
+
+/******************************************************************************/
+
+/** Loads binary (float) data to *mask from file.
+ * @param filename Path to file to read.
+ * @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);
+ 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);
+ 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;
+ }
+ return 0;
+}
+
+/******************************************************************************/
+
+/** Frees the memory alloced for masks. */
+void freeMasks(void) {
+ for(int i=0; i<MASKSCNT; i++) {
+ cvReleaseMat(masks+i);
+ }
+ free(masks);
+}
+
+/******************************************************************************/
+
int main(int argc, char *argv[]) {
+ int ret;
/* while (orte_init()) {
perror("orte_init");
usleep(500*1000);
}
*/
- return modeManager(START_MODE);
+ if((ret=loadMasks())) return ret;
+ ret = modeManager(START_MODE);
+ freeMasks();
+ return ret;
}
projects / eurobot / public.git / commitdiff