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