00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011 #include <stdio.h>
00012 #include <stdlib.h>
00013 #include <math.h>
00014 #include <AR/ar.h>
00015 #include <AR/matrix.h>
00016 #include <AR/arMulti.h>
00017
00018 #define debug 0
00019
00020 #define THRESH_1 2.0
00021 #define THRESH_2 20.0
00022 #define THRESH_3 10.0
00023 #define AR_MULTI_GET_TRANS_MAT_MAX_LOOP_COUNT 2
00024 #define AR_MULTI_GET_TRANS_MAT_MAX_FIT_ERROR 10.0
00025
00026 typedef struct {
00027 double pos[4][2];
00028 double thresh;
00029 double err;
00030 int marker;
00031 int dir;
00032 } arMultiEachMarkerInternalInfoT;
00033
00034 static int verify_markers(ARMarkerInfo *marker_info, int marker_num,
00035 ARMultiMarkerInfoT *config);
00036
00037
00038 double arMultiGetTransMat(ARMarkerInfo *marker_info, int marker_num,
00039 ARMultiMarkerInfoT *config)
00040 {
00041 double *pos2d, *pos3d;
00042 double rot[3][3], trans1[3][4], trans2[3][4];
00043 double err, err2;
00044 int max, max_area, max_marker, vnum;
00045 int dir;
00046 int i, j, k;
00047
00048 if( config->prevF ) {
00049 verify_markers( marker_info, marker_num, config );
00050 }
00051
00052 max = -1;
00053 vnum = 0;
00054 for( i = 0; i < config->marker_num; i++ ) {
00055 k = -1;
00056 for( j = 0; j < marker_num; j++ ) {
00057 if( marker_info[j].id != config->marker[i].patt_id ) continue;
00058 if( marker_info[j].cf < 0.70 ) continue;
00059
00060 if( k == -1 ) k = j;
00061 else if( marker_info[k].cf < marker_info[j].cf ) k = j;
00062 }
00063 if( (config->marker[i].visible=k) == -1) continue;
00064
00065 err = arGetTransMat(&marker_info[k], config->marker[i].center,
00066 config->marker[i].width, trans1);
00067 #if debug
00068 printf("##err = %10.5f %d %10.5f %10.5f\n", err, marker_info[k].dir, marker_info[k].pos[0], marker_info[k].pos[1]);
00069 #endif
00070 if( err > THRESH_1 ) {
00071 config->marker[i].visible = -1;
00072 continue;
00073 }
00074
00075 vnum++;
00076 if( max == -1
00077 || marker_info[k].area > max_area ) {
00078 max = i;
00079 max_marker = k;
00080 max_area = marker_info[k].area;
00081 for( j = 0; j < 3; j++ ) {
00082 for( k = 0; k < 4; k++ ) {
00083 trans2[j][k] = trans1[j][k];
00084 }
00085 }
00086 }
00087 }
00088 if( max == -1 ) {
00089 config->prevF = 0;
00090 return -1;
00091 }
00092
00093 arMalloc(pos2d, double, vnum*4*2);
00094 arMalloc(pos3d, double, vnum*4*3);
00095
00096 j = 0;
00097 for( i = 0; i < config->marker_num; i++ ) {
00098 if( (k=config->marker[i].visible) < 0 ) continue;
00099
00100 dir = marker_info[k].dir;
00101 pos2d[j*8+0] = marker_info[k].vertex[(4-dir)%4][0];
00102 pos2d[j*8+1] = marker_info[k].vertex[(4-dir)%4][1];
00103 pos2d[j*8+2] = marker_info[k].vertex[(5-dir)%4][0];
00104 pos2d[j*8+3] = marker_info[k].vertex[(5-dir)%4][1];
00105 pos2d[j*8+4] = marker_info[k].vertex[(6-dir)%4][0];
00106 pos2d[j*8+5] = marker_info[k].vertex[(6-dir)%4][1];
00107 pos2d[j*8+6] = marker_info[k].vertex[(7-dir)%4][0];
00108 pos2d[j*8+7] = marker_info[k].vertex[(7-dir)%4][1];
00109 pos3d[j*12+0] = config->marker[i].pos3d[0][0];
00110 pos3d[j*12+1] = config->marker[i].pos3d[0][1];
00111 pos3d[j*12+2] = config->marker[i].pos3d[0][2];
00112 pos3d[j*12+3] = config->marker[i].pos3d[1][0];
00113 pos3d[j*12+4] = config->marker[i].pos3d[1][1];
00114 pos3d[j*12+5] = config->marker[i].pos3d[1][2];
00115 pos3d[j*12+6] = config->marker[i].pos3d[2][0];
00116 pos3d[j*12+7] = config->marker[i].pos3d[2][1];
00117 pos3d[j*12+8] = config->marker[i].pos3d[2][2];
00118 pos3d[j*12+9] = config->marker[i].pos3d[3][0];
00119 pos3d[j*12+10] = config->marker[i].pos3d[3][1];
00120 pos3d[j*12+11] = config->marker[i].pos3d[3][2];
00121 j++;
00122 }
00123
00124 if( config->prevF ) {
00125 for( j = 0; j < 3; j++ ) {
00126 for( i = 0; i < 3; i++ ) {
00127 rot[j][i] = config->trans[j][i];
00128 }
00129 }
00130 for( i = 0; i < AR_MULTI_GET_TRANS_MAT_MAX_LOOP_COUNT; i++ ) {
00131 err = arGetTransMat4( rot, (double (*)[2])pos2d,
00132 (double (*)[3])pos3d,
00133 vnum*4, config->trans );
00134 if( err < AR_MULTI_GET_TRANS_MAT_MAX_FIT_ERROR ) break;
00135 }
00136
00137 if( err < THRESH_2 ) {
00138 config->prevF = 1;
00139 free(pos3d);
00140 free(pos2d);
00141 return err;
00142 }
00143 }
00144
00145 arUtilMatMul( trans2, config->marker[max].itrans, trans1 );
00146 for( j = 0; j < 3; j++ ) {
00147 for( i = 0; i < 3; i++ ) {
00148 rot[j][i] = trans1[j][i];
00149 }
00150 }
00151
00152 for( i = 0; i < AR_MULTI_GET_TRANS_MAT_MAX_LOOP_COUNT; i++ ) {
00153 err2 = arGetTransMat4( rot, (double (*)[2])pos2d, (double (*)[3])pos3d,
00154 vnum*4, trans2 );
00155 if( err2 < AR_MULTI_GET_TRANS_MAT_MAX_FIT_ERROR ) break;
00156 }
00157
00158 if( config->prevF == 0 || err2 < err ) {
00159 for( j = 0; j < 3; j++ ) {
00160 for( i = 0; i < 4; i++ ) {
00161 config->trans[j][i] = trans2[j][i];
00162 }
00163 }
00164 err = err2;
00165 }
00166
00167 if( err < THRESH_3 ) {
00168 config->prevF = 1;
00169 }
00170 else {
00171 config->prevF = 0;
00172 }
00173
00174 free(pos3d);
00175 free(pos2d);
00176 return err;
00177 }
00178
00179 static int verify_markers(ARMarkerInfo *marker_info, int marker_num,
00180 ARMultiMarkerInfoT *config)
00181 {
00182 arMultiEachMarkerInternalInfoT *winfo;
00183 double wtrans[3][4];
00184 double pos3d[4][2];
00185 double wx, wy, wz, hx, hy, h;
00186 int dir1, dir2, marker2;
00187 double err, err1, err2;
00188 double x1, x2, y1, y2;
00189 int w1, w2;
00190 int i, j, k;
00191
00192 arMalloc(winfo,arMultiEachMarkerInternalInfoT,config->marker_num);
00193
00194 for( i = 0; i < config->marker_num; i++ ) {
00195 arUtilMatMul(config->trans, config->marker[i].trans, wtrans);
00196 pos3d[0][0] = config->marker[i].center[0] - config->marker[i].width/2.0;
00197 pos3d[0][1] = config->marker[i].center[1] + config->marker[i].width/2.0;
00198 pos3d[1][0] = config->marker[i].center[0] + config->marker[i].width/2.0;
00199 pos3d[1][1] = config->marker[i].center[1] + config->marker[i].width/2.0;
00200 pos3d[2][0] = config->marker[i].center[0] + config->marker[i].width/2.0;
00201 pos3d[2][1] = config->marker[i].center[1] - config->marker[i].width/2.0;
00202 pos3d[3][0] = config->marker[i].center[0] - config->marker[i].width/2.0;
00203 pos3d[3][1] = config->marker[i].center[1] - config->marker[i].width/2.0;
00204 for( j = 0; j < 4; j++ ) {
00205 wx = wtrans[0][0] * pos3d[j][0]
00206 + wtrans[0][1] * pos3d[j][1]
00207 + wtrans[0][3];
00208 wy = wtrans[1][0] * pos3d[j][0]
00209 + wtrans[1][1] * pos3d[j][1]
00210 + wtrans[1][3];
00211 wz = wtrans[2][0] * pos3d[j][0]
00212 + wtrans[2][1] * pos3d[j][1]
00213 + wtrans[2][3];
00214 hx = arParam.mat[0][0] * wx
00215 + arParam.mat[0][1] * wy
00216 + arParam.mat[0][2] * wz
00217 + arParam.mat[0][3];
00218 hy = arParam.mat[1][0] * wx
00219 + arParam.mat[1][1] * wy
00220 + arParam.mat[1][2] * wz
00221 + arParam.mat[1][3];
00222 h = arParam.mat[2][0] * wx
00223 + arParam.mat[2][1] * wy
00224 + arParam.mat[2][2] * wz
00225 + arParam.mat[2][3];
00226 winfo[i].pos[j][0] = hx / h;
00227 winfo[i].pos[j][1] = hy / h;
00228
00229 if(j ==0) {x1=x2=winfo[i].pos[j][0]; y1=y2=winfo[i].pos[j][1];}
00230 else {
00231 if( winfo[i].pos[j][0] < x1 ) x1 = winfo[i].pos[j][0];
00232 if( winfo[i].pos[j][0] > x2 ) x2 = winfo[i].pos[j][0];
00233 if( winfo[i].pos[j][1] < y1 ) y1 = winfo[i].pos[j][1];
00234 if( winfo[i].pos[j][1] > y2 ) y2 = winfo[i].pos[j][1];
00235 }
00236 }
00237 winfo[i].thresh = (x2 - x1 + 1)*(y2 - y1 + 1) / 2;
00238 }
00239
00240 #if debug
00241 printf("\n");
00242 printf("================================================================\n");
00243 for( i = 0; i < config->marker_num; i++) {
00244 printf("%3d: ", i+1);
00245 for( j = 0; j < 4; j++ ) {
00246 printf("(%5.1f %5.1f) ", winfo[i].pos[j][0], winfo[i].pos[j][1]);
00247 }
00248 printf("\n");
00249 }
00250 printf("--------\n");
00251 for( i = 0; i < marker_num; i++) {
00252 printf("%3d: ", i+1);
00253 for( j = 0; j < 4; j++ ) {
00254 printf("(%5.1f %5.1f) ", marker_info[i].vertex[j][0], marker_info[i].vertex[j][1]);
00255 }
00256 printf("\n");
00257 }
00258 #endif
00259
00260 w1 = w2 = 0;
00261 for( i = 0; i < config->marker_num; i++ ) {
00262 marker2 = -1;
00263 err2 = winfo[i].thresh;
00264 for( j = 0; j < marker_num; j++ ) {
00265 if( marker_info[j].id != -1
00266 && marker_info[j].id != config->marker[i].patt_id
00267 && marker_info[j].cf > 0.7 ) continue;
00268
00269 dir1 = -1;
00270 for( k = 0; k < 4; k++ ) {
00271 err = (winfo[i].pos[0][0] - marker_info[j].vertex[(k+0)%4][0])
00272 * (winfo[i].pos[0][0] - marker_info[j].vertex[(k+0)%4][0])
00273 + (winfo[i].pos[0][1] - marker_info[j].vertex[(k+0)%4][1])
00274 * (winfo[i].pos[0][1] - marker_info[j].vertex[(k+0)%4][1])
00275 + (winfo[i].pos[1][0] - marker_info[j].vertex[(k+1)%4][0])
00276 * (winfo[i].pos[1][0] - marker_info[j].vertex[(k+1)%4][0])
00277 + (winfo[i].pos[1][1] - marker_info[j].vertex[(k+1)%4][1])
00278 * (winfo[i].pos[1][1] - marker_info[j].vertex[(k+1)%4][1])
00279 + (winfo[i].pos[2][0] - marker_info[j].vertex[(k+2)%4][0])
00280 * (winfo[i].pos[2][0] - marker_info[j].vertex[(k+2)%4][0])
00281 + (winfo[i].pos[2][1] - marker_info[j].vertex[(k+2)%4][1])
00282 * (winfo[i].pos[2][1] - marker_info[j].vertex[(k+2)%4][1])
00283 + (winfo[i].pos[3][0] - marker_info[j].vertex[(k+3)%4][0])
00284 * (winfo[i].pos[3][0] - marker_info[j].vertex[(k+3)%4][0])
00285 + (winfo[i].pos[3][1] - marker_info[j].vertex[(k+3)%4][1])
00286 * (winfo[i].pos[3][1] - marker_info[j].vertex[(k+3)%4][1]);
00287 if( dir1 == -1 || err < err1 ) {
00288 err1 = err;
00289 dir1 = k;
00290 }
00291 }
00292 #if debug
00293 printf("%f\n", err1);
00294 #endif
00295 if( err1 < err2 ) {
00296 err2 = err1;
00297 dir2 = dir1;
00298 marker2 = j;
00299 }
00300 }
00301
00302 #if debug
00303 printf("%3d<=>%3d, err = %f(%f)\n", i+1, marker2+1, err2, winfo[i].thresh);
00304 #endif
00305 if( marker2 != -1 ) {
00306 winfo[i].marker = marker2;
00307 winfo[i].dir = dir2;
00308 winfo[i].err = err2;
00309
00310 if( marker_info[marker2].id == config->marker[i].patt_id ) w1++;
00311 else if( marker_info[marker2].id != -1 ) w2++;
00312 }
00313 else {
00314 winfo[i].marker = -1;
00315 }
00316 }
00317 #if debug
00318 printf("w1,w2 = %d,%d\n", w1, w2);
00319 #endif
00320 if( w2 >= w1 ) {
00321 free(winfo);
00322 return -1;
00323 }
00324
00325 for( i = 0; i < config->marker_num; i++ ) {
00326 for( j = 0; j < marker_num; j++ ) {
00327 if( marker_info[j].id == config->marker[i].patt_id ) marker_info[j].id = -1;
00328 }
00329 if( winfo[i].marker != -1 ) {
00330 marker_info[winfo[i].marker].id = config->marker[i].patt_id;
00331 marker_info[winfo[i].marker].dir = (4-winfo[i].dir)%4;
00332 marker_info[winfo[i].marker].cf = 1.0;
00333 }
00334 }
00335
00336 free(winfo);
00337
00338 return 0;
00339 }
