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 static char *get_buff( char *buf, int n, FILE *fp );
00019
00020 ARMultiMarkerInfoT *arMultiReadConfigFile( const char *filename )
00021 {
00022 FILE *fp;
00023 ARMultiEachMarkerInfoT *marker;
00024 ARMultiMarkerInfoT *marker_info;
00025 double wpos3d[4][2];
00026 char buf[256], buf1[256];
00027 int num;
00028 int i, j;
00029
00030 if( (fp=fopen(filename,"r")) == NULL ) return NULL;
00031
00032 get_buff(buf, 256, fp);
00033 if( sscanf(buf, "%d", &num) != 1 ) {fclose(fp); return NULL;}
00034
00035 arMalloc(marker,ARMultiEachMarkerInfoT,num);
00036
00037 for( i = 0; i < num; i++ ) {
00038 get_buff(buf, 256, fp);
00039 if( sscanf(buf, "%s", buf1) != 1 ) {
00040 fclose(fp); free(marker); return NULL;
00041 }
00042 if( (marker[i].patt_id = arLoadPatt(buf1)) < 0 ) {
00043 fclose(fp); free(marker); return NULL;
00044 }
00045
00046 get_buff(buf, 256, fp);
00047 if( sscanf(buf, "%lf", &marker[i].width) != 1 ) {
00048 fclose(fp); free(marker); return NULL;
00049 }
00050
00051 get_buff(buf, 256, fp);
00052 if( sscanf(buf, "%lf %lf", &marker[i].center[0], &marker[i].center[1]) != 2 ) {
00053 fclose(fp); free(marker); return NULL;
00054 }
00055
00056 for( j = 0; j < 3; j++ ) {
00057 get_buff(buf, 256, fp);
00058 if( sscanf(buf, "%lf %lf %lf %lf", &marker[i].trans[j][0],
00059 &marker[i].trans[j][1], &marker[i].trans[j][2],
00060 &marker[i].trans[j][3]) != 4 ) {
00061 fclose(fp); free(marker); return NULL;
00062 }
00063 }
00064 arUtilMatInv( marker[i].trans, marker[i].itrans );
00065
00066 wpos3d[0][0] = marker[i].center[0] - marker[i].width/2.0;
00067 wpos3d[0][1] = marker[i].center[1] + marker[i].width/2.0;
00068 wpos3d[1][0] = marker[i].center[0] + marker[i].width/2.0;
00069 wpos3d[1][1] = marker[i].center[1] + marker[i].width/2.0;
00070 wpos3d[2][0] = marker[i].center[0] + marker[i].width/2.0;
00071 wpos3d[2][1] = marker[i].center[1] - marker[i].width/2.0;
00072 wpos3d[3][0] = marker[i].center[0] - marker[i].width/2.0;
00073 wpos3d[3][1] = marker[i].center[1] - marker[i].width/2.0;
00074 for( j = 0; j < 4; j++ ) {
00075 marker[i].pos3d[j][0] = marker[i].trans[0][0] * wpos3d[j][0]
00076 + marker[i].trans[0][1] * wpos3d[j][1]
00077 + marker[i].trans[0][3];
00078 marker[i].pos3d[j][1] = marker[i].trans[1][0] * wpos3d[j][0]
00079 + marker[i].trans[1][1] * wpos3d[j][1]
00080 + marker[i].trans[1][3];
00081 marker[i].pos3d[j][2] = marker[i].trans[2][0] * wpos3d[j][0]
00082 + marker[i].trans[2][1] * wpos3d[j][1]
00083 + marker[i].trans[2][3];
00084 }
00085 }
00086
00087 fclose(fp);
00088
00089 marker_info = (ARMultiMarkerInfoT *)malloc( sizeof(ARMultiMarkerInfoT) );
00090 if( marker_info == NULL ) {free(marker); return NULL;}
00091 marker_info->marker = marker;
00092 marker_info->marker_num = num;
00093 marker_info->prevF = 0;
00094
00095 return marker_info;
00096 }
00097
00098 static char *get_buff( char *buf, int n, FILE *fp )
00099 {
00100 char *ret;
00101
00102 for(;;) {
00103 ret = fgets( buf, n, fp );
00104 if( ret == NULL ) return(NULL);
00105 if( buf[0] != '\n' && buf[0] != '#' ) return(ret);
00106 }
00107 }
