00001 #include <math.h>
00002 #include <stdio.h>
00003 #include <stdlib.h>
00004 #include <stddef.h>
00005 
00006 #include "mrilib.h"
00007 
00008 #if 0
00009 typedef struct { float r , i ; } complex ;
00010 #endif
00011 
00012 #ifndef MAX
00013 #define MAX(a,b) (((a)<(b)) ? (b) : (a))
00014 #endif
00015 
00016 #define NPOL 1
00017 #define NFIT ((NPOL+1)*(NPOL+1))
00018 
00019 void pfit( int nz , complex * z , int nfit , float * psi[] , float * fit ) ;
00020 
00021 int main( int argc , char * argv[] )
00022 {
00023    MRI_IMAGE * rim , * iim , * cxim ;
00024    complex * cxar ;
00025    int nz , ii,jj,kk,ll  , nx,ny ;
00026    float * psi[NFIT] ;
00027    float xx , yy ;
00028    float fit[NFIT] ;
00029 
00030    rim = mri_read( "re_3.14" ) ; iim = mri_read( "im_3.14" ) ;
00031    cxim = mri_pair_to_complex( rim , iim ) ;
00032    cxar = MRI_COMPLEX_PTR(cxim) ;
00033    nx = cxim->nx ; ny = cxim->ny ; nz = nx * ny ;
00034    mri_free( rim ) ; mri_free( iim ) ;
00035 
00036    for( ii=0 ; ii < NFIT ; ii++ )
00037       psi[ii] = (float *) malloc( sizeof(float) * nz ) ;
00038 
00039    for( jj=0 ; jj < ny ; jj++ ){
00040       yy = (jj - 0.5*ny)/(0.5*ny) ;
00041       for( ii=0 ; ii < nx ; ii++ ){
00042          xx = (ii - 0.5*nx)/(0.5*nx) ;
00043          for( ll=0 ; ll <= NPOL ; ll++ )
00044             for( kk=0 ; kk <= NPOL ; kk++ )
00045                psi[kk+ll*(NPOL+1)][ii+jj*nx] = pow(xx,kk) * pow(yy,ll) ;
00046       }
00047    }
00048 
00049    for( ii=0 ; ii < NFIT ; ii++ ) fit[ii] = 1.234 * ii ;
00050 
00051    pfit( nz,cxar , NFIT , psi , fit ) ;
00052    exit(0) ;
00053 }
00054 
00055 /*------------------------------------------------------------------------
00056    Fit the phase.
00057 
00058       nz    =  Number of complex input points.
00059       z     =  Array of nz complex numbers.
00060       nfit  =  Number of fit parameters.
00061       psi   =  Array of nfit pointers to float arrays of length nz.
00062       fit   =  Array of nfit float fit parameters.
00063 
00064    The phase of z[k] will be fit to the function
00065 
00066      phi[k] = sum( fit[k] * psi[j][k] , j=0..nfit-1 )
00067 
00068    On input, fit[] contains the initial estimates of the fitting
00069    parameters.  On output, it contains the final estimate.
00070 
00071    The method is to maximize the function
00072 
00073      E = sum( Abs(z[k])**2  * cos( phi[k] - Arg(z[k]) ) , k=0..nz-1 )
00074 
00075    using Newton's method.
00076 --------------------------------------------------------------------------*/
00077 
00078 void pfit( int nz , complex * z , int nfit , float * psi[] , float * fit )
00079 {
00080    float * zsqr , * zarg ;
00081    double * dfit , * egrad , * ehess ;
00082    double phi , cph,sph , sum , delta ;
00083    float ztop ;
00084    int ii , jj,kk , nite ;
00085 
00086    /*** Compute Abs(z[k])**2 and Arg(z[k]) ***/
00087 
00088    zsqr = (float *) malloc( sizeof(float) * nz ) ;
00089    zarg = (float *) malloc( sizeof(float) * nz ) ;
00090 
00091    if( zsqr==NULL || zarg==NULL ){
00092       fprintf(stderr,"\nCan't malloc workspace in pfit\n") ;
00093       exit(1) ;
00094    }
00095 
00096    ztop = 0.0 ;
00097    for( ii=0 ; ii < nz ; ii++ ){
00098       zsqr[ii] = z[ii].r * z[ii].r + z[ii].i * z[ii].i ;
00099       zarg[ii] = (zsqr[ii] > 0.0) ? atan2(z[ii].i,z[ii].r) : 0.0 ;
00100       ztop     = MAX( ztop , zsqr[ii] ) ;
00101    }
00102    ztop = 1.0 / ztop ;
00103    for( ii=0 ; ii < nz ; ii++ ) zsqr[ii] *= ztop ;
00104 
00105    /*** Allocate space for Newton's method stuff ***/
00106 
00107    dfit  = (double *) malloc( sizeof(double) * nfit ) ;
00108    egrad = (double *) malloc( sizeof(double) * nfit ) ;
00109    ehess = (double *) malloc( sizeof(double) * nfit * nfit ) ;
00110 
00111    for( ii=0 ; ii < nfit ; ii++ ) dfit[ii] = fit[ii] ;  /* initialize */
00112 
00113    /*** Begin Newton iterations ***/
00114 
00115 fprintf(stderr,"pfit starts with nz=%d\n",nz) ;
00116 
00117    nite = 0 ;
00118    do{
00119 
00120       /*** compute gradient and Hessian ***/
00121 
00122 #define HH(k,j) ehess[(k)+(j)*nfit]
00123 
00124       /*** initialize to zero ***/
00125 
00126       for( jj=0 ; jj < nfit ; jj++ ){
00127          egrad[jj] = 0.0 ;
00128          for( kk=0 ; kk <= jj ; kk++ ) HH(jj,kk) = 0.0 ;
00129       }
00130 
00131       /*** sum them up over all points in z[];
00132            note that only the lower triangle of the Hessian
00133            is computed (HH(jj,kk) for jj >= kk), since
00134            the matrix is symmetric and that's all we'll need ***/
00135 
00136       for( ii=0 ; ii < nz ; ii++ ){
00137 
00138          phi = -zarg[ii] ;                   /* compute fitted phase */
00139          for( jj=0 ; jj < nfit ; jj++ )      /* error at z[ii]       */
00140             phi += dfit[jj] * psi[jj][ii] ;
00141 
00142          cph = cos(phi) * zsqr[ii] ;         /* some useful factors */
00143          sph = sin(phi) * zsqr[ii] ;
00144 
00145          for( jj=0 ; jj < nfit ; jj++ ){
00146             egrad[jj] += sph * psi[jj][ii] ; /* gradient */
00147 
00148             for( kk=0 ; kk <= jj ; kk++ )
00149                HH(jj,kk) += cph * psi[jj][ii] * psi[kk][ii] ; /* Hessian */
00150          }
00151       }
00152 
00153 fprintf(stderr,"\nHessian:\n") ;
00154 for(jj=0;jj<nfit;jj++){
00155    for(kk=0;kk<=jj;kk++) fprintf(stderr," %g",HH(jj,kk)) ;
00156    fprintf(stderr,"\n") ;
00157 }
00158 
00159       sph = cph = 0.0 ;
00160       for( jj=0 ; jj < nfit ; jj++ ){
00161          cph = MIN( cph , HH(jj,jj) ) ;
00162          sph = MAX( sph , HH(jj,jj) ) ;
00163       }
00164       if( cph <= 0 ){
00165          cph = -cph + 0.01*fabs(sph) ;
00166          for( jj=0 ; jj < nfit ; jj++ ) HH(jj,jj) += cph ;
00167       }
00168 
00169       /*** Choleski decompose Hessian ***/
00170 
00171       for( ii=0 ; ii < nfit ; ii++ ){    /* in the ii-th row */
00172          for( jj=0 ; jj < ii ; jj++ ){   /* in the jj-th column */
00173             sum = HH(ii,jj) ;
00174             for( kk=0 ; kk < jj ; kk++ ) sum -= HH(ii,kk) * HH(jj,kk) ;
00175             HH(ii,jj) = sum / HH(jj,jj) ;
00176          }
00177          sum = HH(ii,ii) ;
00178          for( kk=0 ; kk < ii ; kk++ ) sum -= HH(ii,kk) * HH(ii,kk) ;
00179          if( sum <= 0.0 ){fprintf(stderr,"Choleski fails: row %d\n",ii);exit(1);}
00180          HH(ii,ii) = sqrt(sum) ;
00181       }
00182 
00183       /*** forward solve ***/
00184 
00185       for( ii=0 ; ii < nfit ; ii++ ){
00186          sum = egrad[ii] ;
00187          for( jj=0 ; jj < ii ; jj++ ) sum -= HH(ii,jj) * egrad[jj] ;
00188          egrad[ii] = sum / HH(ii,ii) ;
00189       }
00190 
00191       /*** backward solve ***/
00192 
00193       delta = 0.0 ;
00194       for( ii=nfit-1 ; ii >= 0 ; ii-- ){
00195          sum = egrad[ii] ;
00196          for( jj=ii+1 ; jj < nfit ; jj++ ) sum -= HH(jj,ii) * egrad[jj] ;
00197          egrad[ii] = sum / HH(ii,ii) ;
00198          dfit[ii] -= egrad[ii] ;       /* change in fit parameters */
00199 
00200          sum    = fabs(dfit[ii]) ;
00201          delta += fabs(egrad[ii]) / MAX(sum,1.0) ;
00202       }
00203       delta /= nfit ;
00204 
00205       /*** check if we are done ***/
00206 
00207       nite++ ;
00208 
00209 fprintf(stderr,"\nIteration %d:\n",nite) ;
00210 for( ii=0 ; ii < nfit ; ii++ )
00211    fprintf(stderr,"  delta[%d] = %g   dfit[%d] = %g\n",
00212            ii,egrad[ii] , ii,dfit[ii] ) ;
00213 
00214    } while( delta > 1.e-3 && nite < 9 ) ;
00215 
00216    /*** clean up mess and exit ***/
00217 
00218    free(zsqr) ; free(zarg) ;
00219    free(dfit) ; free(egrad) ; free(ehess) ;
00220 
00221    for( ii=0 ; ii < nfit ; ii++ ) fit[ii] = dfit[ii] ;  /* output */
00222    return ;
00223 }