algebraic lens distortion

- Main Page
- Namespaces
- Classes
- Files

- File List
- File Members

All Classes Namespaces Files Functions Variables Defines
lens_distortion.cpp
Go to the documentation of this file.

    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 }