00001 #include <math.h>
00002 #include <stdio.h>
00003 #include <AR/ar.h>
00004 #include <AR/param.h>
00005 #include <AR/matrix.h>
00006 #include "calib_camera.h"
00007
00008 static int calc_inp2( CALIB_PATT_T *patt, CALIB_COORD_T *screen, double *pos2d, double *pos3d,
00009 double dist_factor[4], double x0, double y0, double f[2], double *err );
00010 static void get_cpara( CALIB_COORD_T *world, CALIB_COORD_T *screen, int num, double para[3][3] );
00011 static int get_fl( double *p , double *q, int num, double f[2] );
00012 static int check_rotation( double rot[2][3] );
00013
00014 int calc_inp( CALIB_PATT_T *patt, double dist_factor[4], int xsize, int ysize, double mat[3][4] )
00015 {
00016 CALIB_COORD_T *screen, *sp;
00017 double *pos2d, *pos3d, *pp;
00018 double f[2];
00019 double x0, y0;
00020 double err, minerr;
00021 int res;
00022 int i, j, k;
00023
00024 sp = screen = (CALIB_COORD_T *)malloc(sizeof(CALIB_COORD_T) * patt->h_num * patt->v_num * patt->loop_num);
00025 pp = pos2d = (double *)malloc(sizeof(double) * patt->h_num * patt->v_num * patt->loop_num * 2);
00026 pos3d = (double *)malloc(sizeof(double) * patt->h_num * patt->v_num * 2);
00027 for( k = 0; k < patt->loop_num; k++ ) {
00028 for( j = 0; j < patt->v_num; j++ ) {
00029 for( i = 0; i < patt->h_num; i++ ) {
00030 arParamObserv2Ideal( dist_factor,
00031 patt->point[k][j*patt->h_num+i].x_coord,
00032 patt->point[k][j*patt->h_num+i].y_coord,
00033 &(sp->x_coord), &(sp->y_coord) );
00034 *(pp++) = sp->x_coord;
00035 *(pp++) = sp->y_coord;
00036 sp++;
00037 }
00038 }
00039 }
00040 pp = pos3d;
00041 for( j = 0; j < patt->v_num; j++ ) {
00042 for( i = 0; i < patt->h_num; i++ ) {
00043 *(pp++) = patt->world_coord[j*patt->h_num+i].x_coord;
00044 *(pp++) = patt->world_coord[j*patt->h_num+i].y_coord;
00045 }
00046 }
00047
00048 minerr = 100000000000000000000000.0;
00049 for( j = -50; j <= 50; j++ ) {
00050 printf("-- loop:%d --\n", j);
00051 y0 = dist_factor[1] + j;
00052
00053 if( y0 < 0 || y0 >= ysize ) continue;
00054
00055 for( i = -50; i <= 50; i++ ) {
00056 x0 = dist_factor[0] + i;
00057
00058 if( x0 < 0 || x0 >= xsize ) continue;
00059
00060 res = calc_inp2( patt, screen, pos2d, pos3d, dist_factor, x0, y0, f, &err );
00061 if( res < 0 ) continue;
00062 if( err < minerr ) {
00063 printf("F = (%f,%f), Center = (%f,%f): err = %f\n", f[0], f[1], x0, y0, err);
00064 minerr = err;
00065
00066 mat[0][0] = f[0];
00067 mat[0][1] = 0.0;
00068 mat[0][2] = x0;
00069 mat[0][3] = 0.0;
00070 mat[1][0] = 0.0;
00071 mat[1][1] = f[1];
00072 mat[1][2] = y0;
00073 mat[1][3] = 0.0;
00074 mat[2][0] = 0.0;
00075 mat[2][1] = 0.0;
00076 mat[2][2] = 1.0;
00077 mat[2][3] = 0.0;
00078 }
00079 }
00080 }
00081
00082 free( screen );
00083 free( pos2d );
00084 free( pos3d );
00085
00086 if( minerr >= 100.0 ) return -1;
00087 return 0;
00088 }
00089
00090
00091 static int calc_inp2 ( CALIB_PATT_T *patt, CALIB_COORD_T *screen, double *pos2d, double *pos3d,
00092 double dist_factor[4], double x0, double y0, double f[2], double *err )
00093 {
00094 double x1, y1, x2, y2;
00095 double p[LOOP_MAX], q[LOOP_MAX];
00096 double para[3][3];
00097 double rot[3][3], rot2[3][3];
00098 double cpara[3][4], conv[3][4];
00099 double ppos2d[4][2], ppos3d[4][2];
00100 double d, werr, werr2;
00101 int i, j, k, l;
00102
00103 for( i = 0; i < patt->loop_num; i++ ) {
00104 get_cpara( patt->world_coord, &(screen[i*patt->h_num*patt->v_num]),
00105 patt->h_num*patt->v_num, para );
00106 x1 = para[0][0] / para[2][0];
00107 y1 = para[1][0] / para[2][0];
00108 x2 = para[0][1] / para[2][1];
00109 y2 = para[1][1] / para[2][1];
00110
00111 p[i] = (x1 - x0)*(x2 - x0);
00112 q[i] = (y1 - y0)*(y2 - y0);
00113 }
00114 if( get_fl(p, q, patt->loop_num, f) < 0 ) return -1;
00115
00116 cpara[0][0] = f[0];
00117 cpara[0][1] = 0.0;
00118 cpara[0][2] = x0;
00119 cpara[0][3] = 0.0;
00120 cpara[1][0] = 0.0;
00121 cpara[1][1] = f[1];
00122 cpara[1][2] = y0;
00123 cpara[1][3] = 0.0;
00124 cpara[2][0] = 0.0;
00125 cpara[2][1] = 0.0;
00126 cpara[2][2] = 1.0;
00127 cpara[2][3] = 0.0;
00128
00129 werr = 0.0;
00130 for( i = 0; i < patt->loop_num; i++ ) {
00131 get_cpara( patt->world_coord, &(screen[i*patt->h_num*patt->v_num]),
00132 patt->h_num*patt->v_num, para );
00133 rot[0][0] = (para[0][0] - x0*para[2][0]) / f[0];
00134 rot[0][1] = (para[1][0] - y0*para[2][0]) / f[1];
00135 rot[0][2] = para[2][0];
00136 d = sqrt( rot[0][0]*rot[0][0] + rot[0][1]*rot[0][1] + rot[0][2]*rot[0][2] );
00137 rot[0][0] /= d;
00138 rot[0][1] /= d;
00139 rot[0][2] /= d;
00140 rot[1][0] = (para[0][1] - x0*para[2][1]) / f[0];
00141 rot[1][1] = (para[1][1] - y0*para[2][1]) / f[1];
00142 rot[1][2] = para[2][1];
00143 d = sqrt( rot[1][0]*rot[1][0] + rot[1][1]*rot[1][1] + rot[1][2]*rot[1][2] );
00144 rot[1][0] /= d;
00145 rot[1][1] /= d;
00146 rot[1][2] /= d;
00147 check_rotation( rot );
00148 rot[2][0] = rot[0][1]*rot[1][2] - rot[0][2]*rot[1][1];
00149 rot[2][1] = rot[0][2]*rot[1][0] - rot[0][0]*rot[1][2];
00150 rot[2][2] = rot[0][0]*rot[1][1] - rot[0][1]*rot[1][0];
00151 d = sqrt( rot[2][0]*rot[2][0] + rot[2][1]*rot[2][1] + rot[2][2]*rot[2][2] );
00152 rot[2][0] /= d;
00153 rot[2][1] /= d;
00154 rot[2][2] /= d;
00155 rot2[0][0] = rot[0][0];
00156 rot2[1][0] = rot[0][1];
00157 rot2[2][0] = rot[0][2];
00158 rot2[0][1] = rot[1][0];
00159 rot2[1][1] = rot[1][1];
00160 rot2[2][1] = rot[1][2];
00161 rot2[0][2] = rot[2][0];
00162 rot2[1][2] = rot[2][1];
00163 rot2[2][2] = rot[2][2];
00164
00165 ppos2d[0][0] = pos2d[i*patt->h_num*patt->v_num*2 + 0];
00166 ppos2d[0][1] = pos2d[i*patt->h_num*patt->v_num*2 + 1];
00167 ppos2d[1][0] = pos2d[i*patt->h_num*patt->v_num*2 + (patt->h_num-1)*2 + 0];
00168 ppos2d[1][1] = pos2d[i*patt->h_num*patt->v_num*2 + (patt->h_num-1)*2 + 1];
00169 ppos2d[2][0] = pos2d[i*patt->h_num*patt->v_num*2 + (patt->h_num*(patt->v_num-1))*2 + 0];
00170 ppos2d[2][1] = pos2d[i*patt->h_num*patt->v_num*2 + (patt->h_num*(patt->v_num-1))*2 + 1];
00171 ppos2d[3][0] = pos2d[i*patt->h_num*patt->v_num*2 + (patt->h_num*patt->v_num-1)*2 + 0];
00172 ppos2d[3][1] = pos2d[i*patt->h_num*patt->v_num*2 + (patt->h_num*patt->v_num-1)*2 + 1];
00173 ppos3d[0][0] = pos3d[0];
00174 ppos3d[0][1] = pos3d[1];
00175 ppos3d[1][0] = pos3d[(patt->h_num-1)*2 + 0];
00176 ppos3d[1][1] = pos3d[(patt->h_num-1)*2 + 1];
00177 ppos3d[2][0] = pos3d[(patt->h_num*(patt->v_num-1))*2 + 0];
00178 ppos3d[2][1] = pos3d[(patt->h_num*(patt->v_num-1))*2 + 1];
00179 ppos3d[3][0] = pos3d[(patt->h_num*patt->v_num-1)*2 + 0];
00180 ppos3d[3][1] = pos3d[(patt->h_num*patt->v_num-1)*2 + 1];
00181
00182 for( j = 0; j < 5; j++ ) {
00183 arFittingMode = AR_FITTING_TO_IDEAL;
00184 werr2 = arGetTransMat3( rot2, ppos2d, ppos3d, 4, conv, dist_factor, cpara );
00185 for( k = 0; k < 3; k++ ) {
00186 for( l = 0; l < 3; l++ ) {
00187 rot2[k][l] = conv[k][l];
00188 }}
00189 }
00190 werr += werr2;
00191
00192 }
00193 *err = sqrt( werr / patt->loop_num );
00194
00195 return 0;
00196 }
00197
00198 static void get_cpara( CALIB_COORD_T *world, CALIB_COORD_T *screen, int num, double para[3][3] )
00199 {
00200 ARMat *a, *b, *c;
00201 ARMat *at, *aa, res;
00202 int i;
00203
00204 a = arMatrixAlloc( num*2, 8 );
00205 b = arMatrixAlloc( num*2, 1 );
00206 c = arMatrixAlloc( 8, num*2 );
00207 at = arMatrixAlloc( 8, num*2 );
00208 aa = arMatrixAlloc( 8, 8 );
00209 for( i = 0; i < num; i++ ) {
00210 a->m[i*16+0] = world[i].x_coord;
00211 a->m[i*16+1] = world[i].y_coord;
00212 a->m[i*16+2] = 1.0;
00213 a->m[i*16+3] = 0.0;
00214 a->m[i*16+4] = 0.0;
00215 a->m[i*16+5] = 0.0;
00216 a->m[i*16+6] = -world[i].x_coord * screen[i].x_coord;
00217 a->m[i*16+7] = -world[i].y_coord * screen[i].x_coord;
00218 a->m[i*16+8] = 0.0;
00219 a->m[i*16+9] = 0.0;
00220 a->m[i*16+10] = 0.0;
00221 a->m[i*16+11] = world[i].x_coord;
00222 a->m[i*16+12] = world[i].y_coord;
00223 a->m[i*16+13] = 1.0;
00224 a->m[i*16+14] = -world[i].x_coord * screen[i].y_coord;
00225 a->m[i*16+15] = -world[i].y_coord * screen[i].y_coord;
00226 b->m[i*2+0] = screen[i].x_coord;
00227 b->m[i*2+1] = screen[i].y_coord;
00228 }
00229 arMatrixTrans( at, a );
00230 arMatrixMul( aa, at, a );
00231 arMatrixSelfInv( aa );
00232 arMatrixMul( c, aa, at );
00233 res.row = 8;
00234 res.clm = 1;
00235 res.m = &(para[0][0]);
00236 arMatrixMul( &res, c, b );
00237 para[2][2] = 1.0;
00238
00239 arMatrixFree( a );
00240 arMatrixFree( b );
00241 arMatrixFree( c );
00242 arMatrixFree( at );
00243 arMatrixFree( aa );
00244 }
00245
00246 static int get_fl( double *p , double *q, int num, double f[2] )
00247 {
00248 ARMat *a, *b, *c;
00249 ARMat *at, *aa, *res;
00250 int i;
00251
00252 #if 1
00253 a = arMatrixAlloc( num, 2 );
00254 b = arMatrixAlloc( num, 1 );
00255 c = arMatrixAlloc( 2, num );
00256 at = arMatrixAlloc( 2, num );
00257 aa = arMatrixAlloc( 2, 2 );
00258 res = arMatrixAlloc( 2, 1 );
00259 for( i = 0; i < num; i++ ) {
00260 a->m[i*2+0] = *(p++);
00261 a->m[i*2+1] = *(q++);
00262 b->m[i] = -1.0;
00263 }
00264 #else
00265 a = arMatrixAlloc( num-1, 2 );
00266 b = arMatrixAlloc( num-1, 1 );
00267 c = arMatrixAlloc( 2, num-1 );
00268 at = arMatrixAlloc( 2, num-1 );
00269 aa = arMatrixAlloc( 2, 2 );
00270 res = arMatrixAlloc( 2, 1 );
00271 p++; q++;
00272 for( i = 0; i < num-1; i++ ) {
00273 a->m[i*2+0] = *(p++);
00274 a->m[i*2+1] = *(q++);
00275 b->m[i] = -1.0;
00276 }
00277 #endif
00278 arMatrixTrans( at, a );
00279 arMatrixMul( aa, at, a );
00280 arMatrixSelfInv( aa );
00281 arMatrixMul( c, aa, at );
00282 arMatrixMul( res, c, b );
00283
00284 if( res->m[0] < 0 || res->m[1] < 0 ) return -1;
00285
00286 f[0] = sqrt( 1.0 / res->m[0] );
00287 f[1] = sqrt( 1.0 / res->m[1] );
00288
00289 arMatrixFree( a );
00290 arMatrixFree( b );
00291 arMatrixFree( c );
00292 arMatrixFree( at );
00293 arMatrixFree( aa );
00294 arMatrixFree( res );
00295
00296 return 0;
00297 }
00298
00299 static int check_rotation( double rot[2][3] )
00300 {
00301 double v1[3], v2[3], v3[3];
00302 double ca, cb, k1, k2, k3, k4;
00303 double a, b, c, d;
00304 double p1, q1, r1;
00305 double p2, q2, r2;
00306 double p3, q3, r3;
00307 double p4, q4, r4;
00308 double w;
00309 double e1, e2, e3, e4;
00310 int f;
00311
00312 v1[0] = rot[0][0];
00313 v1[1] = rot[0][1];
00314 v1[2] = rot[0][2];
00315 v2[0] = rot[1][0];
00316 v2[1] = rot[1][1];
00317 v2[2] = rot[1][2];
00318 v3[0] = v1[1]*v2[2] - v1[2]*v2[1];
00319 v3[1] = v1[2]*v2[0] - v1[0]*v2[2];
00320 v3[2] = v1[0]*v2[1] - v1[1]*v2[0];
00321 w = sqrt( v3[0]*v3[0]+v3[1]*v3[1]+v3[2]*v3[2] );
00322 if( w == 0.0 ) return -1;
00323 v3[0] /= w;
00324 v3[1] /= w;
00325 v3[2] /= w;
00326
00327 cb = v1[0]*v2[0] + v1[1]*v2[1] + v1[2]*v2[2];
00328 if( cb < 0 ) cb *= -1.0;
00329 ca = (sqrt(cb+1.0) + sqrt(1.0-cb)) * 0.5;
00330
00331 if( v3[1]*v1[0] - v1[1]*v3[0] != 0.0 ) {
00332 f = 0;
00333 }
00334 else {
00335 if( v3[2]*v1[0] - v1[2]*v3[0] != 0.0 ) {
00336 w = v1[1]; v1[1] = v1[2]; v1[2] = w;
00337 w = v3[1]; v3[1] = v3[2]; v3[2] = w;
00338 f = 1;
00339 }
00340 else {
00341 w = v1[0]; v1[0] = v1[2]; v1[2] = w;
00342 w = v3[0]; v3[0] = v3[2]; v3[2] = w;
00343 f = 2;
00344 }
00345 }
00346 if( v3[1]*v1[0] - v1[1]*v3[0] == 0.0 ) return -1;
00347 k1 = (v1[1]*v3[2] - v3[1]*v1[2]) / (v3[1]*v1[0] - v1[1]*v3[0]);
00348 k2 = (v3[1] * ca) / (v3[1]*v1[0] - v1[1]*v3[0]);
00349 k3 = (v1[0]*v3[2] - v3[0]*v1[2]) / (v3[0]*v1[1] - v1[0]*v3[1]);
00350 k4 = (v3[0] * ca) / (v3[0]*v1[1] - v1[0]*v3[1]);
00351
00352 a = k1*k1 + k3*k3 + 1;
00353 b = k1*k2 + k3*k4;
00354 c = k2*k2 + k4*k4 - 1;
00355
00356 d = b*b - a*c;
00357 if( d < 0 ) return -1;
00358 r1 = (-b + sqrt(d))/a;
00359 p1 = k1*r1 + k2;
00360 q1 = k3*r1 + k4;
00361 r2 = (-b - sqrt(d))/a;
00362 p2 = k1*r2 + k2;
00363 q2 = k3*r2 + k4;
00364 if( f == 1 ) {
00365 w = q1; q1 = r1; r1 = w;
00366 w = q2; q2 = r2; r2 = w;
00367 w = v1[1]; v1[1] = v1[2]; v1[2] = w;
00368 w = v3[1]; v3[1] = v3[2]; v3[2] = w;
00369 f = 0;
00370 }
00371 if( f == 2 ) {
00372 w = p1; p1 = r1; r1 = w;
00373 w = p2; p2 = r2; r2 = w;
00374 w = v1[0]; v1[0] = v1[2]; v1[2] = w;
00375 w = v3[0]; v3[0] = v3[2]; v3[2] = w;
00376 f = 0;
00377 }
00378
00379 if( v3[1]*v2[0] - v2[1]*v3[0] != 0.0 ) {
00380 f = 0;
00381 }
00382 else {
00383 if( v3[2]*v2[0] - v2[2]*v3[0] != 0.0 ) {
00384 w = v2[1]; v2[1] = v2[2]; v2[2] = w;
00385 w = v3[1]; v3[1] = v3[2]; v3[2] = w;
00386 f = 1;
00387 }
00388 else {
00389 w = v2[0]; v2[0] = v2[2]; v2[2] = w;
00390 w = v3[0]; v3[0] = v3[2]; v3[2] = w;
00391 f = 2;
00392 }
00393 }
00394 if( v3[1]*v2[0] - v2[1]*v3[0] == 0.0 ) return -1;
00395 k1 = (v2[1]*v3[2] - v3[1]*v2[2]) / (v3[1]*v2[0] - v2[1]*v3[0]);
00396 k2 = (v3[1] * ca) / (v3[1]*v2[0] - v2[1]*v3[0]);
00397 k3 = (v2[0]*v3[2] - v3[0]*v2[2]) / (v3[0]*v2[1] - v2[0]*v3[1]);
00398 k4 = (v3[0] * ca) / (v3[0]*v2[1] - v2[0]*v3[1]);
00399
00400 a = k1*k1 + k3*k3 + 1;
00401 b = k1*k2 + k3*k4;
00402 c = k2*k2 + k4*k4 - 1;
00403
00404 d = b*b - a*c;
00405 if( d < 0 ) return -1;
00406 r3 = (-b + sqrt(d))/a;
00407 p3 = k1*r3 + k2;
00408 q3 = k3*r3 + k4;
00409 r4 = (-b - sqrt(d))/a;
00410 p4 = k1*r4 + k2;
00411 q4 = k3*r4 + k4;
00412 if( f == 1 ) {
00413 w = q3; q3 = r3; r3 = w;
00414 w = q4; q4 = r4; r4 = w;
00415 w = v2[1]; v2[1] = v2[2]; v2[2] = w;
00416 w = v3[1]; v3[1] = v3[2]; v3[2] = w;
00417 f = 0;
00418 }
00419 if( f == 2 ) {
00420 w = p3; p3 = r3; r3 = w;
00421 w = p4; p4 = r4; r4 = w;
00422 w = v2[0]; v2[0] = v2[2]; v2[2] = w;
00423 w = v3[0]; v3[0] = v3[2]; v3[2] = w;
00424 f = 0;
00425 }
00426
00427 e1 = p1*p3+q1*q3+r1*r3; if( e1 < 0 ) e1 = -e1;
00428 e2 = p1*p4+q1*q4+r1*r4; if( e2 < 0 ) e2 = -e2;
00429 e3 = p2*p3+q2*q3+r2*r3; if( e3 < 0 ) e3 = -e3;
00430 e4 = p2*p4+q2*q4+r2*r4; if( e4 < 0 ) e4 = -e4;
00431 if( e1 < e2 ) {
00432 if( e1 < e3 ) {
00433 if( e1 < e4 ) {
00434 rot[0][0] = p1;
00435 rot[0][1] = q1;
00436 rot[0][2] = r1;
00437 rot[1][0] = p3;
00438 rot[1][1] = q3;
00439 rot[1][2] = r3;
00440 }
00441 else {
00442 rot[0][0] = p2;
00443 rot[0][1] = q2;
00444 rot[0][2] = r2;
00445 rot[1][0] = p4;
00446 rot[1][1] = q4;
00447 rot[1][2] = r4;
00448 }
00449 }
00450 else {
00451 if( e3 < e4 ) {
00452 rot[0][0] = p2;
00453 rot[0][1] = q2;
00454 rot[0][2] = r2;
00455 rot[1][0] = p3;
00456 rot[1][1] = q3;
00457 rot[1][2] = r3;
00458 }
00459 else {
00460 rot[0][0] = p2;
00461 rot[0][1] = q2;
00462 rot[0][2] = r2;
00463 rot[1][0] = p4;
00464 rot[1][1] = q4;
00465 rot[1][2] = r4;
00466 }
00467 }
00468 }
00469 else {
00470 if( e2 < e3 ) {
00471 if( e2 < e4 ) {
00472 rot[0][0] = p1;
00473 rot[0][1] = q1;
00474 rot[0][2] = r1;
00475 rot[1][0] = p4;
00476 rot[1][1] = q4;
00477 rot[1][2] = r4;
00478 }
00479 else {
00480 rot[0][0] = p2;
00481 rot[0][1] = q2;
00482 rot[0][2] = r2;
00483 rot[1][0] = p4;
00484 rot[1][1] = q4;
00485 rot[1][2] = r4;
00486 }
00487 }
00488 else {
00489 if( e3 < e4 ) {
00490 rot[0][0] = p2;
00491 rot[0][1] = q2;
00492 rot[0][2] = r2;
00493 rot[1][0] = p3;
00494 rot[1][1] = q3;
00495 rot[1][2] = r3;
00496 }
00497 else {
00498 rot[0][0] = p2;
00499 rot[0][1] = q2;
00500 rot[0][2] = r2;
00501 rot[1][0] = p4;
00502 rot[1][1] = q4;
00503 rot[1][2] = r4;
00504 }
00505 }
00506 }
00507
00508 return 0;
00509 }
