C_RESULT output_gtk_stage_transform(vp_stages_gtk_config_t *cfg, vp_api_io_data_t *in, vp_api_io_data_t *out)
{
// Pull a frame from the camera
// IplImage* cvImage = cvCreateImage(cvSize(640,360), IPL_DEPTH_8U, 3);
IplImage *cvImage = ipl_image_from_data( (uint8_t*)in->buffers[0] );
if(!cvImage)
{
printf("Could not pull image from camera");
}
// If an image was successfully gathered,
// then draw it on the screen
cvShowImage( "BetaCamera", cvImage );
gtk_widget_show_all(ihm_ImageWin);
gdk_threads_leave();
return (SUCCESS);
}
C_RESULT display_stage_transform (display_stage_cfg_t *cfg, vp_api_io_data_t *in, vp_api_io_data_t *out)
{
if(count > 1)
{
temp_x = cur_x;
temp_y = cur_y;
}
int i = 0;
const char *cascade_name = "/usr/share/opencv/haarcascades/haarcascade_frontalface_alt.xml";
CvHaarClassifierCascade *cascade = 0;
CvMemStorage *storage = 0;
CvSeq *faces;
cascade = (CvHaarClassifierCascade *) cvLoad (cascade_name, 0, 0, 0);
if(!cascade)
{
printf("Could not load cascade.\n");
return -1;
}
uint32_t width = 0, height = 0;
getPicSizeFromBufferSize (in->size, &width, &height);
//Obtain image from ARDrone and convert it to OpenCV format.
IplImage *img = ipl_image_from_data((uint8_t*)in->buffers[0], 1, 640, 360);
storage = cvCreateMemStorage (0);
cvClearMemStorage (storage);
//DetectFaces
faces = cvHaarDetectObjects (img, cascade, storage, 1.11, 4, 0, cvSize(30, 30), cvSize (0, 0));
for (i = 0; i < (faces ? faces->total : 0); i++)
{
CvRect *r = (CvRect *) cvGetSeqElem (faces, i);
doMosaic(img, r->x, r->y, r->width, r->height, 10);
cur_x = r->x;
cur_y = r->y;
//printf("In the loop.\n");
count++; //increases count when detect faces
}
if(count > 2)
{
printf("prev_x = %d, prev_y = %d, cur_x = %d, cur_y = %d\n", temp_x, temp_y, cur_x, cur_y);
if(cur_x - temp_x > 3)
{
//printf("Move Right.\n");
ardrone_at_set_progress_cmd( 1, 1.0, 0.0, 0.0, 0.0 );
}
if(cur_x - temp_x < -3)
{
//printf("Move Left.\n");
ardrone_at_set_progress_cmd( 1, -1.0, 0.0, 0.0, 0.0 );
}
if(cur_y - temp_y > 3)
{
//printf("Move Down.\n");
ardrone_at_set_progress_cmd( 1, 0.0, 0.0, -1.0, 0.0 );
}
if(cur_y - temp_y < -5)
{
//printf("Move Up.\n");
//ardrone_at_reset_com_watchdog();
ardrone_tool_set_progressive_cmd( 1, 0.0, 0.0, 0.3, 0.0 );
//vp_os_delay(1000);
}
}
//emergency++;p
if(count > 50)
{
printf("Exit\n");
ardrone_tool_set_ui_pad_start(0);
return -1;
}
cvNamedWindow("FaceDetect", CV_WINDOW_AUTOSIZE);
cvShowImage("FaceDetect", img);
cvWaitKey(1);
cvReleaseImage(&img);
return C_OK;
}