main.cpp

00001 #include "mbed.h"
00002 #include "DisplayBace.h"
00003 #include "rtos.h"
00004 #include "define.h"
00005 #include "mStorage.hpp"
00006 
00007 // Include for graphic
00008 #include "graphicFramework/myImage.h"
00009 #include "graphicFramework/mGraphic.hpp"
00010 
00011 // Define for store facial xml file
00012 #include "haar_cascade.h"
00013 #include "FATFileSystem.h"
00014 #include "HeapBlockDevice.h"
00015 
00016 // Include for opencv
00017 #include "cProcess/cProcess.hpp"
00018 #include "opencv_3_1/opencv2/core.hpp"
00019 #include "opencv_3_1/opencv2/imgproc.hpp"
00020 #include "opencv_3_1/opencv2/objdetect.hpp"
00021 #include "opencv_3_1/opencv2/face.hpp"
00022 using namespace cv;
00023 using namespace face;
00024 
00025 typedef struct {
00026     int x;
00027     int y;
00028     int radius;
00029     uint8_t color[2];
00030 }mCircle;
00031 
00032 typedef struct {
00033     int x1;
00034     int y1;
00035     int x2;
00036     int y2;
00037     uint8_t color[2];
00038 }mLine;
00039 
00040 typedef struct {
00041     vector<Mat> database_image;
00042     vector<int> database_label;
00043 }face_database_t;
00044 
00045 typedef struct {
00046     Rect obj;
00047     int label;
00048 }obj_detect_result;
00049 
00050 typedef struct {
00051     vector<mCircle> circles;
00052     vector<mLine> lines;
00053     vector<Point> contour;//only have max contour
00054 }gesture_result;
00055 
00056 typedef struct {
00057     vector<Point> Pointstart;
00058     vector<Point> Pointdepth;
00059     vector<int> Pointindex;
00060     int detecthand;
00061 }ConvexPoint;
00062 
00063 typedef struct
00064 {
00065     double hand_max;
00066     double hand_min;
00067 }Sampling;
00068 
00069 /* Hardware */
00070 static DisplayBase Display;
00071 static DigitalOut  lcd_pwon(P7_15);
00072 static DigitalOut  lcd_blon(P8_1);
00073 static PwmOut      lcd_cntrst(P8_15);
00074 static Serial      pc(USBTX, USBRX);
00075 static TouckKey_LCD_shield touch(P4_0, P2_13, I2C_SDA, I2C_SCL);
00076 
00077 /* Semaphore and mutex */
00078 rtos::Mutex mMutexProcess;
00079 rtos::Mutex mMutexObjects;
00080 rtos::Mutex mMutexClicked;
00081 
00082 static Semaphore semProcessThread(0);
00083 static Semaphore semTouch(0);
00084 static Semaphore semDrawAction(0);
00085 
00086 /* Threads */
00087 static Thread *    p_VideoLcdTask = NULL;
00088 static Thread *    p_DrawObjects = NULL;
00089 static Thread *    p_Touch = NULL;
00090 
00091 static cStorage *myStorage = NULL;
00092 static vector<obj_detect_result> gObjectDetects;
00093 static gesture_result gGestureResult;
00094 static bool gIsProcessing = false;
00095 static bool gIsDrawing = false;
00096 static int write_buff_num = 0;
00097 static int read_buff_num = 0;
00098 static bool graphics_init_end = false;
00099 static APP_MODE appMode = GUESTURE_RECOGNITION;
00100 static CLICKED_CODE clickedCode = CLICKED_UNKNOWN;
00101 
00102 /****** cache control ******/
00103 static void dcache_clean(void * p_buf, uint32_t size) {
00104     uint32_t start_addr = (uint32_t)p_buf & 0xFFFFFFE0;
00105     uint32_t end_addr   = (uint32_t)p_buf + size;
00106     uint32_t addr;
00107 
00108     /* Data cache clean */
00109     for (addr = start_addr; addr < end_addr; addr += 0x20) {
00110         __v7_clean_dcache_mva((void *)addr);
00111     }
00112 }
00113 
00114 /****** LCD ******/
00115 #if(0) /* When needing LCD Vsync interrupt, please make it effective. */
00116 static void IntCallbackFunc_LoVsync(DisplayBase::int_type_t int_type) {
00117     /* Interrupt callback function for Vsync interruption */
00118 }
00119 #endif
00120 
00121 static void Init_LCD_Display(void) {
00122     DisplayBase::graphics_error_t error;
00123     DisplayBase::lcd_config_t lcd_config;
00124     PinName lvds_pin[8] = {
00125         /* data pin */
00126         P5_7, P5_6, P5_5, P5_4, P5_3, P5_2, P5_1, P5_0
00127     };
00128 
00129     lcd_pwon = 0;
00130     lcd_blon = 0;
00131     Thread::wait(100);
00132     lcd_pwon = 1;
00133     lcd_blon = 1;
00134 
00135     Display.Graphics_Lvds_Port_Init(lvds_pin, 8);
00136 
00137     /* Graphics initialization process */
00138     lcd_config = LcdCfgTbl_LCD_shield;
00139     error = Display.Graphics_init(&lcd_config);
00140     if (error != DisplayBase::GRAPHICS_OK ) {
00141         printf("Line %d, error %d\n", __LINE__, error);
00142         mbed_die();
00143     }
00144     graphics_init_end = true;
00145 
00146 #if(0) /* When needing LCD Vsync interrupt, please make it effective. */
00147     /* Interrupt callback function setting (Vsync signal output from scaler 0)*/
00148     error = Display.Graphics_Irq_Handler_Set(DisplayBase::INT_TYPE_S0_LO_VSYNC ,
00149                                                     0, IntCallbackFunc_LoVsync);
00150     if (error != DisplayBase::GRAPHICS_OK ) {
00151         printf("Line %d, error %d\n", __LINE__, error);
00152         mbed_die();
00153     }
00154 #endif
00155 }
00156 
00157 static void Start_LCD_Display(uint8_t * p_buf) {
00158     DisplayBase::rect_t rect;
00159 
00160     rect.vs  = 0;
00161     rect.vw  = LCD_PIXEL_HEIGHT;
00162     rect.hs  = 0;
00163     rect.hw  = LCD_PIXEL_WIDTH;
00164     Display.Graphics_Read_Setting(
00165         DisplayBase::GRAPHICS_LAYER_0 ,
00166         (void *)p_buf,
00167         FRAME_BUFFER_STRIDE,
00168         GRAPHICS_FORMAT,
00169         WR_RD_WRSWA,
00170         &rect
00171     );
00172     Display.Graphics_Start(DisplayBase::GRAPHICS_LAYER_0 );
00173 }
00174 
00175 /****** Video ******/
00176 #if(0) /* When needing video Vsync interrupt, please make it effective. */
00177 static void IntCallbackFunc_ViVsync(DisplayBase::int_type_t int_type) {
00178     /* Interrupt callback function for Vsync interruption */
00179 }
00180 #endif
00181 
00182 static void IntCallbackFunc_Vfield(DisplayBase::int_type_t int_type) {
00183     /* Interrupt callback function */
00184 #if VIDEO_INPUT_METHOD == VIDEO_CVBS
00185     if (vfield_count == 0) {
00186         vfield_count = 1;
00187     } else {
00188         vfield_count = 0;
00189 #else
00190     {
00191 #endif
00192         if (p_VideoLcdTask != NULL) {
00193             p_VideoLcdTask->signal_set(1);
00194         }
00195     }
00196 }
00197 
00198 static void Init_Video(void) {
00199     DisplayBase::graphics_error_t error;
00200 
00201     /* Graphics initialization process */
00202     if (graphics_init_end == false) {
00203         /* When not initializing LCD, this processing is needed. */
00204         error = Display.Graphics_init(NULL);
00205         if (error != DisplayBase::GRAPHICS_OK ) {
00206             printf("Line %d, error %d\n", __LINE__, error);
00207             mbed_die();
00208         }
00209         graphics_init_end = true;
00210     }
00211 
00212 #if VIDEO_INPUT_METHOD == VIDEO_CVBS
00213     error = Display.Graphics_Video_init( DisplayBase::INPUT_SEL_VDEC , NULL);
00214     if( error != DisplayBase::GRAPHICS_OK  ) {
00215         printf("Line %d, error %d\n", __LINE__, error);
00216         mbed_die();
00217     }
00218 #elif VIDEO_INPUT_METHOD == VIDEO_CMOS_CAMERA
00219     DisplayBase::video_ext_in_config_t ext_in_config;
00220     PinName cmos_camera_pin[11] = {
00221         /* data pin */
00222         P2_7, P2_6, P2_5, P2_4, P2_3, P2_2, P2_1, P2_0,
00223         /* control pin */
00224         P10_0,      /* DV0_CLK   */
00225         P1_0,       /* DV0_Vsync */
00226         P1_1        /* DV0_Hsync */
00227     };
00228 
00229     /* MT9V111 camera input config */
00230     ext_in_config.inp_format      = DisplayBase::VIDEO_EXTIN_FORMAT_BT601 ; /* BT601 8bit YCbCr format */
00231     ext_in_config.inp_pxd_edge    = DisplayBase::EDGE_RISING ;              /* Clock edge select for capturing data          */
00232     ext_in_config.inp_vs_edge     = DisplayBase::EDGE_RISING ;              /* Clock edge select for capturing Vsync signals */
00233     ext_in_config.inp_hs_edge     = DisplayBase::EDGE_RISING ;              /* Clock edge select for capturing Hsync signals */
00234     ext_in_config.inp_endian_on   = DisplayBase::OFF ;                      /* External input bit endian change on/off       */
00235     ext_in_config.inp_swap_on     = DisplayBase::OFF ;                      /* External input B/R signal swap on/off         */
00236     ext_in_config.inp_vs_inv      = DisplayBase::SIG_POL_NOT_INVERTED ;     /* External input DV_VSYNC inversion control     */
00237     ext_in_config.inp_hs_inv      = DisplayBase::SIG_POL_INVERTED ;         /* External input DV_HSYNC inversion control     */
00238     ext_in_config.inp_f525_625    = DisplayBase::EXTIN_LINE_525 ;           /* Number of lines for BT.656 external input */
00239     ext_in_config.inp_h_pos       = DisplayBase::EXTIN_H_POS_CRYCBY ;       /* Y/Cb/Y/Cr data string start timing to Hsync reference */
00240     ext_in_config.cap_vs_pos      = 6;                                     /* Capture start position from Vsync */
00241     ext_in_config.cap_hs_pos      = 150;                                   /* Capture start position form Hsync */
00242 #if (LCD_TYPE == 0)
00243     /* The same screen ratio as the screen ratio of the LCD. */
00244     ext_in_config.cap_width       = 640;                                   /* Capture width */
00245     ext_in_config.cap_height      = 363;                                   /* Capture height Max 468[line]
00246                                                                              Due to CMOS(MT9V111) output signal timing and VDC5 specification */
00247 #else
00248     ext_in_config.cap_width       = 640;                                   /* Capture width  */
00249     ext_in_config.cap_height      = 468;                                   /* Capture height Max 468[line]
00250                                                                              Due to CMOS(MT9V111) output signal timing and VDC5 specification */
00251 #endif
00252     error = Display.Graphics_Video_init( DisplayBase::INPUT_SEL_EXT , &ext_in_config);
00253     if( error != DisplayBase::GRAPHICS_OK  ) {
00254         printf("Line %d, error %d\n", __LINE__, error);
00255         mbed_die();
00256     }
00257 
00258     /* Camera input port setting */
00259     error = Display.Graphics_Dvinput_Port_Init(cmos_camera_pin, 11);
00260     if( error != DisplayBase::GRAPHICS_OK  ) {
00261         printf("Line %d, error %d\n", __LINE__, error);
00262         mbed_die();
00263     }
00264 #endif
00265 
00266 #if(0) /* When needing video Vsync interrupt, please make it effective. */
00267     /* Interrupt callback function setting (Vsync signal input to scaler 0) */
00268     error = Display.Graphics_Irq_Handler_Set(DisplayBase::INT_TYPE_S0_VI_VSYNC ,
00269                                                  0, IntCallbackFunc_ViVsync);
00270     if (error != DisplayBase::GRAPHICS_OK ) {
00271         printf("Line %d, error %d\n", __LINE__, error);
00272         mbed_die();
00273     }
00274 #endif
00275 
00276     /* Interrupt callback function setting (Field end signal for recording function in scaler 0) */
00277     error = Display.Graphics_Irq_Handler_Set(VIDEO_INT_TYPE, 0, 
00278                                                         IntCallbackFunc_Vfield);
00279     if (error != DisplayBase::GRAPHICS_OK ) {
00280         printf("Line %d, error %d\n", __LINE__, error);
00281         mbed_die();
00282     }
00283 }
00284 
00285 static void initDrawResultLayer(void)
00286 {
00287     DisplayBase::rect_t rect;
00288     /* The layer by which the touch panel location is drawn */
00289     memset(user_frame_buffer_draw, 0, sizeof(user_frame_buffer_draw));
00290     dcache_clean(user_frame_buffer_draw, sizeof(user_frame_buffer_draw));
00291     rect.vs  = 0;
00292     rect.vw  = LCD_PIXEL_HEIGHT;
00293     rect.hs  = 0;
00294     rect.hw  = LCD_PIXEL_WIDTH;
00295     Display.Graphics_Read_Setting(
00296         DisplayBase::GRAPHICS_LAYER_1 ,
00297         (void *)user_frame_buffer_draw,
00298         DRAW_BUFFER_STRIDE,
00299         DisplayBase::GRAPHICS_FORMAT_ARGB4444 ,
00300         DisplayBase::WR_RD_WRSWA_32_16BIT ,
00301         &rect
00302     );
00303     Display.Graphics_Start(DisplayBase::GRAPHICS_LAYER_1 );
00304 }
00305 
00306 void initDrawAction32(void)
00307 {
00308     // Init draw control
00309     DisplayBase::rect_t rect;
00310     rect.vs  = 0;
00311     rect.vw  = LCD_PIXEL_HEIGHT;
00312     rect.hs  = 0;
00313     rect.hw  = LCD_PIXEL_WIDTH;
00314     
00315     memset(user_buf_draw_action_888, 0, 
00316                                         sizeof(user_buf_draw_action_888));     
00317     /* Clean cache */
00318     dcache_clean(user_buf_draw_action_888, 
00319                                         sizeof(user_buf_draw_action_888));
00320     
00321     Display.Graphics_Read_Setting(
00322         DisplayBase::GRAPHICS_LAYER_3 ,
00323         (void *)user_buf_draw_action_888,
00324         DRAW_BUFFER_STRIDE_LAYER_3,
00325         DisplayBase::GRAPHICS_FORMAT_ARGB8888 ,
00326         DisplayBase::WR_RD_WRSWA_32_16BIT ,
00327         &rect
00328     );
00329     Display.Graphics_Start(DisplayBase::GRAPHICS_LAYER_3 );
00330 }
00331 static void initDrawButtonLayer(void)
00332 {
00333     DisplayBase::rect_t rect;
00334     /* The layer by which the touch panel location is drawn */
00335     memset(user_frame_buffer_draw_button, 0, 
00336                                         sizeof(user_frame_buffer_draw_button));
00337     dcache_clean(user_frame_buffer_draw_button, 
00338                                         sizeof(user_frame_buffer_draw_button));
00339     rect.vs  = 0;
00340     rect.vw  = LCD_PIXEL_HEIGHT;
00341     rect.hs  = 0;
00342     rect.hw  = LCD_PIXEL_WIDTH;
00343     Display.Graphics_Read_Setting(
00344         DisplayBase::GRAPHICS_LAYER_2 ,
00345         (void *)user_frame_buffer_draw_button,
00346         DRAW_BUFFER_STRIDE,
00347         DisplayBase::GRAPHICS_FORMAT_ARGB4444 ,
00348         DisplayBase::WR_RD_WRSWA_32_16BIT ,
00349         &rect
00350     );
00351     Display.Graphics_Start(DisplayBase::GRAPHICS_LAYER_2 );
00352 }
00353 
00354 static void Start_Video(uint8_t * p_buf) {
00355     DisplayBase::graphics_error_t error;
00356 
00357     /* Video capture setting (progressive form fixed) */
00358     error = Display.Video_Write_Setting(
00359                 VIDEO_INPUT_CH,
00360                 COL_SYS,
00361                 p_buf,
00362                 FRAME_BUFFER_STRIDE,
00363                 VIDEO_FORMAT,
00364                 WR_RD_WRSWA,
00365                 VIDEO_PIXEL_VW,
00366                 VIDEO_PIXEL_HW
00367             );
00368     if (error != DisplayBase::GRAPHICS_OK ) {
00369         printf("Line %d, error %d\n", __LINE__, error);
00370         mbed_die();
00371     }
00372 
00373     /* Video write process start */
00374     error = Display.Video_Start(VIDEO_INPUT_CH);
00375     if (error != DisplayBase::GRAPHICS_OK ) {
00376         printf("Line %d, error %d\n", __LINE__, error);
00377         mbed_die();
00378     }
00379 
00380     /* Video write process stop */
00381     error = Display.Video_Stop(VIDEO_INPUT_CH);
00382     if (error != DisplayBase::GRAPHICS_OK ) {
00383         printf("Line %d, error %d\n", __LINE__, error);
00384         mbed_die();
00385     }
00386 
00387     /* Video write process start */
00388     error = Display.Video_Start(VIDEO_INPUT_CH);
00389     if (error != DisplayBase::GRAPHICS_OK ) {
00390         printf("Line %d, error %d\n", __LINE__, error);
00391         mbed_die();
00392     }
00393 }
00394 
00395 /* Select function from button */
00396 APP_MODE getFunctionSelected(int x, int y)
00397 {
00398     int w = 100; //width of image button
00399     if ( MODE_BTN_Y > y )
00400     {
00401         return MODE_UNKNOWN;
00402     }
00403     
00404     if ( MODE_BTN_X <= x && x <= (MODE_BTN_X + w))
00405     {
00406         return FACE_DETECTION;
00407     }
00408     else if ( (2*MODE_BTN_X + w) <= x && x <= (2*MODE_BTN_X + 2*w))
00409     {
00410         return FACE_RECOGNITION;
00411     }
00412     else if ( (3*MODE_BTN_X + 2*w) <= x && x <= (3*MODE_BTN_X + 3*w) )
00413     {
00414         return MOTION_DETECTION;
00415     }
00416     else if ((4*MODE_BTN_X + 3*w) <= x && x <= (4*MODE_BTN_X + 4*w))
00417     {
00418         return GUESTURE_RECOGNITION;
00419     }
00420     else
00421         return MODE_UNKNOWN;
00422 }
00423 
00424 /* Click action in face recognition */
00425 void clickActionFaceReg(int x, int y)
00426 {
00427     mMutexClicked.lock();
00428     if(REGIS_FACE_BTN_X <= x && x <= (REGIS_FACE_BTN_X + 30) &&
00429        REGIS_FACE_BTN_Y <= y && y <= (REGIS_FACE_BTN_Y + 30))
00430     {
00431         clickedCode = CLICKED_REGIS_FACE;
00432     }
00433     else if (FACE_REG_ACT_MENU_X <= x && x <= (FACE_REG_ACT_MENU_X + 60) &&
00434         FACE_REG_ACT_MENU_Y <= y && y <= (FACE_REG_ACT_MENU_Y + 20))
00435     {
00436         clickedCode = CLICKED_CHANGE_ID;
00437     }
00438     else if (FACE_REG_ACT_MENU_X <= x && x <= (FACE_REG_ACT_MENU_X + 60) &&
00439         (FACE_REG_ACT_MENU_Y + 20 + 12)<= y && 
00440         y <= (FACE_REG_ACT_MENU_Y + 2*20 + 12))//space:12
00441     {
00442         clickedCode = CLICKED_ADD;
00443     }
00444     else if (FACE_REG_ACT_MENU_X <= x && x <= (FACE_REG_ACT_MENU_X + 60) &&
00445         (FACE_REG_ACT_MENU_Y + 2*20 + 2*12)<= y && 
00446         y <= (FACE_REG_ACT_MENU_Y + 3*20 + 2*12))
00447     {
00448         clickedCode = CLICKED_IGNORE;
00449     }
00450     else
00451     {
00452         clickedCode = CLICKED_UNKNOWN;
00453     }
00454     mMutexClicked.unlock();
00455 }
00456 
00457 /* Click on screen gesture recognition */
00458 void clickActionGestureReg(int x, int y)
00459 {
00460     mMutexClicked.lock();
00461     if(GESTURE_SAMPLING_BTN_X <= x && x <= (GESTURE_SAMPLING_BTN_X + 60) &&
00462        GESTURE_SAMPLING_BTN_Y <= y && y <= (GESTURE_SAMPLING_BTN_Y + 20))
00463     {
00464         clickedCode = CLICKED_HAND_SAMPLING;
00465     }
00466     else
00467     {
00468         clickedCode = CLICKED_UNKNOWN;
00469     }
00470     mMutexClicked.unlock();
00471 }
00472 
00473 /**** Draw button controls ****/
00474 void drawButtonControls(graphicFrameworkCanvas myCanvasButton)
00475 {
00476     int space = 16;
00477     int x = 0;
00478     int y = MODE_BTN_Y;
00479     
00480     x+=space;
00481     myCanvasButton.drawImage(my_img_button_face_detect,x,y); // 100x40
00482     x+=100;
00483     
00484     x+=space;
00485     myCanvasButton.drawImage(my_img_button_face_recognition,x,y);
00486     x+=100;
00487 
00488     x+=space;
00489     myCanvasButton.drawImage(my_img_button_obj_motion,x,y);
00490     x+=100;
00491     
00492     x+=space;
00493     myCanvasButton.drawImage(my_img_button_gesture_reg,x,y);
00494     x+=100;
00495 }
00496 
00497 void drawTextScreen(graphicFrameworkCanvas myCanvasButton)
00498 {
00499     int x = 160;
00500     int y = 5;
00501     
00502     if( appMode == MOTION_DETECTION )
00503     {
00504         myCanvasButton.drawImage(my_img_text_obj_motion,x, y);
00505     }
00506     else if ( appMode == FACE_DETECTION )
00507     {
00508         myCanvasButton.drawImage(my_img_text_face_detection,x, y);
00509     }
00510     else if ( appMode == FACE_RECOGNITION )
00511     {
00512         myCanvasButton.drawImage(my_img_text_face_recognition,x, y);
00513     }
00514     else if ( appMode == GUESTURE_RECOGNITION )
00515     {
00516         myCanvasButton.drawImage(my_img_text_gesture_reg,x, y);
00517     }
00518 }
00519 
00520 void clearGestureResult(gesture_result res)
00521 {
00522     res.circles.clear();
00523     res.lines.clear();
00524     res.contour.clear();
00525 }
00526 /****** Thread image process ******/
00527 static void img_draw_objects(void)
00528 {
00529     graphicFrameworkCanvas myCanvas(user_frame_buffer_draw, 0x01E0,
00530                     0x0110, (uint8_t)DRAW_BUFFER_BYTE_PER_PIXEL, 
00531                     (uint8_t)DRAW_POINT,0x06);
00532 
00533     initDrawResultLayer();
00534     
00535     uint8_t color[2] = {0x0B,0xFF};// Color Pink
00536     APP_MODE oldMode = appMode;
00537     while(1)
00538     {
00539         Thread::signal_wait(1);        
00540         memset(user_frame_buffer_draw, 0, sizeof(user_frame_buffer_draw));
00541 
00542         if ( oldMode != appMode)
00543         {
00544             oldMode = appMode;
00545             
00546             mMutexObjects.lock();
00547             gIsDrawing = false;
00548             mMutexObjects.unlock();     
00549                    
00550             // Data cache clean 
00551             dcache_clean(user_frame_buffer_draw, sizeof(user_frame_buffer_draw));
00552             continue;
00553         }
00554 
00555         // Draw faces on screen
00556         switch(appMode)
00557         {
00558             case FACE_DETECTION:
00559             case MOTION_DETECTION:
00560             {
00561                 if(gObjectDetects.size() > 0)
00562                 {
00563                     for(int i = 0;i < gObjectDetects.size();i++)
00564                     {
00565                         int _x = gObjectDetects[i].obj.x*IMG_DOWN_SAMPLE;
00566                         int _y = gObjectDetects[i].obj.y*IMG_DOWN_SAMPLE;
00567                         int _w = gObjectDetects[i].obj.width*IMG_DOWN_SAMPLE;
00568                         int _h = gObjectDetects[i].obj.height*IMG_DOWN_SAMPLE;
00569                         myCanvas.drawRect(_x,_y,_w,_h,color);
00570                     }
00571                 }
00572                 gObjectDetects.clear();
00573                 break;
00574             }
00575             case FACE_RECOGNITION:
00576             {
00577                 if(gObjectDetects.size() > 0)
00578                 {
00579                     for(int i = 0;i < gObjectDetects.size();i++)
00580                     {
00581                         int _x = gObjectDetects[i].obj.x*IMG_DOWN_SAMPLE;
00582                         int _y = gObjectDetects[i].obj.y*IMG_DOWN_SAMPLE;
00583                         int _w = gObjectDetects[i].obj.width*IMG_DOWN_SAMPLE;
00584                         int _h = gObjectDetects[i].obj.height*IMG_DOWN_SAMPLE;
00585                         if(gObjectDetects[i].label == 1)
00586                         {
00587                             // Yellow
00588                             color[0] = 0xF0;
00589                             color[1] = 0xFF;
00590                         }
00591                         else if (gObjectDetects[i].label == 2)
00592                         {
00593                             // Green
00594                             color[0] = 0xF0;
00595                             color[1] = 0xF0;
00596                         }
00597                         else
00598                         {
00599                             // Pink
00600                             color[0] = 0x0B;
00601                             color[1] = 0xFF;
00602                         }
00603                         myCanvas.drawRect(_x,_y,_w,_h,color);
00604                     }
00605                 }
00606                 gObjectDetects.clear();
00607                 break;
00608             }
00609             case GUESTURE_RECOGNITION:
00610             {
00611                 uint8_t contoursColor[2] = {0xF0,0xFF};
00612                     
00613                 //Draw max contour
00614                 for(int i = 0; i < gGestureResult.contour.size();i++)//i=0
00615                 {
00616                     myCanvas.draw_pixel(gGestureResult.contour[i].x,
00617                             gGestureResult.contour[i].y,contoursColor);
00618                 }
00619                 // Draw lines, circles
00620                 for(int i = 0;i < gGestureResult.lines.size();i++)
00621                 {
00622                     myCanvas.drawLine(gGestureResult.lines[i].x1,
00623                                       gGestureResult.lines[i].y1,
00624                                       gGestureResult.lines[i].x2,
00625                                       gGestureResult.lines[i].y2,
00626                                       gGestureResult.lines[i].color);
00627                 }
00628                 
00629                 for(int i = 0;i < gGestureResult.circles.size();i++)
00630                 {
00631                     myCanvas.drawCircle(gGestureResult.circles[i].x,
00632                                         gGestureResult.circles[i].y,
00633                                         gGestureResult.circles[i].radius,
00634                                         gGestureResult.circles[i].color);
00635                 }
00636                 break;
00637             }
00638             default:
00639             break;
00640         } // switch
00641 
00642         mMutexObjects.lock();
00643         gIsDrawing = false;
00644         mMutexObjects.unlock();
00645         
00646         // Data cache clean 
00647         dcache_clean(user_frame_buffer_draw, sizeof(user_frame_buffer_draw));
00648     }//while
00649 }
00650 
00651 /* Touch task*/
00652 static void touch_int_callback(void) 
00653 {
00654     semTouch.release();
00655 }
00656 static void touch_task(void) 
00657 {
00658     graphicFrameworkCanvas myCanvasButton(user_frame_buffer_draw_button, 0x01E0,
00659                     0x0110, (uint8_t)DRAW_BUFFER_BYTE_PER_PIXEL, 
00660                     (uint8_t)DRAW_POINT,0x06);
00661     TouchKey::touch_pos_t touch_pos[TOUCH_NUM];
00662     
00663     /* Callback setting */
00664     touch.SetCallback(&touch_int_callback);
00665 
00666     /* Reset touch IC */
00667     touch.Reset();
00668     
00669     initDrawButtonLayer();
00670     drawTextScreen(myCanvasButton);
00671     drawButtonControls(myCanvasButton);
00672     
00673     int x_touch = -1, y_touch = -1;
00674 
00675     set_time(0);//set_time(1498450609);//11:15,Mon-26-June
00676     double seconds = (double)time(NULL);
00677 
00678     while(1)
00679     {
00680         /* Wait touch event */
00681         semTouch.wait();
00682 
00683         touch.GetCoordinates(TOUCH_NUM, touch_pos);
00684 
00685         for (int i = 0; i < TOUCH_NUM; i ++) {
00686             if (touch_pos[i].valid) {
00687                 int new_touch_x = touch_pos[i].x;
00688                 int new_touch_y = touch_pos[i].y;
00689                 APP_MODE mode = getFunctionSelected(new_touch_x, new_touch_y);
00690 
00691                 if(mode != appMode && mode != MODE_UNKNOWN)
00692                 {
00693                     appMode = mode;
00694                     memset(user_frame_buffer_draw_button, 0, sizeof(uint8_t)*LCD_PIXEL_WIDTH*(MODE_BTN_Y)*FRAME_BUFFER_BYTE_PER_PIXEL);
00695                     drawTextScreen(myCanvasButton);
00696                     dcache_clean(user_frame_buffer_draw_button, sizeof(user_frame_buffer_draw_button));
00697                 }
00698                 else if (appMode == FACE_RECOGNITION)
00699                 {
00700                     double time_1 = (double)time(NULL) - seconds;
00701                     int posAbs = abs(new_touch_x - x_touch) + 
00702                                       abs(new_touch_y - y_touch); 
00703                     
00704                     if(posAbs > 5 || time_1 > 0.5) //1s
00705                     {
00706                         x_touch = new_touch_x;
00707                         y_touch = new_touch_y;
00708 
00709                         clickActionFaceReg(new_touch_x,new_touch_y);
00710 
00711                     }
00712                     seconds = (double)time(NULL);
00713                 }
00714                 else if (appMode == GUESTURE_RECOGNITION)
00715                 {
00716                     double time_1 = (double)time(NULL) - seconds;
00717                     int posAbs = abs(new_touch_x - x_touch) + 
00718                                       abs(new_touch_y - y_touch); 
00719 
00720                     if(posAbs > 5 || time_1 > 0.5) //1s
00721                     {
00722                         x_touch = new_touch_x;
00723                         y_touch = new_touch_y;
00724                         clickActionGestureReg(new_touch_x,new_touch_y);
00725                     }
00726 
00727                     seconds = (double)time(NULL);
00728                 }
00729             }
00730         }
00731     }
00732 }
00733 
00734 /****** Video input is output to LCD ******/
00735 static void video_lcd_task(void) {
00736     DisplayBase::graphics_error_t error;
00737     int wk_num;
00738 
00739     /* Initialization memory */
00740     for (int i = 0; i < FRAME_BUFFER_NUM; i++) {
00741         memset(FrameBufferTbl[i], 0, (FRAME_BUFFER_STRIDE * LCD_PIXEL_HEIGHT));
00742         dcache_clean(FrameBufferTbl[i],(FRAME_BUFFER_STRIDE * LCD_PIXEL_HEIGHT));
00743     }
00744 
00745     /* Start of Video */
00746     Start_Video(FrameBufferTbl[write_buff_num]);
00747 
00748     /* Wait for first video drawing */
00749     Thread::signal_wait(1);
00750     write_buff_num++;
00751     if (write_buff_num >= FRAME_BUFFER_NUM) {
00752         write_buff_num = 0;
00753     }
00754     error = Display.Video_Write_Change(VIDEO_INPUT_CH, 
00755                         FrameBufferTbl[write_buff_num], FRAME_BUFFER_STRIDE);
00756     if (error != DisplayBase::GRAPHICS_OK ) {
00757         printf("Line %d, error %d\n", __LINE__, error);
00758         mbed_die();
00759     }
00760 
00761     /* Start of LCD */
00762     Start_LCD_Display(FrameBufferTbl[read_buff_num]);
00763 
00764     /* Backlight on */
00765     Thread::wait(200);
00766     lcd_cntrst.write(1.0);
00767 
00768     while (1) {
00769         Thread::signal_wait(1);
00770         wk_num = write_buff_num + 1;
00771         if (wk_num >= FRAME_BUFFER_NUM) {
00772             wk_num = 0;
00773         }
00774         /* If the next buffer is empty, it's changed. */
00775         if (wk_num != read_buff_num) {
00776             read_buff_num  = write_buff_num;
00777             write_buff_num = wk_num;
00778             
00779             /* Change video buffer */
00780             error = Display.Video_Write_Change(VIDEO_INPUT_CH, 
00781                     FrameBufferTbl[0/*write_buff_num*/], FRAME_BUFFER_STRIDE);
00782             if (error != DisplayBase::GRAPHICS_OK ) {
00783                 printf("Line %d, error %d\n", __LINE__, error);
00784                 mbed_die();
00785             }
00786             
00787             mMutexProcess.lock();
00788             if ( gIsProcessing == false)
00789             {
00790                 gIsProcessing = true;    
00791                 mMutexProcess.unlock();
00792 
00793                 memcpy((void *)my_frame,(void*)FrameBufferTbl[0],
00794                                         FRAME_BUFFER_STRIDE * LCD_PIXEL_HEIGHT);
00795                 semProcessThread.release();
00796             }
00797             else
00798             {
00799                 mMutexProcess.unlock();
00800             }
00801             
00802             /* Change LCD buffer */
00803             Display.Graphics_Read_Change(DisplayBase::GRAPHICS_LAYER_0 , 
00804                                     (void *)FrameBufferTbl[0/*read_buff_num*/]);
00805         }
00806     }
00807 }
00808 
00809 bool initSdCard()
00810 {
00811     if(myStorage == NULL)
00812     {
00813         return false;
00814     }
00815     if (myStorage->isConnectSdCard() == STORG_PASS) 
00816     {
00817         if(myStorage->mountSdCard() == STORG_PASS)
00818         {
00819             return true;
00820         }
00821         else
00822         {
00823             // mount sdcard failed!
00824             return false;
00825         }
00826     }
00827     else
00828     {
00829         // no sdcard!
00830         togle_led(LED_RED);
00831         return false;
00832     }
00833 }
00834 void faceDectectionApp(void)
00835 {
00836     Size face_size(LCD_PIXEL_WIDTH/IMG_DOWN_SAMPLE,
00837                                             LCD_PIXEL_HEIGHT/IMG_DOWN_SAMPLE);
00838     Mat smallImage = Mat::zeros(face_size, CV_8UC1);
00839     CascadeClassifier haar_cascade;
00840     vector<Rect> faces;
00841     if (!haar_cascade.load(HAAR_CASCADE_FACE_PATH))
00842     {
00843         // load failed
00844         togle_led(LED_RED);
00845         while(1)
00846         {
00847             wait(0.5);
00848         }
00849     }
00850     else{
00851         togle_led(LED_GREEN);
00852     }
00853     togle_reset(LED_RED,LED_BLUE);
00854     faces.clear();
00855     
00856     gObjectDetects.clear();
00857     while (1) {
00858         semProcessThread.wait();
00859         
00860         if ( appMode != FACE_DETECTION )
00861         {
00862             mMutexProcess.lock();
00863             gIsProcessing = false;
00864             mMutexProcess.unlock();
00865             break;
00866         }
00867         
00868         Mat gray;
00869         {
00870             Mat res = _m_cvtRgb5652Rgb(my_frame,(int)LCD_PIXEL_WIDTH,
00871                                                     (int)LCD_PIXEL_HEIGHT,2);
00872             cvtColor(res,gray,COLOR_RGB2GRAY);
00873             res.release();
00874         }
00875 
00876         equalizeHist(gray,gray);
00877         resize(gray, smallImage, face_size);
00878 
00879         haar_cascade.detectMultiScale(smallImage,faces,1.1,
00880                                         2,0|CV_HAAR_SCALE_IMAGE,Size(10,10));        
00881         mMutexObjects.lock();
00882         if(gIsDrawing == false)
00883         {
00884             mMutexObjects.unlock();
00885             for(int i = 0;i < faces.size();i++)
00886             {
00887                 obj_detect_result tmp;
00888                 tmp.obj = faces[i];
00889                 tmp.label = -1;
00890                 gObjectDetects.push_back(tmp);
00891             }
00892             //gObjectDetects = faces;
00893             gIsDrawing = true;
00894             p_DrawObjects->signal_set(1);
00895         }
00896         else
00897         {
00898             mMutexObjects.unlock();
00899         }
00900 
00901         /* Reset flag */
00902         faces.clear();
00903         mMutexProcess.lock();
00904         gIsProcessing = false;
00905         mMutexProcess.unlock();
00906     };
00907 }
00908 
00909 void motionDetectionApp(void)
00910 {
00911     bool isNewStart = true;
00912     int threshValue = 30;
00913     int blurSize = 5;
00914     float mArea = 0.0;
00915     float mMaxArea = 0.0;
00916     int mMaxIndex = 0;
00917     bool _isDrawing;
00918     int mCounter = 0;
00919     Mat curFrameGray;
00920     Mat prevFrameGray;
00921     Mat diffImage;
00922     Mat binImage;
00923     vector<vector<Point> > contours;
00924     vector<Rect> _objects;
00925 
00926     gObjectDetects.clear();
00927     while(1)
00928     {
00929         semProcessThread.wait();
00930         
00931         if ( appMode != MOTION_DETECTION )
00932         {
00933             mMutexProcess.lock();
00934             gIsProcessing = false;
00935             mMutexProcess.unlock();
00936             
00937             break;
00938         }
00939 
00940         _objects.clear();
00941         mCounter++;
00942         if( isNewStart == false)
00943         {
00944             {
00945                 Mat res = _m_cvtRgb5652Rgb(my_frame,(int)LCD_PIXEL_WIDTH,
00946                                                     (int)LCD_PIXEL_HEIGHT,2);
00947                 cvtColor(res,curFrameGray,COLOR_RGB2GRAY);
00948                 res.release();
00949             }
00950             /* differential between foreground and background */
00951             absdiff(curFrameGray, prevFrameGray, diffImage);
00952             blur(diffImage, diffImage, Size(blurSize, blurSize));
00953             threshold(diffImage, binImage, threshValue, 255, CV_THRESH_BINARY);
00954             findContours(binImage, contours, CV_RETR_EXTERNAL, 
00955                                                         CV_CHAIN_APPROX_NONE);
00956 
00957             if(contours.size() > 0)
00958             {
00959                 for (int i = 0; i < contours.size(); i++)
00960                 {
00961                     mArea = contourArea(contours[i]);
00962                     if (mArea > mMaxArea)
00963                     {
00964                         mMaxArea = mArea;
00965                         mMaxIndex = i;
00966                     }
00967                 }
00968                 
00969                 if (mMaxArea > MAX_COUNTOURS)
00970                 {
00971                     Rect objectBoundingRectangle = boundingRect(
00972                                                         contours.at(mMaxIndex));
00973                     _objects.push_back(objectBoundingRectangle);
00974                 }
00975             }
00976             
00977             /* Set display motion objects */
00978             mMutexObjects.lock();
00979             _isDrawing = gIsDrawing;
00980             mMutexObjects.unlock();
00981             
00982             if(_isDrawing == false)
00983             {
00984                 for(int i = 0;i < _objects.size();i++)
00985                 {
00986                     obj_detect_result tmp;
00987                     tmp.obj = _objects[i];
00988                     tmp.label = -1;
00989                     gObjectDetects.push_back(tmp);
00990                 }
00991                 //gObjectDetects = _objects;
00992                 gIsDrawing = true;
00993                 p_DrawObjects->signal_set(1);
00994             }
00995             
00996             /* Reset values */
00997             mMaxArea = 0;
00998             mArea = 0;
00999             mMaxIndex = 0;
01000             
01001             /* Update background */
01002             if(mCounter == 50)
01003             {
01004                 mCounter = 0;
01005                 isNewStart = true;
01006             }
01007         }
01008         else
01009         {
01010             isNewStart = false;
01011             prevFrameGray = _m_cvtRgb5652Rgb(my_frame,(int)LCD_PIXEL_WIDTH,
01012                                                     (int)LCD_PIXEL_HEIGHT,2);
01013             cvtColor(prevFrameGray,prevFrameGray,COLOR_RGB2GRAY);
01014         }
01015         
01016         mMutexProcess.lock();
01017         gIsProcessing = false;
01018         mMutexProcess.unlock();
01019     };
01020 }
01021 
01022 void faceRecognitionApp(void)
01023 {
01024     // Init draw
01025     graphicFrameworkCanvas myCanvas(user_buf_draw_action_888, 0x01E0,
01026                     0x0110, 0x04, 
01027                     (uint8_t)DRAW_POINT,0x06);
01028 
01029     uint8_t label_id = 1;
01030     bool oldStateFace = false;// no faces
01031     // End init draw
01032     
01033     Size face_size(LCD_PIXEL_WIDTH/IMG_DOWN_SAMPLE,
01034                                             LCD_PIXEL_HEIGHT/IMG_DOWN_SAMPLE);
01035     Mat smallImage = Mat::zeros(face_size, CV_8UC1);
01036     CascadeClassifier haar_cascade;
01037     vector<Rect> faces;
01038     
01039     //Init recognizer
01040     int radius = 2, neighbors = 4, gridx = 8, gridy = 8, thresmax = 50;
01041     Ptr<LBPHFaceRecognizer> modelReg = createLBPHFaceRecognizer (radius, neighbors,
01042                                     gridx, gridy, thresmax);
01043     face_database_t face_database;
01044     Size regSize(70,70);
01045     bool flag_class = false;
01046     bool flag_train = false;
01047     bool isFirstTrain = true;
01048     bool changeApp = false;
01049     
01050     if (!haar_cascade.load(HAAR_CASCADE_FACE_PATH))
01051     {
01052         // load failed
01053         togle_led(LED_RED);
01054         while(1)
01055         {
01056             wait(0.5);
01057         }
01058     }
01059     else{
01060         togle_led(LED_GREEN);
01061     }
01062     togle_reset(LED_RED,LED_BLUE);
01063     faces.clear();
01064     
01065     gObjectDetects.clear();
01066     while(1)
01067     {
01068         semProcessThread.wait();
01069 
01070         if ( appMode != FACE_RECOGNITION )
01071         {
01072             mMutexProcess.lock();
01073             gIsProcessing = false;
01074             mMutexProcess.unlock();
01075             memset(user_buf_draw_action_888, 0, 
01076                                     sizeof(user_buf_draw_action_888));     
01077             break;
01078         }
01079         
01080         Mat gray;
01081         Mat imgRgb = _m_cvtRgb5652Rgb(my_frame,(int)LCD_PIXEL_WIDTH,
01082                                                     (int)LCD_PIXEL_HEIGHT,2);
01083         cvtColor(imgRgb,gray,COLOR_RGB2GRAY);
01084 
01085         equalizeHist(gray,gray);
01086         resize(gray, smallImage, face_size);
01087 
01088         haar_cascade.detectMultiScale(smallImage,faces,1.1,
01089                                         2,0|CV_HAAR_SCALE_IMAGE,Size(10,10));
01090         
01091         mMutexObjects.lock();
01092         if(gIsDrawing == false)
01093         {
01094             mMutexObjects.unlock();
01095 
01096             if( faces.size() > 0)
01097             {
01098                 // Show label
01099                 if (oldStateFace == false)
01100                 {
01101                     oldStateFace = true;
01102                     myCanvas.drawImage(my_img_register_face,    
01103                                         REGIS_FACE_BTN_X ,REGIS_FACE_BTN_Y);
01104                 }
01105                 
01106                 // Click update database
01107                 mMutexClicked.lock();
01108                 if(clickedCode == CLICKED_REGIS_FACE)
01109                 {
01110                     mMutexClicked.unlock();
01111                     
01112                     // Clean draw faces
01113                     mMutexObjects.lock();
01114                     if(gIsDrawing == false)
01115                     {
01116                         mMutexObjects.unlock();
01117                         gObjectDetects.clear();
01118                         gIsDrawing = true;
01119                         p_DrawObjects->signal_set(1);
01120                     }
01121                     else
01122                     {
01123                         mMutexObjects.unlock();
01124                     }
01125 
01126                     // Update database/ train data
01127                     face_database.database_image.clear();
01128                     face_database.database_label.clear();
01129                     flag_train = false;
01130                     
01131                     for (int i = 0; i < faces.size(); i++)
01132                     {
01133                         // Check exit app
01134                         if(changeApp == true)
01135                         {
01136                             break;
01137                         }
01138                         mMutexClicked.lock();
01139                         clickedCode = CLICKED_UNKNOWN;
01140                         mMutexClicked.unlock();
01141                         
01142                         int _x = faces[i].x*IMG_DOWN_SAMPLE;
01143                         int _y = faces[i].y*IMG_DOWN_SAMPLE;
01144                         int _w = faces[i].width*IMG_DOWN_SAMPLE;
01145                         int _h = faces[i].height*IMG_DOWN_SAMPLE;
01146                         
01147                         Rect roi(_x,_y,_w,_h);
01148                         
01149                         Mat imgShow = imgRgb(roi);
01150                         if(_w > 100)
01151                         {
01152                             resize(imgShow,imgShow,Size(100,100));
01153                             _w = 100;
01154                             _h = 100;
01155                         }
01156 
01157                         uint8_t* src = cvtMat2RGBA888(_w,_h,imgShow);
01158                         
01159                         label_id = 1;
01160                         
01161                         memset(user_buf_draw_action_888, 0, 
01162                                     sizeof(user_buf_draw_action_888));
01163                         myCanvas.drawImage(src,100, 90);
01164                         myCanvas.drawImage(my_img_id_01,    
01165                                         FACE_REG_ID_MENU_X ,FACE_REG_ID_MENU_Y);
01166                         myCanvas.drawImage(my_img_change_id,    
01167                                 FACE_REG_ACT_MENU_X ,FACE_REG_ACT_MENU_Y);
01168                         myCanvas.drawImage(my_img_add_this,    
01169                             FACE_REG_ACT_MENU_X ,FACE_REG_ACT_MENU_Y + 12 + 20);
01170                         myCanvas.drawImage(my_img_ignore,    
01171                             FACE_REG_ACT_MENU_X ,FACE_REG_ACT_MENU_Y+ 24 + 40);
01172                         
01173                         /* Clean cache */
01174                         dcache_clean(user_buf_draw_action_888, 
01175                                     sizeof(user_buf_draw_action_888));
01176                         while(1)
01177                         {             
01178                             bool isBreak = false;               
01179                             mMutexClicked.lock();
01180                             CLICKED_CODE codeTmp = clickedCode;
01181                             mMutexClicked.unlock();
01182                             
01183                             // Check exit app
01184                             if ( appMode != FACE_RECOGNITION )
01185                             {
01186                                 changeApp = true;     
01187                                 isBreak = true;
01188                                 break;
01189                             }
01190                             
01191                             switch(codeTmp)
01192                             {
01193                                 case CLICKED_CHANGE_ID:
01194                                 {
01195                                     memset(user_buf_draw_action_888 + FACE_REG_ID_MENU_Y*4*LCD_PIXEL_WIDTH, 0, 
01196                                                     21*4*LCD_PIXEL_WIDTH);// clear 21 lines
01197                                     // Change ID label
01198                                     if(label_id == 1)
01199                                     {
01200                                         label_id = 2;
01201                                         myCanvas.drawImage(my_img_id_02,    
01202                                             FACE_REG_ID_MENU_X ,FACE_REG_ID_MENU_Y);
01203                                     }
01204                                     else
01205                                     {
01206                                         label_id = 1;
01207                                         myCanvas.drawImage(my_img_id_01,
01208                                             FACE_REG_ID_MENU_X ,FACE_REG_ID_MENU_Y);
01209                                     }
01210                                     
01211                                     /* Clean cache */
01212                                     dcache_clean(user_buf_draw_action_888, 
01213                                             sizeof(user_buf_draw_action_888));
01214                                             
01215                                     clickedCode = CLICKED_UNKNOWN;
01216                                     break;
01217                                 }
01218                                 case CLICKED_ADD:
01219                                 {
01220                                     
01221                                     Mat imgReg = gray(roi);
01222                                     resize(imgReg,imgReg,regSize);
01223                                     //train face
01224                                     face_database.database_image.push_back(imgReg);
01225                                     face_database.database_label.push_back(label_id);
01226                                     flag_train = true;
01227                                     isBreak = true;
01228                                     break;
01229                                 }
01230                                 case CLICKED_IGNORE:
01231                                 {
01232                                     //ignore
01233                                     isBreak = true;
01234                                     break;
01235                                 }
01236                                 default:
01237                                 break;
01238                             }
01239                             
01240                             if(isBreak)
01241                                 break;
01242                             wait(0.1);
01243                         }
01244                     }// for faces.size()
01245                     
01246                     if( flag_train == true)
01247                     {
01248                         if(isFirstTrain == true)
01249                         {
01250                             isFirstTrain = false;
01251                             modelReg->train(face_database.database_image, face_database.database_label);
01252                         }
01253                         else
01254                         {
01255                             modelReg->update(face_database.database_image, face_database.database_label);
01256                         }
01257                     }
01258                     
01259                     // Clean screen
01260                     memset(user_buf_draw_action_888, 0, 
01261                                     sizeof(user_buf_draw_action_888));
01262                     oldStateFace = false;
01263 
01264                     mMutexClicked.lock();
01265                     clickedCode = CLICKED_UNKNOWN;
01266                     mMutexClicked.unlock();
01267                 } //clickedCode == CLICKED_REGIS_FACE
01268                 else
01269                 {   
01270                     mMutexClicked.unlock();
01271                     if (isFirstTrain == false)
01272                     {
01273                         for(int i = 0; i < faces.size();i++)
01274                         {
01275                             int _x = faces[i].x*IMG_DOWN_SAMPLE;
01276                             int _y = faces[i].y*IMG_DOWN_SAMPLE;
01277                             int _w = faces[i].width*IMG_DOWN_SAMPLE;
01278                             int _h = faces[i].height*IMG_DOWN_SAMPLE;
01279                             
01280                             Rect roi(_x,_y,_w,_h);
01281                             Mat target_face = gray(roi);
01282                             resize(target_face,target_face,regSize);
01283                             int predicted = -1;
01284                             double confidence_level = 0;
01285                             modelReg->predict(target_face, predicted, confidence_level);
01286                             
01287                             obj_detect_result tmp;
01288                             tmp.obj = faces[i];
01289                  
01290                             if(predicted != -1)
01291                             {
01292                                 if (confidence_level < thresmax)
01293                                 {
01294                                     tmp.label = predicted;
01295                                 }
01296                                 else
01297                                 {
01298                                     tmp.label = -1;
01299                                 }
01300                             }
01301                             else
01302                             {
01303                                 tmp.label = -1;
01304                             }
01305                             gObjectDetects.push_back(tmp);
01306                         }
01307                     }
01308                     else
01309                     {
01310                         for(int i = 0;i < faces.size();i++)
01311                         {
01312                             obj_detect_result tmp;
01313                             tmp.obj = faces[i];
01314                             tmp.label = -1;
01315                             gObjectDetects.push_back(tmp);
01316                         }
01317                     }
01318                     
01319                     gIsDrawing = true;
01320                     p_DrawObjects->signal_set(1);
01321                 }//clickedCode != CLICKED_REGIS_FACE
01322             } //faces.size() > 0
01323             else
01324             {
01325                 oldStateFace = false;
01326                 memset(user_buf_draw_action_888, 0, 
01327                                     sizeof(user_buf_draw_action_888));
01328                 mMutexClicked.lock();
01329                 clickedCode = CLICKED_UNKNOWN;
01330                 mMutexClicked.unlock();
01331                 
01332                 // Clean draw faces
01333                 mMutexObjects.lock();
01334                 if(gIsDrawing == false)
01335                 {
01336                     mMutexObjects.unlock();
01337                     gObjectDetects.clear();
01338                     gIsDrawing = true;
01339                     p_DrawObjects->signal_set(1);
01340                 }
01341                 else
01342                 {
01343                     mMutexObjects.unlock();
01344                 }
01345             }
01346         }
01347         else
01348         {
01349             mMutexObjects.unlock();
01350         }
01351         
01352         /* Reset flag */
01353         faces.clear();
01354         mMutexProcess.lock();
01355         gIsProcessing = false;
01356         mMutexProcess.unlock();
01357         
01358         /* Clean cache */
01359         dcache_clean(user_buf_draw_action_888, 
01360                                     sizeof(user_buf_draw_action_888));
01361     }
01362 }
01363 
01364 /* 2 functions for gesture recognition */
01365 int distanceP2P(Point2f  a, Point2f  b)
01366 {
01367     int d = (int)sqrt(fabs(pow(a.x - b.x, 2) + pow(a.y - b.y, 2)));
01368     return d;
01369 }
01370 
01371 float getAngle(Point  s, Point  f, Point  e){
01372     float l1 = distanceP2P(f, s);
01373     float l2 = distanceP2P(f, e);
01374     float dot = (s.x - f.x)*(e.x - f.x) + (s.y - f.y)*(e.y - f.y);
01375     float angle = acos(dot / (l1*l2));
01376     angle = angle * 180 / M_PI;
01377     return angle;
01378 }
01379 
01380 void drawButtonSampling(graphicFrameworkCanvas canvas,bool sampling)
01381 {
01382     // Clear 21 lines
01383     memset(user_buf_draw_action_888 + (GESTURE_SAMPLING_BTN_Y - 1)*
01384         LCD_PIXEL_WIDTH*4, 0, 
01385         21*LCD_PIXEL_WIDTH*4);
01386     if(sampling == true) // Sampling, Clicking stop, draw sampling
01387     { 
01388         canvas.drawImage(my_img_sampling, GESTURE_SAMPLING_BTN_X, 
01389                             GESTURE_SAMPLING_BTN_Y);
01390     }
01391     else
01392     {
01393         canvas.drawImage(my_img_stop, GESTURE_SAMPLING_BTN_X, 
01394                             GESTURE_SAMPLING_BTN_Y);
01395     }
01396 }
01397 
01398 void gestureRegconition(void)
01399 {
01400     // Init draw
01401     graphicFrameworkCanvas myCanvas(user_buf_draw_action_888, 0x01E0,
01402                     0x0110, 0x04, 
01403                     (uint8_t)DRAW_POINT,0x06);
01404 
01405     bool sampling = true;
01406     int kernelSize = 7;
01407     int thresValue = 100;
01408     int maxIndex = 0;
01409     double area = 0.0;
01410     unsigned long lArea = 0, maxArea = 0;
01411     int hullSize = 0;
01412     bool handSamp = true;
01413     bool isFinger = false;
01414     int oneFinger = 0;
01415     int countFinger = 0;
01416     Sampling handdist;
01417     Point2f  mPa;
01418     ConvexPoint checkPoint;
01419     
01420     Mat imgGray;
01421     Mat blurImg;
01422     Mat thresholdImg;
01423     vector<vector<Point> > contours;
01424     vector<mLine> lLines;
01425     vector<mCircle> lCircles;
01426     vector<Point> lMaxContour;
01427     
01428     myCanvas.drawImage(my_img_stop, GESTURE_SAMPLING_BTN_X, 
01429                                     GESTURE_SAMPLING_BTN_Y);
01430     mMutexClicked.lock();
01431     clickedCode = CLICKED_UNKNOWN;
01432     mMutexClicked.unlock();
01433     clearGestureResult(gGestureResult);
01434     while(1)
01435     {
01436         semProcessThread.wait();
01437         
01438         if ( appMode != GUESTURE_RECOGNITION )
01439         {
01440             mMutexProcess.lock();
01441             gIsProcessing = false;
01442             mMutexProcess.unlock();
01443             memset(user_buf_draw_action_888, 0, 
01444                                     sizeof(user_buf_draw_action_888));   
01445             clearGestureResult(gGestureResult);  
01446             break;
01447         }
01448         
01449         //Draw button sampling
01450         mMutexClicked.lock();
01451         if (clickedCode == CLICKED_HAND_SAMPLING)
01452         {
01453             clickedCode = CLICKED_UNKNOWN;
01454             mMutexClicked.unlock();
01455             drawButtonSampling(myCanvas,sampling);
01456             sampling = !sampling;
01457         }
01458         else
01459         {
01460             mMutexClicked.unlock();
01461         }
01462 
01463         // Get frame camera
01464         {
01465             Mat res = _m_cvtRgb5652Rgb(my_frame,(int)LCD_PIXEL_WIDTH,
01466                                                 (int)LCD_PIXEL_HEIGHT,2);
01467             cvtColor(res,imgGray,COLOR_RGB2GRAY);
01468             res.release();
01469         }
01470         
01471         //Clear old data
01472         lLines.clear();
01473         lCircles.clear();
01474         lMaxContour.clear();
01475         
01476         // Start processing
01477         blur(imgGray,blurImg,Size(kernelSize,kernelSize));
01478         threshold(blurImg,thresholdImg,thresValue,255,CV_THRESH_BINARY);
01479 
01480         checkPoint.Pointstart.clear();
01481         checkPoint.Pointdepth.clear();
01482         checkPoint.Pointindex.clear();
01483         checkPoint.detecthand = 0;
01484         
01485         findContours(thresholdImg,contours,CV_RETR_EXTERNAL,
01486                                             CV_CHAIN_APPROX_SIMPLE,Point (0,0));
01487 
01488         if(contours.size() > 0)
01489         {
01490             //togle_led(LED_GREEN);
01491             maxIndex = 0;
01492             vector<vector<int> > hull(contours.size());
01493             vector<vector<Vec4i> > convDef(contours.size());
01494             vector<vector<Point> > hull_point(contours.size());
01495             vector<vector<Point> > detectPoint(contours.size());
01496             
01497             for(int i = 0;i < contours.size();i++)
01498             {
01499                 area = contourArea(contours[i]);
01500                 lArea = long(area);
01501                 if(lArea > maxArea)
01502                 {
01503                     hullSize++;
01504                     maxArea = lArea;
01505                     maxIndex = i;
01506                 }
01507             }
01508             
01509             if(maxArea > 50000)
01510             {
01511                 //togle_reset(LED_RED,LED_BLUE);
01512                 checkPoint.detecthand = 1;
01513                 convexHull(Mat(contours[maxIndex]),hull[maxIndex],false);
01514                 convexityDefects(contours[maxIndex],hull[maxIndex],convDef[maxIndex]);
01515             }
01516             
01517             for (int i = 0;i < convDef[maxIndex].size();i++)
01518             {
01519                 if(convDef[maxIndex][i][3] > 20*256)
01520                 {
01521                     int ind_0 = convDef[maxIndex][i][0];
01522                     int ind_1 = convDef[maxIndex][i][2];
01523                     checkPoint.Pointstart.push_back(contours[maxIndex][ind_0]);
01524                     checkPoint.Pointdepth.push_back(contours[maxIndex][ind_1]);
01525                     checkPoint.Pointindex.push_back(ind_1);
01526                 }
01527             }
01528 
01529             vector<Moments> mu(contours.size());
01530             vector<Point2d> mc(contours.size());    
01531             mu[maxIndex] = moments(contours[maxIndex],false);
01532             mc[maxIndex] = Point2f (mu[maxIndex].m10 / mu[maxIndex].m00,
01533                                    mu[maxIndex].m01 / mu[maxIndex].m00);
01534             mPa = mc[maxIndex];
01535 
01536             // Reset values
01537             lArea = 0;
01538             maxArea = 0;
01539             hullSize = 0;
01540             area = 0;
01541             
01542             
01543             // Draw contours
01544             Mat imgContour(LCD_PIXEL_HEIGHT,LCD_PIXEL_WIDTH,CV_8UC1,Scalar (0));
01545             drawContours(imgContour,contours,maxIndex,Scalar (255));
01546             for(int i=0;i<imgContour.rows;i++)
01547             {
01548                 for(int j=0;j<imgContour.cols;j++)
01549                 {
01550                     if(imgContour.at<uchar>(i,j) == 255)
01551                     {
01552                         lMaxContour.push_back(Point (j,i));
01553                     }
01554                 }
01555             }
01556             
01557             
01558         }// contours.size() > 0
01559         else
01560         {
01561             checkPoint.detecthand = 0;
01562         }
01563         
01564         if(sampling == true)
01565         {
01566             if (checkPoint.Pointstart.size() > 1)
01567             {
01568                 for(int i = 0;i < checkPoint.Pointstart.size() -1;i++)
01569                 {
01570                     float pAngle = getAngle(checkPoint.Pointstart[i],mPa,
01571                                                     checkPoint.Pointstart[i+1]);
01572                     float fAngle = getAngle(checkPoint.Pointstart[1],mPa,
01573                                                     checkPoint.Pointstart[i+1]);
01574     
01575                     if(pAngle < 90.00 && pAngle > 5.00 && fAngle < 180)
01576                     {
01577                         double pFdist = distanceP2P(mPa,
01578                                                     checkPoint.Pointstart[i]);
01579                         double pLdist = distanceP2P(mPa,
01580                                                     checkPoint.Pointdepth[i]);
01581                         double _Pmax = handdist.hand_max;
01582                         double _Pmin = handdist.hand_min;
01583                         if (pFdist > pLdist)
01584                         {
01585                             if (pFdist > _Pmax)
01586                             {
01587                                 handdist.hand_max = pFdist;
01588                             }
01589                             if (pLdist < _Pmin)
01590                             {
01591                                 handdist.hand_min = pLdist;
01592                             }
01593                         }
01594                         else
01595                         {
01596                             if (pLdist > _Pmin)
01597                             {
01598                                 handdist.hand_max = pLdist;
01599                             }
01600                             if (pFdist < _Pmin)
01601                             {
01602                                 handdist.hand_min = pFdist;
01603                             }
01604                         }
01605                     }
01606                 }
01607             }
01608         }//sampling == true
01609         else
01610         {
01611             if(checkPoint.detecthand == 1)
01612             {
01613                 if (checkPoint.Pointstart.size() > 1)
01614                 {
01615                     for(int i = 0;i < checkPoint.Pointstart.size() -1;i++)
01616                     {
01617                         float pAngle = getAngle(checkPoint.Pointstart[i],mPa,
01618                                                     checkPoint.Pointstart[i+1]);
01619                         float fAngle = getAngle(checkPoint.Pointstart[1],mPa,
01620                                                     checkPoint.Pointstart[i+1]);
01621 
01622                         // Draw line
01623                         mLine aLine;
01624                         aLine.x1 = (int)checkPoint.Pointstart[i].x;
01625                         aLine.y1 = (int)checkPoint.Pointstart[i].y;
01626                         aLine.x2 = (int)mPa.x;
01627                         aLine.y2 = (int)mPa.y;
01628                         aLine.color[0] = 0x00;//Color red
01629                         aLine.color[1] = 0xFF;
01630                         lLines.push_back(aLine);
01631                         //line(showRes,checkPoint.Pointstart[i],mPa,
01632 //                                                            Scalar(255,255,0));
01633                         //Draw circle
01634                         mCircle cir;
01635                         cir.x = (int)checkPoint.Pointstart[i].x;
01636                         cir.y = (int)checkPoint.Pointstart[i].y;
01637                         cir.radius = 10;
01638                         cir.color[0] = 0xF0;
01639                         cir.color[1] = 0xF0;//Color green
01640                         lCircles.push_back(cir);
01641                         
01642                         
01643                         //circle(showRes,checkPoint.Pointstart[i],10,
01644 //                                                            Scalar(0,255,0));
01645     
01646                         if(pAngle < 90.00 && pAngle > 5.00 && fAngle < 180)
01647                         {
01648                             double pFdist = distanceP2P(mPa,
01649                                                     checkPoint.Pointstart[i]);
01650                             if(pFdist > (handdist.hand_min + 110) && 
01651                                 pFdist < (handdist.hand_max + 40))
01652                             {
01653                                 isFinger = true;
01654                                 //line(showRes,checkPoint.Pointstart[i],mPa,Scalar(255,255,255));
01655                                 aLine.x1 = (int)checkPoint.Pointstart[i].x;
01656                                 aLine.y1 = (int)checkPoint.Pointstart[i].y;
01657                                 aLine.x2 = (int)mPa.x;
01658                                 aLine.y2 = (int)mPa.y;// Keep color red
01659                                 lLines.push_back(aLine);
01660                                 
01661                                 oneFinger++;
01662                             }
01663                             else
01664                             {
01665                                 isFinger = false;
01666                             }
01667     
01668                             if(isFinger == true)
01669                             {
01670                                 //circle(showRes,mPa,handdist.hand_min,Scalar(255,0,0));
01671                                 cir.x = (int)mPa.x;
01672                                 cir.y = (int)mPa.y;
01673                                 cir.radius = (int)handdist.hand_min;
01674                                 lCircles.push_back(cir);//color green
01675                                 
01676                                 //circle(showRes,mPa,handdist.hand_min + 110,Scalar(255,0,0));
01677                                 cir.radius = (int)handdist.hand_min + 110;
01678                                 lCircles.push_back(cir);//color green
01679                                 
01680                                 double pLdist = distanceP2P(mPa,checkPoint.Pointdepth[i]);
01681                                 if(pLdist > handdist.hand_min - 5)
01682                                 {
01683                                     countFinger++;
01684                                     //circle(showRes,contours[maxIndex][checkPoint.Pointindex[i]],2,Scalar(0,255,255));
01685                                     cir.x = (int)contours[maxIndex][checkPoint.Pointindex[i]].x;
01686                                     cir.y = (int)contours[maxIndex][checkPoint.Pointindex[i]].y;
01687                                     cir.radius = 2;
01688                                     lCircles.push_back(cir);//color green
01689                                 }
01690                             }
01691                         }
01692                     }// for
01693                     if(isFinger == false)
01694                     {
01695                         for(int i = 0; i < checkPoint.Pointstart.size() -1;i++)
01696                         {
01697                             double pLdist = distanceP2P(mPa,
01698                                                     checkPoint.Pointdepth[i]);
01699                             if (pLdist > (handdist.hand_min - 5))
01700                             {
01701                                 countFinger = 0;
01702                                 isFinger = true;
01703                             }
01704                         }
01705                     }
01706                 }//checkPoint.Pointstart.size() > 1
01707             }//checkPoint.detecthand == 1
01708         }
01709         
01710         // Draw
01711         mMutexObjects.lock();
01712         if(gIsDrawing == false)
01713         {
01714             mMutexObjects.unlock();
01715             
01716             // Assign data
01717             gGestureResult.circles = lCircles;
01718             gGestureResult.lines = lLines;
01719             gGestureResult.contour = lMaxContour;
01720             
01721             gIsDrawing = true;
01722             p_DrawObjects->signal_set(1);
01723         }
01724         else
01725         {
01726             mMutexObjects.unlock();
01727         }
01728         
01729         mMutexProcess.lock();
01730         gIsProcessing = false;
01731         mMutexProcess.unlock();
01732 
01733         /* Clean cache */
01734         dcache_clean(user_buf_draw_action_888, 
01735                                     sizeof(user_buf_draw_action_888));
01736     }
01737 }
01738 
01739 /* Init a block on RAM to store file */
01740 HeapBlockDevice myBd(2*128 * 512, 512);// ~128 Kb
01741 
01742 /****** main image process here******/
01743 int main(void) {
01744     
01745     /* Format block on RAM */
01746     if(FATFileSystem::format(&myBd))
01747     {
01748         togle_led(LED_RED);
01749     }
01750     else
01751     {
01752         togle_led(LED_GREEN);
01753     }
01754     FATFileSystem myFs(STORAGE_NAME,&myBd); /* Start the block*/
01755     
01756     /* Create file .xml*/
01757     if(arr2File(haar_cascafe_frontal_face,HAAR_FILE_SIZE,HAAR_CASCADE_FACE_PATH))
01758     {
01759         togle_led(LED_BLUE);
01760     }
01761     else
01762     {
01763         togle_led(LED_RED);
01764     }
01765     
01766     /* Init SD card*/
01767     /*myStorage = new cStorage(SDCARD_NAME);
01768     if( initSdCard() == false)
01769     {
01770         while(1);
01771     }*/
01772 
01773     /* Initialization of LCD */
01774     Init_LCD_Display();    /* When using LCD, please call before than Init_Video(). */
01775 
01776     /* Initialization of Video */
01777     Init_Video();
01778     
01779     /* Start Video and Lcd processing */
01780     p_VideoLcdTask = new Thread();
01781     p_VideoLcdTask->start(video_lcd_task);
01782     
01783     p_Touch = new Thread();
01784     p_Touch->start(touch_task);
01785     
01786     /* Start image processor*/
01787     p_DrawObjects = new Thread();
01788     p_DrawObjects->signal_set(0);
01789     p_DrawObjects->start(img_draw_objects);
01790     
01791     initDrawAction32();
01792 
01793     while(1)
01794     {
01795         switch(appMode)
01796         {
01797             case FACE_DETECTION:
01798             {
01799                 faceDectectionApp();
01800                 break;
01801             }
01802             case MOTION_DETECTION:
01803             {
01804                 motionDetectionApp();
01805                 break;
01806             }
01807             case FACE_RECOGNITION:
01808             {
01809                 faceRecognitionApp();
01810                 break;
01811             }
01812             case GUESTURE_RECOGNITION:
01813             {
01814                 gestureRegconition();
01815                 break;
01816             }
01817             default:
01818             {
01819                 wait(0.5);
01820                 break;
01821             }
01822         }
01823     }
01824     return 1;
01825 }