artoolkit: arDetectMarker2.c Source File

00001 /*******************************************************
00002  *
00003  * Author: Hirokazu Kato
00004  *
00005  *         kato@sys.im.hiroshima-cu.ac.jp
00006  *
00007  * Revision: 2.11
00008  * Date: 00/05/06
00009  *
00010 *******************************************************/
00011 
00012 #include <AR/ar.h>
00013 
00014 static int check_square( int area, ARMarkerInfo2 *marker_info2, double factor );
00015 
00016 static int get_vertex( int x_coord[], int y_coord[], int st, int ed,
00017                        double thresh, int vertex[], int *vnum );
00018 
00019 static ARMarkerInfo2    marker_info2[AR_SQUARE_MAX];
00020 
00021 ARMarkerInfo2 *arDetectMarker2( ARInt16 *limage, int label_num, int *label_ref,
00022                                 int *warea, double *wpos, int *wclip,
00023                                 int area_max, int area_min, double factor, int *marker_num )
00024 {
00025     ARMarkerInfo2     *pm;
00026     int               xsize, ysize;
00027     int               marker_num2;
00028     int               i, j, ret;
00029     double            d;
00030 
00031     if( arImageProcMode == AR_IMAGE_PROC_IN_HALF ) {
00032         area_min /= 4;
00033         area_max /= 4;
00034         xsize = arImXsize / 2;
00035         ysize = arImYsize / 2;
00036     }
00037     else {
00038         xsize = arImXsize;
00039         ysize = arImYsize;
00040     }
00041     marker_num2 = 0;
00042     for(i=0; i<label_num; i++ ) {
00043         if( warea[i] < area_min || warea[i] > area_max ) continue;
00044         if( wclip[i*4+0] == 1 || wclip[i*4+1] == xsize-2 ) continue;
00045         if( wclip[i*4+2] == 1 || wclip[i*4+3] == ysize-2 ) continue;
00046 
00047         ret = arGetContour( limage, label_ref, i+1,
00048                             &(wclip[i*4]), &(marker_info2[marker_num2]));
00049         if( ret < 0 ) continue;
00050 
00051         ret = check_square( warea[i], &(marker_info2[marker_num2]), factor );
00052         if( ret < 0 ) continue;
00053 
00054         marker_info2[marker_num2].area   = warea[i];
00055         marker_info2[marker_num2].pos[0] = wpos[i*2+0];
00056         marker_info2[marker_num2].pos[1] = wpos[i*2+1];
00057         marker_num2++;
00058         if( marker_num2 == AR_SQUARE_MAX ) break;
00059     }
00060 
00061     for( i=0; i < marker_num2; i++ ) {
00062         for( j=i+1; j < marker_num2; j++ ) {
00063             d = (marker_info2[i].pos[0] - marker_info2[j].pos[0])
00064               * (marker_info2[i].pos[0] - marker_info2[j].pos[0])
00065               + (marker_info2[i].pos[1] - marker_info2[j].pos[1])
00066               * (marker_info2[i].pos[1] - marker_info2[j].pos[1]);
00067             if( marker_info2[i].area > marker_info2[j].area ) {
00068                 if( d < marker_info2[i].area / 4 ) {
00069                     marker_info2[j].area = 0;
00070                 }
00071             }
00072             else {
00073                 if( d < marker_info2[j].area / 4 ) {
00074                     marker_info2[i].area = 0;
00075                 }
00076             }
00077         }
00078     }
00079     for( i=0; i < marker_num2; i++ ) {
00080         if( marker_info2[i].area == 0.0 ) {
00081             for( j=i+1; j < marker_num2; j++ ) {
00082                 marker_info2[j-1] = marker_info2[j];
00083             }
00084             marker_num2--;
00085         }
00086     }
00087 
00088     if( arImageProcMode == AR_IMAGE_PROC_IN_HALF ) {
00089         pm = &(marker_info2[0]);
00090         for( i = 0; i < marker_num2; i++ ) {
00091             pm->area *= 4;
00092             pm->pos[0] *= 2.0;
00093             pm->pos[1] *= 2.0;
00094             for( j = 0; j< pm->coord_num; j++ ) {
00095                 pm->x_coord[j] *= 2;
00096                 pm->y_coord[j] *= 2;
00097             }
00098             pm++;
00099         }
00100     }
00101 
00102     *marker_num = marker_num2;
00103     return( &(marker_info2[0]) );
00104 }
00105 
00106 int arGetContour( ARInt16 *limage, int *label_ref,
00107                   int label, int clip[4], ARMarkerInfo2 *marker_info2 )
00108 {
00109     static int      xdir[8] = { 0, 1, 1, 1, 0,-1,-1,-1};
00110     static int      ydir[8] = {-1,-1, 0, 1, 1, 1, 0,-1};
00111     static int      wx[AR_CHAIN_MAX];
00112     static int      wy[AR_CHAIN_MAX];
00113     ARInt16         *p1;
00114     int             xsize, ysize;
00115     int             sx, sy, dir;
00116     int             dmax, d, v1;
00117     int             i, j;
00118 
00119     if( arImageProcMode == AR_IMAGE_PROC_IN_HALF ) {
00120         xsize = arImXsize / 2;
00121         ysize = arImYsize / 2;
00122     }
00123     else {
00124         xsize = arImXsize;
00125         ysize = arImYsize;
00126     }
00127     j = clip[2];
00128     p1 = &(limage[j*xsize+clip[0]]);
00129     for( i = clip[0]; i <= clip[1]; i++, p1++ ) {
00130         if( *p1 > 0 && label_ref[(*p1)-1] == label ) {
00131             sx = i; sy = j; break;
00132         }
00133     }
00134     if( i > clip[1] ) {
00135         printf("??? 1\n"); return(-1);
00136     }
00137 
00138     marker_info2->coord_num = 1;
00139     marker_info2->x_coord[0] = sx;
00140     marker_info2->y_coord[0] = sy;
00141     dir = 5;
00142     for(;;) {
00143         p1 = &(limage[marker_info2->y_coord[marker_info2->coord_num-1] * xsize
00144                     + marker_info2->x_coord[marker_info2->coord_num-1]]);
00145         dir = (dir+5)%8;
00146         for(i=0;i<8;i++) {
00147             if( p1[ydir[dir]*xsize+xdir[dir]] > 0 ) break;
00148             dir = (dir+1)%8;
00149         }
00150         if( i == 8 ) {
00151             printf("??? 2\n"); return(-1);
00152         }
00153         marker_info2->x_coord[marker_info2->coord_num]
00154             = marker_info2->x_coord[marker_info2->coord_num-1] + xdir[dir];
00155         marker_info2->y_coord[marker_info2->coord_num]
00156             = marker_info2->y_coord[marker_info2->coord_num-1] + ydir[dir];
00157         if( marker_info2->x_coord[marker_info2->coord_num] == sx
00158          && marker_info2->y_coord[marker_info2->coord_num] == sy ) break;
00159         marker_info2->coord_num++;
00160         if( marker_info2->coord_num == AR_CHAIN_MAX-1 ) {
00161             printf("??? 3\n"); return(-1);
00162         }
00163     }
00164 
00165     dmax = 0;
00166     for(i=1;i<marker_info2->coord_num;i++) {
00167         d = (marker_info2->x_coord[i]-sx)*(marker_info2->x_coord[i]-sx)
00168           + (marker_info2->y_coord[i]-sy)*(marker_info2->y_coord[i]-sy);
00169         if( d > dmax ) {
00170             dmax = d;
00171             v1 = i;
00172         }
00173     }
00174 
00175     for(i=0;i<v1;i++) {
00176         wx[i] = marker_info2->x_coord[i];
00177         wy[i] = marker_info2->y_coord[i];
00178     }
00179     for(i=v1;i<marker_info2->coord_num;i++) {
00180         marker_info2->x_coord[i-v1] = marker_info2->x_coord[i];
00181         marker_info2->y_coord[i-v1] = marker_info2->y_coord[i];
00182     }
00183     for(i=0;i<v1;i++) {
00184         marker_info2->x_coord[i-v1+marker_info2->coord_num] = wx[i];
00185         marker_info2->y_coord[i-v1+marker_info2->coord_num] = wy[i];
00186     }
00187     marker_info2->x_coord[marker_info2->coord_num] = marker_info2->x_coord[0];
00188     marker_info2->y_coord[marker_info2->coord_num] = marker_info2->y_coord[0];
00189     marker_info2->coord_num++;
00190 
00191     return 0;
00192 }
00193 
00194 static int check_square( int area, ARMarkerInfo2 *marker_info2, double factor )
00195 {
00196     int             sx, sy;
00197     int             dmax, d, v1;
00198     int             vertex[10], vnum;
00199     int             wv1[10], wvnum1, wv2[10], wvnum2, v2;
00200     double          thresh;
00201     int             i;
00202 
00203 
00204     dmax = 0;
00205     v1 = 0;
00206     sx = marker_info2->x_coord[0];
00207     sy = marker_info2->y_coord[0];
00208     for(i=1;i<marker_info2->coord_num-1;i++) {
00209         d = (marker_info2->x_coord[i]-sx)*(marker_info2->x_coord[i]-sx)
00210           + (marker_info2->y_coord[i]-sy)*(marker_info2->y_coord[i]-sy);
00211         if( d > dmax ) {
00212             dmax = d;
00213             v1 = i;
00214         }
00215     }
00216 
00217     thresh = (area/0.75) * 0.01 * factor;
00218     vnum = 1;
00219     vertex[0] = 0;
00220     wvnum1 = 0;
00221     wvnum2 = 0;
00222     if( get_vertex(marker_info2->x_coord, marker_info2->y_coord, 0,  v1,
00223                    thresh, wv1, &wvnum1) < 0 ) {
00224         return(-1);
00225     }
00226     if( get_vertex(marker_info2->x_coord, marker_info2->y_coord,
00227                    v1,  marker_info2->coord_num-1, thresh, wv2, &wvnum2) < 0 ) {
00228         return(-1);
00229     }
00230 
00231     if( wvnum1 == 1 && wvnum2 == 1 ) {
00232         vertex[1] = wv1[0];
00233         vertex[2] = v1;
00234         vertex[3] = wv2[0];
00235     }
00236     else if( wvnum1 > 1 && wvnum2 == 0 ) {
00237         v2 = v1 / 2;
00238         wvnum1 = wvnum2 = 0;
00239         if( get_vertex(marker_info2->x_coord, marker_info2->y_coord,
00240                        0,  v2, thresh, wv1, &wvnum1) < 0 ) {
00241             return(-1);
00242         }
00243         if( get_vertex(marker_info2->x_coord, marker_info2->y_coord,
00244                        v2,  v1, thresh, wv2, &wvnum2) < 0 ) {
00245             return(-1);
00246         }
00247         if( wvnum1 == 1 && wvnum2 == 1 ) {
00248             vertex[1] = wv1[0];
00249             vertex[2] = wv2[0];
00250             vertex[3] = v1;
00251         }
00252         else {
00253             return(-1);
00254         }
00255     }
00256     else if( wvnum1 == 0 && wvnum2 > 1 ) {
00257         v2 = (v1 + marker_info2->coord_num-1) / 2;
00258         wvnum1 = wvnum2 = 0;
00259         if( get_vertex(marker_info2->x_coord, marker_info2->y_coord,
00260                    v1, v2, thresh, wv1, &wvnum1) < 0 ) {
00261             return(-1);
00262         }
00263         if( get_vertex(marker_info2->x_coord, marker_info2->y_coord,
00264                    v2, marker_info2->coord_num-1, thresh, wv2, &wvnum2) < 0 ) {
00265             return(-1);
00266         }
00267         if( wvnum1 == 1 && wvnum2 == 1 ) {
00268             vertex[1] = v1;
00269             vertex[2] = wv1[0];
00270             vertex[3] = wv2[0];
00271         }
00272         else {
00273             return(-1);
00274         }
00275     }
00276     else {
00277         return(-1);
00278     }
00279 
00280     marker_info2->vertex[0] = vertex[0];
00281     marker_info2->vertex[1] = vertex[1];
00282     marker_info2->vertex[2] = vertex[2];
00283     marker_info2->vertex[3] = vertex[3];
00284     marker_info2->vertex[4] = marker_info2->coord_num-1;
00285 
00286     return(0);
00287 }
00288 
00289 static int get_vertex( int x_coord[], int y_coord[], int st,  int ed,
00290                        double thresh, int vertex[], int *vnum)
00291 {
00292     double   d, dmax;
00293     double   a, b, c;
00294     int      i, v1;
00295 
00296     a = y_coord[ed] - y_coord[st];
00297     b = x_coord[st] - x_coord[ed];
00298     c = x_coord[ed]*y_coord[st] - y_coord[ed]*x_coord[st];
00299     dmax = 0;
00300     for(i=st+1;i<ed;i++) {
00301         d = a*x_coord[i] + b*y_coord[i] + c;
00302         if( d*d > dmax ) {
00303             dmax = d*d;
00304             v1 = i;
00305         }
00306     }
00307     if( dmax/(a*a+b*b) > thresh ) {
00308         if( get_vertex(x_coord, y_coord, st,  v1, thresh, vertex, vnum) < 0 )
00309             return(-1);
00310 
00311         if( (*vnum) > 5 ) return(-1);
00312         vertex[(*vnum)] = v1;
00313         (*vnum)++;
00314 
00315         if( get_vertex(x_coord, y_coord, v1,  ed, thresh, vertex, vnum) < 0 )
00316             return(-1);
00317     }
00318 
00319     return(0);
00320 }