|
algebraic lens distortion
|
|
algebraic lens distortion
|
00001 /* 00002 * Copyright 2009, 2010 IPOL Image Processing On Line http://www.ipol.im/ 00003 * 00004 * This program is free software: you can redistribute it and/or modify 00005 * it under the terms of the GNU General Public License as published by 00006 * the Free Software Foundation, either version 3 of the License, or 00007 * (at your option) any later version. 00008 * 00009 * This program is distributed in the hope that it will be useful, 00010 * but WITHOUT ANY WARRANTY; without even the implied warranty of 00011 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 00012 * GNU General Public License for more details. 00013 * 00014 * You should have received a copy of the GNU General Public License 00015 * along with this program. If not, see <http://www.gnu.org/licenses/>. 00016 */ 00017 00036 #include "lens_distortion.h" 00037 00041 int n_iterations=0; 00042 int f_evaluations=0; 00043 double tol_lambda= 1e-8; 00052 int test_compatibility_lens_distortion_model(double *a,int Na,double max_radius) 00053 { 00054 int k; 00055 double r,*b;/* AUXILIAR VARIABLES */ 00056 /* WE INIT TO NULL ALL POINTERS */ 00057 b=NULL; 00058 00059 /* WE ALLOCATE MEMORY */ 00060 b=(double*)malloc( sizeof(double)*(Na+1) ); 00061 // WE BUILD THE DERIVATIVE OF THE POLYNOMIAL 00062 for(k=1;k<=Na;k++) b[k]=(k+1)*a[k]; 00063 for(r=1;r<max_radius;r+=1.){ 00064 if( ami_polynomial_evaluation(b,Na,r)<0 ) {free(b); return(-1.);} 00065 } 00066 free(b); 00067 return(0); 00068 00069 } 00070 00071 00079 int ami_line2d_calculation( 00080 double line[3] , 00081 double **Points2D , 00082 int N 00083 ) 00084 { 00085 int i,j,k; /* AUXILIAR VARIABLES */ 00086 double suu,suv,svv,um,vm,h,r[4][3],min,aux,norm; /* AUXILIAR VARIABLES */ 00087 double zero=0.00000000000001; /* AUXILIAR VARIABLES */ 00088 00089 if(N<2) { 00090 printf("Number of point to calculate the line must be >2\n"); 00091 return(-1); 00092 } 00093 00094 suu=0; 00095 suv=0; 00096 svv=0; 00097 um=0; 00098 vm=0; 00099 00100 for(i=0; i<N; i++) { 00101 um+=Points2D[i][0]; 00102 vm+=Points2D[i][1]; 00103 } 00104 00105 um/=N; 00106 vm/=N; 00107 00108 for(i=0; i<N; i++) { 00109 suu+=(Points2D[i][0]-um)*(Points2D[i][0]-um); 00110 svv+=(Points2D[i][1]-vm)*(Points2D[i][1]-vm); 00111 suv+=(Points2D[i][0]-um)*(Points2D[i][1]-vm); 00112 } 00113 00114 suu/=N; 00115 svv/=N; 00116 suv/=N; 00117 00118 if(fabs(suv)<= zero) { 00119 if(suu<svv && svv>zero) { 00120 line[0]=1; 00121 line[1]=0; 00122 line[2]=-um; 00123 return(0); 00124 } 00125 if(svv<suu && suu>zero) { 00126 line[0]=0; 00127 line[1]=1; 00128 line[2]=-vm; 00129 return(0); 00130 } 00131 printf(" It is not possible to calculate the 2D line\n"); 00132 return(-1); 00133 } 00134 00135 r[2][1]=r[3][1]=r[0][0]=r[1][0]=1.; 00136 h=0.5*(suu-svv)/suv; 00137 00138 if(h>0) { 00139 r[0][1]=-h-sqrt(1.+h*h); 00140 r[0][2]=-(um+r[0][1]*vm); 00141 r[1][1]=-1./r[0][1]; 00142 r[1][2]=-(um+r[1][1]*vm); 00143 r[2][0]=h+sqrt(1.+h*h); 00144 r[2][2]=-(r[2][0]*um+vm); 00145 r[3][0]=-1./r[2][0]; 00146 r[3][2]=-(r[3][0]*um+vm); 00147 } 00148 else { 00149 r[0][1]=-h+sqrt(1+h*h); 00150 r[0][2]=-(um+r[0][1]*vm); 00151 r[1][1]=-1./r[0][1]; 00152 r[1][2]=-(um+r[1][1]*vm); 00153 r[2][0]=h-sqrt(1+h*h); 00154 r[2][2]=-(r[2][0]*um+vm); 00155 r[3][0]=-1./r[2][0]; 00156 r[3][2]=-(r[3][0]*um+vm); 00157 } 00158 00159 for(j=0; j<4; j++) { 00160 norm=sqrt(r[j][0]*r[j][0]+r[j][1]*r[j][1]); 00161 for(i=0; i<3; i++) 00162 r[j][i]/=norm; 00163 } 00164 00165 min=0.; 00166 k=0; 00167 00168 for(i=0; i<N; i++) { 00169 aux=r[0][0]*Points2D[i][0]+r[0][1]*Points2D[i][1]+r[0][2]; 00170 min+=fabs(aux); 00171 } 00172 00173 for(j=1; j<4; j++) { 00174 h=0; 00175 for(i=0; i<N; i++) { 00176 aux=r[j][0]*Points2D[i][0]+r[j][1]*Points2D[i][1]+r[j][2]; 00177 h+=fabs(aux); 00178 } 00179 if(h<min) { 00180 k=j; 00181 min=h; 00182 } 00183 } 00184 00185 line[0]=r[k][0]; 00186 line[1]=r[k][1]; 00187 line[2]=r[k][2]; 00188 return(0); 00189 } 00190 00191 00192 00203 int ami_lens_distortion_polynomial_update_distance_2v( 00204 double *x, double *y, 00205 int Np , 00206 double *a , 00207 int Na , 00208 double x0,double y0 , 00209 int k1 , 00210 int k2 , 00211 double **pol , 00212 double alpha) 00213 { 00214 int i,j,k; /* AUXILIAR VARIABLES */ 00215 double *A,*x2,*y2,*d1,*d2; /* AUXILIAR VARIABLES */ 00216 double aux,**pol1,**pol2,**p_xx,**p_xy,**p_yy; /* AUXILIAR VARIABLES */ 00217 00218 A=x2=y2=d1=d2=NULL; 00219 pol1=pol2=p_xx=p_xy=p_yy=NULL; 00220 00221 /* WE CHECK alpha VALUE */ 00222 if(alpha==0) return(0); 00223 00224 /* WE ALLOCATE MEMORY */ 00225 A=(double*)malloc( sizeof(double)*Np ); 00226 x2=(double*)malloc( sizeof(double)*Np ); 00227 y2=(double*)malloc( sizeof(double)*Np ); 00228 d1=(double*)malloc( sizeof(double)*Np ); 00229 d2=(double*)malloc( sizeof(double)*Np ); 00230 ami_calloc2d(pol1,double,2,2); 00231 ami_calloc2d(pol2,double,2,2); 00232 ami_calloc2d(p_xx,double,3,3); 00233 ami_calloc2d(p_xy,double,3,3); 00234 ami_calloc2d(p_yy,double,3,3); 00235 00236 /* WE COMPUTE THE DISTANCE TO THE IMAGE CENTER */ 00237 for(i=0; i<Np; i++) 00238 d1[i]=sqrt( (x[i]-x0)*(x[i]-x0)+(y[i]-y0)*(y[i]-y0) ); 00239 00240 /* WE COMPUTE THE POINT TRANSFORMATION WITH THE CURRENT LENS DISTORTION MODEL */ 00241 for(i=0; i<Np; i++) { 00242 A[i]=ami_polynomial_evaluation(a,Na,d1[i]); 00243 x2[i]=x0+(x[i]-x0)*A[i]; 00244 y2[i]=y0+(y[i]-y0)*A[i]; 00245 } 00246 /* WE COMPUTE THE POLYNOMS CORRESPONDING TO THE DISTANCE ERROR */ 00247 for(i=0; i<=2; i++) for(j=0; j<=2; j++) p_xx[i][j]=0.; 00248 00249 for(i=0; i<Np; i++) { 00250 aux=0; 00251 for(k=1; k<=Na; k++) if(k!=k1 && k!=k2) aux+=a[k]*pow(d1[i],(double) k); 00252 pol1[0][0]=aux*d1[i]; 00253 pol1[1][0]=pow(d1[i],(double) k1+1); 00254 pol1[0][1]=pow(d1[i],(double) k2+1); 00255 ami_2v_polynom_multiplication(pol1,1,pol1,1,p_xx); 00256 } 00257 00258 for(i=0; i<=2; i++) for(j=0; j<=2; j++) p_xx[i][j]=alpha*p_xx[i][j]/Np; 00259 00260 /* WE UPDATE THE ERROR POLYNOM */ 00261 ami_2v_polynom_multiplication(p_xx,2,p_xx,2,pol); 00262 00263 /* WE FREE THE MEMORY */ 00264 if(A!=NULL) free(A); 00265 if(x2!=NULL)free(x2); 00266 if(y2!=NULL) free(y2); 00267 if(d1!=NULL) free(d1); 00268 if(d2!=NULL) free(d2); 00269 if(p_xx!=NULL) ami_free2d(p_xx); 00270 if(p_xy!=NULL) ami_free2d(p_xy); 00271 if(p_yy!=NULL)ami_free2d(p_yy); 00272 if(pol1!=NULL)ami_free2d(pol1); 00273 if(pol2!=NULL) ami_free2d(pol2); 00274 00275 return(0); 00276 } 00277 00278 00289 double ami_lens_distortion_estimation_2v( 00290 double **x,double **y , 00291 int Nl , 00292 int *Np , 00293 double x0,double y0 , 00294 double *a , 00295 int Na , 00296 int k1 , 00297 int k2 , 00298 double alpha , 00299 double max_radius 00300 ) 00301 { 00302 int i,k,m; /* AUXILIAR VARIABLES */ 00303 double **pol_v2,d,sum_Ad,sum_dd,A,Error=0; /* AUXILIAR VARIABLES */ 00304 00305 /* WE ALLOCATE MEMORY */ 00306 ami_calloc2d(pol_v2,double,5,5); 00307 00308 /* WE UPDATE a[0] BY MINIMIZING THE DISTANCE OF THE DISTORTED POINTS TO 00309 THE UNDISTORTED POINTS */ 00310 if(alpha>0) { 00311 sum_dd=sum_Ad=0; 00312 for(m=0; m<Nl; m++) { 00313 for(i=0; i<Np[m]; i++) { 00314 d=sqrt( (x[m][i]-x0)*(x[m][i]-x0)+(y[m][i]-y0)*(y[m][i]-y0) ); 00315 A=0; 00316 for(k=1; k<=Na; k++) A+=a[k]*pow(d,(double) k+1); 00317 sum_dd+=d*d; 00318 sum_Ad+=A*d; 00319 } 00320 } 00321 a[0]=1-sum_Ad/sum_dd; 00322 } 00323 00324 for(m=0; m<Nl; m++) { 00325 /* WE UPDATE DE POLYNOM TO MINIMIZE */ 00326 ami_lens_distortion_polynomial_update_2v(x[m],y[m],Np[m],a,Na,x0,y0,k1,k2,pol_v2); 00327 ami_lens_distortion_polynomial_update_distance_2v(x[m],y[m],Np[m],a,Na,x0,y0,k1,k2,pol_v2,alpha); 00328 } 00329 00330 /* WE UPDATE THE POLYNOMIAL LENS DISTORTION MODEL */ 00331 ami_lens_distortion_model_update_2v(a,Na,k1,k2,pol_v2,max_radius); 00332 ami_free2d(pol_v2); 00333 for(m=0; m<Nl; m++) 00334 Error+=ami_LensDistortionEnergyError(x[m],y[m],Np[m],x0,y0,a,Na); 00335 return(Error/Nl); 00336 } 00337 00346 int ami_lens_distortion_model_update_2v( 00347 double *a , 00348 int Na , 00349 int k1 , 00350 int k2 , 00351 double **pol , 00352 double max_radius 00353 ) 00354 { 00355 int j,i,M,Nr=0,m=Na; /* AUXILIAR VARIABLES */ 00356 double *x,**pol_x,**pol_y,*pol_r,xr,yr,Emin,*rx,*ry,*b2,*p3,*b;/* AUXILIAR VARIABLES */ 00357 double sx,sy,Energy; /* NORMALIZATION FACTORS */ /* AUXILIAR VARIABLES */ 00358 double p_r3[6][6][19]; /* AUXILIAR VARIABLES */ 00359 00360 x=pol_r=rx=ry=b2=p3=NULL; /*Added by Luis Gomez*/ 00361 pol_x=pol_y=NULL; /*Added by Luis Gomez*/ 00362 00363 for(i=0; i<6; i++) for(j=0; j<6; j++) for(m=0; m<19; m++) p_r3[i][j][m]=0.; 00364 00365 /* WE ALLOCATE MEMORY */ 00366 x=(double*)malloc( sizeof(double)*3 ); 00367 ami_calloc2d(pol_x,double,5,5); 00368 ami_calloc2d(pol_y,double,5,5); 00369 p3=(double*) malloc(sizeof(double)*5); 00370 b=(double*)malloc( sizeof(double)*(Na+1) ); 00371 // WE FILL THE AUXILIARY LENS DISTORTION MODEL 00372 for(i=0;i<=Na;i++) b[i]=a[i]; 00373 00374 /* WE NORMALIZE POLYNOM COEFFICIENT */ 00375 sx=pow(pol[4][0],(double) 0.25); 00376 sy=pow(pol[0][4],(double) 0.25); 00377 for(i=0; i<=4; i++) { 00378 for(j=0; j<=4; j++) { 00379 if(i>0) pol[i][j]/=pow(sx,(double) i); 00380 if(j>0) pol[i][j]/=pow(sy,(double) j); 00381 } 00382 } 00383 00384 /* WE COMPUTE THE DERIVATIVES OF THE POLYNOM */ 00385 ami_2v_polynom_derivatives(pol,4,pol_x, pol_y); 00386 00387 /* WE FILL THE MATRIX TO COMPUTE THE DETERMINANT */ 00388 for(i=0; i<=3; i++) { 00389 for(m=0; m<=4; m++) { 00390 p_r3[2][i+2][m]=p_r3[1][i+1][m]=p_r3[0][i][m]=pol_x[3-i][m]; 00391 p_r3[5][i+2][m]=p_r3[4][i+1][m]=p_r3[3][i][m]=pol_y[3-i][m]; 00392 } 00393 } 00394 /* WE COMPUTE THE RESOLVENT POLYNOM */ 00395 pol_r=(double*) malloc(sizeof(double)*19); 00396 ami_polynom_determinant(p_r3,18,6,pol_r); 00397 00398 /* WE COMPUTE THE RESOLVENT POLYNOM DEGREE */ 00399 for(i=0; i<=18; i++) { 00400 if(pol_r[i]!=0) Nr=i; 00401 } 00402 /* WE COMPUTE THE ROOT OF THE RESOLVENT POLYNOM */ 00403 rx=(double*) malloc(sizeof(double)*Nr); 00404 ry=(double*) malloc(sizeof(double)*Nr); 00405 b2=(double*) malloc(sizeof(double)*(Nr+1)); 00406 for(i=0; i<=Nr; i++) b2[i]=pol_r[Nr-i]; 00407 for(i=0; i<Nr; i++) { 00408 rx[i]=0.0; /*Added by Luis Gomez*/ 00409 ry[i]=0.0; 00410 } 00411 Nr=ami_polynomial_root(b2,Nr,rx,ry); 00412 /* WE COMPUTE THE X COMPONENT BY REPLACING THE ROOTS IN THE DERIVATIVES 00413 OF THE POLYNOM */ 00414 xr=0; 00415 yr=0; 00416 Emin=10e90; 00417 for(i=0; i<Nr; i++) { 00418 if(fabs(ry[i])> 0.000000000000001) continue; 00419 ami_2v_polynom_to_1v_polynom(pol_x,4,p3,rx[i],1); 00420 M=ami_RootCubicPolynomial(p3,3,x); 00421 for(m=0; m<M; m++) { 00422 Energy=ami_2v_polynom_evaluation(pol,4,x[m],rx[i]); 00423 if(Energy<Emin) { 00424 b[k1]=a[k1]+(x[m]/sx); 00425 b[k2]=a[k2]+(rx[i]/sy); 00426 if(test_compatibility_lens_distortion_model(b,Na,max_radius)==0){ 00427 Emin=Energy; 00428 xr=rx[i]; 00429 yr=x[m]; 00430 } 00431 } 00432 } 00433 ami_2v_polynom_to_1v_polynom(pol_y,4,p3,rx[i],1); 00434 M=ami_RootCubicPolynomial(p3,3,x); 00435 for(m=0; m<M; m++) { 00436 Energy=ami_2v_polynom_evaluation(pol,4,x[m],rx[i]); 00437 if(Energy<Emin) { 00438 b[k1]=a[k1]+(x[m]/sx); 00439 b[k2]=a[k2]+(rx[i]/sy); 00440 if(test_compatibility_lens_distortion_model(b,Na,max_radius)==0){ 00441 Emin=Energy; 00442 xr=rx[i]; 00443 yr=x[m]; 00444 } 00445 } 00446 } 00447 } 00448 /* WE UPDATE THE DISTORSION POLYNOMIAL MODEL */ 00449 a[k1]+=(yr/sx); 00450 a[k2]+=(xr/sy); 00451 00452 /* WE FREE THE MEMORY */ 00453 if(x!=NULL) free(x); 00454 if(pol_x!=NULL) ami_free2d(pol_x); 00455 if(pol_y!=NULL) ami_free2d(pol_y); 00456 if(pol_r!=NULL) free(pol_r); 00457 if(p3!=NULL) free(p3); 00458 if(rx!=NULL) free(rx); 00459 if(ry!=NULL) free(ry); 00460 if(b2!=NULL) free(b2); 00461 if(b!=NULL) free(b); 00462 return(0); 00463 } 00464 00465 00474 int ami_lens_distortion_polynomial_update_2v( 00475 double *x, double *y , 00476 int Np , 00477 double *a , 00478 int Na , 00479 double x0,double y0 , 00480 int k1 , 00481 int k2 , 00482 double **pol 00483 ) 00484 { 00485 int i,j; /* AUXILIAR VARIABLES */ 00486 double *A,*x2,*y2,*d1,*d2,x2_m,y2_m,s_xx,s_yy,xA_m; /* AUXILIAR VARIABLES */ 00487 double xd1_m,yA_m,yd1_m,xd2_m,yd2_m; /* AUXILIAR VARIABLES */ 00488 double paso,**pol1,**pol2,**p_xx,**p_xy,**p_yy; /* AUXILIAR VARIABLES */ 00489 00490 A=x2=y2=d1=d2=NULL; 00491 pol1=pol2=p_xx=p_xy=p_yy=NULL; 00492 00493 /* WE ALLOCATE MEMORY */ 00494 A=(double*)malloc( sizeof(double)*Np ); 00495 x2=(double*)malloc( sizeof(double)*Np ); 00496 y2=(double*)malloc( sizeof(double)*Np ); 00497 d1=(double*)malloc( sizeof(double)*Np ); 00498 d2=(double*)malloc( sizeof(double)*Np ); 00499 ami_calloc2d(pol1,double,2,2); 00500 ami_calloc2d(pol2,double,2,2); 00501 ami_calloc2d(p_xx,double,3,3); 00502 ami_calloc2d(p_xy,double,3,3); 00503 ami_calloc2d(p_yy,double,3,3); 00504 00505 /* WE COMPUTE THE DISTANCE TO THE IMAGE CENTER */ 00506 for(i=0; i<Np; i++) 00507 d1[i]=sqrt( (x[i]-x0)*(x[i]-x0)+(y[i]-y0)*(y[i]-y0) ); 00508 00509 /* WE COMPUTE THE POINT TRANSFORMATION WITH THE CURRENT LENS DISTORTION MODEL */ 00510 for(i=0; i<Np; i++) { 00511 A[i]=ami_polynomial_evaluation(a,Na,d1[i]); 00512 x2[i]=x0+(x[i]-x0)*A[i]; 00513 y2[i]=y0+(y[i]-y0)*A[i]; 00514 } 00515 00516 /* WE COMPUTE THE DISTANCE POWER k1 AND k2 (THE COEFFICIENT OF THE LENS 00517 DISTORTION MODEL TO BE UPDATED */ 00518 for(i=0; i<Np; i++) { 00519 paso=d1[i]; 00520 d1[i]=pow(paso,(double) k1); 00521 d2[i]=pow(paso,(double) k2); 00522 } 00523 00524 /* WE COMPUTE THE VARIANCE OF THE TRANSFORMED POINTS */ 00525 x2_m=0; 00526 for(i=0; i<Np; i++) x2_m+=x2[i]; 00527 x2_m/=Np; 00528 s_xx=0; 00529 for(i=0; i<Np; i++) s_xx+=(x2[i]-x2_m)*(x2[i]-x2_m); 00530 s_xx/=Np; 00531 y2_m=0; 00532 for(i=0; i<Np; i++) y2_m+=y2[i]; 00533 y2_m/=Np; 00534 s_yy=0; 00535 for(i=0; i<Np; i++) s_yy+=(y2[i]-y2_m)*(y2[i]-y2_m); 00536 s_yy/=Np; 00537 00538 /* WE COMPUTE SOME AVERAGES WE NEED */ 00539 xA_m=0; 00540 for(i=0; i<Np; i++) xA_m+=(x[i]-x0)*A[i]; 00541 xA_m/=Np; 00542 xd1_m=0; 00543 for(i=0; i<Np; i++) xd1_m+=(x[i]-x0)*d1[i]; 00544 xd1_m/=Np; 00545 xd2_m=0; 00546 for(i=0; i<Np; i++) xd2_m+=(x[i]-x0)*d2[i]; 00547 xd2_m/=Np; 00548 yA_m=0; 00549 for(i=0; i<Np; i++) yA_m+=(y[i]-y0)*A[i]; 00550 yA_m/=Np; 00551 yd1_m=0; 00552 for(i=0; i<Np; i++) yd1_m+=(y[i]-y0)*d1[i]; 00553 yd1_m/=Np; 00554 yd2_m=0; 00555 for(i=0; i<Np; i++) yd2_m+=(y[i]-y0)*d2[i]; 00556 yd2_m/=Np; 00557 00558 /* WE COMPUTE THE POLYNOMS OF THE SECOND ORDER MOMENT OF THE POINT 00559 p_xx p_xy AND p_yy DISTRIBUTION */ 00560 for(i=0; i<Np; i++) { 00561 pol1[0][0]=(x[i]-x0)*A[i]-xA_m; 00562 pol1[1][0]=(x[i]-x0)*d1[i]-xd1_m; 00563 pol1[0][1]=(x[i]-x0)*d2[i]-xd2_m; 00564 pol2[0][0]=(y[i]-y0)*A[i]-yA_m; 00565 pol2[1][0]=(y[i]-y0)*d1[i]-yd1_m; 00566 pol2[0][1]=(y[i]-y0)*d2[i]-yd2_m; 00567 ami_2v_polynom_multiplication(pol1,1,pol1,1,p_xx); 00568 ami_2v_polynom_multiplication(pol1,1,pol2,1,p_xy); 00569 ami_2v_polynom_multiplication(pol2,1,pol2,1,p_yy); 00570 } 00571 for(i=0; i<=2; i++) for(j=0; j<=2; j++) p_xx[i][j]/=1. /*s_max*/ ; 00572 ami_2v_polynom_multiplication(p_xx,2,p_yy,2,pol); 00573 for(i=0; i<=2; i++) for(j=0; j<=2; j++) p_xx[i][j]=-p_xy[i][j]/1. /*s_max*/; 00574 ami_2v_polynom_multiplication(p_xy,2,p_xx,2,pol); 00575 00576 /* WE FREE THE MEMORY */ 00577 if(A!=NULL) free(A); 00578 if(x2!=NULL) free(x2); 00579 if(y2!=NULL) free(y2); 00580 if(d1!=NULL) free(d1); 00581 if(d2!=NULL) free(d2); 00582 ami_free2d(p_xx); 00583 ami_free2d(p_xy); 00584 ami_free2d(p_yy); 00585 ami_free2d(pol1); 00586 ami_free2d(pol2); 00587 00588 return(0); 00589 } 00590 00600 void ami_2v_polynom_derivatives( 00601 double **p , 00602 int N , 00603 double **p_x , 00604 double **p_y 00605 ) 00606 { 00607 int i,j; /* AUXILIAR VARIABLES */ 00608 00609 for(i=0; i<=N; i++) 00610 for(j=0; j<=N; j++) 00611 p_x[i][j]=p_y[i][j]=0; 00612 00613 for(i=1; i<=N; i++) 00614 for(j=0; j<=N; j++) 00615 p_x[i-1][j]=i*p[i][j]; 00616 00617 for(i=0; i<=N; i++) 00618 for(j=1; j<=N; j++) 00619 p_y[i][j-1]=j*p[i][j]; 00620 00621 } 00622 00623 00630 void ami_polynom_determinant( 00631 double p[6][6][19], 00632 int Np, 00633 int Nd, 00634 double *q 00635 ) 00636 { 00637 int i,j,k,l,m,cont; /* AUXILIAR VARIABLES */ 00638 double *q2; /* AUXILIAR VARIABLES */ 00639 double p2[6][6][19]; /* AUXILIAR VARIABLES */ 00640 00641 q2=NULL; 00642 00643 if(Nd==1) { 00644 for(i=0; i<=18; i++) q[i]=p[0][0][i]; 00645 return; 00646 } 00647 00648 for(i=0; i<6; i++) for(j=0; j<6; j++) for(m=0; m<19; m++) p2[i][j][m]=0.; 00649 q2=(double*)malloc(sizeof(double)* (Np+1)); 00650 00651 for(i=0; i<=Np; i++) q[i]=0; 00652 cont=-1; 00653 for(i=0; i<Nd; i++) { 00654 for(k=0; k<=Np; k++) q2[k]=0; 00655 cont*=-1; 00656 for(k=0; k<(Nd-1); k++) { 00657 for(l=0; l<(Nd-1); l++) { 00658 for(m=0; m<=Np; m++) { 00659 p2[k][l][m]= p[k+1][l>=i?l+1:l][m]; 00660 } 00661 } 00662 } 00663 ami_polynom_determinant(p2,Np,Nd-1,q2); 00664 if(cont<0) for(m=0; m<=Np; m++) q2[m]=-q2[m]; 00665 q=ami_1v_polynom_multiplication(p[0][i],Np,q2,Np,q); 00666 } 00667 if(q2!=NULL) free(q2); 00668 } 00669 00676 double ami_2v_polynom_evaluation( 00677 double **p1 , 00678 int N1 , 00679 double x,double y 00680 ) 00681 { 00682 int i,j; /* AUXILIAR VARIABLES */ 00683 double *p,*q,eval; /* AUXILIAR VARIABLES */ 00684 00685 p=q=NULL; 00686 00687 p=(double*)malloc(sizeof(double)*(N1+1)); 00688 q=(double*)malloc(sizeof(double)*(N1+1)); 00689 00690 for(i=0; i<=N1; i++) { 00691 for(j=0; j<=N1; j++) p[j]=p1[i][j]; 00692 q[i]=ami_polynomial_evaluation(p,N1,y); 00693 } 00694 eval=ami_polynomial_evaluation(q,N1,x); 00695 if(p!=NULL) free(p); 00696 if(q!=NULL) free(q); 00697 return(eval); 00698 } 00699 00708 void ami_2v_polynom_to_1v_polynom( 00709 double **p1 , 00710 int N1 , 00711 double *p3 , 00712 double z , 00713 int flat) 00714 { 00715 int i,j; /* AUXILIAR VARIABLES */ 00716 double *p; /* AUXILIAR VARIABLES */ 00717 00718 p=NULL; 00719 p=(double*)malloc(sizeof(double)*(N1+1)); 00720 if(flat==1) { 00721 for(i=0; i<=N1; i++) { 00722 for(j=0; j<=N1; j++) p[j]=p1[i][j]; 00723 p3[i]=ami_polynomial_evaluation(p,N1,z); 00724 } 00725 } 00726 else { 00727 for(i=0; i<=N1; i++) { 00728 for(j=0; j<=N1; j++) p[j]=p1[j][i]; 00729 p3[i]=ami_polynomial_evaluation(p,N1,z); 00730 } 00731 } 00732 if(p!=NULL) free(p); 00733 00734 } 00735 00744 double* ami_1v_polynom_multiplication( 00745 double *p1 , 00746 int N1 , 00747 double *p2 , 00748 int N2 , 00749 double *p3 00750 ) 00751 { 00752 int i,j; /* AUXILIAR VARIABLES */ 00753 00754 /* WE MULTIPLY THE POLYNOMS */ 00755 for(i=0; i<=N1; i++) { 00756 if(p1[i]!=0) { 00757 for(j=0; j<=N2; j++) 00758 if(p2[j]!=0) 00759 p3[i+j]+=p1[i]*p2[j]; 00760 } 00761 } 00762 return(p3); 00763 } 00771 void ami_2v_polynom_multiplication( 00772 double **p1 , 00773 int N1 , 00774 double **p2 , 00775 int N2 , 00776 double **p3 00777 ) 00778 { 00779 int i,j,k,l; 00780 for(i=0; i<=N1; i++) { 00781 for(j=0; j<=N1; j++) { 00782 if(p1[i][j]!=0) { 00783 for(k=0; k<=N2; k++) 00784 for(l=0; l<=N2; l++) 00785 if(p2[k][l]!=0 ) 00786 p3[i+k][j+l]+=p1[i][j]*p2[k][l]; 00787 } 00788 } 00789 } 00790 } 00791 00799 int ami_RootCubicPolynomial( 00800 double *a , 00801 int N , 00802 double *x 00803 ) 00804 { 00805 double a1,a2,a3,Q,R,S,T,D,A; /* AUXILIAR VARIABLES */ 00806 00807 if(N!=3 || a[3]==0) return(-100000); 00808 a1=a[2]/a[3]; 00809 a2=a[1]/a[3]; 00810 a3=a[0]/a[3]; 00811 Q=(3*a2-a1*a1)/9.; 00812 R=(9*a1*a2-27*a3-2*a1*a1*a1)/54.; 00813 D=Q*Q*Q+R*R; 00814 00815 if(D>0) { 00816 S=R+sqrt(D); 00817 T=R-sqrt(D); 00818 if(S>0) S=pow(S,(double)1./3.); 00819 else S=-pow(-S,(double)1./3.); 00820 if(T>0) T=pow(T,(double)1./3.); 00821 else T=-pow(-T,(double)1./3.); 00822 x[0]=S+T-a1/3.; 00823 return(1); 00824 } 00825 else { 00826 double PI2=acos(-1.); 00827 if(Q!=0) A=acos(R/sqrt(-Q*Q*Q)); 00828 else A=0; 00829 00830 Q=2.*sqrt(-Q); 00831 x[0]=Q*cos(A/3.)-a1/3.; 00832 x[1]=Q*cos(A/3.+2.*PI2/3.)-a1/3.; 00833 x[2]=Q*cos(A/3+4.*PI2/3.)-a1/3.; 00834 00835 if(fabs(x[0])>fabs(x[1])) { 00836 Q=x[1]; 00837 x[1]=x[0]; 00838 x[0]=Q; 00839 } 00840 if(fabs(x[0])>fabs(x[2])) { 00841 Q=x[2]; 00842 x[2]=x[0]; 00843 x[0]=Q; 00844 } 00845 if(fabs(x[1])>fabs(x[2])) { 00846 Q=x[2]; 00847 x[2]=x[1]; 00848 x[1]=Q; 00849 } 00850 00851 return(3); 00852 } 00853 } 00860 double ami_polynomial_evaluation( 00861 double *a , 00862 int Na , 00863 double x 00864 ) 00865 { 00866 double sol=a[Na]; /* AUXILIAR VARIABLES */ 00867 int i; /* AUXILIAR VARIABLES */ 00868 for(i=Na-1; i>-1; i--) sol=sol*x+a[i]; 00869 return(sol); 00870 } 00871 00872 00881 int ami_lens_distortion_polynomial_update( 00882 double *x, double *y , 00883 int Np , 00884 double *a , 00885 int Na , 00886 double x0,double y0 , 00887 int k , 00888 double *pol 00889 ) 00890 { 00891 int i,j; /* AUXILIAR VARIABLES */ 00892 double *A,*x2,*y2,*d,x2_m,y2_m,s_xx,s_yy,xA_m,xd_m,yA_m,yd_m;/* AUXILIAR VARIABLES */ 00893 double pol1[5],pol2[5],pol3[5]; /* AUXILIAR VARIABLES */ 00894 00895 A=x2=y2=d=NULL; 00896 00897 /* WE ALLOCATE MEMORY */ 00898 A=(double*)malloc( sizeof(double)*Np ); 00899 x2=(double*)malloc( sizeof(double)*Np ); 00900 y2=(double*)malloc( sizeof(double)*Np ); 00901 d=(double*)malloc( sizeof(double)*Np ); 00902 00903 /* WE COMPUTE THE DISTANCE TO THE IMAGE CENTER */ 00904 for(i=0; i<Np; i++) 00905 d[i]=sqrt( (x[i]-x0)*(x[i]-x0)+(y[i]-y0)*(y[i]-y0) ); 00906 00907 /* WE COMPUTE THE POINT TRANSFORMATION WITH THE CURRENT LENS DISTORTION MODEL */ 00908 for(i=0; i<Np; i++) { 00909 A[i]=ami_polynomial_evaluation(a,Na,d[i]); 00910 x2[i]=x0+(x[i]-x0)*A[i]; 00911 y2[i]=y0+(y[i]-y0)*A[i]; 00912 } 00913 00914 /* WE COMPUTE THE DISTANCE POWER k (THE COEFFICIENT OF THE LENS DISTORTION MODEL 00915 TO BE UPDATED */ 00916 for(i=0; i<Np; i++) d[i]=pow(d[i],(double) k); 00917 00918 /* WE COMPUTE THE VARIANCE OF THE TRANSFORMED POINTS */ 00919 x2_m=0; 00920 for(i=0; i<Np; i++) x2_m+=x2[i]; 00921 x2_m/=Np; 00922 s_xx=0; 00923 for(i=0; i<Np; i++) s_xx+=(x2[i]-x2_m)*(x2[i]-x2_m); 00924 s_xx/=Np; 00925 y2_m=0; 00926 for(i=0; i<Np; i++) y2_m+=y2[i]; 00927 y2_m/=Np; 00928 s_yy=0; 00929 for(i=0; i<Np; i++) s_yy+=(y2[i]-y2_m)*(y2[i]-y2_m); 00930 s_yy/=Np; 00931 00932 /* WE COMPUTE SOME AVERAGES WE NEED */ 00933 xA_m=0; 00934 for(i=0; i<Np; i++) xA_m+=(x[i]-x0)*A[i]; 00935 xA_m/=Np; 00936 xd_m=0; 00937 for(i=0; i<Np; i++) xd_m+=(x[i]-x0)*d[i]; 00938 xd_m/=Np; 00939 yA_m=0; 00940 for(i=0; i<Np; i++) yA_m+=(y[i]-y0)*A[i]; 00941 yA_m/=Np; 00942 yd_m=0; 00943 for(i=0; i<Np; i++) yd_m+=(y[i]-y0)*d[i]; 00944 yd_m/=Np; 00945 00946 /* WE COMPUTE THE POLYNOMIAL TO MINIMIZE */ 00947 for(i=0; i<5; i++) pol1[i]=pol2[i]=pol3[i]=0; 00948 for(i=0; i<Np; i++) { 00949 pol1[0]+=((x[i]-x0)*A[i]-xA_m)*((x[i]-x0)*A[i]-xA_m); 00950 pol1[1]+=2.*((x[i]-x0)*A[i]-xA_m)*((x[i]-x0)*d[i]-xd_m); 00951 pol1[2]+=((x[i]-x0)*d[i]-xd_m)*((x[i]-x0)*d[i]-xd_m); 00952 pol2[0]+=((y[i]-y0)*A[i]-yA_m)*((y[i]-y0)*A[i]-yA_m); 00953 pol2[1]+=2.*((y[i]-y0)*A[i]-yA_m)*((y[i]-y0)*d[i]-yd_m); 00954 pol2[2]+=((y[i]-y0)*d[i]-yd_m)*((y[i]-y0)*d[i]-yd_m); 00955 pol3[0]+=((y[i]-y0)*A[i]-yA_m)*((x[i]-x0)*A[i]-xA_m); 00956 pol3[1]+=((y[i]-y0)*A[i]-yA_m)*((x[i]-x0)*d[i]-xd_m)+ 00957 ((y[i]-y0)*d[i]-yd_m)*((x[i]-x0)*A[i]-xA_m); 00958 pol3[2]+=((y[i]-y0)*d[i]-yd_m)*((x[i]-x0)*d[i]-xd_m); 00959 } 00960 00961 for(i=0; i<3; i++) { 00962 for(j=0; j<3; j++) { 00963 pol[i+j]+=(pol1[i]*pol2[j]-pol3[i]*pol3[j])/1. /*s_max*/; 00964 } 00965 } 00966 00967 /* WE FREE MEMORY */ 00968 if(A!=NULL) free(A); 00969 if(x2!=NULL) free(x2); 00970 if(y2!=NULL) free(y2); 00971 if(d!=NULL) free(d); 00972 return(0); 00973 } 00974 00982 int ami_lens_distortion_model_update( 00983 double *a , 00984 int Na , 00985 int k , 00986 double *pol , 00987 double max_radius 00988 ) 00989 { 00990 int j,i,M=Na; /* AUXILIAR VARIABLES */ 00991 double *x,*b,*b2,p[3]; /* AUXILIAR VARIABLES */ 00992 00993 x=b=NULL; 00994 00995 /* WE ALLOCATE MEMORY */ 00996 x=(double*)malloc( sizeof(double)*3 ); 00997 b=(double*)malloc( sizeof(double)*4 ); 00998 b2=(double*)malloc( sizeof(double)*(Na+1) ); 00999 // WE FILL THE AUXILIARY LENS DISTORTION MODEL 01000 for(i=0;i<=Na;i++) b2[i]=a[i]; 01001 01002 b[0]=pol[1]; 01003 b[1]=2*pol[2]; 01004 b[2]=3.*pol[3]; 01005 b[3]=4.*pol[4]; 01006 M=ami_RootCubicPolynomial(b,3,x); 01007 01008 for(i=0; i<M; i++) p[i]=ami_polynomial_evaluation(pol,4,x[i]); 01009 01010 double Error=1e30; 01011 j=M; 01012 for(i=0;i<M;i++){ 01013 b2[k]=a[k]+x[i]; 01014 if(test_compatibility_lens_distortion_model(b2,Na,max_radius)==0){ 01015 if(p[i]<Error){ 01016 j=i; 01017 Error=p[i]; 01018 } 01019 } 01020 } 01021 01022 if(j<M) a[k]+=x[j]; 01023 01024 if(x!=NULL) free(x); 01025 if(b!=NULL) free(b); 01026 if(b2!=NULL) free(b2); 01027 return(0); 01028 } 01029 01038 double ami_LensDistortionEnergyError( 01039 double *x,double *y , 01040 int Np , 01041 double x0,double y0 , 01042 double *a , 01043 int Na 01044 ) 01045 { 01046 int i; /* AUXILIAR VARIABLES */ 01047 double A,*x2,*y2,d,x2_m,y2_m,s_xx,s_yy,s_xy; /* AUXILIAR VARIABLES */ 01048 01049 x2=y2=NULL; 01050 01051 /* WE ALLOCATE MEMORY */ 01052 x2=(double*)malloc( sizeof(double)*Np ); 01053 y2=(double*)malloc( sizeof(double)*Np ); 01054 01055 /* WE COMPUTE THE POINT TRANSFORMATION USING THE LENS DISTORTION MODEL*/ 01056 for(i=0; i<Np; i++) { 01057 d=sqrt( (x[i]-x0)*(x[i]-x0)+(y[i]-y0)*(y[i]-y0) ); 01058 A=ami_polynomial_evaluation(a,Na,d); 01059 x2[i]=x0+(x[i]-x0)*A; 01060 y2[i]=y0+(y[i]-y0)*A; 01061 } 01062 /* WE COMPUTE THE VARIANCE OF THE TRANSFORMED POINTS */ 01063 x2_m=0; 01064 for(i=0; i<Np; i++) x2_m+=x2[i]; 01065 x2_m/=Np; 01066 s_xx=0; 01067 for(i=0; i<Np; i++) s_xx+=(x2[i]-x2_m)*(x2[i]-x2_m); 01068 s_xx/=Np; 01069 y2_m=0; 01070 for(i=0; i<Np; i++) y2_m+=y2[i]; 01071 y2_m/=Np; 01072 s_yy=0; 01073 for(i=0; i<Np; i++) s_yy+=(y2[i]-y2_m)*(y2[i]-y2_m); 01074 s_yy/=Np; 01075 s_xy=0; 01076 for(i=0; i<Np; i++) s_xy+=(y2[i]-y2_m)*(x2[i]-x2_m); 01077 s_xy/=Np; 01078 01079 /* WE FREE MEMORY */ 01080 if(x2!=NULL) free(x2); 01081 if(y2!=NULL) free(y2); 01082 return((s_xx*s_yy-s_xy*s_xy)); 01083 } 01084 01093 double ami_LensDistortionEnergyError_Vmin( 01094 double *x,double *y , 01095 int Np , 01096 double x0,double y0 , 01097 double *a , 01098 int Na 01099 ) 01100 { 01101 int i; /* AUXILIAR VARIABLES */ 01102 double A,*x2,*y2,d,x2_m,y2_m,s_xx,s_yy,s_max,s_xy; /* AUXILIAR VARIABLES */ 01103 01104 x2=y2=NULL; 01105 01106 /* WE ALLOCATE MEMORY */ 01107 x2=(double*)malloc( sizeof(double)*Np ); 01108 y2=(double*)malloc( sizeof(double)*Np ); 01109 01110 /* WE COMPUTE THE POINT TRANSFORMATION USING THE LENS DISTORTION MODEL*/ 01111 for(i=0; i<Np; i++) { 01112 d=sqrt( (x[i]-x0)*(x[i]-x0)+(y[i]-y0)*(y[i]-y0) ); 01113 A=ami_polynomial_evaluation(a,Na,d); 01114 x2[i]=x0+(x[i]-x0)*A; 01115 y2[i]=y0+(y[i]-y0)*A; 01116 } 01117 /* WE COMPUTE THE VARIANCE OF THE TRANSFORMED POINTS */ 01118 x2_m=0; 01119 for(i=0; i<Np; i++) x2_m+=x2[i]; 01120 x2_m/=Np; 01121 s_xx=0; 01122 for(i=0; i<Np; i++) s_xx+=(x2[i]-x2_m)*(x2[i]-x2_m); 01123 s_xx/=Np; 01124 y2_m=0; 01125 for(i=0; i<Np; i++) y2_m+=y2[i]; 01126 y2_m/=Np; 01127 s_yy=0; 01128 for(i=0; i<Np; i++) s_yy+=(y2[i]-y2_m)*(y2[i]-y2_m); 01129 s_yy/=Np; 01130 s_xy=0; 01131 for(i=0; i<Np; i++) s_xy+=(y2[i]-y2_m)*(x2[i]-x2_m); 01132 s_xy/=Np; 01133 s_max=s_xx>s_yy?s_xx:s_yy; 01134 01135 /* WE FREE MEMORY */ 01136 if(x2!=NULL) free(x2); 01137 if(y2!=NULL) free(y2); 01138 return((s_xx*s_yy-s_xy*s_xy)/s_max); 01139 } 01140 01141 01149 double ami_lens_distortion_estimation( 01150 double **x,double **y , 01151 int Nl , 01152 int *Np , 01153 double x0,double y0 , 01154 double *a , 01155 int Na , 01156 int k , 01157 double alpha , 01159 double max_radius 01160 ) 01161 01162 { 01163 double *pol,sum_dd,sum_Ad,d,A,Error=0; /* AUXILIAR VARIABLES */ 01164 int m,i,j; /* AUXILIAR VARIABLES */ 01165 pol=NULL; 01166 01167 /* WE ALLOCATE MEMORY */ 01168 pol=(double*)malloc( sizeof(double)*5 ); 01169 01170 for(i=0; i<=4; i++) pol[i]=0.; 01171 01172 /* WE ADAPT a[0] TO MINIMIZE THE SQUARE OF THE DISTANCE BEWTEEN 01173 DISTORTED AND UNDISTORDED POINTS */ 01174 if(alpha>0) { 01175 sum_dd=sum_Ad=0; 01176 for(m=0; m<Nl; m++) { 01177 for(i=0; i<Np[m]; i++) { 01178 d=sqrt( (x[m][i]-x0)*(x[m][i]-x0)+(y[m][i]-y0)*(y[m][i]-y0) ); 01179 A=0; 01180 for(j=1; j<=Na; j++) A+=a[j]*pow(d,(double) j+1); 01181 sum_dd+=d*d; 01182 sum_Ad+=A*d; 01183 } 01184 } 01185 a[0]=1-sum_Ad/sum_dd; 01186 } 01187 /* WE COMPUTE THE LENS DISTORTION MODEL */ 01188 for(i=0; i<Nl; i++) { 01189 ami_lens_distortion_polynomial_update(x[i],y[i],Np[i],a,Na,x0,y0,k,pol); 01190 } 01191 ami_lens_distortion_model_update(a,Na,k,pol,max_radius); 01192 /* WE FREE THE MEMORY */ 01193 if(pol!=NULL) free(pol); 01194 for(i=0; i<Nl; i++) Error+=ami_LensDistortionEnergyError(x[i],y[i],Np[i],x0,y0,a,Na); 01195 return(Error/Nl); 01196 } 01197 01205 void ami_lens_distortion_zoom_normalization( 01206 double **x,double **y , 01207 int Nl , 01208 int *Np , 01209 double *a , 01210 int Na 01211 ) 01212 { 01213 int i,k,m,N=0; /* AUXILIAR VARIABLES */ 01214 double Z,d,sum_Ad,A,x0,y0; /* AUXILIAR VARIABLES */ 01215 01216 x0=a[5]; 01217 y0=a[6]; 01218 /* WE UPDATE a BY ESTIMATING A ZOOM FACTOR Z */ 01219 sum_Ad=0; 01220 for(m=0; m<Nl; m++) { 01221 for(i=0; i<Np[m]; i++) { 01222 N++; 01223 d=sqrt( (x[m][i]-x0)*(x[m][i]-x0)+(y[m][i]-y0)*(y[m][i]-y0) ); 01224 A=a[0]; 01225 for(k=1; k<=Na; k++) A+=a[k]*pow(d,(double) k); 01226 sum_Ad+=A*A; 01227 } 01228 } 01229 Z=sqrt((double) N/sum_Ad); 01230 for(k=0; k<=Na; k++) a[k]*=Z; 01231 } 01232 01246 int calculate_points( 01247 double *amin , 01248 double **points_2D_modified , 01249 int N , 01250 int Na , 01251 double x0 , 01252 double y0 01253 ) 01254 { 01255 double d1,sol; /* AUXILIAR VARIABLES */ 01256 int i,j; /* AUXILIAR VARIABLES */ 01257 01258 /* Calculate the distance from each point to the center of the image */ 01259 /* WE COMPUTE THE POINT TRANSFORMATION WITH THE CURRENT LENS DISTORTION MODEL */ 01260 for(i=0; i<N; i++) 01261 { 01262 d1=sqrt(pow(points_2D_modified[i][0]-x0,2.0)+pow(points_2D_modified[i][1]-y0,2.0)); 01263 sol=amin[Na]; 01264 for(j=Na-1; j>-1; j--) sol=sol*d1+amin[j]; 01265 points_2D_modified[i][0]=(points_2D_modified[i][0]-x0)*sol+x0; 01266 points_2D_modified[i][1]=(points_2D_modified[i][1]-y0)*sol+y0; 01267 } 01268 return(0); 01269 } 01270 01278 double distance_function( 01279 double *solution , 01280 double **x , 01281 double **y , 01282 int Nl , 01283 int *Np , 01284 int Na 01285 ) 01286 { 01287 double **points_2D_modified,*amin,sum,f_objective,a,b,c,tmp;/* AUXILIAR VARIABLES */ 01288 double line[3],x0,y0; /* AUXILIAR VARIABLES */ 01289 int i,j; /* AUXILIAR VARIABLES */ 01290 points_2D_modified=NULL; 01291 amin=NULL; 01292 01293 ami_calloc1d(amin,double,Na+1); 01294 for(i=0; i<=Na; i++) amin[i]=solution[i]; 01295 x0=solution[5]; 01296 y0=solution[6]; 01297 01298 f_evaluations++; 01299 f_objective=0.0; 01300 for (i=0; i<Nl; i++) { 01301 points_2D_modified=(double**)malloc(sizeof(double*)*Np[i]); 01302 for(j=0; j<Np[i]; j++) { 01303 points_2D_modified[j]=(double*)malloc(sizeof(double)*2); 01304 points_2D_modified[j][0]=x[i][j]; 01305 points_2D_modified[j][1]=y[i][j]; 01306 } 01307 calculate_points(amin,points_2D_modified,Np[i],Na,x0,y0); 01308 ami_line2d_calculation(line,points_2D_modified,Np[i]); 01309 a=line[1]; 01310 b=line[0]; 01311 c=line[2]; 01312 tmp=pow(a,2.0)+pow(b,2.0); 01313 sum=0.0; 01314 for(j=0; j<Np[i]; j++) 01315 sum=sum+pow(b*points_2D_modified[j][0]+a*points_2D_modified[j][1]+c,2.0); 01316 01317 sum=sum/tmp; 01318 sum=sum/(double)Np[i]; 01319 f_objective=f_objective+sum; 01320 for(j=0; j<Np[i]; j++) { 01321 free(points_2D_modified[j]); 01322 } 01323 free(points_2D_modified); 01324 } 01325 f_objective=f_objective/(double)Nl; 01326 free(amin); 01327 return(f_objective); 01328 } 01329 01338 double find_lambda( 01339 double lambda1 , 01340 double lambda2 , 01341 double lambda3 , 01342 double f_1 , 01343 double f_2 , 01344 double f_3 , 01345 double *amin_copy , 01346 double *amin , 01347 double **x , 01348 double **y , 01349 int Nl , 01350 int *Np , 01351 int Na , 01352 double *grad_f , 01353 int *change_k 01354 ) 01355 01356 { 01357 int Naa=7; /* AUXILIAR VARIABLES */ 01358 double f_min,lambda,lambda_tmp1; /* AUXILIAR VARIABLES */ 01359 double lambda_tmp2,f_tmp1=f_3,f_tmp2=f_2,f=f_1; /* AUXILIAR VARIABLES */ 01360 int i; /* AUXILIAR VARIABLES */ 01361 /* minimum is in (lambda1,lambda2) */ 01362 lambda_tmp1=(lambda2+lambda1)/2.0; 01363 for(i=0; i<Naa; i++) { 01364 if(change_k[i]==1) *(amin_copy+i)=*(amin+i)-lambda_tmp1*(*(grad_f+i)); 01365 } 01366 f_tmp1=distance_function(amin_copy,x,y,Nl,Np,Na); 01367 if(f_tmp1<f_1) return(lambda_tmp1); 01368 01369 lambda_tmp2=(lambda3+lambda2)/2.0; 01370 for(i=0; i<Naa; i++) { 01371 if(change_k[i]==1) *(amin_copy+i)=*(amin+i)-lambda_tmp2*(*(grad_f+i)); 01372 } 01373 f_tmp2=distance_function(amin_copy,x,y,Nl,Np,Na); 01374 if(f_tmp2<f_1) return(lambda_tmp2); 01375 f=f_1; 01376 do { 01377 f_min=f; 01378 lambda=lambda1+1e-8; 01379 for(i=0; i<Naa; i++) { 01380 if(change_k[i]==1) *(amin_copy+i)=*(amin+i)-lambda*(*(grad_f+i)); 01381 } 01382 f=distance_function(amin_copy,x,y,Nl,Np,Na); 01383 } while(f<f_min); 01384 return(lambda); 01385 } 01386 01387 01396 double minimize_cuadratic_polynom( 01397 double lambda1 , 01398 double lambda2 , 01399 double lambda3 , 01400 double f_1 , 01401 double f_2 , 01402 double f_3 , 01403 double *amin_copy , 01404 double *amin , 01405 double **x , 01406 double **y , 01407 int Nl , 01408 int *Np , 01409 int Na , 01410 double *grad_f , 01411 int *change_k 01413 ) 01414 { 01415 01416 double a12,a23,a31,b12,b23,b31,min_lambda; /* AUXILIAR VARIABLES */ 01417 01418 a12=lambda1-lambda2; 01419 a23=lambda2-lambda3; 01420 a31=lambda3-lambda1; 01421 b12=pow(lambda1,2.0)-pow(lambda2,2.0); 01422 b23=pow(lambda2,2.0)-pow(lambda3,2.0); 01423 b31=pow(lambda3,2.0)-pow(lambda1,2.0); 01424 01425 min_lambda=0.5*(b23*f_1+b31*f_2+b12*f_3); 01426 min_lambda=min_lambda/(a23*f_1+a31*f_2+a12*f_3); 01427 /* to avoid numerical errors, we check the condition*/ 01428 /* lambda1<min_lambda<lambda3 */ 01429 if((lambda1<min_lambda)&&(min_lambda<lambda3)) return(min_lambda); 01430 else { 01431 min_lambda=find_lambda(lambda1,lambda2,lambda3,f_1,f_2,f_3, 01432 amin_copy,amin,x,y,Nl,Np,Na,grad_f,change_k); 01433 return(min_lambda); 01434 01435 } 01436 } 01437 01447 double cuadratic_fitting( 01448 double *amin_copy , 01449 double *amin , 01450 double **x , 01451 double **y , 01452 int Nl , 01453 int *Np , 01454 int Na , 01455 double lambda1 , 01456 double lambda2 , 01457 double lambda3 , 01458 double f_1 , 01459 double f_2 , 01460 double f_3 , 01461 double *grad_f , 01462 int *change_k 01463 ) 01464 { 01465 double minimo_lambda,f_minimo; /* AUXILIAR VARIABLES */ 01466 double error=1e100; /* AUXILIAR VARIABLES */ 01467 int iterations_lambda,i; /* AUXILIAR VARIABLES */ 01468 int Naa=7; /* AUXILIAR VARIABLES */ 01469 01470 iterations_lambda=0; 01471 /* We loop till getting the minimum */ 01472 while(error>tol_lambda) { 01473 01474 minimo_lambda=minimize_cuadratic_polynom(lambda1,lambda2,lambda3,f_1,f_2,f_3, 01475 amin_copy,amin,x,y,Nl,Np,Na,grad_f,change_k); 01476 for(i=0; i<Naa; i++) { 01477 if(change_k[i]==1) *(amin_copy+i)=*(amin+i)-minimo_lambda*(*(grad_f+i)); 01478 } 01479 f_minimo=distance_function(amin_copy,x,y,Nl,Np,Na); 01480 if(minimo_lambda>lambda2) { 01481 if(f_minimo>f_2) { 01482 lambda3=minimo_lambda; 01483 f_3=f_minimo; 01484 } 01485 else { 01486 lambda1=lambda2; 01487 f_1=f_2; 01488 lambda2=minimo_lambda; 01489 f_2=f_minimo; 01490 } 01491 } 01492 else { 01493 if(f_minimo>=f_2) { 01494 lambda1=minimo_lambda; 01495 f_1=f_minimo; 01496 } 01497 else { 01498 lambda3=lambda2; 01499 f_3=f_2; 01500 lambda2=minimo_lambda; 01501 f_2=f_minimo; 01502 } 01503 } 01504 error=fabs(lambda3-lambda1); 01505 if(f_minimo==f_2) lambda2=lambda2+tol_lambda; 01506 iterations_lambda++; 01507 if(iterations_lambda==max_itera_lambda) return(lambda2); 01508 } 01509 return(lambda2); 01510 } 01511 01520 double minimize_lambda( 01521 double *amin , 01522 double **x , 01523 double **y , 01524 int Nl , 01525 int *Np , 01526 int Na , 01527 double *grad_f , 01528 double f , 01529 int *change_k 01530 ) 01531 { 01532 double lambda1,lambda2,lambda3,lambda; /* AUXILIAR VARIABLES */ 01533 double f_1,f_2,f_3=0.0,last_f3,last_f2; /* AUXILIAR VARIABLES */ 01534 double tol_ff=1.0e-10; /* AUXILIAR VARIABLES */ 01535 double *amin_copy; /* AUXILIAR VARIABLES */ 01536 int i,Naa; /* AUXILIAR VARIABLES */ 01537 01538 Naa=7; /* AUXILIAR VARIABLES */ 01539 amin_copy=NULL; 01540 amin_copy=(double*)malloc( sizeof(double)*(Naa) ); 01541 01542 f_1=f; 01543 /* search the TTP points */ 01544 lambda1=0.0; 01545 /* search lambda2 */ 01546 lambda2=fabs(grad_f[2]); 01547 for(i=0; i<Naa; i++)*(amin_copy+i)=*(amin+i)-lambda2*(*(grad_f+i)); 01548 f_2=distance_function(amin_copy,x,y,Nl,Np,Na); 01549 01550 if(f_2>f_1) { 01551 lambda3=lambda2; 01552 f_3=f_2; 01553 /* search lambda2 by dividing the (lambda1,lambda3) interval */ 01554 lambda2=lambda3/2.0; 01555 while(1) { 01556 last_f2=f_2; 01557 for(i=0; i<Naa; i++) { 01558 if(change_k[i]==1) *(amin_copy+i)=*(amin+i)-lambda2*(*(grad_f+i)); 01559 } 01560 f_2=distance_function(amin_copy,x,y,Nl,Np,Na); 01561 if(f_2<f_1) break; 01562 if(fabs((f_2-last_f2)/last_f2)<=tol_ff) { /* Avoid the flat zone */ 01563 if(amin_copy!=NULL) free(amin_copy); /*free memory*/ 01564 return(lambda2); 01565 } 01566 lambda2=lambda2/2.0; 01567 } 01568 } 01569 else { 01570 /* search lambda3 by increasing the (lambda1,lambda2) interval */ 01571 lambda3=lambda2*2.0; 01572 while(1) { 01573 last_f3=f_3; 01574 for(i=0; i<Naa; i++) { 01575 if(change_k[i]==1) *(amin_copy+i)=*(amin+i)-lambda3*(*(grad_f+i)); 01576 } 01577 f_3=distance_function(amin_copy,x,y,Nl,Np,Na); 01578 if(f_3>f_2) break; 01579 if(fabs((f_3-last_f3)/last_f3)<=tol_ff) { /* Avoid the flat zone */ 01580 if(amin_copy!=NULL) free(amin_copy); /* Free memory */ 01581 return(lambda3); /* Avoid the flat zone */ 01582 } 01583 lambda3=2*lambda3; 01584 } 01585 } 01586 /* We have the points satisfying the TTP condition 01587 lambda1,f_1;lambda_2,f_2;lambda3,f_3 01588 minimize the cuadratic polynom */ 01589 lambda=cuadratic_fitting(amin_copy,amin,x,y,Nl,Np,Na,lambda1,lambda2,lambda3,f_1,f_2,f_3,grad_f,change_k); 01590 if(amin_copy!=NULL) free(amin_copy); 01591 return(lambda); 01592 } 01593 01601 double gradient_method( 01602 double *solution , 01603 double **x , 01604 double **y , 01605 int Nl , 01606 int *Np , 01607 int Na , 01608 int *change_k , 01609 int zoom , 01610 int optimize_center 01611 ) 01612 { 01613 double *grad_f,f_objective,last_f=1.0e100; /* AUXILIAR VARIABLES */ 01614 double kk,lambda; /* AUXILIAR VARIABLES */ 01615 int i; /* AUXILIAR VARIABLES */ 01616 01617 01618 grad_f=NULL; 01619 ami_calloc1d(grad_f,double,7); 01620 01621 f_objective=distance_function(solution,x,y,Nl,Np,Na); 01622 while(fabs((f_objective-last_f)/last_f)>tol_f) { 01623 last_f=f_objective; 01624 for(i=0; i<7; i++) { 01625 /* Move along each axis and incremental step to calculate the derivative */ 01626 if(change_k[i]==1) { 01627 kk=solution[i]; 01628 solution[i]=kk+delta; 01629 grad_f[i]=(distance_function(solution,x,y,Nl,Np,Na)-f_objective)/delta; 01630 solution[i]=kk; 01631 } 01632 } 01633 /* Activate to stop the gradient when the gradient_norm<tol_norma gradient_norm=0.0; 01634 for(i=0;i<7;i++) gradient_norm=gradient_norm+pow(grad_f[i],2.0); 01635 gradient_norm=sqrt(gradient_norm); if(gradient_norm<=tol_norma) break; */ 01636 lambda=minimize_lambda(solution,x,y,Nl,Np,Na,grad_f,f_objective,change_k); 01637 for(i=0; i<7; i++) if(change_k[i]==1) *(solution+i)=*(solution+i)-lambda*(*(grad_f+i)); 01638 n_iterations++; 01639 f_objective=distance_function(solution,x,y,Nl,Np,Na); 01640 /* Activate to have the trace of the execution */ 01641 //printf("\nIteracion=%d\tf=%1.18f\t|grad(f)|=%1.18f",n_iteraciones,f_objective,gradient_norm); 01642 //for(i=0;i<7;i++) printf("\n(in gradient) solution[%d]=%f",i,solution[i]); 01643 //system("pause"); 01644 if(n_iterations==max_itera) break; 01645 } 01647 if(zoom==1) ami_lens_distortion_zoom_normalization(x,y,Nl,Np,solution,Na); 01648 if(grad_f!=NULL) free(grad_f); 01649 return(0); 01650 } 01651 01660 int optimize( 01661 double *solution , 01662 double **x , 01663 double **y , 01664 double **xx , 01665 double **yy , 01666 int Nl , 01667 int *Np , 01668 int Na , 01669 double factor_n , 01670 int zoom , 01671 FILE *fp1 , 01672 int optimize_center 01673 ) 01674 { 01675 01676 int starttime, stoptime,i; /* AUXILIAR VARIABLES */ 01677 double paso,Emin,Vmin,D; /* AUXILIAR VARIABLES */ 01678 double timeused,x0,y0,x00,y00,*amin,factor_n_new; /* AUXILIAR VARIABLES */ 01679 int *change_k; /* TO INDICATE WHAT VARIABLE IS 01680 GOING TO BE OPTIMIZED BY GRADIENT METHOD */ 01681 01682 change_k=NULL; 01683 amin=NULL; 01684 01685 x00=solution[5]; 01686 y00=solution[6]; /* WE COPY THE ORIGINAL CENTER OF DISTORTION (pixels) */ 01687 f_evaluations=0; 01688 n_iterations=0; 01689 01690 solution[5]=0.0; 01691 solution[6]=0.0; /* TO CALCULATE IN NORMALIZED COORDINATES */ 01692 ami_calloc1d(change_k,int,7); /* BY DEFAULT, INITIALIZED TO 0 */ 01693 change_k[2]=1; 01694 change_k[4]=1; /* OPTIMIZED ONLY K2 AND K4 */ 01695 starttime = clock(); 01696 /* GRADIENT METHOD WORKS WITH NORMALIZED UNITS */ 01697 if(optimize_center==1) { 01698 change_k[5]=1; 01699 change_k[6]=1; /*OPTIMIZE THE CENTER OF DISTORTION */ 01700 gradient_method(solution,x,y,Nl,Np,Na,change_k,zoom,optimize_center); 01701 } 01702 else gradient_method(solution,x,y,Nl,Np,Na,change_k,zoom,optimize_center); 01703 stoptime = clock(); 01704 timeused = difftime(stoptime,starttime); 01705 x0=solution[5]; 01706 y0=solution[6]; /* WE RECOVER THE CENTER OF DISTORTION, OPTIMIZED OR NOT */ 01707 01708 /* Final solution is in solution (distortion parameters) and it is normalized */ 01709 /* We undo the un-normalization to have the solution in pixels */ 01710 if(optimize_center==1) { /* WE CALCULATE THE NEW CENTER OF DISTORTION IN PIXELS */ 01711 x0=x0+x00; 01712 y0=y0+y00; 01713 } 01714 else { 01715 x0=x00; 01716 y0=y00; 01717 } 01718 01719 /* WE TRANSFORM THE SOLUTION IN NORMALIZED COORDINATES TO PIXEL UNITS */ 01720 if(optimize_center==0) { 01721 paso=1.0; 01722 for(i=0; i<=Na; i++) { 01723 solution[i]=solution[i]*paso; 01724 paso/=factor_n; 01725 } 01726 } 01727 else { 01728 factor_n_new=calculate_factor_n(xx,yy,Nl,Np,x0,y0); 01729 paso=1.0; 01730 for(i=0; i<=Na; i++) { 01731 solution[i]=solution[i]*paso; 01732 paso/=factor_n_new; 01733 } 01734 } 01735 solution[5]=x0; 01736 solution[6]=y0; /* WE UPDATE THE SOLUTION (CENTER OF DISTORTION) */ 01737 amin=NULL; 01738 ami_calloc1d(amin,double,Na+1); /* WE RECOVER THE amin SOLUTION */ 01739 for(i=0; i<=Na; i++) amin[i]=solution[i]; /* WE UPDATE THE SOLUTION (ldm) */ 01740 /* We get the final solution and print into a file */ 01741 fprintf(fp1,"\nMODIFIED VARIABLES THROUGH OPTIMIZATION:"); 01742 for(i=0; i<=Na; i++) if(change_k[i]==1) fprintf(fp1,"\tk[%d]",i); 01743 if(optimize_center==1) fprintf(fp1,", (x0,y0)"); 01744 Emin=0.0; 01745 for(i=0; i<Nl; i++)Emin+=ami_LensDistortionEnergyError(xx[i],yy[i],Np[i],x0,y0,amin,Na); 01746 Emin=Emin/(double)Nl; 01747 Vmin=0.0; 01748 for(i=0; i<Nl; i++)Vmin+=ami_LensDistortionEnergyError_Vmin(xx[i],yy[i],Np[i],x0,y0,amin,Na); 01749 Vmin=Vmin/(double)Nl; 01750 D=distance_function(solution,xx,yy,Nl,Np,Na); 01751 fprintf(fp1,"\n\n(Emin, Vmin, D) = (%1.4e, %1.4e, %1.4e)",Emin,Vmin,D); 01752 fprintf(fp1,"\n\nDistortion parameters:\n"); 01753 for(i=0; i<=Na; i++) fprintf(fp1,"k[%d] = %1.15e\n",i,amin[i]); 01754 fprintf(fp1,"\nCenter of distortion (x0,y0) = (%f, %f)\n",x0,y0); 01755 fprintf(fp1,"\nNumber of gradient iterations= %d",n_iterations); 01756 fprintf(fp1,"\nCPU_time = %f (seconds)",timeused/(double)CLOCKS_PER_SEC); 01757 fprintf(fp1,"\nf_evaluations = %d",f_evaluations); 01758 if(change_k!=NULL) free(change_k); /* FREE THE MEMORY */ 01759 return(1); 01760 } 01761 01762 01771 int algebraic_method_pre_gradient( 01772 int Nl , 01773 int *Np , 01774 double *a , 01775 double **x , 01776 double **y , 01777 double **xx , 01778 double **yy , 01779 double factor_n , 01780 int zoom , 01781 FILE *fp1 , 01782 int optimize_center , 01783 double max_radius 01784 ) 01785 { 01786 01787 double aux,Emin; /* AUXILIAR VARIABLES */ 01788 int Na=4; /* DEGREE OF THE POLYNOM */ 01789 01790 if(zoom==0) { 01791 fprintf(fp1,"*************************************************************************\n"); 01792 fprintf(fp1," ALGEBRAIC METHOD + GRADIENT: WITHOUT ZOOM (Na = 4) \n"); 01793 fprintf(fp1,"*************************************************************************"); 01794 } 01795 01796 else { 01797 fprintf(fp1,"\n*************************************************************************\n"); 01798 fprintf(fp1,"* ALGEBRAIC METHOD + GRADIENT: WITH ZOOM (Na = 4) \n"); 01799 fprintf(fp1,"*************************************************************************"); 01800 } 01801 01802 01803 double x0,y0,x00,y00; 01804 x0=a[5]; 01805 y0=a[6]; /* WE CAPTURE THE CENTER OF DISTORTION */ 01806 x00=x0; 01807 y00=y0; /* WE COPY THE ORIGINAL CENTER OF DISTORTION */ 01808 01809 fprintf(fp1,"\n2 parameters, 1 iteration, variables updated:\t2, 4"); 01810 01811 if(optimize_center==0){ //IT WORKS FROM THE TRIVIAL SOLUTION 01812 a[0]=1.0; /* trivial solution */ 01813 for(int i=1; i<=Na; i++) a[i]=0.0; 01814 /* WE RUN A SAFE PREVIOUS ITERATION TO AVOID CONVERGENCE PROBLEMS*/ 01815 Emin=ami_lens_distortion_estimation(x,y,Nl,Np,(double) 0.,(double) 0.,a,Na,2,(double) 0.,max_radius); 01816 Emin=ami_lens_distortion_estimation(x,y,Nl,Np,(double) 0.,(double) 0.,a,Na,4,(double) 0.,max_radius); 01817 /* WE RUN THE ALGEBRAIC METHOD FOR BOTH PARAMETERS IN ONE ITERATION */ 01818 Emin=ami_lens_distortion_estimation_2v(x,y,Nl,Np,(double) 0.,(double) 0.,a,Na,2,4,(double) 0.,max_radius); 01819 01820 /* WE SET THE ORIGINAL CENTER OF DISTORTION */ 01821 a[5]=x00; 01822 a[6]=y00; 01823 01824 /* APPLY THE GRADIENT METHOD FROM THE ALGEBRAIC SOLUTION */ 01825 optimize(a,x,y,xx,yy,Nl,Np,Na,factor_n,zoom,fp1,optimize_center); 01826 } 01827 01828 else{ /* A SOLUTION -PIXELS- HAS BEEN CALCULATED WHEN SEARCHING FOR THE 01829 BEST CENTER OF DISTORTION*/ 01830 /* NORMALIZE COORDINATES */ 01831 double paso=1.0; 01832 for(int i=0; i<=Na; i++) { 01833 a[i]=a[i]*paso; 01834 paso/=factor_n; 01835 } 01836 a[5]=x00; /* WE COPY THE ORIGINAL CENTER OF DISTORTION */ 01837 a[6]=y00; /* WE COPY THE ORIGINAL CENTER OF DISTORTION */ 01838 /* APPLY THE GRADIENT METHOD FROM THE SOLUTION */ 01839 optimize(a,x,y,xx,yy,Nl,Np,Na,factor_n,zoom,fp1,optimize_center); 01840 } 01841 return(1); 01842 } 01843 01852 int trivial_solution( 01853 int Nl , 01854 int *Np , 01855 double *a , 01856 double **xx , 01857 double **yy , 01858 double factor_n , 01859 FILE *fp1 , 01860 double *trivial , 01861 int optimize_center 01862 ) 01863 { 01864 int starttime, stoptime,i,m,Na; 01865 double *amin,Emin,Vmin=factor_n,D,x0,y0; 01866 double timeused; 01867 01868 /* WE CAPTURE THE CENTER OF DISTORTION */ 01869 x0=a[5]; 01870 y0=a[6]; 01871 01872 /* WE DEFINE THE GRADE OF THE LENS DISTORTION POLYNOM (MAXIMUM GRADE) */ 01873 /* WE KEEP THE ORIGINAL FORMAT OF THE ALGEBRAIC FUNCTIONS */ 01874 Na=4; 01875 01876 ami_calloc1d(amin,double,Na+1); 01877 for(i=0; i<=Na; i++) amin[i]=a[i]; 01878 01879 fprintf(fp1,"****************************************************************************"); 01880 fprintf(fp1,"\n LENS DISTORTION MODEL: OUTPUT FILE "); 01881 fprintf(fp1,"\n CALCULATED USING NORMALIZED COORDINATES (SOLUTION IN PIXELS) "); 01882 fprintf(fp1,"\n\nALGEBRAIC METHOD: IT DOES NOT OPTIMIZE THE CENTER OF DISTORTION"); 01883 fprintf(fp1,"\nGRADIENT METHOD: IT IMPROVES THE PREVIOUS ALGEBRAIC SOLUTION"); 01884 fprintf(fp1,"\n "); 01885 fprintf(fp1,"\n THE CENTER OF DISTORTION IS OPTIMIZED IF INDICATED"); 01886 fprintf(fp1,"\n****************************************************************************\n"); 01887 01888 if(optimize_center==1) fprintf(fp1,"\n\t\tThe center of distortion is going to be optimized.\n"); 01889 else fprintf(fp1,"\n\t\tThe center of distortion is not going to be optimized.\n"); 01890 fprintf(fp1,"\n****************************************************************************\n"); 01891 01892 01893 /* WE DO THE CALCULATION IN NORMALIZED COORDINATES AND TRANSFORM TO PIXELS AT THE END */ 01894 /* SOLUTION AND DISTORTION PARAMETERS: IN PIXELS */ 01895 /* //////////////////////////////////////////////////////////////////////////////////////////// */ 01896 /* ******************************WE MEASURE CPU TIME FOR CALCULATIONS ONLY********************* */ 01897 /* //////////////////////////////////////////////////////////////////////////////////////////// */ 01898 /* WE COMPUTE THE DISTORTION MODEL */ 01899 /* WE ESTIMATE THE Emin,Vmin IN PIXELS */ 01900 fprintf(fp1," TRIVIAL SOLUTION (Na = 4): "); 01901 fprintf(fp1,"\n****************************************************************************\n"); 01902 fprintf(fp1,"Center of distortion (x0,y0) = (%f,%f)",x0,y0); 01903 starttime = clock(); 01904 Emin=0; 01905 for(m=0; m<Nl; m++) Emin+=ami_LensDistortionEnergyError(xx[m],yy[m],Np[m],x0,y0,amin,Na); 01906 Emin=Emin/(double)Nl; 01907 Vmin=0; 01908 for(m=0; m<Nl; m++) Vmin+=ami_LensDistortionEnergyError_Vmin(xx[m],yy[m],Np[m],x0,y0,amin,Na); 01909 Vmin=Vmin/(double)Nl; 01910 stoptime = clock(); 01911 timeused = difftime(stoptime,starttime); 01912 D=distance_function(a,xx,yy,Nl,Np,Na); 01913 fprintf(fp1,"\n(Emin, Vmin, D) = (%1.4e, %1.4e, %1.4e)",Emin,Vmin,D); 01914 fprintf(fp1,"\nDistortion parameters:\n"); 01915 for(i=0; i<=Na; i++) fprintf(fp1,"k[%d] = %1.15e\n",i,a[i]); 01916 fprintf(fp1,"\nCPU_time=%f (seconds)",timeused/(double)CLOCKS_PER_SEC); 01917 fprintf(fp1," \n*************************************************************************\n\n\n"); 01918 /* SAVE THE TRIVIAL VALUES TO CALCULATE THE % IMPROVEMENT */ 01919 trivial[0]=Emin; 01920 trivial[1]=Vmin; 01921 trivial[2]=D; 01922 free(amin); 01923 return(1); 01924 } 01925 01926 01934 int search_for_best_center( 01935 int Nl , 01936 int *Np , 01937 double *a , 01938 double **xx , 01939 double **yy , 01940 int width , 01941 int height , 01942 double max_radius 01943 ) 01944 { 01945 int Na=4;/* WE DEFINE THE GRADE OF THE LENS DISTORTION POLYNOM (MAXIMUM GRADE) */ 01946 01947 /* WE CALCULATE THE ALGEBRAIC SOLUTION FOR THE GIVEN CENTER OF DISTORTION */ 01948 /* WE WORK USING PIXELS UNITS */ 01949 double initial_Emin=0.0; 01950 int x_c=floor(a[5]); /* WE CAPTURE THE INDICATED CENTER OF DISTORTION (X) */ 01951 int y_c=floor(a[6]); /* WE CAPTURE THE INDICATED CENTER OF DISTORTION (Y) */ 01952 int best_x_c, best_y_c; /* AUXILIAR VARIABLE TO STORE THE BEST SOLUTION FOUND */ 01953 01954 float Emin,timeused; 01955 float **aux_image; /* AUXILIAR VARIABLES */ 01956 int starttime, stoptime,n_iterations=0; /* AUXILIAR VARIABLES */ 01957 01958 aux_image=NULL; 01959 // TO SPEED-UP CALCULATION OF OPTIMIZED CENTER OF DISTORTION, 01960 //WE AVOID TO RECALCULATE ENERGY FUNCTION AT PREVIOUS 01961 // CALCULATED POSITIONS WITHIN THE IMAGE 01962 /* WE ALLOCATE MEMORY FOR AUXIlIAR IMAGE MATRIX */ 01963 aux_image=(float**)malloc(sizeof(float*)*width); 01964 for(int i=0; i<(int)width; i++) aux_image[i]=(float*)malloc( sizeof(float)*height ); 01965 // WE INITIALIZE THE aux_image MATRIX TO -1.0 01966 for(int i=0; i<width; i++) 01967 for(int j=0; j<height; j++) aux_image[i][j]=-1.0; 01968 01969 /* WE RUN A SAFE PREVIOUS ITERATION TO AVOID CONVERGENCE PROBLEMS*/ 01970 initial_Emin=ami_lens_distortion_estimation(xx,yy,Nl,Np,(double) x_c,(double) y_c,a,Na,2,(double) 0.,max_radius); 01971 initial_Emin=ami_lens_distortion_estimation(xx,yy,Nl,Np,(double) x_c,(double) y_c,a,Na,4,(double) 0.,max_radius); 01972 /* WE RUN THE ALGEBRAIC METHOD FOR BOTH PARAMETERS IN ONE ITERATION */ 01973 initial_Emin=ami_lens_distortion_estimation_2v(xx,yy,Nl,Np,(double) x_c,(double) y_c,a,Na,2,4,(double) 0.,max_radius); 01974 01975 /* initial_Emin has the algebraic solution for the given center of distortion. 01976 The corresponding solution is in vector a */ 01977 /* This initial_Emin solution is the initial one to compare with, for the patch search */ 01978 /* we scan iteratively the image looking for the best center of distortion */ 01979 float last_Emin=1e100; 01980 best_x_c=x_c; 01981 best_y_c=y_c; 01982 int patch_half=floor((double)patch_size/2.0); 01983 starttime = clock(); 01984 while(fabs(initial_Emin-last_Emin)>tol_f) { 01985 last_Emin=initial_Emin; 01986 n_iterations++; 01987 int lim_inf_x=x_c-patch_half; 01988 int lim_sup_x=x_c+patch_half; 01989 int lim_inf_y=y_c-patch_half; 01990 int lim_sup_y=y_c+patch_half; 01991 //#pragma omp parallel for 01992 /* we scan the rectangular patch: pixel precission, subpixel is reached through gradient */ 01993 for(int i=lim_inf_x; i<=lim_sup_x; i++) { 01994 if(i>=0 && i<=width) { // we check that we are inside the image (x-axis) 01995 for(int j=lim_inf_y; j<=lim_sup_y; j++) { 01996 if(j>=0 && j<=height) { // we check that we are inside the image (y-axis) 01997 if(aux_image[i][j]==-1.0) { // we check if this value is already calculated 01998 a[2]=0.0; 01999 a[4]=0.0; // we reset the a vector 02000 /* REMARK: ACTIVATE THE NEXT TWO LINES IF THERE ARE CONVERGENCE PROBLEMS */ 02001 Emin=ami_lens_distortion_estimation(xx,yy,Nl,Np,(double) i,(double) j,a,Na,2,(double) 0.,max_radius); 02002 Emin=ami_lens_distortion_estimation(xx,yy,Nl,Np,(double) i,(double) j,a,Na,4,(double) 0.,max_radius); 02003 Emin=ami_lens_distortion_estimation_2v(xx,yy,Nl,Np,(double) i,(double) j,a,Na,2,4,(double) 0., max_radius); 02004 aux_image[i][j]=Emin; //we save the value for possible use in new iterations 02005 } 02006 else Emin=aux_image[i][j]; 02007 if(Emin<initial_Emin) { 02008 initial_Emin=Emin; /* We save the best solution */ 02009 best_x_c=i; 02010 best_y_c=j; /* We save the best center of distortion found */ 02011 } 02012 } 02013 } 02014 } 02015 } 02016 x_c=best_x_c; 02017 y_c=best_y_c; 02018 printf("\nitera=%d,(x_c,y_c)=(%d,%d), Emin=%1.7e, last_Emin=%1.7e",n_iterations,best_x_c,best_y_c,initial_Emin,last_Emin); 02019 if(n_iterations==max_itera_patch) break; 02020 } 02021 stoptime = clock(); 02022 timeused = difftime(stoptime,starttime); 02023 a[5]=best_x_c; 02024 a[6]=best_y_c; /* We update the new distortion center coordinates */ 02025 printf("\n****************************************************************************"); 02026 printf("\nCPU time Optimizing Center of Distortion=%f (seconds)",timeused/(double)CLOCKS_PER_SEC); 02027 printf("\n****************************************************************************"); 02028 //we free the memory 02029 if(aux_image!=NULL) { 02030 for(int j=0; j<height; j++) { 02031 if(aux_image[j]!=NULL) free(aux_image[j]); 02032 } 02033 free(aux_image); 02034 } 02035 return(1); 02036 } 02037 02038 02044 int read_line_primitives( 02045 char filename[300] , 02046 int *Nl , 02047 int **Np , 02048 double ***x , 02049 double ***y ) 02050 { 02051 FILE *fp_1; 02052 int i,j,k; 02053 float tmp1,tmp2; 02054 if(!(fp_1=fopen(filename,"r"))) { 02055 printf("Error while opening the file %s\n",filename); 02056 return(1); 02057 } 02058 02059 fscanf(fp_1,"%d\n",Nl); /*get number of lines*/ 02060 if((*Nl)<=0) { 02061 printf("Error: The number of lines is 0 %s\n",filename); 02062 return(2); 02063 } 02064 *Np=(int*)malloc( sizeof(int)*(*Nl)); 02065 *x=(double**)malloc( sizeof(double*)*(*Nl) ); /* x pixels coordinates */ 02066 *y=(double**)malloc( sizeof(double*)*(*Nl) ); /* y pixels coordinates */ 02067 02068 printf("Nl=%d\n",*Nl); 02069 for(i=0; i<(*Nl); i++) { 02070 fscanf(fp_1,"%d",&k); /*get number of points for the "i" line */ 02071 (*Np)[i]=k; 02072 printf("Np[%d]=%d\n",i,(*Np)[i]); 02073 (*x)[i]=(double*)malloc( sizeof(double)*k ); 02074 (*y)[i]=(double*)malloc( sizeof(double)*k); 02075 for(j=0; j<k; j++) { 02076 fscanf(fp_1,"%f %f\n",&tmp1,&tmp2); 02077 (*x)[i][j]=tmp1; 02078 (*y)[i][j]=tmp2; 02079 printf("x[%d][%d]=%f x[%d][%d]=%f\n",i,j,(*x)[i][j],i,j,(*y)[i][j]); 02080 } 02081 } 02082 fclose(fp_1); 02083 return(0); 02084 } 02085 02093 double calculate_factor_n( 02094 double **xx , 02095 double **yy , 02096 int Nl , 02097 int *Np , 02098 double x0 , 02099 double y0 02100 ) 02101 { 02102 double **x_aux,**y_aux; /* AUXILIAR MATRICES TO COPY THE X,Y COORDINATES */ 02103 double factor_n=0.0; /* AUXILIAR VARIABLE */ 02104 int i,m, cont=0; /* AUXILIAR VARIABLE */ 02105 02106 /* WE ALLOCATE MEMORY FOR AUXILARY POINTS AND WE NORMALIZE ORIGINAL POINTS */ 02107 x_aux=(double**)malloc( sizeof(double*)*Nl); /* x pixels coordinates */ 02108 y_aux=(double**)malloc( sizeof(double*)*Nl); /* y pixels coordinates */ 02109 for(i=0; i<Nl; i++) { 02110 x_aux[i]=(double*)malloc( sizeof(double)*Np[i] ); 02111 y_aux[i]=(double*)malloc( sizeof(double)*Np[i] ); 02112 } 02113 02114 for(m=0; m<Nl; m++) { 02115 for(i=0; i<Np[m]; i++) { 02116 cont++; 02117 x_aux[m][i]=xx[m][i]; 02118 y_aux[m][i]=yy[m][i]; 02119 x_aux[m][i]-=x0; 02120 y_aux[m][i]-=y0; 02121 factor_n+=x_aux[m][i]*x_aux[m][i]+y_aux[m][i]*y_aux[m][i]; 02122 } 02123 } 02124 factor_n=sqrt(factor_n/cont); 02125 /* WE FREE THE MEMORY */ 02126 if(x_aux!=NULL) { 02127 for(i=0; i<Nl; i++) { 02128 if(x_aux[i]!=NULL) free(x_aux[i]); 02129 } 02130 free(x_aux); 02131 } 02132 if(y_aux!=NULL) { 02133 for(i=0; i<Nl; i++) { 02134 if(y_aux[i]!=NULL) free(y_aux[i]); 02135 } 02136 free(y_aux); 02137 } 02138 02139 return(factor_n); 02140 } 02141 02142 02156 void ami_lens_distortion_model_evaluation( 02157 double *a, // INPUT POLYNOMIAL DISTORTION MODEL 02158 int Na, // INPUT DEGREE OF POLYNOMIAL DISTORTION MODEL 02159 double xc,double yc, // INPUT CENTER OF DISTORTION 02160 double x_input,double y_input, // INPUT POINT 02161 double *x_output,double *y_output // OUTPUT UNDISTORTED POINT 02162 ) 02163 { 02164 double norm=sqrt((x_input-xc)*(x_input-xc)+(y_input-yc)*(y_input-yc)); 02165 double A=ami_polynomial_evaluation(a,Na,norm); 02166 *x_output=xc+(x_input-xc)*A; 02167 *y_output=yc+(y_input-yc)*A; 02168 } 02169 02170 02182 double ami_inverse_lens_distortion_newton_raphson( 02183 double x,double y, /* POINT TO INVERSE (INPUT)*/ 02184 double x0,double y0, /* CENTER OF THE IMAGE (INPUT)*/ 02185 double *xt,double *yt, /* INVERVE POINT TRANSFORMED (OUTPUT) */ 02186 double *a, /* LENS DISTORTION MODEL POLYNOM */ 02187 int Na) /* DEGREE OF THE LENS DISTORTION MODEL POLYNOM */ 02188 { 02189 if(a[Na]==0.) return(-1); 02190 02191 // AUXILIARY VARIABLES 02192 double *b,*b2,root2,root; 02193 02194 // WE ALLOCATE MEMORY 02195 b=(double*)malloc( sizeof(double)*(Na+2) ); 02196 b2=(double*)malloc( sizeof(double)*(Na+2) ); 02197 02198 // WE DEFINE THE POLYNOM WE NEED TO COMPUTE ROOTS AND THE DERIVATIVE */ 02199 for(int i=1; i<(Na+2); i++){ b[i]=a[i-1];} 02200 for(int i=0; i<(Na+1); i++){ b2[i]=a[i]*(i+1);} 02201 root=sqrt((x-x0)*(x-x0)+(y-y0)*(y-y0)); 02202 if(root==0){*xt=x; *yt=y; free(b); free(b2);return(0.);} 02203 b[0]=-root; 02204 //NEWTON-RAPHSON TO COMPUTE THE POLYNOMIAL ROOT 02205 for(int k=0;k<10000;k++){ 02206 double pol_eval=ami_polynomial_evaluation(b,Na+1,root); 02207 double pol_der=ami_polynomial_evaluation(b2,Na,root); 02208 if(pol_der==0.) break; 02209 root2=root-pol_eval/pol_der; 02210 if(fabs(root-root2)<(fabs(root)*1e-8)){ 02211 root=root2; 02212 // printf("k=%d ",k); 02213 break; 02214 } 02215 root=root2; 02216 } 02217 02218 *xt=x0+(x-x0)*root/(-b[0]); 02219 *yt=y0+(y-y0)*root/(-b[0]); 02220 02221 // WE CHECK THE RESULT 02222 /* double x_output,y_output; 02223 ami_lens_distortion_model_evaluation(a,Na,x0,y0,*xt,*yt,&x_output,&y_output); 02224 printf("root=%lf (x,y)=(%1.0lf,%1.0lf) output (x,y)=(%lf,%lf) \n",root,x,y,x_output,y_output); 02225 system("pause"); */ 02226 02227 free(b); 02228 free(b2); 02229 return( ( (root+b[0])*(root+b[0]) ) ); 02230 } 02231 02232 02243 int build_l1r_vector(std::vector<double> &l1r,ami::point2d<double> &dc, double max_distance_corner,int Na, double *a) 02244 { 02245 //BUILD INTERMEDIATE VECTOR WITH L-1(r) FROM CENTER TO FURTHEST CORNER 02246 if(a[Na]==0.) return(-1); 02247 l1r.resize((int)(max_distance_corner+1.5)); 02248 02249 // AUXILIARY VARIABLES 02250 double *b,*b2,root2,root=1.; 02251 02252 // WE ALLOCATE MEMORY 02253 b=(double*)malloc( sizeof(double)*(Na+2) ); 02254 b2=(double*)malloc( sizeof(double)*(Na+2) ); 02255 02256 // WE DEFINE THE POLYNOM WE NEED TO COMPUTE ROOTS AND THE DERIVATIVE */ 02257 for(int i=1; i<(Na+2); i++){ b[i]=a[i-1];} 02258 for(int i=0; i<(Na+1); i++){ b2[i]=a[i]*(i+1);} 02259 02260 // WE BUILD THE VECTOR OF INVERSE LENS DISTORTION FUNCTION 02261 for(int dist=1; dist<(int)(max_distance_corner+1.5); dist++) 02262 { 02263 // WE DEFINE THE POLYNOM WE NEED TO COMPUTE ROOTS AND THE DERIVATIVE */ 02264 b[0]=-dist; 02265 02266 //NEWTON-RAPHSON TO COMPUTE THE POLYNOMIAL ROOT 02267 for(int k=0;k<10000;k++){ 02268 double pol_eval=ami_polynomial_evaluation(b,Na+1,root); 02269 double pol_der=ami_polynomial_evaluation(b2,Na,root); 02270 if(pol_der==0.) break; 02271 root2=root-pol_eval/pol_der; 02272 if(fabs(root-root2)<(fabs(root)*1e-8)){ 02273 root=root2; 02274 //printf("k=%d ",k); 02275 break; 02276 } 02277 root=root2; 02278 } 02279 02280 //PUSH RESULT 02281 l1r[dist]=root/dist; 02282 } 02283 free(b); 02284 free(b2); 02285 l1r[0]=l1r[1]; 02286 02287 return 0; 02288 } 02289 02290 02299 ami::image<unsigned char> undistort_image_inverse_fast(ami::image<unsigned char> input_image,int Na, 02300 double *a,ami::point2d<double> dc,const double &image_amplification_factor) 02301 { 02302 int width0=input_image.width(); 02303 int height0=input_image.height(); 02304 int size =width0*height0; 02305 int width=(int) (width0*image_amplification_factor); 02306 int height=(int) (height0*image_amplification_factor); 02307 int sizenew =width*height; 02308 02309 // WE CREATE OUTPUT IMAGE 02310 ami::image<unsigned char> output_image(width,height,0,0,0); 02311 02312 //CALCULATE MAXIMUM DISTANCE FROM CENTRE TO A CORNER 02313 ami::point2d<double> corner(0,0); 02314 double max_distance_corner= (dc-corner).norm(); 02315 corner.y=height0; 02316 double distance_corner=(dc-corner).norm(); 02317 if(distance_corner>max_distance_corner) max_distance_corner=distance_corner; 02318 corner.x=width0; 02319 distance_corner=(dc-corner).norm(); 02320 if(distance_corner>max_distance_corner) max_distance_corner=distance_corner; 02321 corner.y=0; 02322 distance_corner=(dc-corner).norm(); 02323 if(distance_corner>max_distance_corner) max_distance_corner=distance_corner; 02324 02325 //BUILD INTERMEDIATE VECTOR 02326 std::vector<double> l1r; 02327 if(Na==0) { 02328 output_image=input_image; 02329 return(output_image); 02330 } 02331 if(build_l1r_vector(l1r,dc,max_distance_corner,Na,a)==-1) { 02332 output_image=input_image; 02333 return(output_image); 02334 } 02335 02336 int nc,n2,i,j,n2new; 02337 for (nc=0; nc<3; nc++) 02338 { 02339 n2=nc*size; 02340 n2new=nc*sizenew; 02341 #pragma omp parallel for \ 02342 shared(width,height,width0,height0,output_image,input_image,size,nc,n2)\ 02343 private(i,j) 02344 for(i=0; i<height; i++) { 02345 for(j=0; j<width; j++) { 02346 ami::point2d<double> temp((double) j/image_amplification_factor,(double) i/image_amplification_factor); 02347 double distance_centre= (dc-ami::point2d<double>((double) temp.x,(double) temp.y)).norm(); 02348 //INTERPOLATION 02349 int ind=(int)distance_centre; 02350 double dl1r=l1r[ind]+(distance_centre-ind)*(l1r[ind+1]-l1r[ind]); 02351 ami::point2d<double> p; 02352 p.x=dc.x+(temp.x-dc.x)*dl1r; 02353 p.y=dc.y+(temp.y-dc.y)*dl1r; 02354 int m = (int)p.y; 02355 int n = (int)p.x; 02356 02357 if(0<=m && m<height0 && 0<=n && n<width0) 02358 { 02359 //COLOUR INTERPOLATION 02360 double di=p.y-m; 02361 double dj=p.x-n; 02362 unsigned int k=i*width+j; 02363 unsigned int k0=m*width0+n; 02364 double accum=0; 02365 double w_accum=0; 02366 double w=((1.-di)*(1.-dj)); 02367 02368 accum+=(double)w*input_image[k0+n2]; 02369 w_accum+=w; 02370 02371 02372 if( (di*(1.-dj))>0. && (m+1)<height0) 02373 { 02374 k0=(m+1)*width0+n; 02375 w=(di*(1.-dj)); 02376 accum+=(double)w*input_image[k0+n2]; 02377 w_accum+=w; 02378 } 02379 if( ((1-di)*(dj))>0. && (n+1)<width0) 02380 { 02381 k0=(m)*width0+n+1; 02382 w=(1.-di)*(dj); 02383 accum+=(double)w*input_image[k0+n2]; 02384 w_accum+=w; 02385 } 02386 if( ((di)*(dj))>0. && (n+1)<width0 && (m+1)<height0) 02387 { 02388 k0=(m+1)*width0+n+1; 02389 w=(di)*(dj); 02390 accum+=(double)w*input_image[k0+n2]; 02391 w_accum+=w; 02392 } 02393 02394 02395 if(w_accum>0.) output_image[k+n2new]=(unsigned char) (accum/w_accum); 02396 } 02397 } 02398 } 02399 } 02400 02401 return(output_image); 02402 }
1.8.0
