check_dist.c
Go to the documentation of this file.
00001 #ifdef _WIN32
00002 #  include <windows.h>
00003 #endif
00004 #include <math.h>
00005 #include <stdio.h>
00006 #include <stdlib.h>
00007 #include <AR/param.h>
00008 #include <AR/matrix.h>
00009 #include "calib_dist.h"
00010 
00011 static double   get_fitting_error( CALIB_PATT_T *patt, double dist_factor[4] );
00012 static double   check_error( double *x, double *y, int num, double dist_factor[4] );
00013 static double   calc_distortion2( CALIB_PATT_T *patt, double dist_factor[4] );
00014 static double   get_size_factor( double dist_factor[4], int xsize, int ysize );
00015 
00016 void calc_distortion( CALIB_PATT_T *patt, int xsize, int ysize, double dist_factor[4] )
00017 {
00018     int     i, j;
00019     double  bx, by;
00020     double  bf[4];
00021     double  error, min;
00022     double  factor[4];
00023 
00024     bx = xsize / 2;
00025     by = ysize / 2;
00026     factor[0] = bx;
00027     factor[1] = by;
00028     factor[3] = 1.0;
00029     min = calc_distortion2( patt, factor );
00030     bf[0] = factor[0];
00031     bf[1] = factor[1];
00032     bf[2] = factor[2];
00033     bf[3] = 1.0;
00034 printf("[%5.1f, %5.1f, %5.1f] %f\n", bf[0], bf[1], bf[2], min);
00035     for( j = -10; j <= 10; j++ ) {
00036         factor[1] = by + j*5;
00037         for( i = -10; i <= 10; i++ ) {
00038             factor[0] = bx + i*5;
00039             error = calc_distortion2( patt, factor );
00040             if( error < min ) { bf[0] = factor[0]; bf[1] = factor[1];
00041                                 bf[2] = factor[2]; min = error; }
00042         }
00043 printf("[%5.1f, %5.1f, %5.1f] %f\n", bf[0], bf[1], bf[2], min);
00044     }
00045 
00046     bx = bf[0];
00047     by = bf[1];
00048     for( j = -10; j <= 10; j++ ) {
00049         factor[1] = by + 0.5 * j;
00050         for( i = -10; i <= 10; i++ ) {
00051             factor[0] = bx + 0.5 * i;
00052             error = calc_distortion2( patt, factor );
00053             if( error < min ) { bf[0] = factor[0]; bf[1] = factor[1];
00054                                 bf[2] = factor[2]; min = error; }
00055         }
00056 printf("[%5.1f, %5.1f, %5.1f] %f\n", bf[0], bf[1], bf[2], min);
00057     }
00058 
00059     dist_factor[0] = bf[0];
00060     dist_factor[1] = bf[1];
00061     dist_factor[2] = bf[2];
00062     dist_factor[3] = get_size_factor( bf, xsize, ysize );
00063 }
00064 
00065 static double get_size_factor( double dist_factor[4], int xsize, int ysize )
00066 {
00067     double  ox, oy, ix, iy;
00068     double  olen, ilen;
00069     double  sf, sf1;
00070 
00071     sf = 100.0;
00072 
00073     ox = 0.0;
00074     oy = dist_factor[1];
00075     olen = dist_factor[0];
00076     arParamObserv2Ideal( dist_factor, ox, oy, &ix, &iy );
00077     ilen = dist_factor[0] - ix;
00078 printf("Olen = %f, Ilen = %f\n", olen, ilen);
00079     if( ilen > 0 ) {
00080         sf1 = ilen / olen;
00081         if( sf1 < sf ) sf = sf1;
00082     }
00083 
00084     ox = xsize;
00085     oy = dist_factor[1];
00086     olen = xsize - dist_factor[0];
00087     arParamObserv2Ideal( dist_factor, ox, oy, &ix, &iy );
00088     ilen = ix - dist_factor[0];
00089 printf("Olen = %f, Ilen = %f\n", olen, ilen);
00090     if( ilen > 0 ) {
00091         sf1 = ilen / olen;
00092         if( sf1 < sf ) sf = sf1;
00093     }
00094 
00095     ox = dist_factor[0];
00096     oy = 0.0;
00097     olen = dist_factor[1];
00098     arParamObserv2Ideal( dist_factor, ox, oy, &ix, &iy );
00099     ilen = dist_factor[1] - iy;
00100 printf("Olen = %f, Ilen = %f\n", olen, ilen);
00101     if( ilen > 0 ) {
00102         sf1 = ilen / olen;
00103         if( sf1 < sf ) sf = sf1;
00104     }
00105 
00106     ox = dist_factor[0];
00107     oy = ysize;
00108     olen = ysize - dist_factor[1];
00109     arParamObserv2Ideal( dist_factor, ox, oy, &ix, &iy );
00110     ilen = iy - dist_factor[1];
00111 printf("Olen = %f, Ilen = %f\n", olen, ilen);
00112     if( ilen > 0 ) {
00113         sf1 = ilen / olen;
00114         if( sf1 < sf ) sf = sf1;
00115     }
00116 
00117     if( sf == 0.0 ) sf = 1.0;
00118 
00119     return sf;
00120 }
00121 
00122 static double calc_distortion2( CALIB_PATT_T *patt, double dist_factor[4] )
00123 {
00124     double    min, err, f, fb;
00125     int       i;
00126 
00127     dist_factor[2] = 0.0;
00128     min = get_fitting_error( patt, dist_factor );
00129 
00130     f = dist_factor[2];
00131     for( i = 10; i < 200; i+=10 ) {
00132         dist_factor[2] = i;
00133         err = get_fitting_error( patt, dist_factor );
00134         if( err < min ) { min = err; f = dist_factor[2]; }
00135     }
00136 
00137     fb = f;
00138     for( i = -10; i <= 10; i++ ) {
00139         dist_factor[2] = fb + i;
00140         if( dist_factor[2] < 0 ) continue;
00141         err = get_fitting_error( patt, dist_factor );
00142         if( err < min ) { min = err; f = dist_factor[2]; }
00143     }
00144 
00145     fb = f;
00146     for( i = -10; i <= 10; i++ ) {
00147         dist_factor[2] = fb + 0.1 * i;
00148         if( dist_factor[2] < 0 ) continue;
00149         err = get_fitting_error( patt, dist_factor );
00150         if( err < min ) { min = err; f = dist_factor[2]; }
00151     }
00152 
00153     dist_factor[2] = f;
00154     return min;
00155 }
00156 
00157 static double get_fitting_error( CALIB_PATT_T *patt, double dist_factor[4] )
00158 {
00159     double   *x, *y;
00160     double   error;
00161     int      max;
00162     int      i, j, k, l;
00163     int      p;
00164 
00165     max = (patt->v_num > patt->h_num)? patt->v_num: patt->h_num;
00166     x = (double *)malloc( sizeof(double)*max );
00167     y = (double *)malloc( sizeof(double)*max );
00168     if( x == NULL || y == NULL ) exit(0);
00169 
00170     error = 0.0;
00171     for( i = 0; i < patt->loop_num; i++ ) {
00172         for( j = 0; j < patt->v_num; j++ ) {
00173             for( k = 0; k < patt->h_num; k++ ) {
00174                 x[k] = patt->point[i][j*patt->h_num+k].x_coord;
00175                 y[k] = patt->point[i][j*patt->h_num+k].y_coord;
00176             }
00177             error += check_error( x, y, patt->h_num, dist_factor );
00178         }
00179 
00180         for( j = 0; j < patt->h_num; j++ ) {
00181             for( k = 0; k < patt->v_num; k++ ) {
00182                 x[k] = patt->point[i][k*patt->h_num+j].x_coord;
00183                 y[k] = patt->point[i][k*patt->h_num+j].y_coord;
00184             }
00185             error += check_error( x, y, patt->v_num, dist_factor );
00186         }
00187 
00188         for( j = 3 - patt->v_num; j < patt->h_num - 2; j++ ) {
00189             p = 0;
00190             for( k = 0; k < patt->v_num; k++ ) {
00191                 l = j+k;
00192                 if( l < 0 || l >= patt->h_num ) continue;
00193                 x[p] = patt->point[i][k*patt->h_num+l].x_coord;
00194                 y[p] = patt->point[i][k*patt->h_num+l].y_coord;
00195                 p++;
00196             }
00197             error += check_error( x, y, p, dist_factor );
00198         }
00199 
00200         for( j = 2; j < patt->h_num + patt->v_num - 3; j++ ) {
00201             p = 0;
00202             for( k = 0; k < patt->v_num; k++ ) {
00203                 l = j-k;
00204                 if( l < 0 || l >= patt->h_num ) continue;
00205                 x[p] = patt->point[i][k*patt->h_num+l].x_coord;
00206                 y[p] = patt->point[i][k*patt->h_num+l].y_coord;
00207                 p++;
00208             }
00209             error += check_error( x, y, p, dist_factor );
00210         }
00211     }
00212 
00213     free( x );
00214     free( y );
00215 
00216     return error;
00217 }
00218 
00219 static double check_error( double *x, double *y, int num, double dist_factor[4] )
00220 {
00221     ARMat    *input, *evec;
00222     ARVec    *ev, *mean;
00223     double   a, b, c;
00224     double   error;
00225     int      i;
00226 
00227     ev     = arVecAlloc( 2 );
00228     mean   = arVecAlloc( 2 );
00229     evec   = arMatrixAlloc( 2, 2 );
00230 
00231     input  = arMatrixAlloc( num, 2 );
00232     for( i = 0; i < num; i++ ) {
00233         arParamObserv2Ideal( dist_factor, x[i], y[i],
00234                              &(input->m[i*2+0]), &(input->m[i*2+1]) );
00235     }
00236     if( arMatrixPCA(input, evec, ev, mean) < 0 ) exit(0);
00237     a =  evec->m[1];
00238     b = -evec->m[0];
00239     c = -(a*mean->v[0] + b*mean->v[1]);
00240 
00241     error = 0.0;
00242     for( i = 0; i < num; i++ ) {
00243         error += (a*input->m[i*2+0] + b*input->m[i*2+1] + c)
00244                * (a*input->m[i*2+0] + b*input->m[i*2+1] + c);
00245     }
00246     error /= (a*a + b*b);
00247 
00248     arMatrixFree( input );
00249     arMatrixFree( evec );
00250     arVecFree( mean );
00251     arVecFree( ev );
00252 
00253     return error;
00254 }
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Properties Friends Defines


ar_recog
Author(s): Graylin Trevor Jay and Christopher Crick
autogenerated on Fri Jan 25 2013 12:15:00