algebraic lens distortion

- Main Page
- Namespaces
- Classes
- Files

- File List
- File Members

All Classes Namespaces Files Functions Variables Defines
ami_pol.c
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
    00024 #include "ami_pol.h"
    00025 #include "stdlib.h"
    00026 #include "stdio.h"
    00027
    00028
    00035 long double ami_horner(
    00036     long double *pol ,
    00037     int degree ,
    00038     long double x ,
    00039     long double *fp )
    00040 {
    00041     long double PPX=pol[degree],PX=pol[degree];
    00042     int k;
    00043     for(k=degree-1; k>0; k--) {
    00044         PX=PX*x+pol[k];
    00045         PPX=PPX*x+PX;
    00046     }
    00047     PX=PX*x+pol[0];
    00048     *fp=PPX;
    00049     return(PX);
    00050 }
    00051
    00060 long double ami_root_bisection(
    00061     long double *pol  ,
    00062     int degree  ,
    00063     long double a  ,
    00064     long double b ,
    00065     long double TOL )
    00066 {
    00067     long double a2=a,b2=b,fa,fb,c,c0,fc,fp;
    00068     int iter=0,maxiter=100000;
    00069
    00070     fp=0.0; /*Added by Luis Gomez*/
    00071
    00072     /* we evaluate the polynomial at interval extrema */
    00073     fa=ami_horner(pol,degree,a,&fp);
    00074     fb=ami_horner(pol,degree,b,&fp);
    00075     /* we check if there is a root in the interval */
    00076     if(fa*fb>0) return(0.5*(a+b));
    00077     /* we evaluate the interval middle point */
    00078     c=(a2+b2)*0.5;
    00079     c0=c+1e30;
    00080     /* we perform Newton-Raphson or Bisection iterations */
    00081     while( (b2-a2)>(TOL*fabs(b2)+1e-10) &&  fabs(c-c0)>(TOL*fabs(c0)+1e-10) && (iter++)<maxiter) {
    00082         fc=ami_horner(pol,degree,c,&fp);
    00083         /* we try Newton-Raphson */
    00084         c0=c;
    00085         if(fp!=0.) {
    00086             c=c-fc/fp;
    00087             /* printf("c=%f\n",(double) c); */
    00088             if( c>=a2 && c<=b2 ) continue;
    00089         }
    00090         /* if Newton-Raphson fails, we try bisection */
    00091         c=(a2+b2)*0.5;
    00092         fc=ami_horner(pol,degree,c,&fp);
    00093         c0=c+1e30;
    00094         /* we check if we have arrive to the root */
    00095         if(fabs(fc)<TOL) break;
    00096         /* we perform bisection iteration */
    00097         if((fa*fc)<0) {
    00098             b2=c;
    00099             fb=fc;
    00100         }
    00101         else {
    00102             a2=c;
    00103             fa=fc;
    00104         }
    00105         c=(a2+b2)*0.5;
    00106     }
    00107     if(iter>=maxiter){return((a2+b2)*0.5);}
    00108     return(c);
    00109 }
    00110
    00119 int ami_polynomial_root(
    00120     double *pol ,
    00123     int degree ,
    00124     double *root_r ,
    00125     double *root_i )
    00128 {
    00129
    00130     long double a,b,*p,*p_aux,*ap,*f,fp,*pol2,fa,fb,TOL=1e-14;
    00131     long double max,n_factor;
    00132     int m,k,l,Nr,N=degree;
    00133
    00134     p=p_aux=ap=f=pol2=NULL;
    00135     fp=0.0;
    00136     a=b=fa=fb=max=n_factor=0.0; /*Added by Luis Gomez*/
    00137     Nr=0;/*Added by Luis Gomez*/
    00138
    00139     /* WE ALLOCATE MEMORY */
    00140     p=(long double*)malloc(sizeof(long double)*(N+2));
    00141     p_aux=(long double*)malloc(sizeof(long double)*(N+2));
    00142     ap=(long double*)malloc(sizeof(long double)*(N+1));
    00143     f=(long double*)malloc(sizeof(long double)*(N+1));
    00144     pol2=(long double*)malloc(sizeof(long double)*(N+2));
    00145
    00146     /* POLYNOMIAL NORMALIZATION */
    00147     if(pol[0]!=1.) {
    00148         for(k=0; k<=N; k++) pol[k]/=pol[0];
    00149     }
    00150
    00151     /* WE REORDER THE POLYNOM */
    00152     for(k=0; k<=N; k++) pol2[k]=pol[N-k];
    00153     pol2[N+1]=0.0; /*Added by Luis Gomez*/
    00154
    00155     /* WE NORMALIZE POLINOMIAL COEFFICIENTS TO AVOID POLYNOMIAL EVALUATION IN LARGE NUMBERS*/
    00156     n_factor=0;
    00157     for(k=0; k<N; k++) {
    00158         if(fabs(pol2[k])>10.) {
    00159             max=log(fabs((double) pol2[k])/10.)/(N-k);
    00160             if(max>n_factor) n_factor=max;
    00161         }
    00162     }
    00163
    00164     n_factor=exp((double) n_factor);
    00165     max=n_factor;
    00166     for(k=N-1; k>=0; k--) {
    00167         pol2[k]/=max;
    00168         max*=n_factor;
    00169     }
    00170
    00171     /* WE COMPUTE FACTORIAL NUMBERS */
    00172     f[0]=1.;
    00173     for(k=1; k<=N; k++) f[k]=f[k-1]*k;
    00174
    00175     /* WE COMPUTE THE INITIAL INTERVAL WHERE ALL ROOTS ARE INCLUDED */
    00176     max=fabs(pol2[0]);
    00177     for(k=1; k<N; k++) {
    00178         if(fabs(pol2[k])>max)
    00179             max=fabs(pol2[k]);
    00180     }
    00181     /* WE INITIALIZE THE INTERVALS */
    00182     p[0]=-1.-max/fabs(pol2[N]);
    00183     /* WE COMPUTE THE POLYNOMIAL DEGREE-1 DERIVATE ROOT */
    00184     p[1]=-(pol2[N-1]*f[N-1])/(pol2[N]*f[N]);
    00185     for(k=2; k<=(N+1); k++) p[k]=-p[0];
    00186     for(k=0; k<=N; k++)p_aux[k]=p[k];
    00187     p_aux[N+1]=0.0; /*Added by Luis Gomez*/
    00188     /* WE COMPUTE THE DERIVATIVE POLINOMIAL ROOTS IN A RECURSIVE WAY */
    00189     for(k=N-2; k>=0; k--) {
    00190         /* we compute polynomial derivative coefficients */
    00191         for(l=0; l<=(N-k); l++) {
    00192             ap[l]=pol2[l+k]*(f[k+l]/f[l]);
    00193         }
    00194         /* we check the valid intervals to compute derivative polynomial roots */
    00195         m=1;
    00196         fa=ami_horner(ap,N-k,p_aux[0],&fp);
    00197         for(l=1; l<=(N-k); l++) {
    00198             fb=ami_horner(ap,N-k,p_aux[l],&fp);
    00199             if((fa*fb)<=0 || fabs(fb)<=TOL) {
    00200                 p[m++]=p_aux[l];
    00201                 fa=fb;
    00202             }
    00203         }
    00204         for(l=m; l<N; l++) p[l]=p[N];
    00205         /* we compute the derivative polynomial roots in each interval */
    00206         for(l=1; l<=(N-k); l++) {
    00207             p_aux[l]=ami_root_bisection(ap,N-k,p[l-1],p[l],TOL);
    00208         }
    00209     }
    00210
    00211     /* WE STORE THE ROOTS */
    00212     root_i[0]=0.; /* we fit the complex component of the root to 0 */
    00213     Nr=0;
    00214     for(k=1; k<=N; k++) {
    00215         /* we check if the polynomial has a root in the point */
    00216         a=(p_aux[k]+p_aux[k-1])*0.5;
    00217         b=(p_aux[k]+p_aux[k+1])*0.5;
    00218         fa=ami_horner(pol2,degree,a,&fp);
    00219         fb=ami_horner(pol2,degree,b,&fp);
    00220         if(fa*fb<0 || fabs(ami_horner(pol2,degree,p_aux[k],&fp))<TOL) {
    00221             root_i[Nr]=0; /* we fit the complex component of the root to 0 */
    00222             root_r[Nr++]=p_aux[k]*n_factor; /* we denormalize the root */
    00223         }
    00224     }
    00225     /* we free the memory */
    00226     free(p);
    00227     free(ap);
    00228     free(f);
    00229     free(pol2);
    00230     free(p_aux);
    00231     /* we return the number of real roots estimated */
    00232     return(Nr);
    00233
    00234 }