00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012 #include <stdio.h>
00013 #include <math.h>
00014 #include <AR/ar.h>
00015 #include <AR/matrix.h>
00016
00017 #define DEBUG 0
00018 #define EVEC_MAX 10
00019
00020 static int pattern_num = -1;
00021 static int patf[AR_PATT_NUM_MAX] = { 0 };
00022 static int pat[AR_PATT_NUM_MAX][4][AR_PATT_SIZE_Y*AR_PATT_SIZE_X*3];
00023 static double patpow[AR_PATT_NUM_MAX][4];
00024 static int patBW[AR_PATT_NUM_MAX][4][AR_PATT_SIZE_Y*AR_PATT_SIZE_X*3];
00025 static double patpowBW[AR_PATT_NUM_MAX][4];
00026
00027 static double evec[EVEC_MAX][AR_PATT_SIZE_Y*AR_PATT_SIZE_X*3];
00028 static double epat[AR_PATT_NUM_MAX][4][EVEC_MAX];
00029 static int evec_dim;
00030 static int evecf = 0;
00031
00032
00033
00034 static int evecBWf = 0;
00035
00036 static void get_cpara( double world[4][2], double vertex[4][2],
00037 double para[3][3] );
00038 static int pattern_match( ARUint8 *data, int *code, int *dir, double *cf );
00039 static void put_zero( ARUint8 *p, int size );
00040 static void gen_evec(void);
00041
00042
00043 int arLoadPatt( const char *filename )
00044 {
00045 FILE *fp;
00046 int patno;
00047 int h, i, j, l, m;
00048 int i1, i2, i3;
00049
00050 if(pattern_num == -1 ) {
00051 for( i = 0; i < AR_PATT_NUM_MAX; i++ ) patf[i] = 0;
00052 pattern_num = 0;
00053 }
00054
00055 for( i = 0; i < AR_PATT_NUM_MAX; i++ ) {
00056 if(patf[i] == 0) break;
00057 }
00058 if( i == AR_PATT_NUM_MAX ) return -1;
00059 patno = i;
00060
00061 if( (fp=fopen(filename, "r")) == NULL ) {
00062 printf("\"%s\" not found!!\n", filename);
00063 return(-1);
00064 }
00065
00066 for( h=0; h<4; h++ ) {
00067 l = 0;
00068 for( i3 = 0; i3 < 3; i3++ ) {
00069 for( i2 = 0; i2 < AR_PATT_SIZE_Y; i2++ ) {
00070 for( i1 = 0; i1 < AR_PATT_SIZE_X; i1++ ) {
00071 if( fscanf(fp, "%d", &j) != 1 ) {
00072 printf("Pattern Data read error!!\n");
00073 return -1;
00074 }
00075 j = 255-j;
00076 pat[patno][h][(i2*AR_PATT_SIZE_X+i1)*3+i3] = j;
00077 if( i3 == 0 ) patBW[patno][h][i2*AR_PATT_SIZE_X+i1] = j;
00078 else patBW[patno][h][i2*AR_PATT_SIZE_X+i1] += j;
00079 if( i3 == 2 ) patBW[patno][h][i2*AR_PATT_SIZE_X+i1] /= 3;
00080 l += j;
00081 }
00082 }
00083 }
00084 l /= (AR_PATT_SIZE_Y*AR_PATT_SIZE_X*3);
00085
00086 m = 0;
00087 for( i = 0; i < AR_PATT_SIZE_Y*AR_PATT_SIZE_X*3; i++ ) {
00088 pat[patno][h][i] -= l;
00089 m += (pat[patno][h][i]*pat[patno][h][i]);
00090 }
00091 patpow[patno][h] = sqrt((double)m);
00092 if( patpow[patno][h] == 0.0 ) patpow[patno][h] = 0.0000001;
00093
00094 m = 0;
00095 for( i = 0; i < AR_PATT_SIZE_Y*AR_PATT_SIZE_X; i++ ) {
00096 patBW[patno][h][i] -= l;
00097 m += (patBW[patno][h][i]*patBW[patno][h][i]);
00098 }
00099 patpowBW[patno][h] = sqrt((double)m);
00100 if( patpowBW[patno][h] == 0.0 ) patpowBW[patno][h] = 0.0000001;
00101 }
00102 fclose(fp);
00103
00104 patf[patno] = 1;
00105 pattern_num++;
00106
00107
00108
00109
00110
00111 return( patno );
00112 }
00113
00114 int arFreePatt( int patno )
00115 {
00116 if( patf[patno] == 0 ) return -1;
00117
00118 patf[patno] = 0;
00119 pattern_num--;
00120
00121 gen_evec();
00122
00123 return 1;
00124 }
00125
00126 int arActivatePatt( int patno )
00127 {
00128 if( patf[patno] == 0 ) return -1;
00129
00130 patf[patno] = 1;
00131
00132 return 1;
00133 }
00134
00135 int arDeactivatePatt( int patno )
00136 {
00137 if( patf[patno] == 0 ) return -1;
00138
00139 patf[patno] = 2;
00140
00141 return 1;
00142 }
00143
00144 int arGetCode( ARUint8 *image, int *x_coord, int *y_coord, int *vertex,
00145 int *code, int *dir, double *cf )
00146 {
00147 #if DEBUG
00148 static int count = 0;
00149 static double a1 = 0.0;
00150 static double a2 = 0.0;
00151 double b1, b2, b3;
00152 #endif
00153 ARUint8 ext_pat[AR_PATT_SIZE_Y][AR_PATT_SIZE_X][3];
00154
00155 #if DEBUG
00156 b1 = arUtilTimer();
00157 #endif
00158 arGetPatt(image, x_coord, y_coord, vertex, ext_pat);
00159 #if DEBUG
00160 b2 = arUtilTimer();
00161 #endif
00162
00163 pattern_match((ARUint8 *)ext_pat, code, dir, cf);
00164 #if DEBUG
00165 b3 = arUtilTimer();
00166 #endif
00167
00168 #if DEBUG
00169 a1 += (b2 - b1);
00170 a2 += (b3 - b2);
00171 count++;
00172 if( count == 60 ) {
00173 printf("%10.5f[msec], %10.5f[msec]\n", a1*1000.0/60.0, a2*1000.0/60.0);
00174 count = 0;
00175 a1 = a2 = 0.0;
00176 }
00177 #endif
00178
00179 return(0);
00180 }
00181
00182 #if 1
00183 int arGetPatt( ARUint8 *image, int *x_coord, int *y_coord, int *vertex,
00184 ARUint8 ext_pat[AR_PATT_SIZE_Y][AR_PATT_SIZE_X][3] )
00185 {
00186 ARUint32 ext_pat2[AR_PATT_SIZE_Y][AR_PATT_SIZE_X][3];
00187 double world[4][2];
00188 double local[4][2];
00189 double para[3][3];
00190 double d, xw, yw;
00191 int xc, yc;
00192 int xdiv, ydiv;
00193 int xdiv2, ydiv2;
00194 int lx1, lx2, ly1, ly2;
00195 int i, j;
00196
00197 double xdiv2_reciprocal;
00198 double ydiv2_reciprocal;
00199 int ext_pat2_x_index;
00200 int ext_pat2_y_index;
00201 int image_index;
00202
00203 world[0][0] = 100.0;
00204 world[0][1] = 100.0;
00205 world[1][0] = 100.0 + 10.0;
00206 world[1][1] = 100.0;
00207 world[2][0] = 100.0 + 10.0;
00208 world[2][1] = 100.0 + 10.0;
00209 world[3][0] = 100.0;
00210 world[3][1] = 100.0 + 10.0;
00211 for( i = 0; i < 4; i++ ) {
00212 local[i][0] = x_coord[vertex[i]];
00213 local[i][1] = y_coord[vertex[i]];
00214 }
00215 get_cpara( world, local, para );
00216
00217 lx1 = (int)((local[0][0] - local[1][0])*(local[0][0] - local[1][0])
00218 + (local[0][1] - local[1][1])*(local[0][1] - local[1][1]));
00219 lx2 = (int)((local[2][0] - local[3][0])*(local[2][0] - local[3][0])
00220 + (local[2][1] - local[3][1])*(local[2][1] - local[3][1]));
00221 ly1 = (int)((local[1][0] - local[2][0])*(local[1][0] - local[2][0])
00222 + (local[1][1] - local[2][1])*(local[1][1] - local[2][1]));
00223 ly2 = (int)((local[3][0] - local[0][0])*(local[3][0] - local[0][0])
00224 + (local[3][1] - local[0][1])*(local[3][1] - local[0][1]));
00225 if( lx2 > lx1 ) lx1 = lx2;
00226 if( ly2 > ly1 ) ly1 = ly2;
00227 xdiv2 = AR_PATT_SIZE_X;
00228 ydiv2 = AR_PATT_SIZE_Y;
00229 if( arImageProcMode == AR_IMAGE_PROC_IN_FULL ) {
00230 while( xdiv2*xdiv2 < lx1/4 ) xdiv2*=2;
00231 while( ydiv2*ydiv2 < ly1/4 ) ydiv2*=2;
00232 }
00233 else {
00234 while( xdiv2*xdiv2*4 < lx1/4 ) xdiv2*=2;
00235 while( ydiv2*ydiv2*4 < ly1/4 ) ydiv2*=2;
00236 }
00237 if( xdiv2 > AR_PATT_SAMPLE_NUM ) xdiv2 = AR_PATT_SAMPLE_NUM;
00238 if( ydiv2 > AR_PATT_SAMPLE_NUM ) ydiv2 = AR_PATT_SAMPLE_NUM;
00239
00240 xdiv = xdiv2/AR_PATT_SIZE_X;
00241 ydiv = ydiv2/AR_PATT_SIZE_Y;
00242
00243
00244
00245
00246 xdiv2_reciprocal = 1.0 / xdiv2;
00247 ydiv2_reciprocal = 1.0 / ydiv2;
00248
00249 put_zero( (ARUint8 *)ext_pat2, AR_PATT_SIZE_Y*AR_PATT_SIZE_X*3*sizeof(ARUint32) );
00250 for( j = 0; j < ydiv2; j++ ) {
00251 yw = 102.5 + 5.0 * (j+0.5) * ydiv2_reciprocal;
00252 for( i = 0; i < xdiv2; i++ ) {
00253 xw = 102.5 + 5.0 * (i+0.5) * xdiv2_reciprocal;
00254 d = para[2][0]*xw + para[2][1]*yw + para[2][2];
00255 if( d == 0 ) return(-1);
00256 xc = (int)((para[0][0]*xw + para[0][1]*yw + para[0][2])/d);
00257 yc = (int)((para[1][0]*xw + para[1][1]*yw + para[1][2])/d);
00258 if( arImageProcMode == AR_IMAGE_PROC_IN_HALF ) {
00259 xc = ((xc+1)/2)*2;
00260 yc = ((yc+1)/2)*2;
00261 }
00262 if( xc >= 0 && xc < arImXsize && yc >= 0 && yc < arImYsize ) {
00263 ext_pat2_y_index = j/ydiv;
00264 ext_pat2_x_index = i/xdiv;
00265 image_index = (yc*arImXsize+xc)*AR_PIX_SIZE_DEFAULT;
00266 #if (AR_DEFAULT_PIXEL_FORMAT == AR_PIXEL_FORMAT_ARGB)
00267 ext_pat2[ext_pat2_y_index][ext_pat2_x_index][0] += image[image_index+3];
00268 ext_pat2[ext_pat2_y_index][ext_pat2_x_index][1] += image[image_index+2];
00269 ext_pat2[ext_pat2_y_index][ext_pat2_x_index][2] += image[image_index+1];
00270 #elif (AR_DEFAULT_PIXEL_FORMAT == AR_PIXEL_FORMAT_ABGR)
00271 ext_pat2[ext_pat2_y_index][ext_pat2_x_index][0] += image[image_index+1];
00272 ext_pat2[ext_pat2_y_index][ext_pat2_x_index][1] += image[image_index+2];
00273 ext_pat2[ext_pat2_y_index][ext_pat2_x_index][2] += image[image_index+3];
00274 #elif (AR_DEFAULT_PIXEL_FORMAT == AR_PIXEL_FORMAT_BGRA)
00275 ext_pat2[ext_pat2_y_index][ext_pat2_x_index][0] += image[image_index+0];
00276 ext_pat2[ext_pat2_y_index][ext_pat2_x_index][1] += image[image_index+1];
00277 ext_pat2[ext_pat2_y_index][ext_pat2_x_index][2] += image[image_index+2];
00278 #elif (AR_DEFAULT_PIXEL_FORMAT == AR_PIXEL_FORMAT_BGR)
00279 ext_pat2[ext_pat2_y_index][ext_pat2_x_index][0] += image[image_index+0];
00280 ext_pat2[ext_pat2_y_index][ext_pat2_x_index][1] += image[image_index+1];
00281 ext_pat2[ext_pat2_y_index][ext_pat2_x_index][2] += image[image_index+2];
00282 #elif (AR_DEFAULT_PIXEL_FORMAT == AR_PIXEL_FORMAT_RGBA)
00283 ext_pat2[ext_pat2_y_index][ext_pat2_x_index][0] += image[image_index+2];
00284 ext_pat2[ext_pat2_y_index][ext_pat2_x_index][1] += image[image_index+1];
00285 ext_pat2[ext_pat2_y_index][ext_pat2_x_index][2] += image[image_index+0];
00286 #elif (AR_DEFAULT_PIXEL_FORMAT == AR_PIXEL_FORMAT_RGB)
00287 ext_pat2[ext_pat2_y_index][ext_pat2_x_index][0] += image[image_index+2];
00288 ext_pat2[ext_pat2_y_index][ext_pat2_x_index][1] += image[image_index+1];
00289 ext_pat2[ext_pat2_y_index][ext_pat2_x_index][2] += image[image_index+0];
00290 #elif (AR_DEFAULT_PIXEL_FORMAT == AR_PIXEL_FORMAT_MONO)
00291 ext_pat2[ext_pat2_y_index][ext_pat2_x_index][0] += image[image_index];
00292 ext_pat2[ext_pat2_y_index][ext_pat2_x_index][1] += image[image_index];
00293 ext_pat2[ext_pat2_y_index][ext_pat2_x_index][2] += image[image_index];
00294 #elif (AR_DEFAULT_PIXEL_FORMAT == AR_PIXEL_FORMAT_2vuy)
00295 ext_pat2[ext_pat2_y_index][ext_pat2_x_index][0] += image[image_index+1];
00296 ext_pat2[ext_pat2_y_index][ext_pat2_x_index][1] += image[image_index+1];
00297 ext_pat2[ext_pat2_y_index][ext_pat2_x_index][2] += image[image_index+1];
00298 #elif (AR_DEFAULT_PIXEL_FORMAT == AR_PIXEL_FORMAT_yuvs)
00299 ext_pat2[ext_pat2_y_index][ext_pat2_x_index][0] += image[image_index+0];
00300 ext_pat2[ext_pat2_y_index][ext_pat2_x_index][1] += image[image_index+0];
00301 ext_pat2[ext_pat2_y_index][ext_pat2_x_index][2] += image[image_index+0];
00302 #else
00303 # error Unknown default pixel format defined in config.h
00304 #endif
00305 }
00306 }
00307 }
00308
00309 for( j = 0; j < AR_PATT_SIZE_Y; j++ ) {
00310 for( i = 0; i < AR_PATT_SIZE_X; i++ ) {
00311 ext_pat[j][i][0] = ext_pat2[j][i][0] / (xdiv*ydiv);
00312 ext_pat[j][i][1] = ext_pat2[j][i][1] / (xdiv*ydiv);
00313 ext_pat[j][i][2] = ext_pat2[j][i][2] / (xdiv*ydiv);
00314 }
00315 }
00316
00317 return(0);
00318 }
00319 #else
00320 int arGetPatt( ARUint8 *image, int *x_coord, int *y_coord, int *vertex,
00321 ARUint8 ext_pat[AR_PATT_SIZE_Y][AR_PATT_SIZE_X][3] )
00322 {
00323 double world[4][2];
00324 double local[4][2];
00325 double para[3][3];
00326 double d, xw, yw;
00327 int xc, yc;
00328 int i, j;
00329 int k1, k2, k3;
00330
00331 world[0][0] = 100.0;
00332 world[0][1] = 100.0;
00333 world[1][0] = 100.0 + 10.0;
00334 world[1][1] = 100.0;
00335 world[2][0] = 100.0 + 10.0;
00336 world[2][1] = 100.0 + 10.0;
00337 world[3][0] = 100.0;
00338 world[3][1] = 100.0 + 10.0;
00339 for( i = 0; i < 4; i++ ) {
00340 local[i][0] = x_coord[vertex[i]];
00341 local[i][1] = y_coord[vertex[i]];
00342 }
00343 get_cpara( world, local, para );
00344
00345 put_zero( (ARUint8 *)ext_pat, AR_PATT_SIZE_Y*AR_PATT_SIZE_X*3 );
00346 for( j = 0; j < AR_PATT_SAMPLE_NUM; j++ ) {
00347 yw = 102.5 + 5.0 * (j+0.5) / (double)AR_PATT_SAMPLE_NUM;
00348 for( i = 0; i < AR_PATT_SAMPLE_NUM; i++ ) {
00349 xw = 102.5 + 5.0 * (i+0.5) / (double)AR_PATT_SAMPLE_NUM;
00350 d = para[2][0]*xw + para[2][1]*yw + para[2][2];
00351 if( d == 0 ) return(-1);
00352 xc = (int)((para[0][0]*xw + para[0][1]*yw + para[0][2])/d);
00353 yc = (int)((para[1][0]*xw + para[1][1]*yw + para[1][2])/d);
00354 if( arImageProcMode == AR_IMAGE_PROC_IN_HALF ) {
00355 xc = ((xc+1)/2)*2;
00356 yc = ((yc+1)/2)*2;
00357 }
00358 if( xc >= 0 && xc < arImXsize && yc >= 0 && yc < arImYsize ) {
00359 #if (AR_DEFAULT_PIXEL_FORMAT == AR_PIXEL_FORMAT_ARGB)
00360 k1 = image[(yc*arImXsize+xc)*AR_PIX_SIZE_DEFAULT+3];
00361 k1 = ext_pat[j*AR_PATT_SIZE_Y/AR_PATT_SAMPLE_NUM][i*AR_PATT_SIZE_X/AR_PATT_SAMPLE_NUM][0]
00362 + k1*(AR_PATT_SIZE_Y*AR_PATT_SIZE_X)/(AR_PATT_SAMPLE_NUM*AR_PATT_SAMPLE_NUM);
00363 ext_pat[j*AR_PATT_SIZE_Y/AR_PATT_SAMPLE_NUM][i*AR_PATT_SIZE_X/AR_PATT_SAMPLE_NUM][0] = (k1 > 255)? 255: k1;
00364 k2 = image[(yc*arImXsize+xc)*AR_PIX_SIZE_DEFAULT+2];
00365 k2 = ext_pat[j*AR_PATT_SIZE_Y/AR_PATT_SAMPLE_NUM][i*AR_PATT_SIZE_X/AR_PATT_SAMPLE_NUM][1]
00366 + k2*(AR_PATT_SIZE_Y*AR_PATT_SIZE_X)/(AR_PATT_SAMPLE_NUM*AR_PATT_SAMPLE_NUM);
00367 ext_pat[j*AR_PATT_SIZE_Y/AR_PATT_SAMPLE_NUM][i*AR_PATT_SIZE_X/AR_PATT_SAMPLE_NUM][1] = (k2 > 255)? 255: k2;
00368 k3 = image[(yc*arImXsize+xc)*AR_PIX_SIZE_DEFAULT+1];
00369 k3 = ext_pat[j*AR_PATT_SIZE_Y/AR_PATT_SAMPLE_NUM][i*AR_PATT_SIZE_X/AR_PATT_SAMPLE_NUM][2]
00370 + k3*(AR_PATT_SIZE_Y*AR_PATT_SIZE_X)/(AR_PATT_SAMPLE_NUM*AR_PATT_SAMPLE_NUM);
00371 ext_pat[j*AR_PATT_SIZE_Y/AR_PATT_SAMPLE_NUM][i*AR_PATT_SIZE_X/AR_PATT_SAMPLE_NUM][2] = (k3 > 255)? 255: k3;
00372 #elif (AR_DEFAULT_PIXEL_FORMAT == AR_PIXEL_FORMAT_ABGR)
00373 k1 = image[(yc*arImXsize+xc)*AR_PIX_SIZE_DEFAULT+1];
00374 k1 = ext_pat[j*AR_PATT_SIZE_Y/AR_PATT_SAMPLE_NUM][i*AR_PATT_SIZE_X/AR_PATT_SAMPLE_NUM][0]
00375 + k1*(AR_PATT_SIZE_Y*AR_PATT_SIZE_X)/(AR_PATT_SAMPLE_NUM*AR_PATT_SAMPLE_NUM);
00376 ext_pat[j*AR_PATT_SIZE_Y/AR_PATT_SAMPLE_NUM][i*AR_PATT_SIZE_X/AR_PATT_SAMPLE_NUM][0] = (k1 > 255)? 255: k1;
00377 k2 = image[(yc*arImXsize+xc)*AR_PIX_SIZE_DEFAULT+2];
00378 k2 = ext_pat[j*AR_PATT_SIZE_Y/AR_PATT_SAMPLE_NUM][i*AR_PATT_SIZE_X/AR_PATT_SAMPLE_NUM][1]
00379 + k2*(AR_PATT_SIZE_Y*AR_PATT_SIZE_X)/(AR_PATT_SAMPLE_NUM*AR_PATT_SAMPLE_NUM);
00380 ext_pat[j*AR_PATT_SIZE_Y/AR_PATT_SAMPLE_NUM][i*AR_PATT_SIZE_X/AR_PATT_SAMPLE_NUM][1] = (k2 > 255)? 255: k2;
00381 k3 = image[(yc*arImXsize+xc)*AR_PIX_SIZE_DEFAULT+3];
00382 k3 = ext_pat[j*AR_PATT_SIZE_Y/AR_PATT_SAMPLE_NUM][i*AR_PATT_SIZE_X/AR_PATT_SAMPLE_NUM][2]
00383 + k3*(AR_PATT_SIZE_Y*AR_PATT_SIZE_X)/(AR_PATT_SAMPLE_NUM*AR_PATT_SAMPLE_NUM);
00384 ext_pat[j*AR_PATT_SIZE_Y/AR_PATT_SAMPLE_NUM][i*AR_PATT_SIZE_X/AR_PATT_SAMPLE_NUM][2] = (k3 > 255)? 255: k3;
00385 #elif (AR_DEFAULT_PIXEL_FORMAT == AR_PIXEL_FORMAT_BGRA)
00386 k1 = image[(yc*arImXsize+xc)*AR_PIX_SIZE_DEFAULT+0];
00387 k1 = ext_pat[j*AR_PATT_SIZE_Y/AR_PATT_SAMPLE_NUM][i*AR_PATT_SIZE_X/AR_PATT_SAMPLE_NUM][0]
00388 + k1*(AR_PATT_SIZE_Y*AR_PATT_SIZE_X)/(AR_PATT_SAMPLE_NUM*AR_PATT_SAMPLE_NUM);
00389 ext_pat[j*AR_PATT_SIZE_Y/AR_PATT_SAMPLE_NUM][i*AR_PATT_SIZE_X/AR_PATT_SAMPLE_NUM][0] = (k1 > 255)? 255: k1;
00390 k2 = image[(yc*arImXsize+xc)*AR_PIX_SIZE_DEFAULT+1];
00391 k2 = ext_pat[j*AR_PATT_SIZE_Y/AR_PATT_SAMPLE_NUM][i*AR_PATT_SIZE_X/AR_PATT_SAMPLE_NUM][1]
00392 + k2*(AR_PATT_SIZE_Y*AR_PATT_SIZE_X)/(AR_PATT_SAMPLE_NUM*AR_PATT_SAMPLE_NUM);
00393 ext_pat[j*AR_PATT_SIZE_Y/AR_PATT_SAMPLE_NUM][i*AR_PATT_SIZE_X/AR_PATT_SAMPLE_NUM][1] = (k2 > 255)? 255: k2;
00394 k3 = image[(yc*arImXsize+xc)*AR_PIX_SIZE_DEFAULT+2];
00395 k3 = ext_pat[j*AR_PATT_SIZE_Y/AR_PATT_SAMPLE_NUM][i*AR_PATT_SIZE_X/AR_PATT_SAMPLE_NUM][2]
00396 + k3*(AR_PATT_SIZE_Y*AR_PATT_SIZE_X)/(AR_PATT_SAMPLE_NUM*AR_PATT_SAMPLE_NUM);
00397 ext_pat[j*AR_PATT_SIZE_Y/AR_PATT_SAMPLE_NUM][i*AR_PATT_SIZE_X/AR_PATT_SAMPLE_NUM][2] = (k3 > 255)? 255: k3;
00398 #elif (AR_DEFAULT_PIXEL_FORMAT == AR_PIXEL_FORMAT_BGR)
00399 k1 = image[(yc*arImXsize+xc)*AR_PIX_SIZE_DEFAULT+0];
00400 k1 = ext_pat[j*AR_PATT_SIZE_Y/AR_PATT_SAMPLE_NUM][i*AR_PATT_SIZE_X/AR_PATT_SAMPLE_NUM][0]
00401 + k1*(AR_PATT_SIZE_Y*AR_PATT_SIZE_X)/(AR_PATT_SAMPLE_NUM*AR_PATT_SAMPLE_NUM);
00402 ext_pat[j*AR_PATT_SIZE_Y/AR_PATT_SAMPLE_NUM][i*AR_PATT_SIZE_X/AR_PATT_SAMPLE_NUM][0] = (k1 > 255)? 255: k1;
00403 k2 = image[(yc*arImXsize+xc)*AR_PIX_SIZE_DEFAULT+1];
00404 k2 = ext_pat[j*AR_PATT_SIZE_Y/AR_PATT_SAMPLE_NUM][i*AR_PATT_SIZE_X/AR_PATT_SAMPLE_NUM][1]
00405 + k2*(AR_PATT_SIZE_Y*AR_PATT_SIZE_X)/(AR_PATT_SAMPLE_NUM*AR_PATT_SAMPLE_NUM);
00406 ext_pat[j*AR_PATT_SIZE_Y/AR_PATT_SAMPLE_NUM][i*AR_PATT_SIZE_X/AR_PATT_SAMPLE_NUM][1] = (k2 > 255)? 255: k2;
00407 k3 = image[(yc*arImXsize+xc)*AR_PIX_SIZE_DEFAULT+2];
00408 k3 = ext_pat[j*AR_PATT_SIZE_Y/AR_PATT_SAMPLE_NUM][i*AR_PATT_SIZE_X/AR_PATT_SAMPLE_NUM][2]
00409 + k3*(AR_PATT_SIZE_Y*AR_PATT_SIZE_X)/(AR_PATT_SAMPLE_NUM*AR_PATT_SAMPLE_NUM);
00410 ext_pat[j*AR_PATT_SIZE_Y/AR_PATT_SAMPLE_NUM][i*AR_PATT_SIZE_X/AR_PATT_SAMPLE_NUM][2] = (k3 > 255)? 255: k3;
00411 #elif (AR_DEFAULT_PIXEL_FORMAT == AR_PIXEL_FORMAT_RGBA)
00412 k1 = image[(yc*arImXsize+xc)*AR_PIX_SIZE_DEFAULT+2];
00413 k1 = ext_pat[j*AR_PATT_SIZE_Y/AR_PATT_SAMPLE_NUM][i*AR_PATT_SIZE_X/AR_PATT_SAMPLE_NUM][0]
00414 + k1*(AR_PATT_SIZE_Y*AR_PATT_SIZE_X)/(AR_PATT_SAMPLE_NUM*AR_PATT_SAMPLE_NUM);
00415 ext_pat[j*AR_PATT_SIZE_Y/AR_PATT_SAMPLE_NUM][i*AR_PATT_SIZE_X/AR_PATT_SAMPLE_NUM][0] = (k1 > 255)? 255: k1;
00416 k2 = image[(yc*arImXsize+xc)*AR_PIX_SIZE_DEFAULT+1];
00417 k2 = ext_pat[j*AR_PATT_SIZE_Y/AR_PATT_SAMPLE_NUM][i*AR_PATT_SIZE_X/AR_PATT_SAMPLE_NUM][1]
00418 + k2*(AR_PATT_SIZE_Y*AR_PATT_SIZE_X)/(AR_PATT_SAMPLE_NUM*AR_PATT_SAMPLE_NUM);
00419 ext_pat[j*AR_PATT_SIZE_Y/AR_PATT_SAMPLE_NUM][i*AR_PATT_SIZE_X/AR_PATT_SAMPLE_NUM][1] = (k2 > 255)? 255: k2;
00420 k3 = image[(yc*arImXsize+xc)*AR_PIX_SIZE_DEFAULT+0];
00421 k3 = ext_pat[j*AR_PATT_SIZE_Y/AR_PATT_SAMPLE_NUM][i*AR_PATT_SIZE_X/AR_PATT_SAMPLE_NUM][2]
00422 + k3*(AR_PATT_SIZE_Y*AR_PATT_SIZE_X)/(AR_PATT_SAMPLE_NUM*AR_PATT_SAMPLE_NUM);
00423 ext_pat[j*AR_PATT_SIZE_Y/AR_PATT_SAMPLE_NUM][i*AR_PATT_SIZE_X/AR_PATT_SAMPLE_NUM][2] = (k3 > 255)? 255: k3;
00424 #elif (AR_DEFAULT_PIXEL_FORMAT == AR_PIXEL_FORMAT_RGB)
00425 k1 = image[(yc*arImXsize+xc)*AR_PIX_SIZE_DEFAULT+2];
00426 k1 = ext_pat[j*AR_PATT_SIZE_Y/AR_PATT_SAMPLE_NUM][i*AR_PATT_SIZE_X/AR_PATT_SAMPLE_NUM][0]
00427 + k1*(AR_PATT_SIZE_Y*AR_PATT_SIZE_X)/(AR_PATT_SAMPLE_NUM*AR_PATT_SAMPLE_NUM);
00428 ext_pat[j*AR_PATT_SIZE_Y/AR_PATT_SAMPLE_NUM][i*AR_PATT_SIZE_X/AR_PATT_SAMPLE_NUM][0] = (k1 > 255)? 255: k1;
00429 k2 = image[(yc*arImXsize+xc)*AR_PIX_SIZE_DEFAULT+1];
00430 k2 = ext_pat[j*AR_PATT_SIZE_Y/AR_PATT_SAMPLE_NUM][i*AR_PATT_SIZE_X/AR_PATT_SAMPLE_NUM][1]
00431 + k2*(AR_PATT_SIZE_Y*AR_PATT_SIZE_X)/(AR_PATT_SAMPLE_NUM*AR_PATT_SAMPLE_NUM);
00432 ext_pat[j*AR_PATT_SIZE_Y/AR_PATT_SAMPLE_NUM][i*AR_PATT_SIZE_X/AR_PATT_SAMPLE_NUM][1] = (k2 > 255)? 255: k2;
00433 k3 = image[(yc*arImXsize+xc)*AR_PIX_SIZE_DEFAULT+0];
00434 k3 = ext_pat[j*AR_PATT_SIZE_Y/AR_PATT_SAMPLE_NUM][i*AR_PATT_SIZE_X/AR_PATT_SAMPLE_NUM][2]
00435 + k3*(AR_PATT_SIZE_Y*AR_PATT_SIZE_X)/(AR_PATT_SAMPLE_NUM*AR_PATT_SAMPLE_NUM);
00436 ext_pat[j*AR_PATT_SIZE_Y/AR_PATT_SAMPLE_NUM][i*AR_PATT_SIZE_X/AR_PATT_SAMPLE_NUM][2] = (k3 > 255)? 255: k3;
00437 #elif (AR_DEFAULT_PIXEL_FORMAT == AR_PIXEL_FORMAT_MONO)
00438 k1 = image[(yc*arImXsize+xc)*AR_PIX_SIZE_DEFAULT];
00439 k1 = ext_pat[j*AR_PATT_SIZE_Y/AR_PATT_SAMPLE_NUM][i*AR_PATT_SIZE_X/AR_PATT_SAMPLE_NUM][0]
00440 + k1*(AR_PATT_SIZE_Y*AR_PATT_SIZE_X)/(AR_PATT_SAMPLE_NUM*AR_PATT_SAMPLE_NUM);
00441 ext_pat[j*AR_PATT_SIZE_Y/AR_PATT_SAMPLE_NUM][i*AR_PATT_SIZE_X/AR_PATT_SAMPLE_NUM][0] = (k1 > 255)? 255: k1;
00442 k2 = image[(yc*arImXsize+xc)*AR_PIX_SIZE_DEFAULT];
00443 k2 = ext_pat[j*AR_PATT_SIZE_Y/AR_PATT_SAMPLE_NUM][i*AR_PATT_SIZE_X/AR_PATT_SAMPLE_NUM][1]
00444 + k2*(AR_PATT_SIZE_Y*AR_PATT_SIZE_X)/(AR_PATT_SAMPLE_NUM*AR_PATT_SAMPLE_NUM);
00445 ext_pat[j*AR_PATT_SIZE_Y/AR_PATT_SAMPLE_NUM][i*AR_PATT_SIZE_X/AR_PATT_SAMPLE_NUM][1] = (k2 > 255)? 255: k2;
00446 k3 = image[(yc*arImXsize+xc)*AR_PIX_SIZE_DEFAULT];
00447 k3 = ext_pat[j*AR_PATT_SIZE_Y/AR_PATT_SAMPLE_NUM][i*AR_PATT_SIZE_X/AR_PATT_SAMPLE_NUM][2]
00448 + k3*(AR_PATT_SIZE_Y*AR_PATT_SIZE_X)/(AR_PATT_SAMPLE_NUM*AR_PATT_SAMPLE_NUM);
00449 ext_pat[j*AR_PATT_SIZE_Y/AR_PATT_SAMPLE_NUM][i*AR_PATT_SIZE_X/AR_PATT_SAMPLE_NUM][2] = (k3 > 255)? 255: k3;
00450 #elif (AR_DEFAULT_PIXEL_FORMAT == AR_PIXEL_FORMAT_2vuy)
00451 k1 = image[(yc*arImXsize+xc)*AR_PIX_SIZE_DEFAULT+1];
00452 k1 = ext_pat[j*AR_PATT_SIZE_Y/AR_PATT_SAMPLE_NUM][i*AR_PATT_SIZE_X/AR_PATT_SAMPLE_NUM][0]
00453 + k1*(AR_PATT_SIZE_Y*AR_PATT_SIZE_X)/(AR_PATT_SAMPLE_NUM*AR_PATT_SAMPLE_NUM);
00454 ext_pat[j*AR_PATT_SIZE_Y/AR_PATT_SAMPLE_NUM][i*AR_PATT_SIZE_X/AR_PATT_SAMPLE_NUM][0] = (k1 > 255)? 255: k1;
00455 k2 = image[(yc*arImXsize+xc)*AR_PIX_SIZE_DEFAULT+1];
00456 k2 = ext_pat[j*AR_PATT_SIZE_Y/AR_PATT_SAMPLE_NUM][i*AR_PATT_SIZE_X/AR_PATT_SAMPLE_NUM][1]
00457 + k2*(AR_PATT_SIZE_Y*AR_PATT_SIZE_X)/(AR_PATT_SAMPLE_NUM*AR_PATT_SAMPLE_NUM);
00458 ext_pat[j*AR_PATT_SIZE_Y/AR_PATT_SAMPLE_NUM][i*AR_PATT_SIZE_X/AR_PATT_SAMPLE_NUM][1] = (k2 > 255)? 255: k2;
00459 k3 = image[(yc*arImXsize+xc)*AR_PIX_SIZE_DEFAULT+1];
00460 k3 = ext_pat[j*AR_PATT_SIZE_Y/AR_PATT_SAMPLE_NUM][i*AR_PATT_SIZE_X/AR_PATT_SAMPLE_NUM][2]
00461 + k3*(AR_PATT_SIZE_Y*AR_PATT_SIZE_X)/(AR_PATT_SAMPLE_NUM*AR_PATT_SAMPLE_NUM);
00462 ext_pat[j*AR_PATT_SIZE_Y/AR_PATT_SAMPLE_NUM][i*AR_PATT_SIZE_X/AR_PATT_SAMPLE_NUM][2] = (k3 > 255)? 255: k3;
00463 #elif (AR_DEFAULT_PIXEL_FORMAT == AR_PIXEL_FORMAT_yuvs)
00464 k1 = image[(yc*arImXsize+xc)*AR_PIX_SIZE_DEFAULT+0];
00465 k1 = ext_pat[j*AR_PATT_SIZE_Y/AR_PATT_SAMPLE_NUM][i*AR_PATT_SIZE_X/AR_PATT_SAMPLE_NUM][0]
00466 + k1*(AR_PATT_SIZE_Y*AR_PATT_SIZE_X)/(AR_PATT_SAMPLE_NUM*AR_PATT_SAMPLE_NUM);
00467 ext_pat[j*AR_PATT_SIZE_Y/AR_PATT_SAMPLE_NUM][i*AR_PATT_SIZE_X/AR_PATT_SAMPLE_NUM][0] = (k1 > 255)? 255: k1;
00468 k2 = image[(yc*arImXsize+xc)*AR_PIX_SIZE_DEFAULT+0];
00469 k2 = ext_pat[j*AR_PATT_SIZE_Y/AR_PATT_SAMPLE_NUM][i*AR_PATT_SIZE_X/AR_PATT_SAMPLE_NUM][1]
00470 + k2*(AR_PATT_SIZE_Y*AR_PATT_SIZE_X)/(AR_PATT_SAMPLE_NUM*AR_PATT_SAMPLE_NUM);
00471 ext_pat[j*AR_PATT_SIZE_Y/AR_PATT_SAMPLE_NUM][i*AR_PATT_SIZE_X/AR_PATT_SAMPLE_NUM][1] = (k2 > 255)? 255: k2;
00472 k3 = image[(yc*arImXsize+xc)*AR_PIX_SIZE_DEFAULT+0];
00473 k3 = ext_pat[j*AR_PATT_SIZE_Y/AR_PATT_SAMPLE_NUM][i*AR_PATT_SIZE_X/AR_PATT_SAMPLE_NUM][2]
00474 + k3*(AR_PATT_SIZE_Y*AR_PATT_SIZE_X)/(AR_PATT_SAMPLE_NUM*AR_PATT_SAMPLE_NUM);
00475 ext_pat[j*AR_PATT_SIZE_Y/AR_PATT_SAMPLE_NUM][i*AR_PATT_SIZE_X/AR_PATT_SAMPLE_NUM][2] = (k3 > 255)? 255: k3;
00476 #else
00477 # error Unknown default pixel format defined in config.h
00478 #endif
00479 }
00480 }
00481 }
00482
00483 return(0);
00484 }
00485 #endif
00486
00487 static void get_cpara( double world[4][2], double vertex[4][2],
00488 double para[3][3] )
00489 {
00490 ARMat *a, *b, *c;
00491 int i;
00492
00493 a = arMatrixAlloc( 8, 8 );
00494 b = arMatrixAlloc( 8, 1 );
00495 c = arMatrixAlloc( 8, 1 );
00496 for( i = 0; i < 4; i++ ) {
00497 a->m[i*16+0] = world[i][0];
00498 a->m[i*16+1] = world[i][1];
00499 a->m[i*16+2] = 1.0;
00500 a->m[i*16+3] = 0.0;
00501 a->m[i*16+4] = 0.0;
00502 a->m[i*16+5] = 0.0;
00503 a->m[i*16+6] = -world[i][0] * vertex[i][0];
00504 a->m[i*16+7] = -world[i][1] * vertex[i][0];
00505 a->m[i*16+8] = 0.0;
00506 a->m[i*16+9] = 0.0;
00507 a->m[i*16+10] = 0.0;
00508 a->m[i*16+11] = world[i][0];
00509 a->m[i*16+12] = world[i][1];
00510 a->m[i*16+13] = 1.0;
00511 a->m[i*16+14] = -world[i][0] * vertex[i][1];
00512 a->m[i*16+15] = -world[i][1] * vertex[i][1];
00513 b->m[i*2+0] = vertex[i][0];
00514 b->m[i*2+1] = vertex[i][1];
00515 }
00516 arMatrixSelfInv( a );
00517 arMatrixMul( c, a, b );
00518 for( i = 0; i < 2; i++ ) {
00519 para[i][0] = c->m[i*3+0];
00520 para[i][1] = c->m[i*3+1];
00521 para[i][2] = c->m[i*3+2];
00522 }
00523 para[2][0] = c->m[2*3+0];
00524 para[2][1] = c->m[2*3+1];
00525 para[2][2] = 1.0;
00526 arMatrixFree( a );
00527 arMatrixFree( b );
00528 arMatrixFree( c );
00529 }
00530
00531 static int pattern_match( ARUint8 *data, int *code, int *dir, double *cf )
00532 {
00533 double invec[EVEC_MAX];
00534 int input[AR_PATT_SIZE_Y*AR_PATT_SIZE_X*3];
00535 int i, j, l;
00536 int k = 0;
00537 int ave, sum, res, res2;
00538 double datapow, sum2, min;
00539 double max = 0.0;
00540
00541 sum = ave = 0;
00542 for(i=0;i<AR_PATT_SIZE_Y*AR_PATT_SIZE_X*3;i++) {
00543 ave += (255-data[i]);
00544 }
00545 ave /= (AR_PATT_SIZE_Y*AR_PATT_SIZE_X*3);
00546
00547 if( arTemplateMatchingMode == AR_TEMPLATE_MATCHING_COLOR ) {
00548 for(i=0;i<AR_PATT_SIZE_Y*AR_PATT_SIZE_X*3;i++) {
00549 input[i] = (255-data[i]) - ave;
00550 sum += input[i]*input[i];
00551 }
00552 }
00553 else {
00554 for(i=0;i<AR_PATT_SIZE_Y*AR_PATT_SIZE_X;i++) {
00555 input[i] = ((255-data[i*3+0]) + (255-data[i*3+1]) + (255-data[i*3+02]))/3 - ave;
00556 sum += input[i]*input[i];
00557 }
00558 }
00559
00560 datapow = sqrt( (double)sum );
00561 if( datapow == 0.0 ) {
00562 *code = 0;
00563 *dir = 0;
00564 *cf = -1.0;
00565 return -1;
00566 }
00567
00568 res = res2 = -1;
00569 if( arTemplateMatchingMode == AR_TEMPLATE_MATCHING_COLOR ) {
00570 if( arMatchingPCAMode == AR_MATCHING_WITH_PCA && evecf ) {
00571
00572 for( i = 0; i < evec_dim; i++ ) {
00573 invec[i] = 0.0;
00574 for( j = 0; j < AR_PATT_SIZE_Y*AR_PATT_SIZE_X*3; j++ ) {
00575 invec[i] += evec[i][j] * input[j];
00576 }
00577 invec[i] /= datapow;
00578 }
00579
00580 min = 10000.0;
00581 k = -1;
00582 for( l = 0; l < pattern_num; l++ ) {
00583 k++;
00584 while( patf[k] == 0 ) k++;
00585 if( patf[k] == 2 ) continue;
00586 #if DEBUG
00587 printf("%3d: ", k);
00588 #endif
00589 for( j = 0; j < 4; j++ ) {
00590 sum2 = 0;
00591 for(i = 0; i < evec_dim; i++ ) {
00592 sum2 += (invec[i] - epat[k][j][i]) * (invec[i] - epat[k][j][i]);
00593 }
00594 #if DEBUG
00595 printf("%10.7f ", sum2);
00596 #endif
00597 if( sum2 < min ) { min = sum2; res = j; res2 = k; }
00598 }
00599 #if DEBUG
00600 printf("\n");
00601 #endif
00602 }
00603 sum = 0;
00604 for(i=0;i<AR_PATT_SIZE_Y*AR_PATT_SIZE_X*3;i++) sum += input[i]*pat[res2][res][i];
00605 max = sum / patpow[res2][res] / datapow;
00606 }
00607 else {
00608 k = -1;
00609 max = 0.0;
00610 for( l = 0; l < pattern_num; l++ ) {
00611 k++;
00612 while( patf[k] == 0 ) k++;
00613 if( patf[k] == 2 ) continue;
00614 for( j = 0; j < 4; j++ ) {
00615 sum = 0;
00616 for(i=0;i<AR_PATT_SIZE_Y*AR_PATT_SIZE_X*3;i++) sum += input[i]*pat[k][j][i];
00617 sum2 = sum / patpow[k][j] / datapow;
00618 if( sum2 > max ) { max = sum2; res = j; res2 = k; }
00619 }
00620 }
00621 }
00622 }
00623 else {
00624 for( l = 0; l < pattern_num; l++ ) {
00625 k++;
00626 while( patf[k] == 0 ) k++;
00627 if( patf[k] == 2 ) continue;
00628 for( j = 0; j < 4; j++ ) {
00629 sum = 0;
00630 for(i=0;i<AR_PATT_SIZE_Y*AR_PATT_SIZE_X;i++) sum += input[i]*patBW[k][j][i];
00631 sum2 = sum / patpowBW[k][j] / datapow;
00632 if( sum2 > max ) { max = sum2; res = j; res2 = k; }
00633 }
00634 }
00635 }
00636
00637 *code = res2;
00638 *dir = res;
00639 *cf = max;
00640
00641 #if DEBUG
00642 printf("%d %d %f\n", res2, res, max);
00643 #endif
00644
00645 return 0;
00646 }
00647
00648 static void put_zero( ARUint8 *p, int size )
00649 {
00650 while( (size--) > 0 ) *(p++) = 0;
00651 }
00652
00653 static void gen_evec(void)
00654 {
00655 int i, j, k, ii, jj;
00656 ARMat *input, *wevec;
00657 ARVec *wev;
00658 double sum, sum2;
00659 int dim;
00660
00661 if( pattern_num < 4 ) {
00662 evecf = 0;
00663 evecBWf = 0;
00664 return;
00665 }
00666
00667 #if DEBUG
00668 printf("------------------------------------------\n");
00669 #endif
00670
00671 dim = (pattern_num*4 < AR_PATT_SIZE_Y*AR_PATT_SIZE_X*3)? pattern_num*4: AR_PATT_SIZE_Y*AR_PATT_SIZE_X*3;
00672 input = arMatrixAlloc( pattern_num*4, AR_PATT_SIZE_Y*AR_PATT_SIZE_X*3 );
00673 wevec = arMatrixAlloc( dim, AR_PATT_SIZE_Y*AR_PATT_SIZE_X*3 );
00674 wev = arVecAlloc( dim );
00675
00676 for( j = jj = 0; jj < AR_PATT_NUM_MAX; jj++ ) {
00677 if( patf[jj] == 0 ) continue;
00678 for( k = 0; k < 4; k++ ) {
00679 for( i = 0; i < AR_PATT_SIZE_Y*AR_PATT_SIZE_X*3; i++ ) {
00680 input->m[(j*4+k)*AR_PATT_SIZE_Y*AR_PATT_SIZE_X*3+i] = pat[j][k][i] / patpow[j][k];
00681 }
00682 }
00683 j++;
00684 }
00685
00686 if( arMatrixPCA2(input, wevec, wev) < 0 ) {
00687 arMatrixFree( input );
00688 arMatrixFree( wevec );
00689 arVecFree( wev );
00690 evecf = 0;
00691 evecBWf = 0;
00692 return;
00693 }
00694
00695 sum = 0.0;
00696 for( i = 0; i < dim; i++ ) {
00697 sum += wev->v[i];
00698 #if DEBUG
00699 printf("%2d(%10.7f): \n", i+1, sum);
00700 #endif
00701 if( sum > 0.90 ) break;
00702 if( i == EVEC_MAX-1 ) break;
00703 }
00704 evec_dim = i+1;
00705
00706 for( j = 0; j < evec_dim; j++ ) {
00707 for( i = 0; i < AR_PATT_SIZE_Y*AR_PATT_SIZE_X*3; i++ ) {
00708 evec[j][i] = wevec->m[j*AR_PATT_SIZE_Y*AR_PATT_SIZE_X*3+i];
00709 }
00710 }
00711
00712 for( i = 0; i < AR_PATT_NUM_MAX; i++ ) {
00713 if(patf[i] == 0) continue;
00714 for( j = 0; j < 4; j++ ) {
00715 #if DEBUG
00716 printf("%2d[%d]: ", i+1, j+1);
00717 #endif
00718 sum2 = 0.0;
00719 for( k = 0; k < evec_dim; k++ ) {
00720 sum = 0.0;
00721 for(ii=0;ii<AR_PATT_SIZE_Y*AR_PATT_SIZE_X*3;ii++) {
00722 sum += evec[k][ii] * pat[i][j][ii] / patpow[i][j];
00723 }
00724 #if DEBUG
00725 printf("%10.7f ", sum);
00726 #endif
00727 epat[i][j][k] = sum;
00728 sum2 += sum*sum;
00729 }
00730 #if DEBUG
00731 printf(":: %10.7f\n", sqrt(sum2));
00732 #endif
00733 }
00734 }
00735
00736 arMatrixFree( input );
00737 arMatrixFree( wevec );
00738 arVecFree( wev );
00739
00740 evecf = 1;
00741 evecBWf = 0;
00742
00743 return;
00744 }
00745
