00001 #include <stdio.h>
00002 #include <stdlib.h>
00003 #if defined(_WIN32)
00004 #include <windows.h>
00005 #endif
00006 #ifndef __APPLE__
00007 # include <GL/glut.h>
00008 # ifdef GL_VERSION_1_2
00009 # include <GL/glext.h>
00010 # endif
00011 #else
00012 # include <GLUT/glut.h>
00013 # include <OpenGL/glext.h>
00014 #endif
00015 #include <AR/config.h>
00016 #include <AR/param.h>
00017 #include <AR/ar.h>
00018 #include <AR/gsub.h>
00019 #include <AR/video.h>
00020 #include <AR/gsubUtil.h>
00021
00022 #define CALIB_POS1_NUM 5 // Use 5 positions in the display image plane (x-y)
00023 #define CALIB_POS2_NUM 2 // Use 2 positions in the display depth axis (z)
00024
00025
00026 static double calib_pos[CALIB_POS1_NUM][2] = { { 160, 120 },
00027 { 480, 120 },
00028 { 320, 240 },
00029 { 160, 360 },
00030 { 480, 360 } };
00031 static double calib_pos2d[CALIB_POS1_NUM][CALIB_POS2_NUM][2];
00032 static double calib_pos3d[CALIB_POS1_NUM][CALIB_POS2_NUM][3];
00033 static int co1;
00034 static int co2;
00035 static int left_right;
00036 static double target_trans[3][4];
00037 static int target_id;
00038 static int target_visible;
00039 static double target_center[2] = { 0.0, 0.0 };
00040 static double target_width = 80.0;
00041
00042 static ARParam hmd_param[2];
00043 static int thresh;
00044 static int arFittingModeBak;
00045
00046 static int hmdMode;
00047 static int gMiniXnum, gMiniYnum;
00048 static void (*gMouseFunc)(int button, int state, int x, int y);
00049 static void (*gKeyFunc)(unsigned char key, int x, int y);
00050 static void (*gMainFunc)(void);
00051 static void (*gCalibPostFunc)(ARParam *lpara, ARParam *rpara);
00052
00053 static void argCalibMouseFunc(int button, int state, int x, int y);
00054 static void argCalibMainFunc(void);
00055 static int argDrawAttention(double pos[2], int color);
00056
00057 void argUtilCalibHMD( int targetId, int thresh2,
00058 void (*postFunc)(ARParam *lpara, ARParam *rpara) )
00059 {
00060 argInqSetting( &hmdMode, &gMiniXnum, &gMiniYnum,
00061 &gMouseFunc, &gKeyFunc, &gMainFunc );
00062
00063 if( hmdMode == 0 ) return;
00064
00065 target_id = targetId;
00066 thresh = thresh2;
00067 gCalibPostFunc = postFunc;
00068 arFittingModeBak = arFittingMode;
00069
00070 arFittingMode = AR_FITTING_TO_IDEAL;
00071 co1 = 0;
00072 co2 = 0;
00073 left_right = 0;
00074 target_visible = 0;
00075
00076 glutKeyboardFunc( NULL );
00077 glutMouseFunc( argCalibMouseFunc );
00078 glutIdleFunc( argCalibMainFunc );
00079 glutDisplayFunc( argCalibMainFunc );
00080 }
00081
00082 static void argCalibMouseFunc(int button, int state, int x, int y)
00083 {
00084 if( button == GLUT_LEFT_BUTTON && state == GLUT_DOWN ) {
00085 if( target_visible ) {
00086 calib_pos3d[co1][co2][0] = target_trans[0][3];
00087 calib_pos3d[co1][co2][1] = target_trans[1][3];
00088 calib_pos3d[co1][co2][2] = target_trans[2][3];
00089 calib_pos2d[co1][co2][0] = calib_pos[co1][0];
00090 calib_pos2d[co1][co2][1] = calib_pos[co1][1];
00091 co2++;
00092 if( co2 == CALIB_POS2_NUM ) {
00093 co1++;
00094 co2 = 0;
00095 }
00096
00097 if( co1 == CALIB_POS1_NUM ) {
00098 hmd_param[left_right].xsize = AR_HMD_XSIZE;
00099 hmd_param[left_right].ysize = AR_HMD_YSIZE;
00100 hmd_param[left_right].dist_factor[0] = AR_HMD_XSIZE / 2.0;
00101 hmd_param[left_right].dist_factor[1] = AR_HMD_YSIZE / 2.0;
00102 hmd_param[left_right].dist_factor[2] = 0.0;
00103 hmd_param[left_right].dist_factor[3] = 1.0;
00104 if( arParamGet( (double (*)[3])calib_pos3d, (double (*)[2])calib_pos2d,
00105 CALIB_POS1_NUM*CALIB_POS2_NUM, hmd_param[left_right].mat) < 0 ) {
00106 (*gCalibPostFunc)( NULL, NULL );
00107 arFittingMode = arFittingModeBak;
00108 glutKeyboardFunc( gKeyFunc );
00109 glutMouseFunc( gMouseFunc );
00110 glutIdleFunc( gMainFunc );
00111 glutDisplayFunc( gMainFunc );
00112 return;
00113 }
00114
00115 co1 = 0;
00116 co2 = 0;
00117 left_right++;
00118 if( left_right == 2 ) {
00119 argLoadHMDparam( &hmd_param[0], &hmd_param[1] );
00120 arFittingMode = arFittingModeBak;
00121
00122 if( gCalibPostFunc != NULL ) {
00123 (*gCalibPostFunc)( &hmd_param[0], &hmd_param[1] );
00124 }
00125 glutKeyboardFunc( gKeyFunc );
00126 glutMouseFunc( gMouseFunc );
00127 glutIdleFunc( gMainFunc );
00128 glutDisplayFunc( gMainFunc );
00129 return;
00130 }
00131 }
00132 }
00133 }
00134
00135 if( button == GLUT_RIGHT_BUTTON && state == GLUT_DOWN ) {
00136 (*gCalibPostFunc)( NULL, NULL );
00137 arFittingMode = arFittingMode;
00138 glutKeyboardFunc( gKeyFunc );
00139 glutMouseFunc( gMouseFunc );
00140 glutIdleFunc( gMainFunc );
00141 glutDisplayFunc( gMainFunc );
00142 return;
00143 }
00144 }
00145
00146 static void argCalibMainFunc(void)
00147 {
00148 ARUint8 *dataPtr;
00149 ARMarkerInfo *marker_info;
00150 int marker_num;
00151 int i, j;
00152 double cfmax;
00153 double err;
00154
00155
00156 if( (dataPtr = (ARUint8 *)arVideoGetImage()) == NULL ) {
00157 arUtilSleep(2);
00158 return;
00159 }
00160 target_visible = 0;
00161
00162
00163 if( arDetectMarker(dataPtr, thresh,
00164 &marker_info, &marker_num) < 0 ) {
00165 (*gCalibPostFunc)( NULL, NULL );
00166 arFittingMode = arFittingModeBak;
00167 glutKeyboardFunc( gKeyFunc );
00168 glutMouseFunc( gMouseFunc );
00169 glutIdleFunc( gMainFunc );
00170 glutDisplayFunc( gMainFunc );
00171 return;
00172 }
00173 arVideoCapNext();
00174
00175 glClearColor( 0.0, 0.0, 0.0, 0.0 );
00176 glClear(GL_COLOR_BUFFER_BIT);
00177
00178
00179
00180 if( arDebug && gMiniXnum >= 2 && gMiniYnum >= 1 ) {
00181 argDispImage( dataPtr, 1, 1 );
00182 if( arImageProcMode == AR_IMAGE_PROC_IN_HALF )
00183 argDispHalfImage( arImage, 2, 1 );
00184 else
00185 argDispImage( arImage, 2, 1);
00186
00187 glColor3f( 1.0, 0.0, 0.0 );
00188 glLineWidth( 3.0 );
00189 for( i = 0; i < marker_num; i++ ) {
00190 if( marker_info[i].id < 0 ) continue;
00191 argDrawSquare( marker_info[i].vertex, 2, 1 );
00192 }
00193 glLineWidth( 1.0 );
00194 }
00195
00196 if( left_right == 0 ) argDraw2dLeft();
00197 else argDraw2dRight();
00198 glLineWidth( 3.0 );
00199 glColor3f( 1.0, 1.0, 1.0 );
00200 argLineSegHMD( 0, calib_pos[co1][1], AR_HMD_XSIZE, calib_pos[co1][1] );
00201 argLineSegHMD( calib_pos[co1][0], 0, calib_pos[co1][0], AR_HMD_YSIZE );
00202 glLineWidth( 1.0 );
00203 argDrawMode2D();
00204
00205 cfmax = 0.0;
00206 j = -1;
00207 for( i = 0; i < marker_num; i++ ) {
00208 if( marker_info[i].id != target_id ) continue;
00209
00210 if( marker_info[i].cf > cfmax ) {
00211 cfmax = marker_info[i].cf;
00212 j = i;
00213 }
00214 }
00215 if( j < 0 ) {
00216 argSwapBuffers();
00217 return;
00218 }
00219 err = arGetTransMat(&marker_info[j], target_center, target_width, target_trans);
00220 if( err >= 0.0 ) {
00221 target_visible = 1;
00222
00223 if( left_right == 0 ) argDraw2dLeft();
00224 else argDraw2dRight();
00225 argDrawAttention( calib_pos[co1], co2 );
00226 argDrawMode2D();
00227
00228 if( arDebug && gMiniXnum >= 2 && gMiniYnum >= 1 ) {
00229 glColor3f( 0.0, 1.0, 0.0 );
00230 glLineWidth( 3.0 );
00231 argDrawSquare( marker_info[j].vertex, 1, 1 );
00232 glLineWidth( 1.0 );
00233 }
00234 }
00235
00236 argSwapBuffers();
00237 }
00238
00239 static int argDrawAttention( double pos[2], int color )
00240 {
00241 switch( color%7 ) {
00242 case 0: glColor3f( 1.0, 0.0, 0.0 ); break;
00243 case 1: glColor3f( 0.0, 1.0, 0.0 ); break;
00244 case 2: glColor3f( 0.0, 0.0, 1.0 ); break;
00245 case 3: glColor3f( 1.0, 1.0, 0.0 ); break;
00246 case 4: glColor3f( 1.0, 0.0, 1.0 ); break;
00247 case 5: glColor3f( 0.0, 1.0, 1.0 ); break;
00248 case 6: glColor3f( 1.0, 1.0, 1.0 ); break;
00249 }
00250
00251 glLineWidth( 5.0 );
00252 argLineSegHMD( pos[0]-20, pos[1]-20, pos[0]+20, pos[1]-20 );
00253 argLineSegHMD( pos[0]-20, pos[1]+20, pos[0]+20, pos[1]+20 );
00254 argLineSegHMD( pos[0]-20, pos[1]-20, pos[0]-20, pos[1]+20 );
00255 argLineSegHMD( pos[0]+20, pos[1]-20, pos[0]+20, pos[1]+20 );
00256 glLineWidth( 1.0 );
00257
00258 return(0);
00259 }
