If the game_color is changed during recognition, load appropriate masks.

[eurobot/public.git] / src / camera / rozkuk / rozkuk.cxx

diff --git a/src/camera/rozkuk/rozkuk.cxx b/src/camera/rozkuk/rozkuk.cxx
index 4c5ccc4cac9f9bfdb4de30bc5fe59f71af101f8b..cc2d30d1456594293201715b8322c6b9744cf0f7 100644 (file)
--- a/src/camera/rozkuk/rozkuk.cxx
+++ b/src/camera/rozkuk/rozkuk.cxx
@@ -21,6 +21,7 @@
 
 #include "clr2float.h"
 #include "masks_globals.h"
+#include "t_decision_box.h"
 
 extern "C" {
 #include <roboorte_robottype.h>
@@ -97,6 +98,16 @@ extern "C" {
 #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
@@ -108,7 +119,7 @@ void displayMasksSum(const CvMat *mat1, const CvMat *mat2, const char *windowNam
 void displayFloatMat(const CvMat *floatMat, const char *windowName);
 int modeRealtime(CvCapture* capture);
 #endif                    /*----------- DEBUG SESSION ONLY -----------*/
-int modeRecognize(CvCapture* capture);
+int modeRecognize(CvCapture* capture, CORBA_octet currentColor);
 int modeWait(CvCapture *capture);
 int modeManager(int defaultMode);
 int countThreshold(const IplImage *frame);
@@ -260,18 +271,25 @@ int modeRealtime(CvCapture* capture) {
 /** 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) {
+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.on) {
+       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 */
@@ -333,16 +351,22 @@ int modeRecognize(CvCapture* capture) {
                // 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
+               //recognize side and center configurations
                sideConfig = recognize(floatMat, sideMasks, SIDEMASKSCNT, &sideDist);
                centerConfig = recognize(floatMat, centerMasks, CENTERMASKSCNT, &centerDist);
+               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(&centerConfig, &centerDist);
                //publish results
-               fprintf(stderr, "thr: %d, side: %d (%f), center: %d (%f)\n", threshold, sideConfig, sideDist, centerConfig, centerDist);
                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);
@@ -372,7 +396,7 @@ int modeRecognize(CvCapture* capture) {
   * @param capture Pointer to a camera capture.
   * @return Code of mode to switch to. */
 int modeWait(CvCapture *capture) {
-       while(!orte.camera_control.on) {
+       while(!ORTE_CAMERA_CONTROL) {
 #ifdef ROZKUK_DEBUG       /************ DEBUG SESSION ONLY ************/
                //process keyboard events
                switch(cvWaitKey(10) & 0xFF) {                      // wait 10ms for an event
@@ -401,6 +425,7 @@ int modeManager(int defaultMode) {
        int mode=defaultMode;
        int ret;
        CvCapture* capture;
+       CORBA_octet color;
 
        // connect to a camera
        while(!((capture=cvCaptureFromCAM(0)) || (capture=cvCaptureFromCAM(1)))) {
@@ -439,8 +464,9 @@ int modeManager(int defaultMode) {
                        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;