main.cpp
Go to the documentation of this file.
00001 #include <windows.h>
00002 #include <stdio.h>
00003 #include <stdlib.h>
00004 #include <string.h>
00005 
00006 #include <GL/gl.h>
00007 #include <GL/glut.h>
00008 //#include <AR/gsub.h>
00009 //#include <ARToolKitPlus/param.h>
00010 //#include <ARToolKitPlus/ar.h>
00011 //#include <ARToolKitPlus/matrix.h>
00012 //#include <AR/ARFrameGrabber.h>
00013 
00014 #include <ARToolKitPlus/Tracker.h>
00015 
00016 #include "ARFileGrabber.h"
00017 #include "calib_camera.h"
00018 
00019 //#define   USE_TEXMAP  0
00020 
00021 #ifndef GL_ABGR
00022 #define GL_ABGR GL_ABGR_EXT
00023 #endif
00024 #ifndef GL_BGRA
00025 #define GL_BGRA GL_BGRA_EXT
00026 #endif
00027 #ifndef GL_BGR
00028 #define GL_BGR GL_BGR_EXT
00029 #endif
00030 
00031 int             win;
00032 char            *vconf = "";
00033 int             xsize, ysize;
00034 int             thresh = THRESH;
00035 int             count = 0;
00036 unsigned char   *clipImage;
00037 
00038 CALIB_PATT_T    patt;          
00039 ARFloat          dist_factor[4];
00040 ARFloat          mat[3][4];
00041 
00042 int             point_num;
00043 int             sx, sy, ex, ey;
00044 
00045 int             status;
00046 int             check_num;
00047 
00048 #ifdef USE_TEXMAP
00049 static GLuint   glid[2];
00050 static int      tex1Xsize1 = 1;
00051 static int      tex1Xsize2 = 1;
00052 static int      tex1Ysize  = 1;
00053 static void dispImageTex1( unsigned char *pimage );
00054 static void dispImageTex2( unsigned char *pimage );
00055 #endif
00056 
00057 static void     init(void);
00058 static void     mouseEvent(int button, int state, int x, int y);
00059 static void     motionEvent( int x, int y );
00060 static void     keyEvent(unsigned char key, int x, int y);
00061 static void     dispImage(void);
00062 static void     dispImage2( unsigned char *pimage );
00063 static void     dispClipImage( int sx, int sy, int xsize, int ysize, ARToolKitPlus::ARUint8 *clipImage );
00064 static void     draw_warp_line( ARFloat a, ARFloat b , ARFloat c );
00065 static void     draw_line(void);
00066 static void     draw_line2( ARFloat *x, ARFloat *y, int num );
00067 static void     draw_warp_line( ARFloat a, ARFloat b , ARFloat c );
00068 static void     print_comment( int status );
00069 static void     save_param(void);
00070 
00071 
00072 #pragma message("WARNING: make sure that you compile ARToolKitPlus with:")
00073 #pragma message("  - no 8-bit image support")
00074 #pragma message("  - ARFloat as double")
00075 #pragma message("--> otherwise linker errors will occur")
00076 
00077 // we don't use ARFrameGrabber...
00078 //
00079 // ARFrameGrabber camera;
00080 
00081 // this camera calib version uses the ARFileGrabber class
00082 // instead of ARFrameGrabber. which will load files as specified
00083 // in the constructor...
00084 // (filename pattern, width, height, bytes_per_component)
00085 //
00086 #define CAM_W 640
00087 #define CAM_H 480
00088 ARFileGrabber camera("dump_%02d.raw", CAM_W,CAM_H,4);
00089 
00090 ARToolKitPlus::Tracker *theTracker = new ARToolKitPlus::Tracker();
00091 
00092 static void     save_param(void)
00093 {
00094     char     name[256];
00095     ARToolKitPlus::ARParam  param;
00096     int      i, j;
00097 
00098     param.xsize = xsize;
00099     param.ysize = ysize;
00100     for( i = 0; i < 4; i++ ) param.dist_factor[i] = dist_factor[i];
00101     for( j = 0; j < 3; j++ ) {
00102         for( i = 0; i < 4; i++ ) {
00103             param.mat[j][i] = mat[j][i];
00104         }
00105     }
00106 
00107         ARToolKitPlus::Param::display( &param );
00108 
00109     printf("Filename: ");
00110     scanf( "%s", name ); while( getchar()!= '\n' );
00111         ARToolKitPlus::Tracker::arParamSave( name, 1, &param );
00112 
00113     return;
00114 }
00115 
00116 int main()
00117 {
00118         if(ARToolKitPlus::getPixelFormat()!=ARToolKitPlus::PIXEL_FORMAT_ABGR &&
00119            ARToolKitPlus::getPixelFormat()!=ARToolKitPlus::PIXEL_FORMAT_BGRA &&
00120            ARToolKitPlus::getPixelFormat()!=ARToolKitPlus::PIXEL_FORMAT_BGR)
00121         {
00122                 printf("ERROR: CameraCalib needs one of: PIXEL_FORMAT_ABGR, PIXEL_FORMAT_BGRA or PIXEL_FORMAT_BGR\n");
00123                 exit(-1);
00124         }
00125 
00126         if(ARToolKitPlus::usesSinglePrecision())
00127         {
00128                 printf("ERROR: CameraCalib needs double precision\n");
00129                 exit(-1);
00130         }
00131 
00132 
00133 
00134         CoInitialize(NULL);
00135         //initialize applications
00136     init();
00137 
00138     glutKeyboardFunc(keyEvent);
00139     glutMouseFunc(mouseEvent);
00140     glutMotionFunc(motionEvent);
00141     glutIdleFunc(dispImage);
00142     glutDisplayFunc(dispImage);
00143 
00144     print_comment(0);
00145     status = 0;
00146     point_num = 0;
00147  
00148         //start the video capture
00149         //camera.Init(0);
00150         //camera.DisplayProperties();
00151         //camera.SetFlippedImage(true);
00152 
00153         //start the main event loop
00154     glutMainLoop();
00155         CoUninitialize();
00156         return 0;
00157 }
00158 
00159 static void init(void)
00160 {
00161     ARFloat  length;
00162     int     i, j;
00163 
00164     patt.h_num    = H_NUM;
00165     patt.v_num    = V_NUM;
00166     patt.loop_num = 0;
00167     if( patt.h_num < 3 || patt.v_num < 3 ) exit(0);
00168 
00169     printf("Input the length between each markers: ");
00170     scanf("%lf", &length); while( getchar()!='\n' );
00171     patt.world_coord = (CALIB_COORD_T *)malloc( sizeof(CALIB_COORD_T)*patt.h_num*patt.v_num );
00172     for( j = 0; j < patt.v_num; j++ ) {
00173         for( i = 0; i < patt.h_num; i++ ) {
00174             patt.world_coord[j*patt.h_num+i].x_coord =  length * i;
00175             patt.world_coord[j*patt.h_num+i].y_coord =  length * j;
00176         }
00177     }
00178 
00179         /* Note: the video image captured is hard coded to CAM_WxCAM_H */
00180         xsize = CAM_W; ysize = CAM_H;
00181     printf("Image size (x,y) = (%d,%d)\n", xsize, ysize);
00182 
00183     glutInitDisplayMode(GLUT_RGB | GLUT_DOUBLE);
00184     glutInitWindowSize(xsize, ysize);
00185     glutInitWindowPosition(100,100);
00186     win = glutCreateWindow("Calib distotion param");
00187 
00188     glMatrixMode(GL_MODELVIEW);
00189     glLoadIdentity();
00190     glMatrixMode(GL_PROJECTION);
00191     glLoadIdentity();
00192     glOrtho(-0.5, xsize-0.5, -0.5, ysize-0.5, -1.0, 1.0);
00193     glViewport(0, 0, xsize, ysize);
00194 
00195     clipImage = (unsigned char *)malloc( xsize*ysize*PIX_SIZE );
00196     if( clipImage == NULL ) exit(0);
00197 
00198 #ifdef USE_TEXMAP
00199     glGenTextures(2, glid);
00200     glBindTexture( GL_TEXTURE_2D, glid[0] );
00201     glPixelStorei( GL_UNPACK_ALIGNMENT, 1 );
00202     glTexParameterf( GL_TEXTURE_2D, GL_TEXTURE_WRAP_S, GL_CLAMP );
00203     glTexParameterf( GL_TEXTURE_2D, GL_TEXTURE_WRAP_T, GL_CLAMP );
00204     glTexParameterf( GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_NEAREST );
00205     glTexParameterf( GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_NEAREST );
00206     glTexEnvf( GL_TEXTURE_ENV, GL_TEXTURE_ENV_MODE, GL_DECAL );
00207     glBindTexture( GL_TEXTURE_2D, glid[1] );
00208     glPixelStorei( GL_UNPACK_ALIGNMENT, 1 );
00209     glTexParameterf( GL_TEXTURE_2D, GL_TEXTURE_WRAP_S, GL_CLAMP );
00210     glTexParameterf( GL_TEXTURE_2D, GL_TEXTURE_WRAP_T, GL_CLAMP );
00211     glTexParameterf( GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_NEAREST );
00212     glTexParameterf( GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_NEAREST );
00213     glTexEnvf( GL_TEXTURE_ENV, GL_TEXTURE_ENV_MODE, GL_DECAL );
00214     if( xsize > 512 ) {
00215         tex1Xsize1 = 512;
00216         tex1Xsize2 = 1;
00217         while( tex1Xsize2 < xsize - tex1Xsize1 ) tex1Xsize2 *= 2;
00218     }
00219     else {
00220         tex1Xsize1 = 1;
00221         while( tex1Xsize1 < xsize ) tex1Xsize1 *= 2;
00222     }
00223     tex1Ysize  = 1;
00224     while( tex1Ysize < ysize ) tex1Ysize *= 2;
00225 #endif
00226 }
00227 
00228 static void motionEvent( int x, int y )
00229 {
00230     unsigned char   *p, *p1;
00231     int             ssx, ssy, eex, eey;
00232     int             i, j, k;
00233 
00234     if( status == 1 && sx != -1 && sy != -1 ) {
00235         ex = x;
00236         ey = y;
00237 
00238         if( sx < ex ) { ssx = sx; eex = ex; }
00239          else         { ssx = ex; eex = sx; }
00240         if( sy < ey ) { ssy = sy; eey = ey; }
00241          else         { ssy = ey; eey = sy; }
00242         p1 = clipImage;
00243         for( j = ssy; j <= eey; j++ ) {
00244             p = &(patt.savedImage[patt.loop_num-1][(j*xsize+ssx)*PIX_SIZE]);
00245             for( i = ssx; i <= eex; i++ ) {
00246 #ifdef  AR_PIX_FORMAT_BGRA
00247                 k = (255*3 - (*(p+0) + *(p+1) + *(p+2))) / 3;
00248                 if( k < thresh ) k = 0;
00249                 else k = 255;
00250                 *(p1+0) = *(p1+1) = *(p1+2) = k;
00251 #endif
00252 #ifdef  AR_PIX_FORMAT_ABGR
00253                 k = (255*3 - (*(p+1) + *(p+2) + *(p+3))) / 3;
00254                 if( k < thresh ) k = 0;
00255                 else k = 255;
00256                 *(p1+1) = *(p1+2) = *(p1+3) = k;
00257 #endif
00258 #ifdef  AR_PIX_FORMAT_BGR
00259                 k = (255*3 - (*(p+0) + *(p+1) + *(p+2))) / 3;
00260                 if( k < thresh ) k = 0;
00261                 else k = 255;
00262                 *(p1+0) = *(p1+1) = *(p1+2) = k;
00263 #endif
00264                 p  += PIX_SIZE;
00265                 p1 += PIX_SIZE;
00266             }
00267         }
00268     }
00269 }
00270 
00271 static void mouseEvent(int button, int state, int x, int y)
00272 {
00273     unsigned char   *p, *p1;
00274     int             ssx, ssy, eex, eey;
00275     int             i, j, k;
00276 
00277     if( button == GLUT_RIGHT_BUTTON  && state == GLUT_UP ) {
00278         if( status == 0 ) {
00279             if( patt.loop_num > 0 ) {
00280                 calc_distortion( &patt, xsize, ysize, dist_factor );
00281                 printf("--------------\n");
00282                 printf("Center X: %f\n", dist_factor[0]);
00283                 printf("       Y: %f\n", dist_factor[1]);
00284                 printf("Dist Factor: %f\n", dist_factor[2]);
00285                 printf("Size Adjust: %f\n", dist_factor[3]);
00286                 printf("--------------\n");
00287                 status = 2;
00288                 check_num = 0;
00289                 print_comment(5);
00290             }
00291             else {
00292                 glutDestroyWindow( win );
00293                 exit(0);
00294             }
00295         }
00296         else if( status == 1 ) {
00297             if( patt.loop_num == 0 ) {printf("error!!\n"); exit(0);}
00298             patt.loop_num--;
00299             free( patt.point[patt.loop_num] );
00300             free( patt.savedImage[patt.loop_num] );
00301             status = 0;
00302             point_num = 0;
00303 
00304             if( patt.loop_num == 0 ) print_comment(0);
00305              else                    print_comment(4);
00306         }
00307     }
00308 
00309     if( button == GLUT_LEFT_BUTTON  && state == GLUT_DOWN ) {
00310         if( status == 1 && point_num < patt.h_num*patt.v_num ) {
00311             sx = ex = x;
00312             sy = ey = y;
00313 
00314             p  = &(patt.savedImage[patt.loop_num-1][(y*xsize+x)*PIX_SIZE]);
00315             p1 = &(clipImage[0]);
00316 #ifdef  AR_PIX_FORMAT_BGRA
00317             k = (255*3 - (*(p+0) + *(p+1) + *(p+2))) / 3;
00318             if( k < thresh ) k = 0;
00319                 else k = 255;
00320             *(p1+0) = *(p1+1) = *(p1+2) = k;
00321 #endif
00322 #ifdef  AR_PIX_FORMAT_ABGR
00323             k = (255*3 - (*(p+1) + *(p+2) + *(p+3))) / 3;
00324             if( k < thresh ) k = 0;
00325                 else k = 255;
00326             *(p1+1) = *(p1+2) = *(p1+3) = k;
00327 #endif
00328 #ifdef  AR_PIX_FORMAT_BGR
00329             k = (255*3 - (*(p+0) + *(p+1) + *(p+2))) / 3;
00330             if( k < thresh ) k = 0;
00331                 else k = 255;
00332             *(p1+0) = *(p1+1) = *(p1+2) = k;
00333 #endif
00334         }
00335     }
00336 
00337         if( button == GLUT_MIDDLE_BUTTON  && state == GLUT_DOWN )
00338                 camera.NextFile();
00339 
00340     if( button == GLUT_LEFT_BUTTON  && state == GLUT_UP ) {
00341         if( status == 0 && patt.loop_num < LOOP_MAX ) {
00342                         camera.GrabFrame();
00343             while( (p = (unsigned char *)camera.GetBuffer()) == NULL ) {
00344                                 camera.GrabFrame();
00345                 //arUtilSleep(2);
00346                                 Sleep(2);
00347             }
00348 #ifdef USE_TEXMAP
00349             patt.savedImage[patt.loop_num] = (unsigned char *)malloc( xsize*tex1Ysize*PIX_SIZE );
00350 #else
00351             patt.savedImage[patt.loop_num] = (unsigned char *)malloc( xsize*ysize*PIX_SIZE );
00352 #endif
00353             if( patt.savedImage[patt.loop_num] == NULL ) exit(0);
00354 
00355             p1 = patt.savedImage[patt.loop_num];
00356             for(i=0;i<xsize*ysize*PIX_SIZE;i++) *(p1++) = *(p++);
00357             //arVideoCapStop();
00358 
00359             patt.point[patt.loop_num] = (CALIB_COORD_T *)malloc( sizeof(CALIB_COORD_T)*patt.h_num*patt.v_num );
00360             if( patt.point[patt.loop_num] == NULL ) exit(0);
00361 
00362             patt.loop_num++;
00363             status = 1;
00364             sx = sy = ex= ey = -1;
00365 
00366             print_comment(1);
00367         }
00368         else if( status == 1 && point_num == patt.h_num*patt.v_num ) {
00369             status = 0;
00370             point_num = 0;
00371             //arVideoCapStart();
00372 
00373             printf("### No.%d ###\n", patt.loop_num);
00374             for( j = 0; j < patt.v_num; j++ ) {
00375                 for( i = 0; i < patt.h_num; i++ ) {
00376                     printf("%2d, %2d: %6.2f, %6.2f\n", i+1, j+1,
00377                            patt.point[patt.loop_num-1][j*patt.h_num+i].x_coord,
00378                            patt.point[patt.loop_num-1][j*patt.h_num+i].y_coord);
00379                 }
00380             }
00381             printf("\n\n");
00382             if( patt.loop_num < LOOP_MAX ) print_comment(4);
00383              else                          print_comment(6);
00384         }
00385         else if( status == 1 ) {
00386             if( sx < ex ) { ssx = sx; eex = ex; }
00387              else         { ssx = ex; eex = sx; }
00388             if( sy < ey ) { ssy = sy; eey = ey; }
00389              else         { ssy = ey; eey = sy; }
00390 
00391             patt.point[patt.loop_num-1][point_num].x_coord = 0.0;
00392             patt.point[patt.loop_num-1][point_num].y_coord = 0.0;
00393             p = clipImage;
00394             k = 0;
00395             for( j = 0; j < (eey-ssy+1); j++ ) {
00396                 for( i = 0; i < (eex-ssx+1); i++ ) {
00397                     patt.point[patt.loop_num-1][point_num].x_coord += i * *(p+1);
00398                     patt.point[patt.loop_num-1][point_num].y_coord += j * *(p+1);
00399                     k += *(p+1);
00400                     p += PIX_SIZE;
00401                 }
00402             }
00403             if( k != 0 ) {
00404                 patt.point[patt.loop_num-1][point_num].x_coord /= k;
00405                 patt.point[patt.loop_num-1][point_num].y_coord /= k;
00406                 patt.point[patt.loop_num-1][point_num].x_coord += ssx;
00407                 patt.point[patt.loop_num-1][point_num].y_coord += ssy;
00408                 point_num++;
00409             }
00410             sx = sy = ex= ey = -1;
00411 
00412             printf(" # %d/%d\n", point_num, patt.h_num*patt.v_num);
00413             if( point_num == patt.h_num*patt.v_num ) print_comment(2);
00414         }
00415         else if( status == 2 ) {
00416             check_num++;
00417             if( check_num == patt.loop_num ) {
00418                 if(patt.loop_num >= 2) {
00419                     if( calc_inp(&patt, dist_factor, xsize, ysize, mat) < 0 ) {
00420                         printf("Calibration failed.\n");
00421                         exit(0);
00422                     }
00423                     save_param();
00424                 }
00425                 glutDestroyWindow( win );
00426                 exit(0);
00427             }
00428 
00429             if( check_num+1 == patt.loop_num ) {
00430                 printf("\nLeft Mouse Button: Next Step.\n");
00431             }
00432             else {
00433                 printf("   %d/%d.\n", check_num+1, patt.loop_num);
00434             }
00435         }
00436     }
00437 }
00438 
00439 static void keyEvent(unsigned char key, int x, int y)
00440 {
00441     if( key == 't' ) {
00442         printf("Enter new threshold value (now = %d): ", thresh);
00443         scanf("%d",&thresh); while( getchar()!='\n' );
00444         printf("\n");
00445     }
00446 }
00447 
00448 static void dispImage(void)
00449 {
00450     unsigned char  *dataPtr;
00451     ARFloat         x, y;
00452     int            ssx, eex, ssy, eey;
00453     int            i;
00454 
00455     if( status == 0 ) {
00456                 /* grab a video frame */
00457                 camera.GrabFrame();
00458                 if( (dataPtr = (ARToolKitPlus::ARUint8 *)camera.GetBuffer()) == NULL ) {
00459                         //arUtilSleep(2); 
00460                         Sleep(2);
00461                         return;
00462                 }
00463         dispImage2( dataPtr );
00464     }
00465 
00466     else if( status == 1 ) {
00467         dispImage2( patt.savedImage[patt.loop_num-1] );
00468 
00469         for( i = 0; i < point_num; i++ ) {
00470             x = patt.point[patt.loop_num-1][i].x_coord;
00471             y = patt.point[patt.loop_num-1][i].y_coord;
00472             glColor3f( 1.0, 0.0, 0.0 );
00473             glBegin(GL_LINES);
00474               glVertex2f( (float)(x-10), (float)(ysize-1-y) );
00475               glVertex2f( (float)(x+10), (float)(ysize-1-y) );
00476               glVertex2f( (float)x, (float)((ysize-1)-(y-10)) );
00477               glVertex2f( (float)x, (float)((ysize-1)-(y+10)) );
00478             glEnd();
00479         }
00480 
00481         if( sx != -1 && sy != -1 ) {
00482             if( sx < ex ) { ssx = sx; eex = ex; }
00483              else         { ssx = ex; eex = sx; }
00484             if( sy < ey ) { ssy = sy; eey = ey; }
00485              else         { ssy = ey; eey = sy; }
00486             dispClipImage( ssx, ysize-1-ssy, eex-ssx+1, eey-ssy+1, clipImage );
00487 #if 0
00488             glColor3f( 0.0, 0.0, 1.0 );
00489             glBegin(GL_LINE_LOOP);
00490               glVertex2f( sx, (ysize-1)-sy );
00491               glVertex2f( ex, (ysize-1)-sy );
00492               glVertex2f( ex, (ysize-1)-ey );
00493               glVertex2f( sx, (ysize-1)-ey );
00494             glEnd();
00495 #endif
00496         }
00497     }
00498 
00499     else if( status == 2 ) {
00500         dispImage2( patt.savedImage[check_num] );
00501         for( i = 0; i < patt.h_num*patt.v_num; i++ ) {
00502             x = patt.point[check_num][i].x_coord;
00503             y = patt.point[check_num][i].y_coord;
00504             glColor3f( 1.0, 0.0, 0.0 );
00505             glBegin(GL_LINES);
00506               glVertex2f( (float)(x-10), (float)((ysize-1)-y) );
00507               glVertex2f( (float)(x+10), (float)((ysize-1)-y) );
00508               glVertex2f( (float)x, (float)((ysize-1)-(y-10)) );
00509               glVertex2f( (float)x, (float)((ysize-1)-(y+10)) );
00510             glEnd();
00511         }
00512         draw_line();
00513     }
00514 
00515     glutSwapBuffers();
00516 }
00517 
00518 /* cleanup function called when program exits */
00519 static void cleanup(void)
00520 {
00521     //argCleanup();
00522 }
00523 
00524 static void dispImage2( unsigned char *pimage )
00525 {
00526 #ifndef USE_TEXMAP
00527     int      sx, sy;
00528 
00529     sx = 0;
00530     sy = ysize - 1;
00531     glPixelZoom( 1.0, -1.0);
00532     glRasterPos3i( sx, sy, 0 );
00533 
00534 #ifdef  AR_PIX_FORMAT_ABGR
00535     glDrawPixels( xsize, ysize, GL_ABGR, GL_UNSIGNED_BYTE, pimage );
00536 #endif
00537 #ifdef  AR_PIX_FORMAT_BGRA
00538     glDrawPixels( xsize, ysize, GL_BGRA, GL_UNSIGNED_BYTE, pimage );
00539 #endif
00540 #ifdef  AR_PIX_FORMAT_BGR
00541     glDrawPixels( xsize, ysize, GL_BGR, GL_UNSIGNED_BYTE, pimage );
00542 #endif
00543 #else
00544     glDisable( GL_DEPTH_TEST );
00545     if( xsize > tex1Xsize1 ) dispImageTex1( pimage );
00546      else                    dispImageTex2( pimage );
00547 #endif
00548 }
00549 
00550 #ifdef USE_TEXMAP
00551 static void dispImageTex1( unsigned char *pimage )
00552 {
00553     float    sx, sy, ex, ey, z;
00554     float    tx, ty;
00555 
00556     glEnable( GL_TEXTURE_2D );
00557     glMatrixMode(GL_TEXTURE);
00558     glLoadIdentity();
00559     glMatrixMode(GL_MODELVIEW);
00560 
00561     glPixelStorei( GL_UNPACK_ROW_LENGTH, xsize );
00562     glPixelStorei( GL_UNPACK_IMAGE_HEIGHT, ysize );
00563 
00564     glBindTexture( GL_TEXTURE_2D, glid[0] );
00565 #ifdef  AR_PIX_FORMAT_ABGR
00566     glTexImage2D( GL_TEXTURE_2D, 0, 3, tex1Xsize1, tex1Ysize, 0, GL_ABGR, GL_UNSIGNED_BYTE, pimage );
00567 #endif
00568 #ifdef  AR_PIX_FORMAT_BGRA
00569     glTexImage2D( GL_TEXTURE_2D, 0, 3, tex1Xsize1, tex1Ysize, 0, GL_BGRA, GL_UNSIGNED_BYTE, pimage );
00570 #endif
00571 #ifdef  AR_PIX_FORMAT_BGR
00572     glTexImage2D( GL_TEXTURE_2D, 0, 3, tex1Xsize1, tex1Ysize, 0, GL_BGR, GL_UNSIGNED_BYTE, pimage );
00573 #endif
00574     tx = 1.0;
00575     ty = (ARFloat)ysize / (ARFloat)tex1Ysize;
00576     sx = -1.0;
00577     sy = ysize - 1;
00578     ex = sx + tex1Xsize1;
00579     ey = sy - ysize;
00580     z = 1.0;
00581     glBegin(GL_QUADS );
00582       glTexCoord2f( 0.0, 0.0 ); glVertex3f( sx, sy, z );
00583       glTexCoord2f(  tx, 0.0 ); glVertex3f( ex, sy, z );
00584       glTexCoord2f(  tx,  ty ); glVertex3f( ex, ey, z );
00585       glTexCoord2f( 0.0,  ty ); glVertex3f( sx, ey, z );
00586     glEnd();
00587 
00588     glBindTexture( GL_TEXTURE_2D, glid[1] );
00589 #ifdef  AR_PIX_FORMAT_ABGR
00590     glTexImage2D( GL_TEXTURE_2D, 0, 3, tex1Xsize2, tex1Ysize, 0, GL_ABGR, GL_UNSIGNED_BYTE, pimage+tex1Xsize1*PIX_SIZE );
00591 #endif
00592 #ifdef  AR_PIX_FORMAT_BGRA
00593     glTexImage2D( GL_TEXTURE_2D, 0, 3, tex1Xsize2, tex1Ysize, 0, GL_BGRA, GL_UNSIGNED_BYTE, pimage+tex1Xsize1*PIX_SIZE );
00594 #endif
00595 #ifdef  AR_PIX_FORMAT_BGR
00596     glTexImage2D( GL_TEXTURE_2D, 0, 3, tex1Xsize2, tex1Ysize, 0, GL_BGR, GL_UNSIGNED_BYTE, pimage+tex1Xsize1*PIX_SIZE );
00597 #endif
00598     tx = (ARFloat)(xsize-tex1Xsize1) / (ARFloat)tex1Xsize2;
00599     ty = (ARFloat)ysize / (ARFloat)tex1Ysize;
00600     sx = tex1Xsize1-1;
00601     sy = ysize - 1;
00602     ex = sx + tex1Xsize2;
00603     ey = sy - ysize;
00604     z = 1.0;
00605     glBegin(GL_QUADS );
00606       glTexCoord2f( 0.0, 0.0 ); glVertex3f( sx, sy, z );
00607       glTexCoord2f(  tx, 0.0 ); glVertex3f( ex, sy, z );
00608       glTexCoord2f(  tx,  ty ); glVertex3f( ex, ey, z );
00609       glTexCoord2f( 0.0,  ty ); glVertex3f( sx, ey, z );
00610     glEnd();
00611 
00612     glBindTexture( GL_TEXTURE_2D, 0 );
00613     glDisable( GL_TEXTURE_2D );
00614     glPixelStorei( GL_UNPACK_ROW_LENGTH, 0 );
00615     glPixelStorei( GL_UNPACK_IMAGE_HEIGHT, 0 );
00616 }
00617 
00618 static void dispImageTex2( unsigned char *pimage )
00619 {
00620     float    sx, sy, ex, ey, z;
00621     float    tx, ty;
00622 
00623     glEnable( GL_TEXTURE_2D );
00624     glMatrixMode(GL_TEXTURE);
00625     glLoadIdentity();
00626     glMatrixMode(GL_MODELVIEW);
00627 
00628     glPixelStorei( GL_UNPACK_ROW_LENGTH, xsize );
00629     glPixelStorei( GL_UNPACK_IMAGE_HEIGHT, ysize );
00630 
00631     glBindTexture( GL_TEXTURE_2D, glid[0] );
00632 #ifdef  AR_PIX_FORMAT_ABGR
00633     glTexImage2D( GL_TEXTURE_2D, 0, 3, tex1Xsize1, tex1Ysize, 0, GL_ABGR, GL_UNSIGNED_BYTE, pimage );
00634 #endif
00635 #ifdef  AR_PIX_FORMAT_BGRA
00636     glTexImage2D( GL_TEXTURE_2D, 0, 3, tex1Xsize1, tex1Ysize, 0, GL_BGRA, GL_UNSIGNED_BYTE, pimage );
00637 #endif
00638 #ifdef  AR_PIX_FORMAT_BGR
00639     glTexImage2D( GL_TEXTURE_2D, 0, 3, tex1Xsize1, tex1Ysize, 0, GL_BGR, GL_UNSIGNED_BYTE, pimage );
00640 #endif
00641     tx = (ARFloat)xsize / (ARFloat)tex1Xsize1;
00642     ty = (ARFloat)ysize / (ARFloat)tex1Ysize;
00643     sx = -1;
00644     sy = ysize - 1;
00645     ex = sx + xsize;
00646     ey = sy - ysize;
00647     z = 1.0;
00648     glBegin(GL_QUADS );
00649       glTexCoord2f( 0.0, 0.0 ); glVertex3f( sx, sy, z );
00650       glTexCoord2f(  tx, 0.0 ); glVertex3f( ex, sy, z );
00651       glTexCoord2f(  tx,  ty ); glVertex3f( ex, ey, z );
00652       glTexCoord2f( 0.0,  ty ); glVertex3f( sx, ey, z );
00653     glEnd();
00654 
00655     glBindTexture( GL_TEXTURE_2D, 0 );
00656     glDisable( GL_TEXTURE_2D );
00657     glPixelStorei( GL_UNPACK_ROW_LENGTH, 0 );
00658     glPixelStorei( GL_UNPACK_IMAGE_HEIGHT, 0 );
00659 }
00660 #endif
00661 
00662 static void draw_line(void)
00663 {
00664     ARFloat   *x, *y;
00665     int      max;
00666     int      i, j, k, l;
00667     int      p;
00668 
00669     max = (patt.v_num > patt.h_num)? patt.v_num: patt.h_num;
00670     x = (ARFloat *)malloc( sizeof(ARFloat)*max );
00671     y = (ARFloat *)malloc( sizeof(ARFloat)*max );
00672     if( x == NULL || y == NULL ) exit(0);
00673 
00674     i = check_num;
00675 
00676     for( j = 0; j < patt.v_num; j++ ) {
00677         for( k = 0; k < patt.h_num; k++ ) {
00678             x[k] = patt.point[i][j*patt.h_num+k].x_coord;
00679             y[k] = patt.point[i][j*patt.h_num+k].y_coord;
00680         }
00681         draw_line2( x, y, patt.h_num );
00682     }
00683 
00684     for( j = 0; j < patt.h_num; j++ ) {
00685         for( k = 0; k < patt.v_num; k++ ) {
00686             x[k] = patt.point[i][k*patt.h_num+j].x_coord;
00687             y[k] = patt.point[i][k*patt.h_num+j].y_coord;
00688         }
00689         draw_line2( x, y, patt.v_num );
00690     }
00691 
00692     for( j = 3 - patt.v_num; j < patt.h_num - 2; j++ ) {
00693         p = 0;
00694         for( k = 0; k < patt.v_num; k++ ) {
00695             l = j+k;
00696             if( l < 0 || l >= patt.h_num ) continue;
00697             x[p] = patt.point[i][k*patt.h_num+l].x_coord;
00698             y[p] = patt.point[i][k*patt.h_num+l].y_coord;
00699             p++;
00700         }
00701         draw_line2( x, y, p );
00702     }
00703 
00704     for( j = 2; j < patt.h_num + patt.v_num - 3; j++ ) {
00705         p = 0;
00706         for( k = 0; k < patt.v_num; k++ ) {
00707             l = j-k;
00708             if( l < 0 || l >= patt.h_num ) continue;
00709             x[p] = patt.point[i][k*patt.h_num+l].x_coord;
00710             y[p] = patt.point[i][k*patt.h_num+l].y_coord;
00711             p++;
00712         }
00713         draw_line2( x, y, p );
00714     }
00715 
00716     free( x );
00717     free( y );
00718 }
00719 
00720 static void draw_line2( ARFloat *x, ARFloat *y, int num )
00721 {
00722         ARToolKitPlus::ARMat    *input, *evec;
00723     ARToolKitPlus::ARVec    *ev, *mean;
00724     ARFloat   a, b, c;
00725     int      i;
00726 
00727     ev     = ARToolKitPlus::Vector::alloc( 2 );
00728     mean   = ARToolKitPlus::Vector::alloc( 2 );
00729     evec   = ARToolKitPlus::Matrix::alloc( 2, 2 );
00730 
00731     input  = ARToolKitPlus::Matrix::alloc( num, 2 );
00732     for( i = 0; i < num; i++ ) {
00733         ARToolKitPlus::Tracker::arParamObserv2Ideal( dist_factor, x[i], y[i],
00734                              &(input->m[i*2+0]), &(input->m[i*2+1]) );
00735     }
00736     if( ARToolKitPlus::Tracker::arMatrixPCA(input, evec, ev, mean) < 0 ) exit(0);
00737     a =  evec->m[1];
00738     b = -evec->m[0];
00739     c = -(a*mean->v[0] + b*mean->v[1]);
00740 
00741     ARToolKitPlus::Matrix::free( input );
00742     ARToolKitPlus::Matrix::free( evec );
00743     ARToolKitPlus::Vector::free( mean );
00744     ARToolKitPlus::Vector::free( ev );
00745 
00746     draw_warp_line(a, b, c);
00747 }
00748 
00749 static void draw_warp_line( ARFloat a, ARFloat b , ARFloat c )
00750 {
00751     ARFloat   x, y;
00752     ARFloat   x1, y1;
00753     int      i;
00754 
00755     glLineWidth( 1.0 );
00756     glBegin(GL_LINE_STRIP);
00757     if( a*a >= b*b ) {
00758         for( i = -20; i <= ysize+20; i+=10 ) {
00759             x = -(b*i + c)/a;
00760             y = i;
00761 
00762             ARToolKitPlus::Tracker::arParamIdeal2Observ( dist_factor, x, y, &x1, &y1 );
00763             glVertex2f( (float)x1, (float)(ysize-1-y1) );
00764         }
00765     }
00766     else {
00767         for( i = -20; i <= xsize+20; i+=10 ) {
00768             x = i;
00769             y = -(a*i + c)/b;
00770 
00771             ARToolKitPlus::Tracker::arParamIdeal2Observ( dist_factor, x, y, &x1, &y1 );
00772             glVertex2f( (float)x1, (float)(ysize-1-y1) );
00773         }
00774     }
00775     glEnd();
00776 }
00777 
00778 static void     print_comment( int status )
00779 {
00780     printf("\n-----------\n");
00781     switch( status ) {
00782         case 0:
00783            printf("Mouse Button\n");
00784            printf(" Left   : Grab image.\n");
00785            printf(" Right  : Quit.\n");
00786            break;
00787         case 1:
00788            printf("Mouse Button\n");
00789            printf(" Left   : Rubber-bounding of feature. (%d x %d)\n", patt.h_num, patt.v_num);
00790            printf(" Right  : Cansel rubber-bounding & Retry grabbing.\n");
00791            break;
00792         case 2:
00793            printf("Mouse Button\n");
00794            printf(" Left   : Save feature position.\n");
00795            printf(" Right  : Discard & Retry grabbing.\n");
00796            break;
00797         case 4:
00798            printf("Mouse Button\n");
00799            printf(" Left   : Grab next image.\n");
00800            printf(" Right  : Calc parameter.\n");
00801            break;
00802         case 5:
00803            printf("Mouse Button\n");
00804            printf(" Left   : Check fittness.\n");
00805            printf(" Right  :\n");
00806            printf("   %d/%d.\n", check_num+1, patt.loop_num);
00807            break;
00808         case 6:
00809            printf("Mouse Button\n");
00810            printf(" Left   :\n");
00811            printf(" Right  : Calc parameter.\n");
00812            printf("   %d/%d.\n", check_num+1, patt.loop_num);
00813            break;
00814     }
00815     printf("-----------\n");
00816 }
00817 
00818 static void dispClipImage( int sx, int sy, int xsize, int ysize, ARToolKitPlus::ARUint8 *clipImage )
00819 {
00820     glPixelZoom( (GLfloat)1.0, (GLfloat)-1.0);
00821     glRasterPos3i( sx, sy, 0 );
00822 #ifdef  AR_PIX_FORMAT_ABGR
00823     glDrawPixels( xsize, ysize, GL_ABGR, GL_UNSIGNED_BYTE, clipImage );
00824 #endif
00825 #ifdef  AR_PIX_FORMAT_BGRA
00826     glDrawPixels( xsize, ysize, GL_BGRA, GL_UNSIGNED_BYTE, clipImage );
00827 #endif
00828 #ifdef  AR_PIX_FORMAT_BGR
00829     glDrawPixels( xsize, ysize, GL_BGR, GL_UNSIGNED_BYTE, clipImage );
00830 #endif
00831 }
00832 
00833 
00834 


tuw_artoolkitplus
Author(s): Markus Bader
autogenerated on Sun May 29 2016 02:50:12