00001 #include "mrilib.h"
00002 
00003 /*** NOT 7D SAFE ***/
00004 
00005 /*************************************************************************
00006  **   warp3D version of mri_3dalign.c -- RWCox -- November 2004         **
00007  *************************************************************************/
00008 
00009 /*-----------------------------------------------------------------------*/
00010 /*! Compute the pseudo-inverse of a matrix stored in a 2D float image.
00011     If the input is mXn, the output is nXm.  wt[] is an optional array
00012     of positive weights, m of them.  The result can be used to solve
00013     the weighted least squares problem
00014       [imc] [b] = [v]
00015     where [b] is an n-vector and [v] is an m-vector, where m > n.
00016 -------------------------------------------------------------------------*/
00017 
00018 static MRI_IMAGE * mri_psinv( MRI_IMAGE *imc , float *wt )
00019 {
00020    float *rmat=MRI_FLOAT_PTR(imc) ;
00021    int m=imc->nx , n=imc->ny , ii,jj,kk ;
00022    double *amat , *umat , *vmat , *sval , *xfac , smax,del,ww ;
00023    MRI_IMAGE *imp ; float *pmat ;
00024    register double sum ;
00025    int do_svd=0 ;
00026 
00027    amat = (double *)calloc( sizeof(double),m*n ) ;  /* input matrix */
00028    xfac = (double *)calloc( sizeof(double),n   ) ;  /* column norms of [a] */
00029 
00030 #define R(i,j) rmat[(i)+(j)*m]   /* i=0..m-1 , j=0..n-1 */
00031 #define A(i,j) amat[(i)+(j)*m]   /* i=0..m-1 , j=0..n-1 */
00032 #define P(i,j) pmat[(i)+(j)*n]   /* i=0..n-1 , j=0..m-1 */
00033 
00034    /* copy input matrix into amat */
00035 
00036    for( ii=0 ; ii < m ; ii++ )
00037      for( jj=0 ; jj < n ; jj++ ) A(ii,jj) = R(ii,jj) ;
00038 
00039    /* weight rows? */
00040 
00041    if( wt != NULL ){
00042      for( ii=0 ; ii < m ; ii++ ){
00043        ww = wt[ii] ;
00044        for( jj=0 ; jj < n ; jj++ ) A(ii,jj) *= ww ;
00045      }
00046    }
00047 
00048    /* scale each column to have norm 1 */
00049 
00050    for( jj=0 ; jj < n ; jj++ ){
00051      sum = 0.0 ;
00052      for( ii=0 ; ii < m ; ii++ ) sum += A(ii,jj)*A(ii,jj) ;
00053      if( sum > 0.0 ) sum = 1.0/sqrt(sum) ; else do_svd = 1 ;
00054      xfac[jj] = sum ;
00055      for( ii=0 ; ii < m ; ii++ ) A(ii,jj) *= sum ;
00056    }
00057 
00058    /*** compute using Choleski or SVD ***/
00059 
00060    if( do_svd || AFNI_yesenv("AFNI_WARPDRIVE_SVD") ){ /***--- SVD method ---***/
00061 
00062 #define U(i,j) umat[(i)+(j)*m]
00063 #define V(i,j) vmat[(i)+(j)*n]
00064 
00065      umat = (double *)calloc( sizeof(double),m*n ); /* left singular vectors */
00066      vmat = (double *)calloc( sizeof(double),n*n ); /* right singular vectors */
00067      sval = (double *)calloc( sizeof(double),n   ); /* singular values */
00068 
00069      /* compute SVD of scaled matrix */
00070 
00071      svd_double( m , n , amat , sval , umat , vmat ) ;
00072 
00073      free((void *)amat) ;  /* done with this */
00074 
00075      /* find largest singular value */
00076 
00077      smax = sval[0] ;
00078      for( ii=1 ; ii < n ; ii++ ) if( sval[ii] > smax ) smax = sval[ii] ;
00079 
00080      if( smax <= 0.0 ){                        /* this is bad */
00081        fprintf(stderr,"** ERROR: SVD fails in mri_warp3D_align_setup!\n");
00082        free((void *)xfac); free((void *)sval);
00083        free((void *)vmat); free((void *)umat); return NULL;
00084      }
00085 
00086      for( ii=0 ; ii < n ; ii++ )
00087        if( sval[ii] < 0.0 ) sval[ii] = 0.0 ;  /* should not happen */
00088 
00089 #define PSINV_EPS 1.e-8
00090 
00091      /* "reciprocals" of singular values:  1/s is actually s/(s^2+del) */
00092 
00093      del = PSINV_EPS * smax*smax ;
00094      for( ii=0 ; ii < n ; ii++ )
00095        sval[ii] = sval[ii] / ( sval[ii]*sval[ii] + del ) ;
00096 
00097      /* create pseudo-inverse */
00098 
00099      imp  = mri_new( n , m , MRI_float ) ;   /* recall that m > n */
00100      pmat = MRI_FLOAT_PTR(imp) ;
00101 
00102      for( ii=0 ; ii < n ; ii++ ){
00103        for( jj=0 ; jj < m ; jj++ ){
00104          sum = 0.0 ;
00105          for( kk=0 ; kk < n ; kk++ ) sum += sval[kk] * V(ii,kk) * U(jj,kk) ;
00106          P(ii,jj) = (float)sum ;
00107        }
00108      }
00109      free((void *)sval); free((void *)vmat); free((void *)umat);
00110 
00111    } else { /***----- Choleski method -----***/
00112 
00113      vmat = (double *)calloc( sizeof(double),n*n ); /* normal matrix */
00114 
00115      for( ii=0 ; ii < n ; ii++ ){
00116        for( jj=0 ; jj <= ii ; jj++ ){
00117          sum = 0.0 ;
00118          for( kk=0 ; kk < m ; kk++ ) sum += A(kk,ii) * A(kk,jj) ;
00119          V(ii,jj) = sum ;
00120        }
00121        V(ii,ii) += PSINV_EPS ;   /* note V(ii,ii)==1 before this */
00122      }
00123 
00124      /* Choleski factor */
00125 
00126      for( ii=0 ; ii < n ; ii++ ){
00127        for( jj=0 ; jj < ii ; jj++ ){
00128          sum = V(ii,jj) ;
00129          for( kk=0 ; kk < jj ; kk++ ) sum -= V(ii,kk) * V(jj,kk) ;
00130          V(ii,jj) = sum / V(jj,jj) ;
00131        }
00132        sum = V(ii,ii) ;
00133        for( kk=0 ; kk < ii ; kk++ ) sum -= V(ii,kk) * V(ii,kk) ;
00134        if( sum <= 0.0 ){
00135          fprintf(stderr,"** ERROR: Choleski fails in mri_warp3D_align_setup!\n");
00136          free((void *)xfac); free((void *)amat); free((void *)vmat); return NULL ;
00137        }
00138        V(ii,ii) = sqrt(sum) ;
00139      }
00140 
00141      /* create pseudo-inverse */
00142 
00143      imp  = mri_new( n , m , MRI_float ) ;   /* recall that m > n */
00144      pmat = MRI_FLOAT_PTR(imp) ;
00145 
00146      sval = (double *)calloc( sizeof(double),n ) ; /* row #jj of A */
00147 
00148      for( jj=0 ; jj < m ; jj++ ){
00149        for( ii=0 ; ii < n ; ii++ ) sval[ii] = A(jj,ii) ; /* extract row */
00150 
00151        for( ii=0 ; ii < n ; ii++ ){  /* forward solve */
00152          sum = sval[ii] ;
00153          for( kk=0 ; kk < ii ; kk++ ) sum -= V(ii,kk) * sval[kk] ;
00154          sval[ii] = sum / V(ii,ii) ;
00155        }
00156        for( ii=n-1 ; ii >= 0 ; ii-- ){  /* backward solve */
00157          sum = sval[ii] ;
00158          for( kk=ii+1 ; kk < n ; kk++ ) sum -= V(kk,ii) * sval[kk] ;
00159          sval[ii] = sum / V(ii,ii) ;
00160        }
00161 
00162        for( ii=0 ; ii < n ; ii++ ) P(ii,jj) = (float)sval[ii] ;
00163      }
00164      free((void *)amat); free((void *)vmat); free((void *)sval);
00165    }
00166 
00167    /* rescale rows from norming */
00168 
00169    for( ii=0 ; ii < n ; ii++ ){
00170      for( jj=0 ; jj < m ; jj++ ) P(ii,jj) *= xfac[ii] ;
00171    }
00172    free((void *)xfac);
00173 
00174    /* rescale cols for weight? */
00175 
00176    if( wt != NULL ){
00177      for( ii=0 ; ii < m ; ii++ ){
00178        ww = wt[ii] ;
00179        for( jj=0 ; jj < n ; jj++ ) P(jj,ii) *= ww ;
00180      }
00181    }
00182 
00183    return imp;
00184 }
00185 
00186 /*-----------------------------------------------------------------------*/
00187 
00188 #define WW(i,j,k) wf[(i)+(j)*nx+(k)*nxy]
00189 
00190 static void mri_warp3D_align_edging_default( int  nx   , int  ny   , int  nz   ,
00191                                              int *xfade, int *yfade, int *zfade )
00192 {
00193    char *ef=my_getenv("AFNI_VOLREG_EDGING") , *eq ;
00194 
00195    if( ef == NULL ){                  /* the 5% solution */
00196      *xfade = (int)(0.05*nx+0.5) ;
00197      *yfade = (int)(0.05*ny+0.5) ;
00198      *zfade = (int)(0.05*nz+0.5) ;
00199    } else {
00200      float ff = strtod(ef,&eq) ;
00201      if( ff < 0 ){                   /* again, 5% */
00202        *xfade = (int)(0.05*nx+0.5) ;
00203        *yfade = (int)(0.05*ny+0.5) ;
00204        *zfade = (int)(0.05*nz+0.5) ;
00205      } else {
00206        if( *eq == '%' ){            /* the whatever % solution */
00207          *xfade = (int)(0.01*ff*nx+0.5) ;
00208          *yfade = (int)(0.01*ff*ny+0.5) ;
00209          *zfade = (int)(0.01*ff*nz+0.5) ;
00210        } else {                     /* the fixed value solution */
00211          *xfade = (int)( MIN(0.25*nx,ff) ) ;
00212          *yfade = (int)( MIN(0.25*ny,ff) ) ;
00213          *zfade = (int)( MIN(0.25*nz,ff) ) ;
00214        }
00215      }
00216    }
00217 }
00218 
00219 /*--------------------------------------------------------------------------*/
00220 /*! Get a default value of delta for parameter #kpar, such that the
00221     RMS index change is about 1.
00222 ----------------------------------------------------------------------------*/
00223 
00224 static void mri_warp3D_get_delta( MRI_warp3D_align_basis *bas , int kpar )
00225 {
00226    float *pvec , dpar ;
00227    int   ii,jj,kk , nx,ny,nz,nxy , nite , ntot ;
00228    float xx,yy,zz , tx,ty,tz , dt,dtot ;
00229    float *wf ;
00230 
00231    if( bas == NULL || kpar < 0 || kpar >= bas->nparam ) return ;
00232    if( bas->param[kpar].fixed ) return ;
00233 
00234    /* load parameter vector with the identity value for all params */
00235 
00236    pvec = (float *)malloc(sizeof(float) * bas->nparam) ;
00237    for( ii=0 ; ii < bas->nparam ; ii++ )
00238      pvec[ii] = (bas->param[ii].fixed) ? bas->param[ii].val_fixed
00239                                        : bas->param[ii].ident ;
00240 
00241    nx = bas->imbase->nx ; ny = bas->imbase->ny ; nz = bas->imbase->nz ;
00242    nxy = nx*ny ;
00243 
00244    /* initial value for delta */
00245 
00246    dpar = 0.001 * ( fabs(bas->param[kpar].ident) + 1.0 ) ;
00247    nite = 0 ; wf = MRI_FLOAT_PTR(bas->imww) ;
00248 
00249    /* iterate:
00250        - compute transformation over all points with positive weight
00251        - compute distance moved
00252        - compute RMS of positive distances moved
00253        - adjust dpar up or down to move RMS distance moved towards 1.0 */
00254 
00255    if( bas->verb )
00256      fprintf(stderr,"+   get_delta param#%d [%s]: %f" ,
00257             kpar+1, bas->param[kpar].name , dpar ) ;
00258 
00259    while(1){
00260      pvec[kpar] = bas->param[kpar].ident + dpar ;  /* set param */
00261      bas->vwset( bas->nparam , pvec ) ;            /* load transform */
00262      ntot = 0 ; dtot = 0.0f ;
00263      for( kk=0 ; kk < nz ; kk++ ){
00264       zz = (float)kk ;
00265       for( jj=0 ; jj < ny ; jj++ ){
00266        yy = (float)jj ;
00267        for( ii=0 ; ii < nx ; ii++ ){
00268          if( WW(ii,jj,kk) == 0.0f ) continue ;     /* not counted */
00269          xx = (float)ii ;
00270                                                    /* forward transform */
00271          bas->vwfor( xx,yy,zz , &tx,&ty,&tz ) ;
00272          dt = (tx-xx)*(tx-xx) + (ty-yy)*(ty-yy) + (tz-zz)*(tz-zz) ;
00273          if( dt > 0.0f ){ ntot++ ; dtot += dt ; }
00274                                                    /* inverse transform */
00275          bas->vwinv( xx,yy,zz , &tx,&ty,&tz ) ;
00276          dt = (tx-xx)*(tx-xx) + (ty-yy)*(ty-yy) + (tz-zz)*(tz-zz) ;
00277          if( dt > 0.0f ){ ntot++ ; dtot += dt ; }
00278      }}}
00279      if( ntot > 0 ){
00280        dtot = sqrt( dtot/ntot ) ;        /* RMS positive distance moved */
00281        if( dtot > 0.909f && dtot < 1.100f ) break ;     /* good enough! */
00282        dtot = 1.0f / dtot ;                   /* dpar adjustment factor */
00283             if( dtot > 50.0f ) dtot = 50.0f ;
00284        else if( dtot < 0.02f ) dtot = 0.02f ;
00285      } else {
00286        dtot = 50.0f ;                     /* no movement? how peculiar! */
00287      }
00288      dpar *= dtot ;                          /* adjust dpar, up or down */
00289      if( bas->verb ) fprintf(stderr," %f",dpar) ;
00290      nite++ ; if( nite > 9 ) break ;
00291    } /* end of iteration loop */
00292 
00293    if( bas->verb ) fprintf(stderr,"\n") ;
00294 
00295    bas->param[kpar].delta = dpar ;   /* save result, whatever it is */
00296    free((void *)pvec) ;
00297    return ;
00298 }
00299 
00300 /*--------------------------------------------------------------------*/
00301 
00302 static MRI_IMAGE * mri_warp3D_align_fitim( MRI_warp3D_align_basis *bas ,
00303                                            MRI_IMAGE *cim ,
00304                                            int warp_mode , float delfac )
00305 {
00306    MRI_IMAGE *fitim , *pim , *mim ;
00307    float *fitar , *car=MRI_FLOAT_PTR(cim) ;
00308    int nfree=bas->nfree , *ima=MRI_INT_PTR(bas->imap) , nmap=bas->imap->nx ;
00309    int npar =bas->nparam ;
00310    float *pvec , *par , *mar ;
00311    int ii , pp ;
00312    float dpar , delta ;
00313 
00314    /*-- create image containing basis columns --*/
00315 
00316    fitim = mri_new( nmap , nfree+1 , MRI_float ) ;
00317    fitar = MRI_FLOAT_PTR(fitim) ;
00318    pvec  = (float *)malloc(sizeof(float) * npar) ;
00319 
00320 #define FMAT(i,j) fitar[(i)+(j)*nmap]  /* col dim=nmap, row dim=nfree+1 */
00321 
00322    /* column #nfree = base image itself */
00323 
00324    for( ii=0 ; ii < nmap ; ii++ ) FMAT(ii,nfree) = car[ima[ii]] ;
00325 
00326    pvec = (float *)malloc(sizeof(float) * npar) ;
00327 
00328    /* for each parameter:
00329        apply inverse transform to base image with param value up and down
00330        compute central difference to approximate derivative of base
00331         image wrt parameter
00332        store as a column in the fitim matrix */
00333 
00334    mri_warp3D_method( warp_mode ) ;  /* set interpolation mode */
00335    mri_warp3D_set_womask( bas->imsk ) ;
00336 
00337    for( pp=0 ; pp < npar ; pp++ ){
00338 
00339      if( bas->param[pp].fixed ) continue ;  /* don't do this one! */
00340 
00341      /* init all params to their identity transform value */
00342 
00343      for( ii=0 ; ii < npar ; ii++ )
00344        pvec[ii] = (bas->param[ii].fixed) ? bas->param[ii].val_fixed
00345                                          : bas->param[ii].ident ;
00346 
00347      /* change in the pp-th parameter to use for derivative */
00348 
00349      dpar = delfac * bas->param[pp].delta ;
00350 
00351      if( bas->verb )
00352        fprintf(stderr,"+   difference base by %f in param#%d [%s]\n",
00353                dpar , pp+1 , bas->param[pp].name ) ;
00354 
00355      pvec[pp] = bas->param[pp].ident + dpar ;   /* set positive change */
00356      bas->vwset( npar , pvec ) ;                 /* put into transform */
00357      pim = mri_warp3D( cim , 0,0,0 , bas->vwinv ) ;      /* warp image */
00358 
00359      pvec[pp] = bas->param[pp].ident - dpar ;   /* set negative change */
00360      bas->vwset( npar , pvec ) ;
00361      mim = mri_warp3D( cim , 0,0,0 , bas->vwinv ) ;
00362 
00363      /* compute derivative */
00364 
00365      delta = bas->scale_init / ( 2.0f * dpar ) ;
00366      par = MRI_FLOAT_PTR(pim) ; mar = MRI_FLOAT_PTR(mim) ;
00367      for( ii=0 ; ii < nmap ; ii++ )
00368        FMAT(ii,pp) = delta * ( par[ima[ii]] - mar[ima[ii]] ) ;
00369 
00370      mri_free(pim) ; mri_free(mim) ;  /* no longer needed */
00371    }
00372 
00373    mri_warp3D_set_womask( NULL ) ;
00374    free((void *)pvec) ;
00375 
00376    return(fitim) ;
00377 }
00378 
00379 /*-------------------------------------------------------------------------
00380    Input:  pointer to a filled in MRI_warp3D_align_basis struct.
00381    Output: 0 if setup went OK, 1 if it failed.
00382            Some elements in the MRI_warp3D_align_basis struct
00383            will be filled in for internal use in MRI_warp3D_align_one().
00384            They can be freed later with  MRI_warp3D_align_cleanup().
00385 ---------------------------------------------------------------------------*/
00386 
00387 int mri_warp3D_align_setup( MRI_warp3D_align_basis *bas )
00388 {
00389    MRI_IMAGE *cim , *fitim ;
00390    int nx, ny, nz, nxy, nxyz , ii,jj,kk , nmap, *im ;
00391    float *wf , *wtar , clip , clip2 ;
00392    int   *ima , pp , wtproc , npar , nfree ;
00393    byte  *msk ;
00394    int ctstart ;
00395 
00396 ENTRY("mri_warp3D_align_setup") ;
00397 
00398    ctstart = NI_clock_time() ;
00399 
00400    /*- check for good inputs -*/
00401 
00402    if( bas == NULL     || bas->imbase == NULL ) RETURN(1) ;
00403    if( bas->nparam < 1 || bas->param  == NULL ) RETURN(1) ;
00404    if( bas->vwfor == NULL ||
00405        bas->vwinv == NULL || bas->vwset == NULL ) RETURN(1) ;
00406 
00407    /*- set defaults in bas, if values weren't set by user -*/
00408 
00409    if( bas->scale_init <= 0.0f ) bas->scale_init = 1.0f ;
00410    if( bas->delfac     <= 0.0f ) bas->delfac     = 1.0f ;
00411    if( bas->regmode    <= 0    ) bas->regmode    = MRI_LINEAR ;
00412    if( bas->max_iter   <= 0    ) bas->max_iter   = 9 ;
00413 
00414    /* process the weight image? */
00415 
00416    wtproc = (bas->imwt == NULL) ? 1 : bas->wtproc ;
00417    npar   = bas->nparam ;
00418 
00419    nfree = npar ;
00420    for( pp=0 ; pp < npar ; pp++ )
00421      if( bas->param[pp].fixed ) nfree-- ;
00422    if( nfree <= 0 ) RETURN(1) ;
00423    bas->nfree = nfree ;
00424 
00425    /*- clean out anything from last call -*/
00426 
00427    mri_warp3D_align_cleanup( bas ) ;
00428 
00429    /*-- need local copy of input base image --*/
00430 
00431    cim = mri_to_float( bas->imbase ) ;
00432    nx=cim->nx ; ny=cim->ny ; nz=cim->nz ; nxy = nx*ny ; nxyz=nxy*nz ;
00433 
00434    /*-- make weight image up from the base image if it isn't supplied --*/
00435 
00436    if( bas->verb ) fprintf(stderr,"++ mri_warp3D_align_setup ENTRY\n") ;
00437 
00438    if( bas->imwt == NULL   ||
00439        bas->imwt->nx != nx ||
00440        bas->imwt->ny != ny ||
00441        bas->imwt->nz != nz   ) bas->imww = mri_copy( cim ) ;
00442    else                        bas->imww = mri_to_float( bas->imwt ) ;
00443 
00444    if( bas->twoblur > 0.0f ){
00445      float bmax = (float)pow((double)nxyz,0.33333333) * 0.03 ;
00446      if( bmax < bas->twoblur ){
00447        if( bas->verb )
00448          fprintf(stderr,"+   shrink bas->twoblur from %.3f to %.3f\n",
00449                         bas->twoblur , bmax ) ;
00450        bas->twoblur = bmax ;
00451      }
00452    }
00453 
00454    if( bas->verb ) fprintf(stderr,"+   processing weight:") ;
00455 
00456    /* make sure weight is non-negative */
00457 
00458    wf = MRI_FLOAT_PTR(bas->imww) ;
00459    for( ii=0 ; ii < nxyz ; ii++ ) wf[ii] = fabs(wf[ii]) ;
00460 
00461    /* trim off edges of weight */
00462 
00463    if( wtproc ){
00464      int ff ;
00465      int xfade=bas->xedge , yfade=bas->yedge , zfade=bas->zedge ;
00466 
00467      if( xfade < 0 || yfade < 0 || zfade < 0 )
00468        mri_warp3D_align_edging_default(nx,ny,nz,&xfade,&yfade,&zfade) ;
00469 
00470      if( bas->twoblur > 0.0f ){
00471        xfade += (int)rint(1.5*bas->twoblur) ;
00472        yfade += (int)rint(1.5*bas->twoblur) ;
00473        zfade += (int)rint(1.5*bas->twoblur) ;
00474      }
00475 
00476      if( 3*zfade >= nz ) zfade = (nz-1)/3 ;
00477      if( 3*xfade >= nx ) xfade = (nx-1)/3 ;
00478      if( 3*yfade >= ny ) yfade = (ny-1)/3 ;
00479 
00480      if( bas->verb ) fprintf(stderr," [edge(%d,%d,%d)]",xfade,yfade,zfade) ;
00481 
00482      for( jj=0 ; jj < ny ; jj++ )
00483       for( ii=0 ; ii < nx ; ii++ )
00484        for( ff=0 ; ff < zfade ; ff++ )
00485          WW(ii,jj,ff) = WW(ii,jj,nz-1-ff) = 0.0f ;
00486 
00487      for( kk=0 ; kk < nz ; kk++ )
00488       for( jj=0 ; jj < ny ; jj++ )
00489        for( ff=0 ; ff < xfade ; ff++ )
00490          WW(ff,jj,kk) = WW(nx-1-ff,jj,kk) = 0.0f ;
00491 
00492      for( kk=0 ; kk < nz ; kk++ )
00493       for( ii=0 ; ii < nx ; ii++ )
00494        for( ff=0 ; ff < yfade ; ff++ )
00495         WW(ii,ff,kk) = WW(ii,ny-1-ff,kk) = 0.0f ;
00496 
00497    }
00498 
00499    /* spatially blur weight a little */
00500 
00501    if( wtproc ){
00502      float blur ;
00503      blur = 1.0f + MAX(1.5f,bas->twoblur) ;
00504      if( bas->verb ) fprintf(stderr," [blur(%.1f)]",blur) ;
00505      EDIT_blur_volume_3d( nx,ny,nz ,       1.0f,1.0f,1.0f ,
00506                           MRI_float , wf , blur,blur,blur  ) ;
00507    }
00508 
00509    /* get rid of low-weight voxels */
00510 
00511    clip  = 0.035 * mri_max(bas->imww) ;
00512    clip2 = 0.5*THD_cliplevel(bas->imww,0.4) ;
00513    if( clip2 > clip ) clip = clip2 ;
00514    if( bas->verb ) fprintf(stderr," [clip(%.1f)]",clip) ;
00515    for( ii=0 ; ii < nxyz ; ii++ ) if( wf[ii] < clip ) wf[ii] = 0.0f ;
00516 
00517    /* keep only the largest cluster of nonzero voxels */
00518 
00519    { byte *mmm = (byte *)malloc( sizeof(byte)*nxyz ) ;
00520      for( ii=0 ; ii < nxyz ; ii++ ) mmm[ii] = (wf[ii] > 0.0f) ;
00521      THD_mask_clust( nx,ny,nz, mmm ) ;
00522      THD_mask_erode( nx,ny,nz, mmm ) ;  /* cf. thd_automask.c */
00523      THD_mask_clust( nx,ny,nz, mmm ) ;
00524      for( ii=0 ; ii < nxyz ; ii++ ) if( !mmm[ii] ) wf[ii] = 0.0f ;
00525      free((void *)mmm) ;
00526    }
00527 
00528    if( bas->verb ) fprintf(stderr,"\n") ;
00529 
00530    /*-- make integer index map of weight > 0 voxels --*/
00531 
00532    nmap = 0 ;
00533    for( ii=0 ; ii < nxyz ; ii++ ) if( wf[ii] > 0.0f ) nmap++ ;
00534 
00535    if( bas->verb )
00536      fprintf(stderr,"+   using %d [%.3f%%] voxels\n",nmap,(100.0*nmap)/nxyz);
00537 
00538    if( nmap < 7*nfree+13 ){
00539      fprintf(stderr,"** mri_warp3D_align error: weight image too zero-ish!\n") ;
00540      mri_warp3D_align_cleanup( bas ) ; mri_free(cim) ;
00541      RETURN(1) ;
00542    }
00543 
00544    bas->imap = mri_new( nmap , 1 , MRI_int ) ;
00545    ima       = MRI_INT_PTR(bas->imap) ;
00546    bas->imsk = mri_new_conforming( bas->imww , MRI_byte ) ;
00547    msk       = MRI_BYTE_PTR(bas->imsk) ;
00548    for( ii=jj=0 ; ii < nxyz ; ii++ ){
00549      if( wf[ii] > 0.0f ){ ima[jj++] = ii; msk[ii] = 1; }
00550    }
00551 
00552    /* make copy of sqrt(weight), only at mapped indexes */
00553 
00554    wtar = (float *)malloc(sizeof(float)*nmap) ;
00555    for( ii=0 ; ii < nmap ; ii++ ) wtar[ii] = sqrt(wf[ima[ii]]) ;
00556 
00557    /*-- for parameters that don't come with a step size, find one --*/
00558 
00559    clip = bas->tolfac ; if( clip <= 0.0f ) clip = 0.03f ;
00560 
00561    for( ii=0 ; ii < npar ; ii++ ){
00562      if( bas->param[ii].fixed ) continue ; /* don't need this */
00563      if( bas->param[ii].delta <= 0.0f )
00564        mri_warp3D_get_delta( bas , ii ) ;  /* find step size */
00565      if( bas->param[ii].toler <= 0.0f ){   /* and set default tolerance */
00566        bas->param[ii].toler = clip * bas->param[ii].delta ;
00567        if( bas->verb )
00568          fprintf(stderr,"+   set toler param#%d [%s] = %f\n",
00569                  ii+1,bas->param[ii].name,bas->param[ii].toler) ;
00570      }
00571    }
00572 
00573    /* don't need the computed weight image anymore */
00574 
00575    mri_free(bas->imww) ; bas->imww = NULL ; wf = NULL ;
00576 
00577    /*-- create image containing basis columns, then pseudo-invert it --*/
00578 
00579    if( bas->verb ) fprintf(stderr,"+  Compute Derivatives of Base\n") ;
00580    fitim = mri_warp3D_align_fitim( bas , cim , bas->regmode , bas->delfac ) ;
00581    if( bas->verb ) fprintf(stderr,"+   calculate pseudo-inverse\n") ;
00582    bas->imps = mri_psinv( fitim , wtar ) ;
00583    mri_free(fitim) ;
00584 
00585    if( bas->imps == NULL ){  /* bad bad bad */
00586      fprintf(stderr,"** mri_warp3D_align error: can't invert Base matrix!\n") ;
00587      free((void *)wtar) ; mri_warp3D_align_cleanup( bas ) ; mri_free(cim) ;
00588      RETURN(1) ;
00589    }
00590 
00591    /*--- twoblur? ---*/
00592 
00593    if( bas->twoblur > 0.0f ){
00594      float *car=MRI_FLOAT_PTR(cim) ;
00595      float blur = bas->twoblur ;
00596      float bfac = blur ;
00597           if( bfac < 1.1234f ) bfac = 1.1234f ;
00598      else if( bfac > 1.3456f ) bfac = 1.3456f ;
00599 
00600      if( bas->verb ) fprintf(stderr,"+  Compute Derivatives of Blurred Base\n") ;
00601      EDIT_blur_volume_3d( nx,ny,nz ,       1.0f,1.0f,1.0f ,
00602                           MRI_float , car, blur,blur,blur  ) ;
00603      fitim = mri_warp3D_align_fitim( bas , cim , MRI_LINEAR , bfac*bas->delfac ) ;
00604      if( bas->verb ) fprintf(stderr,"+   calculate pseudo-inverse\n") ;
00605      bas->imps_blur = mri_psinv( fitim , wtar ) ;
00606      mri_free(fitim) ;
00607      if( bas->imps_blur == NULL ){  /* bad */
00608        fprintf(stderr,"** mri_warp3D_align error: can't invert Blur matrix!\n") ;
00609      }
00610    }
00611 
00612    /*--- done ---*/
00613 
00614    mri_free(cim) ; free((void *)wtar) ;
00615 
00616    if( bas->verb ){
00617      double st = (NI_clock_time()-ctstart) * 0.001 ;
00618      fprintf(stderr,"++ mri_warp3D_align_setup EXIT: %.2f seconds elapsed\n",st);
00619    }
00620 
00621    RETURN(0);
00622 }
00623 
00624 /*-----------------------------------------------------------------------
00625    Input:   basis  = MRI_warp3D_align_basis *
00626             im     = MRI_IMAGE * to align to base image
00627 
00628    Output:  Return value is aligned image.
00629             Note that returned image is floats.
00630 -------------------------------------------------------------------------*/
00631 
00632 MRI_IMAGE * mri_warp3d_align_one( MRI_warp3D_align_basis *bas, MRI_IMAGE *im )
00633 {
00634    float *fit , *dfit , *qfit , *tol ;
00635    int iter , good,ngood , ii, pp , skip_first ;
00636    MRI_IMAGE *tim , *fim ;
00637    float *pmat=MRI_FLOAT_PTR(bas->imps) , /* pseudo inverse: n X m matrix */
00638          *tar , tv , sfit ;
00639    int n=bas->imps->nx ,          /* = nfree+1 */
00640        m=bas->imps->ny ,          /* = imap->nx = length of ima */
00641     npar=bas->nparam   ,          /* = number of warp parameters */
00642    nfree=bas->nfree    ,          /* = number of free warp parameters */
00643     *ima=MRI_INT_PTR(bas->imap) , /* = indexes in fim of voxels to use */
00644     *pma ;                        /* = map of free to total params */
00645    int ctstart ;
00646    int do_twopass=(bas->imps_blur != NULL && bas->twoblur > 0.0f) , passnum=1 ;
00647    char      *save_prefix ;
00648    static int save_index=0 ;
00649 
00650 #define AITMAX  3.33
00651 #define NMEM    5
00652    float *fitmem[NMEM] ;
00653    int mm , last_aitken , num_aitken=0 ;
00654 
00655 ENTRY("mri_warp3D_align_one") ;
00656 
00657    ctstart = NI_clock_time() ;
00658 
00659    save_prefix = getenv("AFNI_WARPDRIVE_SAVER") ;
00660 
00661    pma = (int *)malloc(sizeof(int) * nfree) ;
00662    for( pp=ii=0 ; ii < npar ; ii++ )
00663      if( !bas->param[ii].fixed ) pma[pp++] = ii ;
00664 
00665    fit  = (float *)malloc(sizeof(float) * npar ) ;
00666    dfit = (float *)malloc(sizeof(float) * npar ) ;
00667    qfit = (float *)malloc(sizeof(float) * nfree) ;
00668    tol  = (float *)malloc(sizeof(float) * npar ) ;
00669 
00670    for( mm=0 ; mm < NMEM ; mm++ ) fitmem[mm] = NULL ;
00671 
00672    /*--- loop back point for two pass alignment ---*/
00673 
00674    bas->num_iter = 0 ;
00675    mri_warp3D_set_womask( bas->imsk ) ;
00676 
00677  ReStart:
00678 
00679    mri_warp3D_method( (do_twopass && passnum==1) ? MRI_LINEAR : bas->regmode ) ;
00680 
00681    /* load initial fit parameters;
00682       if they are all the identity transform value,
00683       then skip the first transformation of the fim volume */
00684 
00685    if( passnum == 1 ){
00686      skip_first = 1 ;
00687      for( pp=0 ; pp < npar ; pp++ ){
00688        if( bas->param[pp].fixed ){
00689          fit[pp] = bas->param[pp].val_fixed ;
00690        } else {
00691          fit[pp] = bas->param[pp].val_init ;
00692          skip_first = skip_first && (fit[pp] == bas->param[pp].ident) ;
00693        }
00694      }
00695    } else {
00696      skip_first = 0 ;  /* and fit[] is unchanged */
00697    }
00698 
00699    fitmem[0] = (float *)malloc(sizeof(float)*npar) ;
00700    memcpy( fitmem[0] , fit , sizeof(float)*npar) ;
00701 
00702    for( pp=0 ; pp < npar ; pp++ ) tol[pp] = bas->param[pp].toler ;
00703 
00704    if( do_twopass && passnum == 1 ){
00705      float fac = (1.0f+bas->twoblur) ;
00706      if( fac < 3.0f ) fac = 3.0f ;
00707      for( pp=0 ; pp < npar ; pp++ ) tol[pp] *= fac ;
00708    }
00709 
00710    if( bas->verb ) fprintf(stderr,"++ mri_warp3d_align_one: START PASS #%d\n",passnum) ;
00711 
00712    /* setup base image for registration into fim,
00713       and pseudo-inverse of base+derivative images into pmat */
00714 
00715    if( do_twopass && passnum==1 ){    /* first pass ==> registering blurred images */
00716      float *far , blur=bas->twoblur ;
00717      int nx=im->nx , ny=im->ny , nz=im->nz ;
00718      fim = mri_to_float( im ) ; far = MRI_FLOAT_PTR(fim) ;
00719      EDIT_blur_volume_3d( nx,ny,nz ,       1.0f,1.0f,1.0f ,
00720                           MRI_float , far, blur,blur,blur  ) ;
00721      pmat = MRI_FLOAT_PTR(bas->imps_blur) ;
00722    } else {                           /* registering original image */
00723      if( im->kind == MRI_float ) fim = im ;
00724      else                        fim = mri_to_float( im ) ;
00725      pmat = MRI_FLOAT_PTR(bas->imps) ;
00726    }
00727 
00728    /*-- iterate fit --*/
00729 
00730    iter = 0 ; good = 1 ; last_aitken = 3 ;
00731    while( good ){
00732      if( skip_first ){
00733        tim = fim ; skip_first = 0 ;
00734      } else {
00735        bas->vwset( npar , fit ) ;
00736        tim = mri_warp3D( fim , 0,0,0 , bas->vwfor ) ; /* warp on current params */
00737      }
00738      tar = MRI_FLOAT_PTR(tim) ;
00739 
00740      /* find least squares fit of base + derivatives to warped image */
00741 
00742      sfit = 0.0f ;
00743      for( pp=0 ; pp < npar  ; pp++ ) dfit[pp] = 0.0f ;
00744      for( pp=0 ; pp < nfree ; pp++ ) qfit[pp] = 0.0f ;
00745      for( ii=0 ; ii < m ; ii++ ){
00746        tv = tar[ima[ii]] ; sfit += P(nfree,ii) * tv ;
00747        for( pp=0 ; pp < nfree ; pp++ ) qfit[pp] += P(pp,ii) * tv ;
00748      }
00749      if( tim != fim ) mri_free( tim ) ;
00750      for( pp=0 ; pp < nfree ; pp++ ) dfit[pp] = qfit[pma[pp]] ;
00751      for( pp=0 ; pp < npar  ; pp++ ){
00752        fit[pp] += dfit[pp] ;
00753             if( fit[pp] > bas->param[pp].max ) fit[pp] = bas->param[pp].max ;
00754        else if( fit[pp] < bas->param[pp].min ) fit[pp] = bas->param[pp].min ;
00755      }
00756 
00757      if( bas->verb ){
00758        fprintf(stderr,"+   Delta:") ;
00759        for( pp=0 ; pp < npar ; pp++ ) fprintf(stderr," %13.6g",dfit[pp]) ;
00760        fprintf(stderr,"\n") ;
00761        fprintf(stderr,"+   Total: scale factor=%g\n"
00762                       "+  #%5d:",sfit,iter+1) ;
00763        for( pp=0 ; pp < npar ; pp++ ) fprintf(stderr," %13.6g", fit[pp]) ;
00764        fprintf(stderr,"\n") ;
00765      }
00766 
00767      /* save fit results for a while into the past, and then maybe do Aitken */
00768 
00769      if( fitmem[NMEM-1] != NULL ) free((void *)fitmem[NMEM-1]) ;
00770      for( mm=NMEM-1 ; mm > 0 ; mm-- ) fitmem[mm] = fitmem[mm-1] ;
00771      fitmem[0] = (float *)malloc(sizeof(float)*npar) ;
00772      memcpy( fitmem[0] , fit , sizeof(float)*npar) ;
00773 
00774      iter++ ;
00775      if( iter > last_aitken+NMEM && !AFNI_noenv("AFNI_WARPDRIVE_AITKEN") ){
00776        double s0,s1,s2 , dd , de,df ;
00777        num_aitken = 0 ;
00778        for( pp=0 ; pp < npar ; pp++ ){
00779          dd = fabs(fitmem[1][pp]-fit[pp]) ;
00780          if( dd <= tol[pp] ) continue ; /* done here */
00781          de = dd ;
00782          for( mm=2 ; mm < NMEM ; mm++ ){
00783            df = fabs(fitmem[mm][pp]-fitmem[mm-1][pp]) ;
00784            if( df <= de ) break ;
00785            de = df ;
00786          }
00787          if( mm == NMEM ){  /* do Aitken */
00788            s2 = fit[pp] ; s1 = fitmem[1][pp] ; s0 = fitmem[2][pp] ;
00789            de = ( (s2-s1) - (s1-s0) ) ;
00790            if( de != 0.0 ){
00791              de = -(s2-s1)*(s2-s1) / de ; dd *= AITMAX ;
00792              if( fabs(de) > dd ){ de = (de > 0.0) ? dd : -dd ; }
00793              fit[pp] += de ;
00794                   if( fit[pp] > bas->param[pp].max ) fit[pp] = bas->param[pp].max ;
00795              else if( fit[pp] < bas->param[pp].min ) fit[pp] = bas->param[pp].min ;
00796              num_aitken++ ;
00797            }
00798          }
00799        }
00800        if( num_aitken > 0 ) last_aitken = iter ;
00801 
00802        if( bas->verb && num_aitken > 0 ){
00803          fprintf(stderr,"+   Aitken on %d params:\n"
00804                         "+________:",num_aitken) ;
00805          for( pp=0 ; pp < npar ; pp++ ){
00806            if( fit[pp] != fitmem[0][pp] ){
00807              fprintf(stderr," %13.6g", fit[pp]) ; fitmem[0][pp] = fit[pp] ;
00808            } else {
00809              fprintf(stderr," _____________") ;
00810            }
00811          }
00812          fprintf(stderr,"\n") ;
00813        }
00814      }
00815 
00816      /* save intermediate result? */
00817 
00818      if( save_prefix != NULL ){
00819        char sname[THD_MAX_NAME] ; FILE *fp ;
00820        mri_warp3D_set_womask( NULL ) ;
00821        bas->vwset( npar , fit ) ;
00822        tim = mri_warp3D( fim , 0,0,0 , bas->vwfor ) ;
00823        tar = MRI_FLOAT_PTR(tim) ;
00824        mri_warp3D_set_womask( bas->imsk ) ;
00825        sprintf(sname,"%s_%04d.mmm",save_prefix,save_index++) ;
00826        fprintf(stderr,"+   Saving intermediate image to binary file %s\n",sname) ;
00827        fp = fopen( sname , "w" ) ;
00828        if( fp != NULL ){
00829          fwrite( tar, tim->pixel_size, tim->nvox, fp ) ; fclose(fp) ;
00830        }
00831 
00832        if( bas->vwdet != NULL ){
00833          int i,j,k , nx=tim->nx,ny=tim->ny,nz=tim->nz ;
00834          for( k=0 ; k < nz ; k++ ){
00835           for( j=0 ; j < ny ; j++ ){
00836            for( i=0 ; i < nx ; i++ ){
00837              tar[i+(j+k*ny)*nx] = bas->vwdet( (float)i,(float)j,(float)k ) ;
00838          }}}
00839          sprintf(sname,"%s_%04d.ddd",save_prefix,save_index-1) ;
00840          fprintf(stderr,"+   Saving determinant image to binary file %s\n",sname) ;
00841          fp = fopen( sname , "w" ) ;
00842          if( fp != NULL ){
00843            fwrite( tar, tim->pixel_size, tim->nvox, fp ) ; fclose(fp) ;
00844          }
00845        }
00846        mri_free( tim ) ;
00847      }
00848 
00849      /* loop back for more iterations? */
00850 
00851      if( last_aitken == iter ) continue ;  /* don't test, just loop */
00852      if( fitmem[2]   == NULL ) continue ;
00853 
00854      ngood = 0 ;
00855      for( pp=0 ; pp < npar ; pp++ )
00856        if( !bas->param[pp].fixed )
00857          ngood += ( ( fabs(fitmem[1][pp]-fitmem[0][pp]) <= tol[pp] ) &&
00858                     ( fabs(fitmem[2][pp]-fitmem[1][pp]) <= tol[pp] )   ) ;
00859 
00860      good = (ngood < nfree) && (iter < bas->max_iter) ;
00861 
00862    } /* end while */
00863 
00864    bas->num_iter += iter ;
00865 
00866    for( mm=0 ; mm < NMEM ; mm++ )
00867      if( fitmem[mm] != NULL ){ free((void *)fitmem[mm]); fitmem[mm] = NULL; }
00868 
00869    /*--- do the second pass? ---*/
00870 
00871    if( do_twopass && passnum == 1 ){
00872      if( bas->verb )
00873        fprintf(stderr,"+++++++++++++ Loop back for next pass +++++++++++++\n");
00874      mri_free(fim) ; fim = NULL ; passnum++ ; goto ReStart ;
00875    } else {
00876      if( bas->verb )
00877        fprintf(stderr,"+++++++++++++ Convergence test passed +++++++++++++\n");
00878    }
00879 
00880    /*--- done! ---*/
00881 
00882    for( pp=0 ; pp < npar ; pp++ ) bas->param[pp].val_out = fit[pp] ;
00883 
00884    /*-- do the actual realignment to get the output image --*/
00885 
00886    if( bas->regfinal > 0 ) mri_warp3D_method( bas->regfinal ) ;
00887    mri_warp3D_set_womask( NULL ) ;
00888    bas->vwset( npar , fit ) ;
00889    tim = mri_warp3D( fim , 0,0,0 , bas->vwfor ) ;
00890 
00891    if( fim != im ) mri_free(fim) ;  /* if it was a copy, junk it */
00892    free((void *)dfit) ; free((void *)fit) ;
00893    free((void *)qfit) ; free((void *)pma) ; free((void *)tol) ;
00894 
00895    if( bas->verb ){
00896      double st = (NI_clock_time()-ctstart) * 0.001 ;
00897      fprintf(stderr,"++ mri_warp3d_align_one EXIT: %.2f seconds elapsed\n",st) ;
00898    }
00899 
00900    RETURN( tim ) ;
00901 }
00902 
00903 /*--------------------------------------------------------------------*/
00904 
00905 void mri_warp3D_align_cleanup( MRI_warp3D_align_basis *bas )
00906 {
00907    if( bas == NULL ) return ;
00908    if( bas->imww != NULL ){ mri_free(bas->imww) ; bas->imww = NULL ; }
00909    if( bas->imap != NULL ){ mri_free(bas->imap) ; bas->imap = NULL ; }
00910    if( bas->imps != NULL ){ mri_free(bas->imps) ; bas->imps = NULL ; }
00911    if( bas->imsk != NULL ){ mri_free(bas->imsk) ; bas->imsk = NULL ; }
00912 
00913    if( bas->imps_blur != NULL ){ mri_free(bas->imps_blur) ; bas->imps_blur = NULL ; }
00914 }