path: root/multi/SRC/Calibrador.cpp

/*
 * Calibrador.cpp
 *
 *  Created on: 8/09/2010
 *      Author: smarichal
 */

#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 <opencv/cv.h>
#include <opencv/highgui.h>
#include "Utilidades.h"
#include "cvUtilities.h"
#include "UtilidadesSDL.h"
#include "stdio.h"
#include "stdlib.h"
#include <unistd.h>
#include <math.h>
#include <formatopixel.h>

#include "cvUtilities.h"

			/*************************************************************************************************************************
			 * 									CONFIGURACIONES GENERALES DE LA APLICACION
			 *************************************************************************************************************************/
int mostrarVideo = 0;
int mostrarRadar = 1;

			/*************************************************************************************************************************
			 * 									CONFIGURACION DE RECONOCIMIENTO DE COLORES
			 *************************************************************************************************************************/
#define MIN_AREA 200
#define MAX_AREA 6000

int min_area = MIN_AREA;
int max_area = MAX_AREA;
#define MIN_H 0
#define MAX_H 0
#define MIN_S 0
#define MAX_S 0
#define MIN_B 0
#define MAX_B 0
#define UMBRAL 5
#define UMBRAL_AREA_APUNTADOR 20  //El area del apuntador y el area del marcador tienen una relacion definida mediante calibracion. Usamos un
									//umbral de error al que declaramos como UMBRAL_AREA_APUNTADOR. Es decir si la zona de estudio donde buscamos el
								//apuntador mide 300 y la relacion es de 1/2 (el apuntador mide la mitad del marcador) entonces buscaremos apuntadores entre
								//las cotas (150 - UMBRAL_AREA_APUNTADOR)  y (150 - UMBRAL_AREA_APUNTADOR)
#define ERODE  1
#define DILATE  1
#define SMOOTH 1
#define OFFSET_ROI 5			//Cuanto mas grande mas grande sera el ROI donde busco el apuntador
#define MIN_AREA_WIDTH_HEIGHT 5	//Este valor sirve para indicar cual es el area minima en la cual se busca un apuntador.
								//Por ejemplo si el marcador se encuentra apenas dentro de la pantalla y el height o el width no superan este valor entonces
								//el area no sera analizada en busca de un apuntador

int umbral = UMBRAL;
int  minH = MIN_H, maxH = MAX_H, minS = MIN_S, maxS = MAX_S, minB = MIN_B,maxB = MAX_B;
int num_apuntadores =0;
char **imagenesPatrones;

			/********************************************************************************************************************************
			 * 								CONFIGURACION DE ARTOOLKIT Y MARCADORES
			 ********************************************************************************************************************************/

#define MAX_MARCADORES 30
/* 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;

char           *cparam_name    = "Data/camera_para.dat";
ARParam         cparam;

char                *config_name = "Data/prototipo1/prototipo1.dat";
ARMultiMarkerInfoT  *config;


IplImage* img =NULL;
uchar* data1 =NULL;
IplImage* mono_Image = NULL;


				/******************************************************************************************************************************************
				 * 												VARIABLES DEL CALIBRADOR
				 *****************************************************************************************************************************************/

PropiedadesCalibracion propiedades;
ResultadosCalibracion resultados;
int marcador_actual = 0;				//ID Marcador sobre el cual se esta calculando la realacion con el apuntador.
int veces_marcador_actual = 0;			//Cantidad de iteracion llevadas a cabo sobre el marcador actual
										//Cuando veces_marcador_actual llega a propiedades.veces_por_marcador , se pasa al siguiente marcador.
int recorrido_completo = 0;				//Indica si termino de recorrer todos los marcadores
int contador_no_detectado = 0; //Cada vez que se busca el apuntador sobre el marcador actual y no se detecta se incrementa esta variable
															//Si la variable alcanza MAX_NO_DETECCION se muestra mensaje en pantalla hasta que sea detectado
															//En caso de que se detecta al menos una vez entonces el contador vuelve a 0

int min_area_encontrada = 0;
int max_area_encontrada = 0;
				/***************************************************************************************************************************************
				 * 												DECALRACIONES DE FUNCIONES AUXILIARES Y VARIABLES AUXILIAREAS
				 ****************************************************************************************************************************************/
static void   init(void);
static void   cleanup(void);
static void   keyEvent( unsigned char key, int x, int y);
static void   mainLoop(void);
static void   draw();

double hXi_hYi[3][4];
double vertexAux[4][2];

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

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

static int cargarDeFichero(void) {
	FILE *fich;
	char c;
	fich=fopen("conf.con","r");
	if(fich != NULL) {
		fscanf(fich," %c",&c);
		if(c=='#'){ //si el separador es correcto y no es fin de fichero carga valores;
			fscanf(fich,"%d",&minH);
			fscanf(fich,"%d",&maxH);
			fscanf(fich,"%d",&minS);
			fscanf(fich,"%d",&maxS);
			fscanf(fich,"%d",&minB);
			fscanf(fich,"%d",&maxB);
		}
		fclose(fich);
		printf("Configuracion HSV cargada:\n");
		printf("minH = %d , maxH = %d\n",minH,maxH);
		printf("minS = %d , maxS = %d\n",minS,maxS);
		printf("minB = %d , maxB = %d\n",minB,maxB);
		return 1;
	}else{
		printf("Se usaran los valores por defecto, el motivo es: No se puede leer el archivo de configuracion\n");
	}
	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 == 'v' ) {
		mostrarVideo = 1 - mostrarVideo;
		if(!mostrarVideo){
			argCleanWindow(0,0);
		}
    }

     if( key == 'r' ) {
    	printf("Mostrar Radar\n");
		mostrarRadar = 1 - mostrarRadar;
		if(!mostrarRadar){
			argCleanWindow(2,1);
		}
	}

    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;
    }

    /* turn on and off the debug mode with d*/
    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 == '+' ) {
			minH += 5;
            printf("*** Aumento hue: %d\n", minH);
	}

    if( key == '-' ) {
			minH -= 5;
			printf("*** Disminuyo hue: %d\n", minH);
	}



}



/* Despliega en pantalla el marcador que se debe tocar */
void mensajeTocarMarcador(int Idmarcador){
	if(Idmarcador>=0){
		EscribirTexto("Por favor toque el marcador y ",200,30,0);
		EscribirTexto("mueva el apuntador sin separarlo del mismo",200,42,0);
		PlayImage(imagenesPatrones[Idmarcador],"Marcador",1);
	}
}

void avanzar(){
	veces_marcador_actual++;
	if(veces_marcador_actual==propiedades.veces_por_marcador){ //Termine con este marcador, paso al siguiente
		if(marcador_actual >= config->marker_num -1){  //Si hay 6 marcadores y el Id del elemento actual es 5 quiere decir que es el ultimo elemento
			recorrido_completo =1;				   //por lo tanto ya los recorri todos y es momento de terminar la calibracion
			printf("Recorrido completo\n");			//Es posible deducir que es el ultimo porque los id de los marcadores empiezan en 0 y son correlativos
		}else{
			marcador_actual++;
			printf("Avanzo de marcador : %d\n",marcador_actual);

			veces_marcador_actual =0;
			mensajeTocarMarcador(marcador_actual);
		}
	}
}



void desplegarResultadosCalibracion()
{	printf("Desplegar resultados...\n");
    EscribirTexto("PROCESANDO DATOS OBTENIDOS.....", 200, 70, 1);
    /*printf("Calculando promedios\n");
    for(int i;i < config->marker_num;i++){
        propiedades.promedios[i] = calcularPromedio(propiedades.relaciones[i], propiedades.veces_por_marcador);
    }*/
    printf("Desplegando promedios\n");
    char aux[100];
    EscribirTexto("---> RESULTADOS", 200, 85, 1);
    int posY = 95;
    for(int i;i < config->marker_num;i++){
        sprintf(aux, "Marcador %d:  * Relacion Area: %f ",config->marker[i].patt_id, resultados.relaciones_promedio[config->marker[i].patt_id]);
        EscribirTexto(aux, 220, posY,0);
        posY += 10;
        sprintf(aux, "             * Min area apuntador: %d -- Max Area Apuntador: %d",resultados.areas_minimas[config->marker[i].patt_id],resultados.areas_maximas[config->marker[i].patt_id]);
        EscribirTexto(aux,220,posY,1);
        posY += 10;
    }
}

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

    /* grab a video frame */
    arUtilSleep(90000);
    if( (dataPtr = (ARUint8 *)arVideoGetImage()) == NULL ) {
	printf("sleep\n");
	arUtilSleep(2);
        return;
    }
    if( count == 0 ) arUtilTimerReset();
    count++;

    if(img==NULL){
    	img = crearImagenCv2(dataPtr,xsize,ysize);
    }
    if(!recorrido_completo){
		/* detect the markers in the video frame */
		if( arDetectMarkerLite(dataPtr, thresh, &marker_info, &marker_num) < 0 ) {
			cleanup();
			exit(0);
		}



		//argSwapBuffers();
		if(mostrarVideo){
			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(6.0);
				for( i = 0; i < marker_num; i++ ) {
					argDrawSquare( marker_info[i].vertex, 0, 0 );
				}
				glLineWidth( 1.0 );
			}
		}

		if(mostrarRadar){
			argDrawMode2D();
			argCleanWindow(2,1);
			dibujarLimitesPantalla(xsize, ysize);
			for( i = 0; i < marker_num; i++ ) {
				dibujarMiniMarcador(0,0,marker_info[i].vertex,1);
			}
		}
		arVideoCapNext();

		if( (err=arMultiGetTransMat(marker_info, marker_num, config)) < 0 ) {
			argSwapBuffers();
			cvShowImage("Debug Image",NULL);
			cvWaitKey(27);
			return;
		}

		if(err > 100.0 ) {
		  printf("saliendo 2\n");
		  argSwapBuffers();
		  return;
		}

		if(mostrarVideo){
			argDrawMode3D();
			argDraw3dCamera( 0, 0 );
			glClearDepth( 1.0 );
			glClear(GL_DEPTH_BUFFER_BIT);
		}
	//	printf("Marcadores detectados = %d\n",marker_num);


		if(marker_num>0){
			if(marker_num>config->marker_num){
				fprintf(stderr, "Atencion: Se estan detectando mas marcadores de los que hay cargados en la configuracion\n Ajustar la posicion de la hoja o la iluminacion\n");
				//TODO Se detectaron mas marcadores de los que hay cargados
			}
			for( i = 0; i < config->marker_num; i++ ) {
				if( (config->marker[i].visible <= 0)&&(config->marker[i].patt_id==marcador_actual) ){
					if(i!=config->marker[i].patt_id){ //Nunca deberia entrar en este if. Si lo hace entonces hay algo mal porque elementos en config deberian estar por orden
						fprintf(stderr, "CUIDADOOOOOOOOO!!! EL patt_id no coincide con el indice en el config->marker[]\n");
					}
					//draw( config->trans, config->marker[i].trans, 0 );
					/*
					*		MARCADOR NO DETECTADO.
					*									* TENEMOS SUS COORDENADAS
					*														- Marker Coordinates (Xm,Ym)
					*														- Camera Coordinates (Xc,Yc)
					*									* NECESITAMOS SUS COORDENADAS
					*														- Ideal Screen Coordinates  (xc,yc)
					*
					*/
					obtenerCoordenadasIdealesMarcadorNoDetectado(config->trans,config->marker[i],hXi_hYi,vertexAux);

					/*Busco el apuntador y guardo su area en propiedades.medidas_apuntador[i][veces_marcador_actual]*/
					double areaZonaEstudio = 0;
					float area = hayApuntador(minH,maxH,minS,maxS,minB,maxB,vertexAux,OFFSET_ROI,(uchar*)dataPtr,img,mono_Image,
							MIN_AREA_WIDTH_HEIGHT,MIN_AREA,MAX_AREA,ERODE,DILATE,SMOOTH,1,&areaZonaEstudio,"Debug Image","ROI Image");

					/*obtengo el area del marcador y la guardo en propiedades.medidas_marcador[config->marker[i].patt_id][veces_marcador_actual]*/
					if(areaZonaEstudio > 0){
						propiedades.medidas_marcador[config->marker[i].patt_id][veces_marcador_actual]=areaZonaEstudio;
					}else{	//Hay algun problema con el area del marcador, no tomo en cuenta esta corrida, no se actualiza veces_marcador_actual
						break;
					}
					if(area>0){
						if(contador_no_detectado==50){ //tenemos que sacar el cartel de la pantalla que dice que no se detecta el apuntador
							OcultarZona(200,150,200,20,1);
						}
						propiedades.medidas_apuntador[config->marker[i].patt_id][veces_marcador_actual] = area;

						//Actualizo min_area_encontrada y max_area_encontrada
						if(area<min_area_encontrada){
							min_area_encontrada = area;
						}
						if(area>max_area_encontrada){
							max_area_encontrada = area;
						}
					}else{
						if(contador_no_detectado<50){
							contador_no_detectado++;
						}
						if(contador_no_detectado == 50){
							EscribirTexto("No se esta detectando el apuntador",200,70,1);
						}
						break;	//No encontro apuntador asi que no se tiene en cuenta esta corrida, no se actualiza veces_marcador_actual
					}

					/*Calculo la relacion y la guardo en propiedades.relaciones[config->marker[i].patt_id][veces_marcador_actual]*/
					propiedades.relaciones[config->marker[i].patt_id][veces_marcador_actual] = areaZonaEstudio/area;

					avanzar();
					break; //No hay necesidad de seguir el for, ya sabemos que el apuntador esta sobre un solo elemento y es marcador_actual
				}else{
					cvShowImage("Debug Image",NULL);
					cvShowImage("ROI Image",NULL);
					cvWaitKey(27);
				}
			}
		}else{
			cvShowImage("Debug Image",NULL);
			cvShowImage("ROI Image",NULL);
			cvWaitKey(27);
		}

		argSwapBuffers();
    }else{
				//Para todos los marcadores calculo las relaciones con el area del apuntador, el area de apuntador minimo y maxima
				procesarDatosCalibracion(&propiedades,&resultados,config->marker_num);
				desplegarResultadosCalibracion();
				guardarDatosCalibracion(&resultados,"datosCalibracion.conf",config->marker_num);
esperar:    	char c =cvWaitKey(0);
				if(c == 27){
					cleanup();
					exit(0);
				}else{
					goto esperar;
				}

    }
}

void switch_hupper( int position ) {
    maxH = position;
}

void switch_hlower( int position ) {
    minH = position;
}

void switch_saturationmax( int position ) {
    maxS = position;
}

void switch_saturation( int position ) {
    minS = position;
}

void switch_brightnessmax( int position ) {
    maxB = position;
}

void switch_brightness( int position ) {
    minB = position;
}

void initDebugImage(){
   cvNamedWindow("Debug Image",0);
   cvResizeWindow("Debug Image",600,600);

   cvNamedWindow("ROI Image",0);
   cvResizeWindow("ROI Image",600,600);
   //Trackbar
   cvCreateTrackbar("Hue Upper","Debug Image",&maxH,390,switch_hupper);
   cvCreateTrackbar("Hue Lower","Debug Image",&minH,360,switch_hlower);

   cvCreateTrackbar("SaturationMax","Debug Image",&maxS,100,switch_saturationmax);
   cvCreateTrackbar("SaturationMin","Debug Image",&minS,100,switch_saturation);

   cvCreateTrackbar("BrightnessMax","Debug Image",&maxB,100,switch_brightnessmax);
   cvCreateTrackbar("BrightnessMin","Debug Image",&minB,100,switch_brightness);
}

static void init( void )
{
	/*ALGO DE HISTOGRAMAS
	 * IplImage* hue = crearImagenCv2(data1,xsize,ysize);
	IplImage* mask = cvCreateImage( cvGetSize(hue), 8, 1 );
	float max_val = 0.0f;
	float min_val = 0.0f;

	float hranges_arr[] = {0,180};
	float* hranges = hranges_arr;
	CvHistogram* hist = cvCreateHist(1,16,CV_HIST_ARRAY,hranges,1 );
	cvCalcHist(&hue,hist,0,mask);
	cvGetMinMaxHistValue( hist, &min_val, &max_val, 0, 0 );
	cvConvertScale( hist->bins, hist->bins, max_val ? 255. / max_val : 0., 0 );
	 */
    ARParam  wparam;
    //Cargo configuracion del color a detectar
    cargarDeFichero();

    /* 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);
    }

    /* Propiedades del calibrado */
    initPropiedadesCalibracion(30,&propiedades,&resultados,config->marker_num,1,1/*debugmode*/);
    imagenesPatrones = new char*[config->marker_num];
    for(int k=0;k<config->marker_num;k++){
    	imagenesPatrones[k] = new char[255];
    }
    leerConfiguracionCalibracion("Data/calibrador/calibrador.config",config->marker_num,imagenesPatrones);

    printf("Imagenes cargadas para los marcadores\n");
    for(int k=0;k<config->marker_num;k++){
    	printf("%s\n",imagenesPatrones[k]);
    }


    /* 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;

    data1= new uchar[xsize* AR_PIX_SIZE_DEFAULT*ysize];   // Creo data1 del maximo tamanio que pueda necesitar
    img = crearImagenCv2(data1,xsize,ysize);  // Idem con la imagenCv
    mono_Image =cvCreateImage( cvGetSize(img), 8, 1 );

    //Inicializacion de sdl
   	if(!initSDL(800,350)){
   		cleanup();
   		exit(0);
   	}
   	CargarFondo("Data/prototipo1/fondoPrototipo1.bmp",1);
   	marcador_actual = 0;

   	mensajeTocarMarcador(0);


   	initDebugImage();

}

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