00001 #include <stdio.h>
00002 #include <AR/ar.h>
00003
00004 static ARMarkerInfo2 *marker_info2;
00005 static ARMarkerInfo *wmarker_info;
00006 static int wmarker_num = 0;
00007
00008 static arPrevInfo prev_info[AR_SQUARE_MAX];
00009 static int prev_num = 0;
00010
00011 static arPrevInfo sprev_info[2][AR_SQUARE_MAX];
00012 static int sprev_num[2] = {0,0};
00013
00014 int arSavePatt( ARUint8 *image, ARMarkerInfo *marker_info, char *filename )
00015 {
00016 FILE *fp;
00017 ARUint8 ext_pat[4][AR_PATT_SIZE_Y][AR_PATT_SIZE_X][3];
00018 int vertex[4];
00019 int i, j, k, x, y;
00020
00021
00022 for( i = 0; i < wmarker_num; i++ ) {
00023 if( marker_info->area == marker_info2[i].area
00024 && marker_info->pos[0] == marker_info2[i].pos[0]
00025 && marker_info->pos[1] == marker_info2[i].pos[1] ) break;
00026 }
00027 if( i == wmarker_num ) return -1;
00028
00029 for( j = 0; j < 4; j++ ) {
00030 for( k = 0; k < 4; k++ ) {
00031 vertex[k] = marker_info2[i].vertex[(k+j+2)%4];
00032 }
00033 arGetPatt( image, marker_info2[i].x_coord,
00034 marker_info2[i].y_coord, vertex, ext_pat[j] );
00035 }
00036
00037 fp = fopen( filename, "w" );
00038 if( fp == NULL ) return -1;
00039
00040
00041 for( i = 0; i < 4; i++ ) {
00042 for( j = 0; j < 3; j++ ) {
00043 for( y = 0; y < AR_PATT_SIZE_Y; y++ ) {
00044 for( x = 0; x < AR_PATT_SIZE_X; x++ ) {
00045 fprintf( fp, "%4d", ext_pat[i][y][x][j] );
00046 }
00047 fprintf(fp, "\n");
00048 }
00049 }
00050 fprintf(fp, "\n");
00051 }
00052
00053 fclose( fp );
00054
00055 return 0;
00056 }
00057
00058 int arDetectMarker( ARUint8 *dataPtr, int thresh,
00059 ARMarkerInfo **marker_info, int *marker_num )
00060 {
00061 ARInt16 *limage;
00062 int label_num;
00063 int *area, *clip, *label_ref;
00064 double *pos;
00065 double rarea, rlen, rlenmin;
00066 double diff, diffmin;
00067 int cid, cdir;
00068 int i, j, k;
00069
00070 *marker_num = 0;
00071
00072 limage = arLabeling( dataPtr, thresh,
00073 &label_num, &area, &pos, &clip, &label_ref );
00074 if( limage == 0 ) return -1;
00075
00076 marker_info2 = arDetectMarker2( limage, label_num, label_ref,
00077 area, pos, clip, AR_AREA_MAX, AR_AREA_MIN,
00078 1.0, &wmarker_num);
00079 if( marker_info2 == 0 ) return -1;
00080
00081 wmarker_info = arGetMarkerInfo( dataPtr, marker_info2, &wmarker_num );
00082 if( wmarker_info == 0 ) return -1;
00083
00084 for( i = 0; i < prev_num; i++ ) {
00085 rlenmin = 10.0;
00086 cid = -1;
00087 for( j = 0; j < wmarker_num; j++ ) {
00088 rarea = (double)prev_info[i].marker.area / (double)wmarker_info[j].area;
00089 if( rarea < 0.7 || rarea > 1.43 ) continue;
00090 rlen = ( (wmarker_info[j].pos[0] - prev_info[i].marker.pos[0])
00091 * (wmarker_info[j].pos[0] - prev_info[i].marker.pos[0])
00092 + (wmarker_info[j].pos[1] - prev_info[i].marker.pos[1])
00093 * (wmarker_info[j].pos[1] - prev_info[i].marker.pos[1]) ) / wmarker_info[j].area;
00094 if( rlen < 0.5 && rlen < rlenmin ) {
00095 rlenmin = rlen;
00096 cid = j;
00097 }
00098 }
00099 if( cid >= 0 && wmarker_info[cid].cf < prev_info[i].marker.cf ) {
00100 wmarker_info[cid].cf = prev_info[i].marker.cf;
00101 wmarker_info[cid].id = prev_info[i].marker.id;
00102 diffmin = 10000.0 * 10000.0;
00103 cdir = -1;
00104 for( j = 0; j < 4; j++ ) {
00105 diff = 0;
00106 for( k = 0; k < 4; k++ ) {
00107 diff += (prev_info[i].marker.vertex[k][0] - wmarker_info[cid].vertex[(j+k)%4][0])
00108 * (prev_info[i].marker.vertex[k][0] - wmarker_info[cid].vertex[(j+k)%4][0])
00109 + (prev_info[i].marker.vertex[k][1] - wmarker_info[cid].vertex[(j+k)%4][1])
00110 * (prev_info[i].marker.vertex[k][1] - wmarker_info[cid].vertex[(j+k)%4][1]);
00111 }
00112 if( diff < diffmin ) {
00113 diffmin = diff;
00114 cdir = (prev_info[i].marker.dir - j + 4) % 4;
00115 }
00116 }
00117 wmarker_info[cid].dir = cdir;
00118 }
00119 }
00120
00121 for( i = 0; i < wmarker_num; i++ ) {
00122
00123
00124
00125 if( wmarker_info[i].cf < 0.5 ) wmarker_info[i].id = -1;
00126 }
00127
00128
00129
00130
00131 for( i = j = 0; i < prev_num; i++ ) {
00132 prev_info[i].count++;
00133 if( prev_info[i].count < 4 ) {
00134 prev_info[j] = prev_info[i];
00135 j++;
00136 }
00137 }
00138 prev_num = j;
00139
00140 for( i = 0; i < wmarker_num; i++ ) {
00141 if( wmarker_info[i].id < 0 ) continue;
00142
00143 for( j = 0; j < prev_num; j++ ) {
00144 if( prev_info[j].marker.id == wmarker_info[i].id ) break;
00145 }
00146 prev_info[j].marker = wmarker_info[i];
00147 prev_info[j].count = 1;
00148 if( j == prev_num ) prev_num++;
00149 }
00150
00151 for( i = 0; i < prev_num; i++ ) {
00152 for( j = 0; j < wmarker_num; j++ ) {
00153 rarea = (double)prev_info[i].marker.area / (double)wmarker_info[j].area;
00154 if( rarea < 0.7 || rarea > 1.43 ) continue;
00155 rlen = ( (wmarker_info[j].pos[0] - prev_info[i].marker.pos[0])
00156 * (wmarker_info[j].pos[0] - prev_info[i].marker.pos[0])
00157 + (wmarker_info[j].pos[1] - prev_info[i].marker.pos[1])
00158 * (wmarker_info[j].pos[1] - prev_info[i].marker.pos[1]) ) / wmarker_info[j].area;
00159 if( rlen < 0.5 ) break;
00160 }
00161 if( j == wmarker_num ) {
00162 wmarker_info[wmarker_num] = prev_info[i].marker;
00163 wmarker_num++;
00164 }
00165 }
00166
00167
00168 *marker_num = wmarker_num;
00169 *marker_info = wmarker_info;
00170
00171 return 0;
00172 }
00173
00174
00175 int arDetectMarkerLite( ARUint8 *dataPtr, int thresh,
00176 ARMarkerInfo **marker_info, int *marker_num )
00177 {
00178 ARInt16 *limage;
00179 int label_num;
00180 int *area, *clip, *label_ref;
00181 double *pos;
00182 int i;
00183
00184 *marker_num = 0;
00185
00186 limage = arLabeling( dataPtr, thresh,
00187 &label_num, &area, &pos, &clip, &label_ref );
00188 if( limage == 0 ) return -1;
00189
00190 marker_info2 = arDetectMarker2( limage, label_num, label_ref,
00191 area, pos, clip, AR_AREA_MAX, AR_AREA_MIN,
00192 1.0, &wmarker_num);
00193 if( marker_info2 == 0 ) return -1;
00194
00195 wmarker_info = arGetMarkerInfo( dataPtr, marker_info2, &wmarker_num );
00196 if( wmarker_info == 0 ) return -1;
00197
00198 for( i = 0; i < wmarker_num; i++ ) {
00199 if( wmarker_info[i].cf < 0.5 ) wmarker_info[i].id = -1;
00200 }
00201
00202
00203 *marker_num = wmarker_num;
00204 *marker_info = wmarker_info;
00205
00206 return 0;
00207 }
00208
00209 int arsDetectMarker( ARUint8 *dataPtr, int thresh,
00210 ARMarkerInfo **marker_info, int *marker_num, int LorR )
00211 {
00212 ARInt16 *limage;
00213 int label_num;
00214 int *area, *clip, *label_ref;
00215 double *pos;
00216 double rarea, rlen, rlenmin;
00217 double diff, diffmin;
00218 int cid, cdir;
00219 int i, j, k;
00220
00221 *marker_num = 0;
00222
00223 limage = arsLabeling( dataPtr, thresh,
00224 &label_num, &area, &pos, &clip, &label_ref, LorR );
00225 if( limage == 0 ) return -1;
00226
00227 marker_info2 = arDetectMarker2( limage, label_num, label_ref,
00228 area, pos, clip, AR_AREA_MAX, AR_AREA_MIN,
00229 1.0, &wmarker_num);
00230 if( marker_info2 == 0 ) return -1;
00231
00232 wmarker_info = arsGetMarkerInfo( dataPtr, marker_info2, &wmarker_num, LorR );
00233 if( wmarker_info == 0 ) return -1;
00234
00235 for( i = 0; i < sprev_num[LorR]; i++ ) {
00236 rlenmin = 10.0;
00237 cid = -1;
00238 for( j = 0; j < wmarker_num; j++ ) {
00239 rarea = (double)sprev_info[LorR][i].marker.area / (double)wmarker_info[j].area;
00240 if( rarea < 0.7 || rarea > 1.43 ) continue;
00241 rlen = ( (wmarker_info[j].pos[0] - sprev_info[LorR][i].marker.pos[0])
00242 * (wmarker_info[j].pos[0] - sprev_info[LorR][i].marker.pos[0])
00243 + (wmarker_info[j].pos[1] - sprev_info[LorR][i].marker.pos[1])
00244 * (wmarker_info[j].pos[1] - sprev_info[LorR][i].marker.pos[1]) ) / wmarker_info[j].area;
00245 if( rlen < 0.5 && rlen < rlenmin ) {
00246 rlenmin = rlen;
00247 cid = j;
00248 }
00249 }
00250 if( cid >= 0 && wmarker_info[cid].cf < sprev_info[LorR][i].marker.cf ) {
00251 wmarker_info[cid].cf = sprev_info[LorR][i].marker.cf;
00252 wmarker_info[cid].id = sprev_info[LorR][i].marker.id;
00253 diffmin = 10000.0 * 10000.0;
00254 cdir = -1;
00255 for( j = 0; j < 4; j++ ) {
00256 diff = 0;
00257 for( k = 0; k < 4; k++ ) {
00258 diff += (sprev_info[LorR][i].marker.vertex[k][0] - wmarker_info[cid].vertex[(j+k)%4][0])
00259 * (sprev_info[LorR][i].marker.vertex[k][0] - wmarker_info[cid].vertex[(j+k)%4][0])
00260 + (sprev_info[LorR][i].marker.vertex[k][1] - wmarker_info[cid].vertex[(j+k)%4][1])
00261 * (sprev_info[LorR][i].marker.vertex[k][1] - wmarker_info[cid].vertex[(j+k)%4][1]);
00262 }
00263 if( diff < diffmin ) {
00264 diffmin = diff;
00265 cdir = (sprev_info[LorR][i].marker.dir - j + 4) % 4;
00266 }
00267 }
00268 wmarker_info[cid].dir = cdir;
00269 }
00270 }
00271
00272 for( i = 0; i < wmarker_num; i++ ) {
00273 if( wmarker_info[i].cf < 0.5 ) wmarker_info[i].id = -1;
00274 }
00275
00276 j = 0;
00277 for( i = 0; i < wmarker_num; i++ ) {
00278 if( wmarker_info[i].id < 0 ) continue;
00279 sprev_info[LorR][j].marker = wmarker_info[i];
00280 sprev_info[LorR][j].count = 1;
00281 j++;
00282 }
00283 sprev_num[LorR] = j;
00284
00285 *marker_num = wmarker_num;
00286 *marker_info = wmarker_info;
00287
00288 return 0;
00289 }
00290
00291 int arsDetectMarkerLite( ARUint8 *dataPtr, int thresh,
00292 ARMarkerInfo **marker_info, int *marker_num, int LorR )
00293 {
00294 ARInt16 *limage;
00295 int label_num;
00296 int *area, *clip, *label_ref;
00297 double *pos;
00298 int i;
00299
00300 *marker_num = 0;
00301
00302 limage = arsLabeling( dataPtr, thresh,
00303 &label_num, &area, &pos, &clip, &label_ref, LorR );
00304 if( limage == 0 ) return -1;
00305
00306 marker_info2 = arDetectMarker2( limage, label_num, label_ref,
00307 area, pos, clip, AR_AREA_MAX, AR_AREA_MIN,
00308 1.0, &wmarker_num);
00309 if( marker_info2 == 0 ) return -1;
00310
00311 wmarker_info = arsGetMarkerInfo( dataPtr, marker_info2, &wmarker_num, LorR );
00312 if( wmarker_info == 0 ) return -1;
00313
00314 for( i = 0; i < wmarker_num; i++ ) {
00315 if( wmarker_info[i].cf < 0.5 ) wmarker_info[i].id = -1;
00316 }
00317
00318
00319 *marker_num = wmarker_num;
00320 *marker_info = wmarker_info;
00321
00322 return 0;
00323 }
