diff options
Diffstat (limited to 'multi/SRC/multiTest.c')
| -rwxr-xr-x | multi/SRC/multiTest.c | 499 |
1 files changed, 0 insertions, 499 deletions
diff --git a/multi/SRC/multiTest.c b/multi/SRC/multiTest.c deleted file mode 100755 index c3d4c4c..0000000 --- a/multi/SRC/multiTest.c +++ /dev/null @@ -1,499 +0,0 @@ -#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(); -} - |