path: root/multi/SRC/multiTest.c

#ifdef _WIN32
#include <windows.h>
#endif
#include <stdio.h>
#include <stdlib.h>
#ifndef __APPLE__
#include <GL/gl.h>
#include <GL/glut.h>
#else
#include <OpenGL/gl.h>
#include <GLUT/glut.h>
#endif
#include <AR/gsub.h>
#include <AR/video.h>
#include <AR/param.h>
#include <AR/ar.h>
#include <AR/arMulti.h>
#include "Utilidades.h"
#include <opencv/cv.h>
//#include <opencv/cxcore.h>
#include <opencv/highgui.h>
#include "stdio.h"
#include "stdlib.h"
#include "detectorCuadrados.cpp"
#include <unistd.h>
#include <math.h>

#include "cvUtilities.h"


/* set up the video format globals */

#ifdef _WIN32
char			*vconf = "Data\\WDM_camera_flipV.xml";
#else
char			*vconf = "";
#endif

int             xsize, ysize;
int             thresh = 100;
int             count = 0;
int             mostrarDebug = 0;
int             probarMatching = 0;
int             procesarNoVisi = 0;
int gris = 0;
char           *cparam_name    = "Data/camera_para.dat";
ARParam         cparam;

char                *config_name = "Data/multi/marker.dat";
ARMultiMarkerInfoT  *config;
IplImage* img = NULL;

IplImage* templateImage;

static void   init(void);
static void   cleanup(void);
static void   keyEvent( unsigned char key, int x, int y);
static void   mainLoop(void);
static void procesarNoVisibles(ARMarkerInfo* marker_info,ARMultiMarkerInfoT* config,int indice,IplImage* img);
static void detectarElementoPorSegmentacion(ARMarkerInfo* marker_info,ARMultiMarkerInfoT* config, int indice,IplImage* img );

int main(int argc, char **argv)
{
	glutInit(&argc, argv);
    init();

    arVideoCapStart();
    argMainLoop( NULL, keyEvent, mainLoop );
	return (0);
}

static void   keyEvent( unsigned char key, int x, int y)
{
    /* quit if the ESC key is pressed */
    if( key == 0x1b ) {
        printf("*** %f (frame/sec)\n", (double)count/arUtilTimer());
        cleanup();
        exit(0);
    }

    if( key == 't' ) {
        printf("count = %d\n",count);
        printf("timer = %f\n",arUtilTimer());
        printf("*** %f (frame/sec)\n", (double)count/arUtilTimer());
        printf("Enter new threshold value (current = %d): ", thresh);
        scanf("%d",&thresh); while( getchar()!='\n' );
        printf("\n");
        count = 0;
    }

    if( key == 'p'){
        procesarNoVisi = 1 - procesarNoVisi;
    }
    /* turn on and off the debug mode with right mouse */
    if( key == 'd' ) {
        printf("*** %f (frame/sec)\n", (double)count/arUtilTimer());
        arDebug = 1 - arDebug;
        if( arDebug == 0 ) {
            glClearColor( 0.0, 0.0, 0.0, 0.0 );
            glClear(GL_COLOR_BUFFER_BIT);
            argSwapBuffers();
            glClear(GL_COLOR_BUFFER_BIT);
            argSwapBuffers();
        }
        count = 0;
    }

    if( key == 'j' ) {
        printf("*** %f (frame/sec)\n", (double)count/arUtilTimer());
        mostrarDebug = 1 - mostrarDebug;
    }

    if( key == 'm' ) {
        printf("*** %f (frame/sec)\n", (double)count/arUtilTimer());
        probarMatching= 1 - probarMatching;
    }

    if( key == 'g' ) {
        printf("*** %f (frame/sec)\n", (double)count/arUtilTimer());
        gris = 1;

    }

}

/* main loop */
static void mainLoop(void)
{
    ARUint8         *dataPtr;
    ARMarkerInfo    *marker_info;
    int             marker_num;
    double          err;
    int             i;

   /* if(count==0){
        printf("\n\n\n +++++++++++++++++++OBTENIENDO UNA IMAGEN NUEVAAAAAAAAAAAAAA +++++++++++++++++++++++++++");
        dataPtr = (ARUint8 *)arVideoGetImage();
    }*/
    /* grab a video frame */

    arUtilSleep(90000);
    if( (dataPtr = (ARUint8 *)arVideoGetImage()) == NULL ) {
	printf("sleep\n");
	arUtilSleep(2);
        return;
    }
    if( count == 0 ) arUtilTimerReset();
    count++;
    //unsigned long int tam =sizeof(*dataPtr);
    //printf("sizeof(*dataPtr) = %ll",sizeof(*dataPtr));


//    if(probarMatching){
//        util->templateMatching(img,templateImage);
//    }
    if(probarMatching){
        probarMatching = 0;
        //IplImage* imgGris = crearImagenCvGris(dataPtr);
        IplImage* imgGris = crearImagenCv(dataPtr,xsize,ysize);
        templateMatching(imgGris,templateImage);
//        cvNamedWindow("ImagenGris",1);
//        cvShowImage("ImagenGris",imgGris);
//        cvWaitKey(0);
//        cvDestroyWindow("ImagenGris");
    }

    if(img==NULL){
		printf("---------------->CREO LA IMAGEN OPENCV PORQUE ERA NULL 111\n\n\n");
		img = crearImagenCv(dataPtr,xsize,ysize);
	}
    /* detect the markers in the video frame */
    cvSetImageData( img, dataPtr, img->width* img->nChannels );
    if( arDetectMarkerLite((ARUint8*)img->imageData, thresh, &marker_info, &marker_num) < 0 ) {
    //if( arDetectMarkerLite(dataPtr, thresh, &marker_info, &marker_num) < 0 ) {
        cleanup();
        exit(0);
    }

    int j;
    for(j = 0;j<marker_num;j++){
        if(marker_info[j].id==-1){
            if(marker_info[j].cf > 0.5){
                printf("ERROR!!! ID = -1 y cf=%f",marker_info[j].cf);
            }
        }
    }

    argDrawMode2D();
    if( !arDebug ) {
        argDispImage( dataPtr, 0,0 );
    }
    else {
        argDispImage( dataPtr, 1, 1 );
        if( arImageProcMode == AR_IMAGE_PROC_IN_HALF )
            argDispHalfImage( arImage, 0, 0 );
        else
            argDispImage( arImage, 0, 0);

        glColor3f( 1.0, 0.0, 0.0 );
        glLineWidth( 1.0 );
        for( i = 0; i < marker_num; i++ ) {
            argDrawSquare( marker_info[i].vertex, 0, 0 );
        }
        glLineWidth( 1.0 );
    }

    arVideoCapNext();

    if( (err=arMultiGetTransMat(marker_info, marker_num, config)) < 0 ) {
        argSwapBuffers();
        return;
    }

    //printf("err = %f\n", err);
    if(err > 100.0 ) {
        printf("saliendo 2\n");
        argSwapBuffers();
        return;
    }

    argDrawMode3D();
    argDraw3dCamera( 0, 0 );
    glClearDepth( 1.0 );
    glClear(GL_DEPTH_BUFFER_BIT);

    int encontre = 0;
    printf("Marcadores detectados = %d\n",marker_num);
    for( i = 0; i < config->marker_num; i++ ) {
        if(marker_info[i].id==0){
        	encontre=1;
        	printf("encontre la A\n");
        	double *ox,*oy;
        	ox = (double*)malloc(sizeof(double));
        	oy = (double*)malloc(sizeof(double));

        	double *ox_calculated,*oy_calculated;
			ox_calculated = (double*)malloc(sizeof(double));
			oy_calculated = (double*)malloc(sizeof(double));


        	arParamIdeal2Observ(arParam.dist_factor,marker_info[i].pos[0],marker_info[i].pos[1],ox,oy);


        	printf("Coordenadas Ideales de A = (%f,%f)\n",marker_info[i].pos[0],marker_info[i].pos[1]);
        	printf("Coordenadas Observadas de A = (%f,%f)\n\n",*ox,*oy);
            double      cam_trans[3][4];
        	arUtilMatInv(config->trans,cam_trans);
        	printf("Matriz config->trans\n");
        	int fila,col;
        	for( fila =0;fila<3;fila++){
        		for(col = 0;col<4;col++){
        			printf(" %f ",config->trans[fila][col]);
        		}
        		printf("\n");
        	}


        	printf("Matriz Inversa config->trans\n");
        	for( fila =0;fila<3;fila++){
				for(col = 0;col<4;col++){
					printf(" %f ",cam_trans[fila][col]);
				}
				printf("\n");
			}
        	/*
        	double transT [4][3];
        	transT[0][0] = config->trans[0][0];
        	transT[1][0] = config->trans[0][1];
        	transT[2][0] = config->trans[0][2];
        	transT[3][0] = config->trans[0][3];

        	transT[0][1] = config->trans[1][0];
			transT[1][1] = config->trans[1][1];
			transT[2][1] = config->trans[1][2];
			transT[3][1] = config->trans[1][3];

			transT[0][2] = config->trans[2][0];
			transT[1][2] = config->trans[2][1];
			transT[2][2] = config->trans[2][2];
			transT[3][2] = config->trans[2][3];




			printf("\nMatriz config->trans Traspuesta!\n");
			for( fila =0;fila<4;fila++){
				for(col = 0;col<3;col++){
					printf(" %f ",transT[fila][col]);
				}
				printf("\n");
			 }

			printf("\nMatriz config->transR \n");
			for( fila =0;fila<3;fila++){
				for(col = 0;col<4;col++){
					printf(" %f ",config->transR[fila][col]);
				}
				printf("\n");
			}*/

        	/*if(img==NULL){
				printf("---------------->CREO LA IMAGEN OPENCV PORQUE ERA NULL\n\n\n");
				img = util->crearImagenCv(dataPtr,xsize,ysize);
			}
			detectarElementoPorSegmentacion(marker_info,config,i,img);*/
		}

        if( config->marker[i].visible >= 0 ){
            draw( config->trans, config->marker[i].trans, 0 );
        }else{
			/*
			*		MARCADOR NO DETECTADO.
			*									* TENEMOS SUS COORDENADAS
			*														- Marker Coordinates (Xm,Ym)
			*														- Camera Coordinates (Xc,Yc)
			*									* NECESITAMOS SUS COORDENADAS
			*														- Ideal Screen Coordinates  (xc,yc)
			*
			*/


			double hXi_hYi[3][4];
			double vertex[4][2];
			obtenerCoordenadasIdealesMarcadorNoDetectado(config->trans,config->marker[i],hXi_hYi,vertex);

			double Xi = hXi_hYi[0][3]/hXi_hYi[2][3];
			double Yi = hXi_hYi[1][3]/hXi_hYi[2][3];
			printf("Coordenadas ideales del marcador %d = (%f,%f)\n",i,Xi,Yi);
			printf("h = %f\n",hXi_hYi[2][3]);


			argDrawMode2D();
			//Dibujo el centro
			glColor3f( 1.0, 0.0, 0.0 );
			argLineSeg( Xi , Yi, Xi + 20, Yi, 0, 0);

			//Dibujo los vertices
			glColor3f( 0.0, 1.0, 0.0 );
			int vertice;
			for(vertice=0;vertice<4;vertice++){
				argLineSeg( vertex[vertice][0] , vertex[vertice][1], vertex[vertice][0] + 20, vertex[vertice][1], 0, 0);
			}



			//draw( config->trans, config->marker[i].trans, 1 );
            if(procesarNoVisi){
                if(img==NULL){
                    printf("---------------->CREO LA IMAGEN OPENCV PORQUE ERA NULL 222\n\n\n");
                    img = crearImagenCv(dataPtr,xsize,ysize);
                }
                procesarNoVisibles(marker_info,config,i,img);
            }
        }

        if( (config->marker[i].patt_id == 0)&&(mostrarDebug)) { //muestro la segmentacion de la A
                if(img==NULL){
                    printf("---------------->CREO LA IMAGEN OPENCV PORQUE ERA NULL 333f\n\n\n");
                    img = crearImagenCv(dataPtr,xsize,ysize);
                }
                detectarElementoPorSegmentacion(marker_info,config,i,img);
                mostrarDebug = 0;
        }
    }
    procesarNoVisi = 0;
    argSwapBuffers();

}

/*
 * @param markerToCameraMatrix corresponde a config->trans, contiene la transformacion desde las coordenadas del marcador a las coordenadas de la camara
 * @param markerCoordinate    corresponde a la matriz config->marker[i].trans . Contiene la transformacion dentro del sistema de coordenadas situado en el plano donde estan detectados los marcadores.
 * 								usualmente tiene la traslacion desde el origen de este sistema hacia el punto donde esta el centro del marcador
 * @param hXi_hYi				Se almacena el resultado hXi_hYi = markerToCameraMatrix * marker_coordinate .
 *
 * Las posicion en Screen Ideal Coordinates se obtienen de la siguiente manera:  Xi = hXi_hYi[0][3]/hXi_hYi[2][3]
 *																				 Yi = hXi_hYi[1][3]/hXi_hYi[2][3]
 *
 */
/*static void obtenerCoordenadasIdealesMarcadorNoDetectado(double markerToCameraMatrix[3][4],double markerCoordinate[3][4],double hXi_hYi[3][4]){
	double Tcm_Por_Xm[3][4];
	arUtilMatMul(markerToCameraMatrix,markerCoordinate,Tcm_Por_Xm);
	arUtilMatMul(arParam.mat,Tcm_Por_Xm,hXi_hYi);
}*/


static void detectarElementoPorSegmentacion(ARMarkerInfo* marker_info,ARMultiMarkerInfoT* config, int indice,IplImage* img ){
    cvNamedWindow("imagenCompleta",1);
    cvShowImage("imagenCompleta",img);
    int resultado[2];
    calcularPuntoOrigen(marker_info[indice].vertex, resultado);
    int puntoOrigen = resultado[0];
    int puntoMasLejos = resultado[1];
    printf("El vertice origen es v%d = (%f,%f)\n",puntoOrigen+1,marker_info[indice].vertex[puntoOrigen][0],marker_info[indice].vertex[puntoOrigen][1]);
    img->origin = IPL_ORIGIN_TL;
    double ancho = abs( marker_info[indice].vertex[puntoOrigen][0] - marker_info[indice].vertex[puntoMasLejos][0]);
    double alto =  abs( marker_info[indice].vertex[puntoOrigen][1] - marker_info[indice].vertex[puntoMasLejos][1]);

    cvSetImageROI(img, cvRect(marker_info[indice].vertex[puntoOrigen][0],marker_info[indice].vertex[puntoOrigen][1], ancho, alto) );
    IplImage* mono_Image = cvCreateImage( cvGetSize(img), 8, 1 );
	segmentarImagen(img,mono_Image,hlower,hupper,1);
	cvNamedWindow( "Imagen Monocromatica", CV_WINDOW_AUTOSIZE );
	cvShowImage( "Imagen Monocromatica", mono_Image );
	cvWaitKey(0);
    if(hayRectangulo(mono_Image,MIN_AREA)){
        printf("******************* ENCONTRO RECTANGULOOOOOOOOOOO *******************\n");
    }
}
static void procesarNoVisibles(ARMarkerInfo* marker_info,ARMultiMarkerInfoT* config, int indice,IplImage* img ){
    cvNamedWindow("imagenCompleta",1);
    cvShowImage("imagenCompleta",img);
    int resultado[2];
    calcularPuntoOrigen(marker_info[indice].vertex, resultado);
    int puntoOrigen = resultado[0];
    int puntoMasLejos = resultado[1];
    printf("El vertice origen es v%d = (%f,%f)\n",puntoOrigen+1,marker_info[indice].vertex[puntoOrigen][0],marker_info[indice].vertex[puntoOrigen][1]);
    img->origin = IPL_ORIGIN_TL;
    double ancho = abs( marker_info[indice].vertex[puntoOrigen][0] - marker_info[indice].vertex[puntoMasLejos][0]);
    double alto =  abs( marker_info[indice].vertex[puntoOrigen][1] - marker_info[indice].vertex[puntoMasLejos][1]);

    cvSetImageROI(img, cvRect(marker_info[indice].vertex[puntoOrigen][0],marker_info[indice].vertex[puntoOrigen][1], ancho, alto) );

    cvNamedWindow("ROI" + indice,1);
    cvShowImage("ROI" + indice,img);
    cvWaitKey(0);
   //cvReleaseImage(&img);
    //cvDestroyWindow("imagenCompleta");
    cvDestroyWindow("ROI" + indice);
    cvResetImageROI(img);
}


static void imprimirNoVisibles(ARMultiMarkerInfoT* config){
    //identifico los marcadores no encontrados
    int i;
    for( i = 0; i < config->marker_num; i++ ) {
        if( config->marker[i].visible < 0 ) {
            printf("El marcador %d no esta visible \n",config->marker[i].patt_id);
        }
    }
}



static void init( void )
{

    ARParam  wparam;
    /* open the video path */
    if( arVideoOpen( vconf ) < 0 ) exit(0);
    /* find the size of the window */
    if( arVideoInqSize(&xsize, &ysize) < 0 ) exit(0);
    printf("Image size (x,y) = (%d,%d)\n", xsize, ysize);
    /* set the initial camera parameters */
    if( arParamLoad(cparam_name, 1, &wparam) < 0 ) {
        printf("Camera parameter load error !!\n");
        exit(0);
    }
    arParamChangeSize( &wparam, xsize, ysize, &cparam );
    arInitCparam( &cparam );
    printf("*** Camera Parameter ***\n");
    arParamDisp( &cparam );

    if( (config = arMultiReadConfigFile(config_name)) == NULL ) {
        printf("config data load error !!\n");
        exit(0);
    }

    /* open the graphics window */
    argInit( &cparam, 1.0, 0, 2, 1, 0 );
    arFittingMode   = AR_FITTING_TO_IDEAL;
    arImageProcMode = AR_IMAGE_PROC_IN_HALF;
    argDrawMode     = AR_DRAW_BY_TEXTURE_MAPPING;
    argTexmapMode   = AR_DRAW_TEXTURE_HALF_IMAGE;


    //SI USO EL METODO DE TEMPLATE MATCHING ENTONCES DEBO CARGAR EL TEMPLATE!!!
    templateImage = NULL;
    templateImage = cvLoadImage("Data/multi/template4.png",CV_LOAD_IMAGE_COLOR);
    if(templateImage==NULL){
        printf("\nERROR CARGANDO TEMPLATE IMAGE \n");
    }
    /*else{
        cvNamedWindow("Template elegido",1);
        cvShowImage("Template elegido",templateImage);
        cvWaitKey(0);
        cvDestroyWindow("Template elegido");
    }*/

}

/* cleanup function called when program exits */
static void cleanup(void)
{
    arVideoCapStop();
    arVideoClose();
    argCleanup();
}