00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012 #include <stdio.h>
00013 #include <math.h>
00014 #include <string.h>
00015 #ifdef _WIN32
00016 #include <sys/timeb.h>
00017 #include <windows.h>
00018 #else
00019 #include <sys/time.h>
00020 #endif
00021 #include <AR/param.h>
00022 #include <AR/matrix.h>
00023 #include <AR/ar.h>
00024
00025
00026 int arDebug = 0;
00027 ARUint8* arImage = NULL;
00028 int arFittingMode = DEFAULT_FITTING_MODE;
00029 int arImageProcMode = DEFAULT_IMAGE_PROC_MODE;
00030 ARParam arParam;
00031 int arImXsize, arImYsize;
00032 int arTemplateMatchingMode = DEFAULT_TEMPLATE_MATCHING_MODE;
00033 int arMatchingPCAMode = DEFAULT_MATCHING_PCA_MODE;
00034
00035 ARUint8* arImageL = NULL;
00036 ARUint8* arImageR = NULL;
00037 ARSParam arsParam;
00038 double arsMatR2L[3][4];
00039
00040 ARUint32 arGetVersion(char **versionStringRef)
00041 {
00042 const char version[] = AR_HEADER_VERSION_STRING;
00043 char *s;
00044
00045 if (versionStringRef) {
00046 arMalloc(s, char, sizeof(version));
00047 strncpy(s, version, sizeof(version));
00048 *versionStringRef = s;
00049 }
00050
00051
00052 return (0x10000000u * ((unsigned int)AR_HEADER_VERSION_MAJOR / 10u) +
00053 0x01000000u * ((unsigned int)AR_HEADER_VERSION_MAJOR % 10u) +
00054 0x00100000u * ((unsigned int)AR_HEADER_VERSION_MINOR / 10u) +
00055 0x00010000u * ((unsigned int)AR_HEADER_VERSION_MINOR % 10u) +
00056 0x00001000u * ((unsigned int)AR_HEADER_VERSION_TINY / 10u) +
00057 0x00000100u * ((unsigned int)AR_HEADER_VERSION_TINY % 10u) +
00058 0x00000010u * ((unsigned int)AR_HEADER_VERSION_BUILD / 10u) +
00059 0x00000001u * ((unsigned int)AR_HEADER_VERSION_BUILD % 10u)
00060 );
00061 }
00062
00063 static int arGetLine2(int x_coord[], int y_coord[], int coord_num,
00064 int vertex[], double line[4][3], double v[4][2], double *dist_factor);
00065
00066 int arInitCparam( ARParam *param )
00067 {
00068 arImXsize = param->xsize;
00069 arImYsize = param->ysize;
00070 arParam = *param;
00071
00072 return(0);
00073 }
00074
00075 int arsInitCparam( ARSParam *sparam )
00076 {
00077 arImXsize = sparam->xsize;
00078 arImYsize = sparam->ysize;
00079 arsParam = *sparam;
00080
00081 arUtilMatInv( arsParam.matL2R, arsMatR2L );
00082
00083 return(0);
00084 }
00085
00086 int arGetLine(int x_coord[], int y_coord[], int coord_num,
00087 int vertex[], double line[4][3], double v[4][2])
00088 {
00089 return arGetLine2( x_coord, y_coord, coord_num, vertex, line, v, arParam.dist_factor );
00090 }
00091
00092 int arsGetLine(int x_coord[], int y_coord[], int coord_num,
00093 int vertex[], double line[4][3], double v[4][2], int LorR)
00094 {
00095 if( LorR )
00096 return arGetLine2( x_coord, y_coord, coord_num, vertex, line, v, arsParam.dist_factorL );
00097 else
00098 return arGetLine2( x_coord, y_coord, coord_num, vertex, line, v, arsParam.dist_factorR );
00099 }
00100
00101 static int arGetLine2(int x_coord[], int y_coord[], int coord_num,
00102 int vertex[], double line[4][3], double v[4][2], double *dist_factor)
00103 {
00104 ARMat *input, *evec;
00105 ARVec *ev, *mean;
00106 double w1;
00107 int st, ed, n;
00108 int i, j;
00109
00110 ev = arVecAlloc( 2 );
00111 mean = arVecAlloc( 2 );
00112 evec = arMatrixAlloc( 2, 2 );
00113 for( i = 0; i < 4; i++ ) {
00114 w1 = (double)(vertex[i+1]-vertex[i]+1) * 0.05 + 0.5;
00115 st = (int)(vertex[i] + w1);
00116 ed = (int)(vertex[i+1] - w1);
00117 n = ed - st + 1;
00118 input = arMatrixAlloc( n, 2 );
00119 for( j = 0; j < n; j++ ) {
00120 arParamObserv2Ideal( dist_factor, x_coord[st+j], y_coord[st+j],
00121 &(input->m[j*2+0]), &(input->m[j*2+1]) );
00122 }
00123 if( arMatrixPCA(input, evec, ev, mean) < 0 ) {
00124 arMatrixFree( input );
00125 arMatrixFree( evec );
00126 arVecFree( mean );
00127 arVecFree( ev );
00128 return(-1);
00129 }
00130 line[i][0] = evec->m[1];
00131 line[i][1] = -evec->m[0];
00132 line[i][2] = -(line[i][0]*mean->v[0] + line[i][1]*mean->v[1]);
00133 arMatrixFree( input );
00134 }
00135 arMatrixFree( evec );
00136 arVecFree( mean );
00137 arVecFree( ev );
00138
00139 for( i = 0; i < 4; i++ ) {
00140 w1 = line[(i+3)%4][0] * line[i][1] - line[i][0] * line[(i+3)%4][1];
00141 if( w1 == 0.0 ) return(-1);
00142 v[i][0] = ( line[(i+3)%4][1] * line[i][2]
00143 - line[i][1] * line[(i+3)%4][2] ) / w1;
00144 v[i][1] = ( line[i][0] * line[(i+3)%4][2]
00145 - line[(i+3)%4][0] * line[i][2] ) / w1;
00146 }
00147
00148 return(0);
00149 }
00150
00151 int arUtilMatMul( double s1[3][4], double s2[3][4], double d[3][4] )
00152 {
00153 int i, j;
00154
00155 for( j = 0; j < 3; j++ ) {
00156 for( i = 0; i < 4; i++) {
00157 d[j][i] = s1[j][0] * s2[0][i]
00158 + s1[j][1] * s2[1][i]
00159 + s1[j][2] * s2[2][i];
00160 }
00161 d[j][3] += s1[j][3];
00162 }
00163
00164 return 0;
00165 }
00166
00167 int arUtilMatInv( double s[3][4], double d[3][4] )
00168 {
00169 ARMat *mat;
00170 int i, j;
00171
00172 mat = arMatrixAlloc( 4, 4 );
00173 for( j = 0; j < 3; j++ ) {
00174 for( i = 0; i < 4; i++ ) {
00175 mat->m[j*4+i] = s[j][i];
00176 }
00177 }
00178 mat->m[3*4+0] = 0; mat->m[3*4+1] = 0;
00179 mat->m[3*4+2] = 0; mat->m[3*4+3] = 1;
00180 arMatrixSelfInv( mat );
00181 for( j = 0; j < 3; j++ ) {
00182 for( i = 0; i < 4; i++ ) {
00183 d[j][i] = mat->m[j*4+i];
00184 }
00185 }
00186 arMatrixFree( mat );
00187
00188 return 0;
00189 }
00190
00191 int arUtilMat2QuatPos( double m[3][4], double q[4], double p[3] )
00192 {
00193 double w;
00194
00195 w = m[0][0] + m[1][1] + m[2][2] + 1;
00196 if( w < 0.0 ) return -1;
00197
00198 w = sqrt( w );
00199 q[0] = (m[1][2] - m[2][1]) / (w*2.0);
00200 q[1] = (m[2][0] - m[0][2]) / (w*2.0);
00201 q[2] = (m[0][1] - m[1][0]) / (w*2.0);
00202 q[3] = w / 2.0;
00203
00204 p[0] = m[0][3];
00205 p[1] = m[1][3];
00206 p[2] = m[2][3];
00207
00208 return 0;
00209 }
00210
00211 int arUtilQuatPos2Mat( double q[4], double p[3], double m[3][4] )
00212 {
00213 double x2, y2, z2;
00214 double xx, xy, xz;
00215 double yy, yz, zz;
00216 double wx, wy, wz;
00217
00218 x2 = q[0] * 2.0;
00219 y2 = q[1] * 2.0;
00220 z2 = q[2] * 2.0;
00221 xx = q[0] * x2;
00222 xy = q[0] * y2;
00223 xz = q[0] * z2;
00224 yy = q[1] * y2;
00225 yz = q[1] * z2;
00226 zz = q[2] * z2;
00227 wx = q[3] * x2;
00228 wy = q[3] * y2;
00229 wz = q[3] * z2;
00230
00231 m[0][0] = 1.0 - (yy + zz);
00232 m[1][1] = 1.0 - (xx + zz);
00233 m[2][2] = 1.0 - (xx + yy);
00234 m[1][0] = xy - wz;
00235 m[0][1] = xy + wz;
00236 m[2][0] = xz + wy;
00237 m[0][2] = xz - wy;
00238 m[2][1] = yz - wx;
00239 m[1][2] = yz + wx;
00240
00241 m[0][3] = p[0];
00242 m[1][3] = p[1];
00243 m[2][3] = p[2];
00244
00245 return 0;
00246 }
00247
00248
00249 static int ss, sms;
00250
00251 double arUtilTimer(void)
00252 {
00253 #ifdef _WIN32
00254 struct _timeb sys_time;
00255 double tt;
00256 int s1, s2;
00257
00258 _ftime(&sys_time);
00259 s1 = sys_time.time - ss;
00260 s2 = sys_time.millitm - sms;
00261 #else
00262 struct timeval time;
00263 double tt;
00264 int s1, s2;
00265
00266 #if defined(__linux) || defined(__APPLE__)
00267 gettimeofday( &time, NULL );
00268 #else
00269 gettimeofday( &time );
00270 #endif
00271 s1 = time.tv_sec - ss;
00272 s2 = time.tv_usec/1000 - sms;
00273 #endif
00274
00275 tt = (double)s1 + (double)s2 / 1000.0;
00276
00277 return( tt );
00278 }
00279
00280 void arUtilTimerReset(void)
00281 {
00282 #ifdef _WIN32
00283 struct _timeb sys_time;
00284
00285 _ftime(&sys_time);
00286 ss = sys_time.time;
00287 sms = sys_time.millitm;
00288 #else
00289 struct timeval time;
00290
00291 #if defined(__linux) || defined(__APPLE__)
00292 gettimeofday( &time, NULL );
00293 #else
00294 gettimeofday( &time );
00295 #endif
00296 ss = time.tv_sec;
00297 sms = time.tv_usec / 1000;
00298 #endif
00299 }
00300
00301 void arUtilSleep( int msec )
00302 {
00303 #ifndef _WIN32
00304 struct timespec req;
00305
00306 req.tv_sec = 0;
00307 req.tv_nsec = msec* 1000;
00308 nanosleep( &req, NULL );
00309 #else
00310 Sleep(msec);
00311 #endif
00312 return;
00313 }
00314
