Details | Last modification | View Log | RSS feed
| Rev | Author | Line No. | Line |
|---|---|---|---|
| 204 | chris | 1 | #include <stdio.h> |
| 2 | #include <stdlib.h> |
||
| 3 | #if defined(_WIN32) |
||
| 4 | #include <windows.h> |
||
| 5 | #endif |
||
| 6 | #ifndef __APPLE__ |
||
| 7 | # include <GL/glut.h> |
||
| 8 | # ifdef GL_VERSION_1_2 |
||
| 9 | # include <GL/glext.h> |
||
| 10 | # endif |
||
| 11 | #else |
||
| 12 | # include <GLUT/glut.h> |
||
| 13 | # include <OpenGL/glext.h> |
||
| 14 | #endif |
||
| 15 | #include <AR/config.h> |
||
| 16 | #include <AR/param.h> |
||
| 17 | #include <AR/ar.h> |
||
| 18 | #include <AR/gsub.h> |
||
| 19 | #include <AR/video.h> |
||
| 20 | #include <AR/gsubUtil.h> |
||
| 21 | |||
| 22 | #define CALIB_POS1_NUM 5 |
||
| 23 | #define CALIB_POS2_NUM 2 |
||
| 24 | |||
| 25 | static double calib_pos[CALIB_POS1_NUM][2] = { { 160, 120 }, |
||
| 26 | { 480, 120 }, |
||
| 27 | { 320, 240 }, |
||
| 28 | { 160, 360 }, |
||
| 29 | { 480, 360 } }; |
||
| 30 | static double calib_pos2d[CALIB_POS1_NUM][CALIB_POS2_NUM][2]; |
||
| 31 | static double calib_pos3d[CALIB_POS1_NUM][CALIB_POS2_NUM][3]; |
||
| 32 | static int co1; |
||
| 33 | static int co2; |
||
| 34 | static int left_right; |
||
| 35 | static double target_trans[3][4]; |
||
| 36 | static int target_id; |
||
| 37 | static int target_visible; |
||
| 38 | static double target_center[2] = { 0.0, 0.0 }; |
||
| 39 | static double target_width = 80.0; |
||
| 40 | |||
| 41 | static ARParam hmd_param[2]; |
||
| 42 | static int thresh; |
||
| 43 | static int arFittingModeBak; |
||
| 44 | |||
| 45 | static int hmdMode; |
||
| 46 | static int gMiniXnum, gMiniYnum; |
||
| 47 | static void (*gMouseFunc)(int button, int state, int x, int y); |
||
| 48 | static void (*gKeyFunc)(unsigned char key, int x, int y); |
||
| 49 | static void (*gMainFunc)(void); |
||
| 50 | static void (*gCalibPostFunc)(ARParam *lpara, ARParam *rpara); |
||
| 51 | |||
| 52 | static void argCalibMouseFunc(int button, int state, int x, int y); |
||
| 53 | static void argCalibMainFunc(void); |
||
| 54 | static int argDrawAttention(double pos[2], int color); |
||
| 55 | |||
| 56 | void argUtilCalibHMD( int targetId, int thresh2, |
||
| 57 | void (*postFunc)(ARParam *lpara, ARParam *rpara) ) |
||
| 58 | { |
||
| 59 | argInqSetting( &hmdMode, &gMiniXnum, &gMiniYnum, |
||
| 60 | &gMouseFunc, &gKeyFunc, &gMainFunc ); |
||
| 61 | |||
| 62 | if( hmdMode == 0 ) return; |
||
| 63 | |||
| 64 | target_id = targetId; |
||
| 65 | thresh = thresh2; |
||
| 66 | gCalibPostFunc = postFunc; |
||
| 67 | arFittingModeBak = arFittingMode; |
||
| 68 | |||
| 69 | arFittingMode = AR_FITTING_TO_IDEAL; |
||
| 70 | co1 = 0; |
||
| 71 | co2 = 0; |
||
| 72 | left_right = 0; |
||
| 73 | target_visible = 0; |
||
| 74 | |||
| 75 | glutKeyboardFunc( NULL ); |
||
| 76 | glutMouseFunc( argCalibMouseFunc ); |
||
| 77 | glutIdleFunc( argCalibMainFunc ); |
||
| 78 | glutDisplayFunc( argCalibMainFunc ); |
||
| 79 | } |
||
| 80 | |||
| 81 | static void argCalibMouseFunc(int button, int state, int x, int y) |
||
| 82 | { |
||
| 83 | if( button == GLUT_LEFT_BUTTON && state == GLUT_DOWN ) { |
||
| 84 | if( target_visible ) { |
||
| 85 | calib_pos3d[co1][co2][0] = target_trans[0][3]; |
||
| 86 | calib_pos3d[co1][co2][1] = target_trans[1][3]; |
||
| 87 | calib_pos3d[co1][co2][2] = target_trans[2][3]; |
||
| 88 | calib_pos2d[co1][co2][0] = calib_pos[co1][0]; |
||
| 89 | calib_pos2d[co1][co2][1] = calib_pos[co1][1]; |
||
| 90 | co2++; |
||
| 91 | if( co2 == CALIB_POS2_NUM ) { |
||
| 92 | co1++; |
||
| 93 | co2 = 0; |
||
| 94 | } |
||
| 95 | |||
| 96 | if( co1 == CALIB_POS1_NUM ) { |
||
| 97 | hmd_param[left_right].xsize = AR_HMD_XSIZE; |
||
| 98 | hmd_param[left_right].ysize = AR_HMD_YSIZE; |
||
| 99 | hmd_param[left_right].dist_factor[0] = AR_HMD_XSIZE / 2.0; |
||
| 100 | hmd_param[left_right].dist_factor[1] = AR_HMD_YSIZE / 2.0; |
||
| 101 | hmd_param[left_right].dist_factor[2] = 0.0; |
||
| 102 | hmd_param[left_right].dist_factor[3] = 1.0; |
||
| 103 | if( arParamGet( (double (*)[3])calib_pos3d, (double (*)[2])calib_pos2d, |
||
| 104 | CALIB_POS1_NUM*CALIB_POS2_NUM, hmd_param[left_right].mat) < 0 ) { |
||
| 105 | (*gCalibPostFunc)( NULL, NULL ); |
||
| 106 | arFittingMode = arFittingModeBak; |
||
| 107 | glutKeyboardFunc( gKeyFunc ); |
||
| 108 | glutMouseFunc( gMouseFunc ); |
||
| 109 | glutIdleFunc( gMainFunc ); |
||
| 110 | glutDisplayFunc( gMainFunc ); |
||
| 111 | return; |
||
| 112 | } |
||
| 113 | |||
| 114 | co1 = 0; |
||
| 115 | co2 = 0; |
||
| 116 | left_right++; |
||
| 117 | if( left_right == 2 ) { |
||
| 118 | argLoadHMDparam( &hmd_param[0], &hmd_param[1] ); |
||
| 119 | arFittingMode = arFittingModeBak; |
||
| 120 | |||
| 121 | if( gCalibPostFunc != NULL ) { |
||
| 122 | (*gCalibPostFunc)( &hmd_param[0], &hmd_param[1] ); |
||
| 123 | } |
||
| 124 | glutKeyboardFunc( gKeyFunc ); |
||
| 125 | glutMouseFunc( gMouseFunc ); |
||
| 126 | glutIdleFunc( gMainFunc ); |
||
| 127 | glutDisplayFunc( gMainFunc ); |
||
| 128 | return; |
||
| 129 | } |
||
| 130 | } |
||
| 131 | } |
||
| 132 | } |
||
| 133 | |||
| 134 | if( button == GLUT_RIGHT_BUTTON && state == GLUT_DOWN ) { |
||
| 135 | (*gCalibPostFunc)( NULL, NULL ); |
||
| 136 | arFittingMode = arFittingMode; |
||
| 137 | glutKeyboardFunc( gKeyFunc ); |
||
| 138 | glutMouseFunc( gMouseFunc ); |
||
| 139 | glutIdleFunc( gMainFunc ); |
||
| 140 | glutDisplayFunc( gMainFunc ); |
||
| 141 | return; |
||
| 142 | } |
||
| 143 | } |
||
| 144 | |||
| 145 | static void argCalibMainFunc(void) |
||
| 146 | { |
||
| 147 | ARUint8 *dataPtr; |
||
| 148 | ARMarkerInfo *marker_info; |
||
| 149 | int marker_num; |
||
| 150 | int i, j; |
||
| 151 | double cfmax; |
||
| 152 | double err; |
||
| 153 | |||
| 154 | /* grab a vide frame */ |
||
| 155 | if( (dataPtr = (ARUint8 *)arVideoGetImage()) == NULL ) { |
||
| 156 | arUtilSleep(2); |
||
| 157 | return; |
||
| 158 | } |
||
| 159 | target_visible = 0; |
||
| 160 | |||
| 161 | /* detect the markers in the video frame */ |
||
| 162 | if( arDetectMarker(dataPtr, thresh, |
||
| 163 | &marker_info, &marker_num) < 0 ) { |
||
| 164 | (*gCalibPostFunc)( NULL, NULL ); |
||
| 165 | arFittingMode = arFittingModeBak; |
||
| 166 | glutKeyboardFunc( gKeyFunc ); |
||
| 167 | glutMouseFunc( gMouseFunc ); |
||
| 168 | glutIdleFunc( gMainFunc ); |
||
| 169 | glutDisplayFunc( gMainFunc ); |
||
| 170 | return; |
||
| 171 | } |
||
| 172 | arVideoCapNext(); |
||
| 173 | |||
| 174 | glClearColor( 0.0, 0.0, 0.0, 0.0 ); |
||
| 175 | glClear(GL_COLOR_BUFFER_BIT); |
||
| 176 | |||
| 177 | /* if the debug mode is on draw squares |
||
| 178 | around the detected squares in the video image */ |
||
| 179 | if( arDebug && gMiniXnum >= 2 && gMiniYnum >= 1 ) { |
||
| 180 | argDispImage( dataPtr, 1, 1 ); |
||
| 181 | if( arImageProcMode == AR_IMAGE_PROC_IN_HALF ) |
||
| 182 | argDispHalfImage( arImage, 2, 1 ); |
||
| 183 | else |
||
| 184 | argDispImage( arImage, 2, 1); |
||
| 185 | |||
| 186 | glColor3f( 1.0, 0.0, 0.0 ); |
||
| 187 | glLineWidth( 3.0 ); |
||
| 188 | for( i = 0; i < marker_num; i++ ) { |
||
| 189 | if( marker_info[i].id < 0 ) continue; |
||
| 190 | argDrawSquare( marker_info[i].vertex, 2, 1 ); |
||
| 191 | } |
||
| 192 | glLineWidth( 1.0 ); |
||
| 193 | } |
||
| 194 | |||
| 195 | if( left_right == 0 ) argDraw2dLeft(); |
||
| 196 | else argDraw2dRight(); |
||
| 197 | glLineWidth( 3.0 ); |
||
| 198 | glColor3f( 1.0, 1.0, 1.0 ); |
||
| 199 | argLineSegHMD( 0, calib_pos[co1][1], AR_HMD_XSIZE, calib_pos[co1][1] ); |
||
| 200 | argLineSegHMD( calib_pos[co1][0], 0, calib_pos[co1][0], AR_HMD_YSIZE ); |
||
| 201 | glLineWidth( 1.0 ); |
||
| 202 | argDrawMode2D(); |
||
| 203 | |||
| 204 | cfmax = 0.0; |
||
| 205 | j = -1; |
||
| 206 | for( i = 0; i < marker_num; i++ ) { |
||
| 207 | if( marker_info[i].id != target_id ) continue; |
||
| 208 | |||
| 209 | if( marker_info[i].cf > cfmax ) { |
||
| 210 | cfmax = marker_info[i].cf; |
||
| 211 | j = i; |
||
| 212 | } |
||
| 213 | } |
||
| 214 | if( j < 0 ) { |
||
| 215 | argSwapBuffers(); |
||
| 216 | return; |
||
| 217 | } |
||
| 218 | err = arGetTransMat(&marker_info[j], target_center, target_width, target_trans); |
||
| 219 | if( err >= 0.0 ) { |
||
| 220 | target_visible = 1; |
||
| 221 | |||
| 222 | if( left_right == 0 ) argDraw2dLeft(); |
||
| 223 | else argDraw2dRight(); |
||
| 224 | argDrawAttention( calib_pos[co1], co2 ); |
||
| 225 | argDrawMode2D(); |
||
| 226 | |||
| 227 | if( arDebug && gMiniXnum >= 2 && gMiniYnum >= 1 ) { |
||
| 228 | glColor3f( 0.0, 1.0, 0.0 ); |
||
| 229 | glLineWidth( 3.0 ); |
||
| 230 | argDrawSquare( marker_info[j].vertex, 1, 1 ); |
||
| 231 | glLineWidth( 1.0 ); |
||
| 232 | } |
||
| 233 | } |
||
| 234 | |||
| 235 | argSwapBuffers(); |
||
| 236 | } |
||
| 237 | |||
| 238 | static int argDrawAttention( double pos[2], int color ) |
||
| 239 | { |
||
| 240 | switch( color%7 ) { |
||
| 241 | case 0: glColor3f( 1.0, 0.0, 0.0 ); break; |
||
| 242 | case 1: glColor3f( 0.0, 1.0, 0.0 ); break; |
||
| 243 | case 2: glColor3f( 0.0, 0.0, 1.0 ); break; |
||
| 244 | case 3: glColor3f( 1.0, 1.0, 0.0 ); break; |
||
| 245 | case 4: glColor3f( 1.0, 0.0, 1.0 ); break; |
||
| 246 | case 5: glColor3f( 0.0, 1.0, 1.0 ); break; |
||
| 247 | case 6: glColor3f( 1.0, 1.0, 1.0 ); break; |
||
| 248 | } |
||
| 249 | |||
| 250 | glLineWidth( 5.0 ); |
||
| 251 | argLineSegHMD( pos[0]-20, pos[1]-20, pos[0]+20, pos[1]-20 ); |
||
| 252 | argLineSegHMD( pos[0]-20, pos[1]+20, pos[0]+20, pos[1]+20 ); |
||
| 253 | argLineSegHMD( pos[0]-20, pos[1]-20, pos[0]-20, pos[1]+20 ); |
||
| 254 | argLineSegHMD( pos[0]+20, pos[1]-20, pos[0]+20, pos[1]+20 ); |
||
| 255 | glLineWidth( 1.0 ); |
||
| 256 | |||
| 257 | return(0); |
||
| 258 | } |