C_RESULT output_gtk_stage_transform( void *cfg, vp_api_io_data_t *in, vp_api_io_data_t *out)
{
static int frame = 0;
static int mass = 0, x_center = -1, y_center = -1;
unsigned char mask_buf[WIDTH*HEIGHT];
uint8_t display_data[WIDTH*HEIGHT*3];
int is_hover;
static int width, height;
static int is_face = 0;
int camshift_error;
static float this_hue_buf[WIDTH*HEIGHT];
static float last_hue_buf[WIDTH*HEIGHT];
int i;
static camshift_frames = 0;
FILE *fp;
char filename[50];
frame++;
// GET FRAME FROM VIDEO FEED
vp_os_mutex_lock(&video_update_lock);
pixbuf_data = (uint8_t*)in->buffers[0];
vp_os_mutex_unlock(&video_update_lock);
// Process frame for orange ball
// UNCOMMENT TO DETECT BALL
//process_frame_ball(pixbuf_data, mask_buf, &mass, &x_center, &y_center);
// UNCOMMENT TO DETECT FACES
if (is_face == 0) {
//printf("Detecting face...\n");
process_frame_face(pixbuf_data, &mass, &x_center, &y_center, &width, &height);
if (mass > 8) {
is_face = 1;
rgb2hue(pixbuf_data, last_hue_buf);
printf("DETECT!!! x,y = %d, %d\n", x_center, y_center);
}
} else {
rgb2hue(pixbuf_data, this_hue_buf);
camshift_error = camshift(last_hue_buf, this_hue_buf, &x_center, &y_center, width, height);
printf("camshift x,y = %d, %d\n", x_center, y_center);
// copy this buffer to last buffer
for (i=0; i<WIDTH*HEIGHT; i++) {
last_hue_buf[i] = this_hue_buf[i];
}
if (camshift_error) {
printf("CAMSHIFT ERROR\n");
is_face = 0;
x_center = -1;
y_center = -1;
}
camshift_frames++;
// about two seconds
if (camshift_frames > 30) {
printf("!!check to see if we are still tracking\n");
camshift_frames = 0;
is_face = 0;
}
}
// we did not use the mask, so make it all black
clear_mask(mask_buf);
// write pixbuf to file
sprintf(filename, "/home/a/pixbuf/pixbuf_%d.ppm", frame);
fp = fopen(filename, "w");
// write header
fprintf(fp, "P6\n320 240\n255\n");
for (i=0; i<320*240*3; i++) {
fprintf(fp, "%c", pixbuf_data[i]);
}
fclose(fp);
fp = fopen("/home/a/meanshift_log.txt", "a");
// write log
// framenum, mass, centroid_x, centroid_y, width, height
fprintf(fp, "%d %d %d %d %d %d\n", frame, mass, x_center, y_center, width, height);
// Get mask display
display_mask(mask_buf, display_data, x_center, y_center);
// Fly drone
is_hover = fly(x_center, y_center, mass);
// Print status
printf("Frame number: %d\n", frame);
printf("Mass: %d; Centroid: (%d, %d)\n", mass, x_center, y_center);
printf("Meanshift Width: %d; Height: %d\n", width, height);
printf("\033[2J");
gdk_threads_enter();
// GdkPixbuf structures to store the displayed picture
static GdkPixbuf *pixbuf = NULL;
static GdkPixbuf *maskbuf = NULL;
if (pixbuf != NULL) {
g_object_unref(pixbuf);
pixbuf=NULL;
}
// Create GdkPixbuf from color frame data
pixbuf = gdk_pixbuf_new_from_data(pixbuf_data,
GDK_COLORSPACE_RGB,
FALSE, // No alpha channel
8, // 8 bits per pixel
320, // Image width
240,
320 * 3, // new pixel every 3 bytes (3channel per pixel)
NULL, // Function pointers
NULL);
// Create the GdkPixbuf from color mask display buffer
maskbuf = gdk_pixbuf_new_from_data(display_data,
GDK_COLORSPACE_RGB,
FALSE, // No alpha channel
8, // 8 bits per pixel
320, // Image width
240,
320 * 3, // new pixel every 3 bytes (3channel per pixel)
NULL, // Function pointers
NULL);
gui_t *gui = get_gui();
// Display the image
if (gui && gui->cam) {
gtk_image_set_from_pixbuf(GTK_IMAGE(gui->cam), pixbuf);
// also display the pixbuf in a second window
if (gui->mask_cam) {
gtk_image_set_from_pixbuf(GTK_IMAGE(gui->mask_cam), maskbuf);
}
}
gdk_threads_leave();
return (SUCCESS);
}
/*-------------------------------------------------------------------------------------------------------------- */
void mexFunction( int nlhs, mxArray *plhs[] , int nrhs, const mxArray *prhs[] )
{
unsigned char *im;
double *H;
const int *dimim;
double *I;
int ny , nx , nynx , dimcolor = 1 , nH = 1 , i;
double norma_default[2] = {1 , 1};
double kernelx_default[3] = {-0.5 , 0 , 0.5};
double kernely_default[3] = {-0.5 , 0 , 0.5};
struct opts options;
mxArray *mxtemp;
double *tmp , temp;
int tempint;
options.nspyr = 1;
options.nori = 9;
options.bndori = 0;
options.norm = 1;
options.kyx = 1;
options.kxx = 3;
options.kyy = 3;
options.kxy = 1;
options.color = 0;
options.interpolate = 1;
options.clamp = 0.2;
/* Input 1 */
if( (mxGetNumberOfDimensions(prhs[0]) > 3) || !mxIsUint8(prhs[0]) )
{
mexErrMsgTxt("I must be (ny x nx x [3]) in UINT8 format\n");
}
im = (unsigned char *)mxGetData(prhs[0]);
dimim = mxGetDimensions(prhs[0]);
ny = dimim[0];
nx = dimim[1];
nynx = ny*nx;
/* Input 2 */
if ((nrhs > 1) && !mxIsEmpty(prhs[1]) )
{
mxtemp = mxGetField(prhs[1] , 0 , "spyr");
if(mxtemp != NULL)
{
if( mxGetN(mxtemp) != 4 )
{
mexErrMsgTxt("spyr must be (nscale x 4) in double format");
}
options.spyr = mxGetPr(mxtemp);
options.nspyr = mxGetM(mxtemp);
}
else
{
options.nspyr = 1;
options.spyr = (double *)mxMalloc(4*sizeof(double));
options.spyr[0] = 1.0;
options.spyr[1] = 1.0;
options.spyr[2] = 1.0;
options.spyr[3] = 1.0;
}
mxtemp = mxGetField( prhs[1], 0, "norma" );
if(mxtemp != NULL)
{
if((mxGetM(mxtemp) != 1) && (mxGetN(mxtemp) != 2) )
{
mexErrMsgTxt("options.norma must be (1 x 2) in double format");
}
options.norma = mxGetPr(mxtemp);
}
else
{
options.norma = (double *)mxMalloc(2*sizeof(double));
for(i = 0 ; i < 2 ; i++)
{
options.norma[i] = norma_default[i];
}
}
mxtemp = mxGetField( prhs[1], 0, "kernelx" );
if(mxtemp != NULL)
{
options.kernelx = mxGetPr(mxtemp);
options.kyx = (int) mxGetM(mxtemp);
options.kxx = (int) mxGetN(mxtemp);
}
else
{
options.kernelx = (double *)mxMalloc(3*sizeof(double));
for(i = 0 ; i < 3 ; i++)
{
options.kernelx[i] = kernelx_default[i];
}
}
mxtemp = mxGetField( prhs[1], 0, "kernely" );
if(mxtemp != NULL)
{
options.kernely = mxGetPr(mxtemp);
options.kyy = (int) mxGetM(mxtemp);
options.kxy = (int) mxGetN(mxtemp);
}
else
{
options.kernely = (double *)mxMalloc(3*sizeof(double));
for(i = 0 ; i < 3 ; i++)
{
options.kernely[i] = kernely_default[i];
}
}
mxtemp = mxGetField(prhs[1] , 0 , "color");
if(mxtemp != NULL)
{
tmp = mxGetPr(mxtemp);
tempint = (int) tmp[0];
if( (tempint < 0) || (tempint > 6))
{
mexPrintf("color = {0,1,2,3,4,5}, force to 0\n");
options.color = 0;
}
else
{
options.color = tempint;
}
}
mxtemp = mxGetField(prhs[1] , 0 , "nori");
if(mxtemp != NULL)
{
tmp = mxGetPr(mxtemp);
tempint = (int) tmp[0];
if( (tempint < 1))
{
mexPrintf("nori > 0, force to 9");
options.nori = 9;
}
else
{
options.nori = tempint;
}
}
mxtemp = mxGetField(prhs[1] , 0 , "norm");
if(mxtemp != NULL)
{
tmp = mxGetPr(mxtemp);
tempint = (int) tmp[0];
if( (tempint < 0) || (tempint > 4) )
{
mexPrintf("norm = {0 , 1 , 2 , 3 , 4}, force to 1");
options.norm = 1;
}
else
{
options.norm = tempint;
}
}
mxtemp = mxGetField(prhs[1] , 0 , "bndori");
if(mxtemp != NULL)
{
tmp = mxGetPr(mxtemp);
tempint = (int) tmp[0];
if( (tempint < 0) || (tempint > 1) )
{
mexErrMsgTxt("bndori = {0 , 1}, force to 1");
options.bndori = 0;
}
else
{
options.bndori = tempint;
}
}
mxtemp = mxGetField(prhs[1] , 0 , "interpolate");
if(mxtemp != NULL)
{
tmp = mxGetPr(mxtemp);
tempint = (int) tmp[0];
if( (tempint < 0) || (tempint > 1))
{
mexPrintf("interpolate = {0,1}, force to 1");
options.interpolate = 1;
}
else
{
options.interpolate = tempint;
}
}
mxtemp = mxGetField(prhs[1] , 0 , "clamp");
if(mxtemp != NULL)
{
tmp = mxGetPr(mxtemp);
temp = tmp[0];
if( (temp < 0.0) )
{
mexPrintf("clamp must be >= 0, force to 0.2");
options.clamp = 0.2;
}
else
{
options.clamp = temp;
}
}
}
else
{
options.nspyr = 1;
options.spyr = (double *)mxMalloc(4*sizeof(double));
options.spyr[0] = 1.0;
options.spyr[1] = 1.0;
options.spyr[2] = 1.0;
options.spyr[3] = 1.0;
options.norma = (double *)mxMalloc(2*sizeof(double));
for(i = 0 ; i < 2 ; i++)
{
options.norma[i] = norma_default[i];
}
options.kernelx = (double *)mxMalloc(3*sizeof(double));
options.kernely = (double *)mxMalloc(3*sizeof(double));
for(i = 0 ; i < 3 ; i++)
{
options.kernelx[i] = kernelx_default[i];
options.kernely[i] = kernely_default[i];
}
}
if((mxGetNumberOfDimensions(prhs[0]) == 2))
{
options.color = 0;
I = (double *)mxMalloc(nynx*sizeof(double));
dimcolor = 1;
for (i = 0 ; i < nynx ; i++)
{
I[i] = (double)im[i];
}
}
else
{
if((options.color == 0) )
{
I = (double *)mxMalloc(nynx*sizeof(double));
rgb2gray(im , ny , nx , I);
dimcolor = 1;
}
else if (options.color == 1)
{
I = (double *)mxMalloc(3*nynx*sizeof(double));
for (i = 0 ; i < 3*nynx ; i++)
{
I[i] = (double)im[i];
}
dimcolor = 3;
}
else if (options.color == 2)
{
I = (double *)mxMalloc(3*nynx*sizeof(double));
rgb2nrgb(im , ny , nx , I);
dimcolor = 3;
}
else if (options.color == 3)
{
I = (double *)mxMalloc(3*nynx*sizeof(double));
rgb2opponent(im , ny , nx , I);
dimcolor = 3;
}
else if(options.color == 4)
{
I = (double *)mxMalloc(2*nynx*sizeof(double));
rgb2nopponent(im , ny , nx , I);
dimcolor = 2;
}
else if(options.color == 5)
{
I = (double *)mxMalloc(nynx*sizeof(double));
rgb2hue(im , ny , nx , I);
dimcolor = 1;
}
}
/*----------------------- Outputs -------------------------------*/
nH = number_subwindows(options.spyr , options.nspyr );
plhs[0] = mxCreateDoubleMatrix(dimcolor*nH*options.nori , 1 , mxREAL);
H = mxGetPr(plhs[0]);
/*------------------------ Main Call ----------------------------*/
mlhoee_spyr(I , H , ny , nx , nH , dimcolor , options );
/*--------------------------- Free memory -----------------------*/
if ( (nrhs > 1) && !mxIsEmpty(prhs[1]) )
{
if ( (mxGetField( prhs[1] , 0 , "spyr" )) == NULL )
{
mxFree(options.spyr);
}
if ( (mxGetField( prhs[1] , 0 , "norma" )) == NULL )
{
mxFree(options.norma);
}
if ( (mxGetField( prhs[1] , 0 , "kernelx" )) == NULL )
{
mxFree(options.kernelx);
}
if ( (mxGetField( prhs[1] , 0 , "kernely" )) == NULL )
{
mxFree(options.kernely);
}
}
else
{
mxFree(options.spyr);
mxFree(options.norma);
mxFree(options.kernelx);
mxFree(options.kernely);
}
mxFree(I);
}
