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


tuw_artoolkitplus
Author(s): Markus Bader
autogenerated on Sun May 29 2016 02:50:12