1 #include "mrilib.h"
2
3 /*** NOT 7D SAFE ***/
4
5 /*************************************************************************
6 ** warp3D version of mri_3dalign.c -- RWCox -- November 2004 **
7 *************************************************************************/
8
9 /*-----------------------------------------------------------------------*/
10 /*! Compute the pseudo-inverse of a matrix stored in a 2D float image.
11 If the input is mXn, the output is nXm. wt[] is an optional array
12 of positive weights, m of them. The result can be used to solve
13 the weighted least squares problem
14 [imc] [b] = [v]
15 where [b] is an n-vector and [v] is an m-vector, where m > n.
16 -------------------------------------------------------------------------*/
17
mri_psinv(MRI_IMAGE * imc,float * wt)18 static MRI_IMAGE * mri_psinv( MRI_IMAGE *imc , float *wt )
19 {
20 float *rmat=MRI_FLOAT_PTR(imc) ;
21 int m=imc->nx , n=imc->ny , ii,jj,kk ;
22 double *amat , *umat , *vmat , *sval , *xfac , smax,del,ww ;
23 MRI_IMAGE *imp ; float *pmat ;
24 register double sum ;
25 int do_svd=0 ;
26
27 amat = (double *)calloc( sizeof(double),m*n ) ; /* input matrix */
28 xfac = (double *)calloc( sizeof(double),n ) ; /* column norms of [a] */
29
30 #define R(i,j) rmat[(i)+(j)*m] /* i=0..m-1 , j=0..n-1 */
31 #define A(i,j) amat[(i)+(j)*m] /* i=0..m-1 , j=0..n-1 */
32 #define P(i,j) pmat[(i)+(j)*n] /* i=0..n-1 , j=0..m-1 */
33
34 /* copy input matrix (float) into amat (double) */
35
36 for( ii=0 ; ii < m ; ii++ )
37 for( jj=0 ; jj < n ; jj++ ) A(ii,jj) = R(ii,jj) ;
38
39 /* weight rows? */
40
41 if( wt != NULL ){
42 for( ii=0 ; ii < m ; ii++ ){
43 ww = wt[ii] ;
44 if( ww > 0.0 ) for( jj=0 ; jj < n ; jj++ ) A(ii,jj) *= ww ;
45 }
46 }
47
48 /* scale each column to have norm 1 */
49
50 for( jj=0 ; jj < n ; jj++ ){
51 sum = 0.0 ;
52 for( ii=0 ; ii < m ; ii++ ) sum += A(ii,jj)*A(ii,jj) ;
53 if( sum > 0.0 ) sum = 1.0/sqrt(sum) ;
54 else { do_svd = 1 ; ERROR_message("mri_psinv[%d]=0\n",jj);}
55 xfac[jj] = sum ;
56 for( ii=0 ; ii < m ; ii++ ) A(ii,jj) *= sum ;
57 }
58
59 /*** compute using Choleski or SVD ***/
60
61 if( do_svd || AFNI_yesenv("AFNI_WARPDRIVE_SVD") ){ /***--- SVD method ---***/
62
63 #define U(i,j) umat[(i)+(j)*m]
64 #define V(i,j) vmat[(i)+(j)*n]
65
66 umat = (double *)calloc( sizeof(double),m*n ); /* left singular vectors */
67 vmat = (double *)calloc( sizeof(double),n*n ); /* right singular vectors */
68 sval = (double *)calloc( sizeof(double),n ); /* singular values */
69
70 /* compute SVD of scaled matrix */
71
72 svd_double( m , n , amat , sval , umat , vmat ) ;
73
74 free((void *)amat) ; /* done with this */
75
76 /* find largest singular value */
77
78 smax = sval[0] ;
79 for( ii=1 ; ii < n ; ii++ ) if( sval[ii] > smax ) smax = sval[ii] ;
80
81 if( smax <= 0.0 ){ /* this is bad */
82 ERROR_message("SVD fails in mri_warp3D_align_setup!\n");
83 free((void *)xfac); free((void *)sval);
84 free((void *)vmat); free((void *)umat); return NULL;
85 }
86
87 for( ii=0 ; ii < n ; ii++ )
88 if( sval[ii] < 0.0 ) sval[ii] = 0.0 ; /* should not happen */
89
90 #define PSINV_EPS 1.e-8
91
92 /* "reciprocals" of singular values: 1/s is actually s/(s^2+del) */
93
94 del = PSINV_EPS * smax*smax ;
95 for( ii=0 ; ii < n ; ii++ )
96 sval[ii] = sval[ii] / ( sval[ii]*sval[ii] + del ) ;
97
98 /* create pseudo-inverse */
99
100 imp = mri_new( n , m , MRI_float ) ; /* recall that m > n */
101 pmat = MRI_FLOAT_PTR(imp) ;
102
103 for( ii=0 ; ii < n ; ii++ ){
104 for( jj=0 ; jj < m ; jj++ ){
105 sum = 0.0 ;
106 for( kk=0 ; kk < n ; kk++ ) sum += sval[kk] * V(ii,kk) * U(jj,kk) ;
107 P(ii,jj) = (float)sum ;
108 }
109 }
110 free((void *)sval); free((void *)vmat); free((void *)umat);
111
112 } else { /***----- Choleski method -----***/
113
114 vmat = (double *)calloc( sizeof(double),n*n ); /* normal matrix */
115
116 for( ii=0 ; ii < n ; ii++ ){
117 for( jj=0 ; jj <= ii ; jj++ ){
118 sum = 0.0 ;
119 for( kk=0 ; kk < m ; kk++ ) sum += A(kk,ii) * A(kk,jj) ;
120 V(ii,jj) = sum ;
121 }
122 V(ii,ii) += PSINV_EPS ; /* note V(ii,ii)==1 before this */
123 }
124
125 /* Choleski factor */
126
127 for( ii=0 ; ii < n ; ii++ ){
128 for( jj=0 ; jj < ii ; jj++ ){
129 sum = V(ii,jj) ;
130 for( kk=0 ; kk < jj ; kk++ ) sum -= V(ii,kk) * V(jj,kk) ;
131 V(ii,jj) = sum / V(jj,jj) ;
132 }
133 sum = V(ii,ii) ;
134 for( kk=0 ; kk < ii ; kk++ ) sum -= V(ii,kk) * V(ii,kk) ;
135 if( sum <= 0.0 ){
136 ERROR_message("Choleski fails in mri_warp3D_align_setup!\n");
137 free((void *)xfac); free((void *)amat); free((void *)vmat); return NULL ;
138 }
139 V(ii,ii) = sqrt(sum) ;
140 }
141
142 /* create pseudo-inverse */
143
144 imp = mri_new( n , m , MRI_float ) ; /* recall that m > n */
145 pmat = MRI_FLOAT_PTR(imp) ;
146
147 sval = (double *)calloc( sizeof(double),n ) ; /* row #jj of A */
148
149 for( jj=0 ; jj < m ; jj++ ){
150 for( ii=0 ; ii < n ; ii++ ) sval[ii] = A(jj,ii) ; /* extract row */
151
152 for( ii=0 ; ii < n ; ii++ ){ /* forward solve */
153 sum = sval[ii] ;
154 for( kk=0 ; kk < ii ; kk++ ) sum -= V(ii,kk) * sval[kk] ;
155 sval[ii] = sum / V(ii,ii) ;
156 }
157 for( ii=n-1 ; ii >= 0 ; ii-- ){ /* backward solve */
158 sum = sval[ii] ;
159 for( kk=ii+1 ; kk < n ; kk++ ) sum -= V(kk,ii) * sval[kk] ;
160 sval[ii] = sum / V(ii,ii) ;
161 }
162
163 for( ii=0 ; ii < n ; ii++ ) P(ii,jj) = (float)sval[ii] ;
164 }
165 free((void *)amat); free((void *)vmat); free((void *)sval);
166 }
167
168 /* rescale rows from norming */
169
170 for( ii=0 ; ii < n ; ii++ ){
171 for( jj=0 ; jj < m ; jj++ ) P(ii,jj) *= xfac[ii] ;
172 }
173 free((void *)xfac);
174
175 /* rescale cols for weight? */
176
177 if( wt != NULL ){
178 for( ii=0 ; ii < m ; ii++ ){
179 ww = wt[ii] ;
180 if( ww > 0.0 ) for( jj=0 ; jj < n ; jj++ ) P(jj,ii) *= ww ;
181 }
182 }
183
184 return imp;
185 }
186
187 /*-----------------------------------------------------------------------*/
188
189 #define WW(i,j,k) wf[(i)+(j)*nx+(k)*nxy]
190
mri_warp3D_align_edging_default(int nx,int ny,int nz,int * xfade,int * yfade,int * zfade)191 static void mri_warp3D_align_edging_default( int nx , int ny , int nz ,
192 int *xfade, int *yfade, int *zfade )
193 {
194 char *ef=my_getenv("AFNI_VOLREG_EDGING") , *eq ;
195
196 if( ef == NULL ){ /* the 5% solution */
197 *xfade = (int)(0.05*nx+0.5) ;
198 *yfade = (int)(0.05*ny+0.5) ;
199 *zfade = (int)(0.05*nz+0.5) ;
200 } else {
201 float ff = strtod(ef,&eq) ;
202 if( ff < 0 ){ /* again, 5% */
203 *xfade = (int)(0.05*nx+0.5) ;
204 *yfade = (int)(0.05*ny+0.5) ;
205 *zfade = (int)(0.05*nz+0.5) ;
206 } else {
207 if( *eq == '%' ){ /* the whatever % solution */
208 *xfade = (int)(0.01*ff*nx+0.5) ;
209 *yfade = (int)(0.01*ff*ny+0.5) ;
210 *zfade = (int)(0.01*ff*nz+0.5) ;
211 } else { /* the fixed value solution */
212 *xfade = (int)( MIN(0.25*nx,ff) ) ;
213 *yfade = (int)( MIN(0.25*ny,ff) ) ;
214 *zfade = (int)( MIN(0.25*nz,ff) ) ;
215 }
216 }
217 }
218 }
219
220 /*--------------------------------------------------------------------------*/
221 /*! Get a default value of delta for parameter #kpar, such that the
222 RMS index change is about 1.
223 ----------------------------------------------------------------------------*/
224
mri_warp3D_get_delta(MRI_warp3D_align_basis * bas,int kpar)225 static void mri_warp3D_get_delta( MRI_warp3D_align_basis *bas , int kpar )
226 {
227 float *pvec , dpar ;
228 int ii,jj,kk , nx,ny,nz,nxy , nite , ntot ;
229 float xx,yy,zz , tx,ty,tz , dt,dtot ;
230 float *wf ;
231
232 if( bas == NULL || kpar < 0 || kpar >= bas->nparam ) return ;
233 if( bas->param[kpar].fixed ) return ;
234
235 /* load parameter vector with the identity value for all params */
236
237 pvec = (float *)malloc(sizeof(float) * bas->nparam) ;
238 for( ii=0 ; ii < bas->nparam ; ii++ )
239 pvec[ii] = (bas->param[ii].fixed) ? bas->param[ii].val_fixed
240 : bas->param[ii].ident ;
241
242 nx = bas->imbase->nx ; ny = bas->imbase->ny ; nz = bas->imbase->nz ;
243 nxy = nx*ny ;
244
245 /* initial value for delta */
246
247 dpar = 0.001 * ( fabs(bas->param[kpar].ident) + 1.0 ) ;
248 nite = 0 ; wf = MRI_FLOAT_PTR(bas->imww) ;
249
250 /* iterate:
251 - compute transformation over all points with positive weight
252 - compute distance moved
253 - compute RMS of positive distances moved
254 - adjust dpar up or down to move RMS distance moved towards 1.0 */
255
256 if( bas->verb )
257 fprintf(stderr,"+ get_delta param#%d [%s]: %f" ,
258 kpar+1, bas->param[kpar].name , dpar ) ;
259
260 while(1){
261 pvec[kpar] = bas->param[kpar].ident + dpar ; /* set param */
262 bas->vwset( bas->nparam , pvec ) ; /* load transform */
263 ntot = 0 ; dtot = 0.0f ;
264 for( kk=0 ; kk < nz ; kk++ ){
265 zz = (float)kk ;
266 for( jj=0 ; jj < ny ; jj++ ){
267 yy = (float)jj ;
268 for( ii=0 ; ii < nx ; ii++ ){
269 if( WW(ii,jj,kk) == 0.0f ) continue ; /* not counted */
270 xx = (float)ii ;
271 /* forward transform */
272 bas->vwfor( xx,yy,zz , &tx,&ty,&tz ) ;
273 dt = (tx-xx)*(tx-xx) + (ty-yy)*(ty-yy) + (tz-zz)*(tz-zz) ;
274 if( dt > 0.0f ){ ntot++ ; dtot += dt ; }
275 /* inverse transform */
276 bas->vwinv( xx,yy,zz , &tx,&ty,&tz ) ;
277 dt = (tx-xx)*(tx-xx) + (ty-yy)*(ty-yy) + (tz-zz)*(tz-zz) ;
278 if( dt > 0.0f ){ ntot++ ; dtot += dt ; }
279 }}}
280 if( ntot > 0 ){
281 dtot = sqrt( dtot/ntot ) ; /* RMS positive distance moved */
282 if( dtot > 0.909f && dtot < 1.100f ) break ; /* good enough! */
283 dtot = 1.0f / dtot ; /* dpar adjustment factor */
284 if( dtot > 50.0f ) dtot = 50.0f ;
285 else if( dtot < 0.02f ) dtot = 0.02f ;
286 } else {
287 dtot = 50.0f ; /* no movement? how peculiar! */
288 }
289 dpar *= dtot ; /* adjust dpar, up or down */
290 if( bas->verb ) fprintf(stderr," %f",dpar) ;
291 nite++ ; if( nite > 9 ) break ;
292 } /* end of iteration loop */
293
294 if( bas->verb ) fprintf(stderr,"\n") ;
295
296 bas->param[kpar].delta = dpar ; /* save result, whatever it is */
297
298 /* 11 Jan 2005: use this result to scale min..max up, if needed */
299
300 dt = AFNI_numenv("AFNI_3dWarpDrive_dfac") ;
301 if( dt <= 1.0f ) dt = 1.666f ;
302 dpar = dt * dpar ;
303
304 if( bas->param[kpar].ident == 0.0f && dpar > bas->param[kpar].max ){
305 bas->param[kpar].min = -dpar ;
306 bas->param[kpar].max = dpar ;
307 if( bas->verb )
308 fprintf(stderr,"+ reset range to %f .. %f\n",
309 bas->param[kpar].min,bas->param[kpar].max) ;
310 }
311
312 free((void *)pvec) ;
313 return ;
314 }
315
316 /*--------------------------------------------------------------------*/
317
mri_warp3D_align_fitim(MRI_warp3D_align_basis * bas,MRI_IMAGE * cim,int warp_mode,float delfac)318 static MRI_IMAGE * mri_warp3D_align_fitim( MRI_warp3D_align_basis *bas ,
319 MRI_IMAGE *cim ,
320 int warp_mode , float delfac )
321 {
322 MRI_IMAGE *fitim , *pim , *mim ;
323 float *fitar , *car=MRI_FLOAT_PTR(cim) ;
324 int nfree=bas->nfree , *ima=MRI_INT_PTR(bas->imap) , nmap=bas->imap->nx ;
325 int npar =bas->nparam ;
326 float *pvec , *par , *mar ;
327 int ii , pp , cc ;
328 float dpar , delta ;
329
330 /*-- create image containing basis columns --*/
331
332 fitim = mri_new( nmap , nfree+1 , MRI_float ) ;
333 fitar = MRI_FLOAT_PTR(fitim) ;
334 pvec = (float *)malloc(sizeof(float) * npar) ;
335
336 #undef FMAT
337 #define FMAT(i,j) fitar[(i)+(j)*nmap] /* col dim=nmap, row dim=nfree+1 */
338
339 /* column #nfree = base image itself */
340
341 for( ii=0 ; ii < nmap ; ii++ ) FMAT(ii,nfree) = car[ima[ii]] ;
342
343 pvec = (float *)malloc(sizeof(float) * npar) ;
344
345 /* for each free parameter:
346 apply inverse transform to base image with param value up and down
347 compute central difference to approximate derivative of base
348 image wrt parameter
349 store as a column in the fitim matrix */
350
351 mri_warp3D_method( warp_mode ) ; /* set interpolation mode */
352 mri_warp3D_set_womask( bas->imsk ) ;
353
354 for( pp=0,cc=0 ; pp < npar ; pp++ ){
355
356 if( bas->param[pp].fixed ) continue ; /* don't do this one! */
357
358 /* init all params to their identity transform value */
359
360 for( ii=0 ; ii < npar ; ii++ )
361 pvec[ii] = (bas->param[ii].fixed) ? bas->param[ii].val_fixed
362 : bas->param[ii].ident ;
363
364 /* change in the pp-th parameter to use for derivative */
365
366 dpar = delfac * bas->param[pp].delta ;
367
368 if( bas->verb )
369 fprintf(stderr,"+ difference base by %f in param#%d [%s]\n",
370 dpar , pp+1 , bas->param[pp].name ) ;
371
372 pvec[pp] = bas->param[pp].ident + dpar ; /* set positive change */
373 bas->vwset( npar , pvec ) ; /* put into transform */
374 pim = mri_warp3D( cim , 0,0,0 , bas->vwinv ) ; /* warp image */
375
376 pvec[pp] = bas->param[pp].ident - dpar ; /* set negative change */
377 bas->vwset( npar , pvec ) ;
378 mim = mri_warp3D( cim , 0,0,0 , bas->vwinv ) ;
379
380 /* compute derivative */
381
382 delta = bas->scale_init / ( 2.0f * dpar ) ;
383 par = MRI_FLOAT_PTR(pim) ; mar = MRI_FLOAT_PTR(mim) ;
384 for( ii=0 ; ii < nmap ; ii++ )
385 FMAT(ii,cc) = delta * ( par[ima[ii]] - mar[ima[ii]] ) ;
386
387 #if 0
388 { float psum=0.0f,msum=0.0f,dsum=0.0f;
389 for( ii=0 ; ii < nmap ; ii++ ){
390 psum += fabsf(par[ima[ii]]) ;
391 msum += fabsf(mar[ima[ii]]) ;
392 dsum += fabsf(FMAT(ii,cc)) ;
393 }
394 fprintf(stderr," pp=%d psum=%g msum=%g dsum=%g\n",pp,psum,msum,dsum) ;
395 }
396 #endif
397
398 mri_free(pim) ; mri_free(mim) ; /* no longer needed */
399
400 cc++ ; /* oopsie */
401 }
402
403 mri_warp3D_set_womask( NULL ) ;
404 free((void *)pvec) ;
405
406 #if 0
407 { int zz , jj ;
408 for( jj=0 ; jj <= nfree ; jj++ ){
409 zz = 0 ;
410 for( ii=0 ; ii < nmap ; ii++ ) if( FMAT(ii,jj) == 0.0 ) zz++ ;
411 fprintf(stderr," fitim: col#%d has %d zeros out of %d\n",jj,zz,nmap) ;
412 }
413 }
414 #endif
415
416 return(fitim) ;
417 }
418
419 /*-------------------------------------------------------------------------
420 Input: pointer to a filled in MRI_warp3D_align_basis struct.
421 Output: 0 if setup went OK, 1 if it failed.
422 Some elements in the MRI_warp3D_align_basis struct
423 will be filled in for internal use in MRI_warp3D_align_one().
424 They can be freed later with MRI_warp3D_align_cleanup().
425 ---------------------------------------------------------------------------*/
426
mri_warp3D_align_setup(MRI_warp3D_align_basis * bas)427 int mri_warp3D_align_setup( MRI_warp3D_align_basis *bas )
428 {
429 MRI_IMAGE *cim , *fitim ;
430 int nx, ny, nz, nxy, nxyz , ii,jj,kk , nmap, *im ;
431 float *wf , *wtar , clip , clip2 ;
432 int *ima , pp , wtproc , npar , nfree ;
433 byte *msk ;
434 int ctstart ;
435
436 ENTRY("mri_warp3D_align_setup") ;
437
438 ctstart = NI_clock_time() ;
439
440 /*- check for good inputs -*/
441
442 if( bas == NULL || bas->imbase == NULL ) RETURN(1) ;
443 if( bas->nparam < 1 || bas->param == NULL ) RETURN(1) ;
444 if( bas->vwfor == NULL ||
445 bas->vwinv == NULL || bas->vwset == NULL ) RETURN(1) ;
446
447 /*- set defaults in bas, if values weren't set by user -*/
448
449 if( bas->scale_init <= 0.0f ) bas->scale_init = 1.0f ;
450 if( bas->delfac <= 0.0f ) bas->delfac = 1.0f ;
451 if( bas->regmode <= 0 ) bas->regmode = MRI_LINEAR ;
452 if( bas->max_iter <= 0 ) bas->max_iter = 9 ;
453
454 /* process the weight image? */
455
456 wtproc = (bas->imwt == NULL) ? 1 : bas->wtproc ;
457 npar = bas->nparam ;
458
459 nfree = npar ;
460 for( pp=0 ; pp < npar ; pp++ )
461 if( bas->param[pp].fixed ) nfree-- ;
462 if( nfree <= 0 ) RETURN(1) ;
463 bas->nfree = nfree ;
464
465 /*- clean out anything from last call -*/
466
467 mri_warp3D_align_cleanup( bas ) ;
468
469 /*-- need local copy of input base image --*/
470
471 cim = mri_to_float( bas->imbase ) ;
472 nx=cim->nx ; ny=cim->ny ; nz=cim->nz ; nxy = nx*ny ; nxyz=nxy*nz ;
473
474 /*-- make weight image up from the base image if it isn't supplied --*/
475
476 if( bas->verb ) fprintf(stderr,"++ mri_warp3D_align_setup ENTRY\n") ;
477
478 if( bas->imwt == NULL ||
479 bas->imwt->nx != nx ||
480 bas->imwt->ny != ny ||
481 bas->imwt->nz != nz ) bas->imww = mri_copy( cim ) ;
482 else bas->imww = mri_to_float( bas->imwt ) ;
483
484 if( bas->twoblur > 0.0f ){
485 float bmax = cbrt((double)nxyz) * 0.03 ;
486 if( bmax < bas->twoblur ){
487 if( bas->verb )
488 fprintf(stderr,"+ shrink bas->twoblur from %.3f to %.3f\n",
489 bas->twoblur , bmax ) ;
490 bas->twoblur = bmax ;
491 }
492 }
493
494 if( bas->verb ) fprintf(stderr,"+ processing weight:") ;
495
496 /* make sure weight is non-negative */
497
498 wf = MRI_FLOAT_PTR(bas->imww) ;
499 for( ii=0 ; ii < nxyz ; ii++ ) wf[ii] = fabs(wf[ii]) ;
500
501 /* trim off edges of weight */
502
503 if( wtproc ){
504 int ff ;
505 int xfade=bas->xedge , yfade=bas->yedge , zfade=bas->zedge ;
506
507 if( xfade < 0 || yfade < 0 || zfade < 0 )
508 mri_warp3D_align_edging_default(nx,ny,nz,&xfade,&yfade,&zfade) ;
509
510 if( bas->twoblur > 0.0f ){
511 xfade += (int)rint(1.5*bas->twoblur) ;
512 yfade += (int)rint(1.5*bas->twoblur) ;
513 zfade += (int)rint(1.5*bas->twoblur) ;
514 }
515
516 if( 3*zfade >= nz ) zfade = (nz-1)/3 ;
517 if( 3*xfade >= nx ) xfade = (nx-1)/3 ;
518 if( 3*yfade >= ny ) yfade = (ny-1)/3 ;
519
520 if( bas->verb ) fprintf(stderr," [edge(%d,%d,%d)]",xfade,yfade,zfade) ;
521
522 for( jj=0 ; jj < ny ; jj++ )
523 for( ii=0 ; ii < nx ; ii++ )
524 for( ff=0 ; ff < zfade ; ff++ )
525 WW(ii,jj,ff) = WW(ii,jj,nz-1-ff) = 0.0f ;
526
527 for( kk=0 ; kk < nz ; kk++ )
528 for( jj=0 ; jj < ny ; jj++ )
529 for( ff=0 ; ff < xfade ; ff++ )
530 WW(ff,jj,kk) = WW(nx-1-ff,jj,kk) = 0.0f ;
531
532 for( kk=0 ; kk < nz ; kk++ )
533 for( ii=0 ; ii < nx ; ii++ )
534 for( ff=0 ; ff < yfade ; ff++ )
535 WW(ii,ff,kk) = WW(ii,ny-1-ff,kk) = 0.0f ;
536
537 }
538
539 /* spatially blur weight a little */
540
541 if( wtproc ){
542 float blur ;
543 blur = 1.0f + MAX(1.5f,bas->twoblur) ;
544 if( bas->verb ) fprintf(stderr," [blur(%.1f)]",blur) ;
545 EDIT_blur_volume_3d( nx,ny,nz , 1.0f,1.0f,1.0f ,
546 MRI_float , wf , blur,blur,blur ) ;
547 }
548
549 /* get rid of low-weight voxels */
550
551 clip = 0.035 * mri_max(bas->imww) ;
552 clip2 = 0.5*THD_cliplevel(bas->imww,0.4) ;
553 if( clip2 > clip ) clip = clip2 ;
554 if( bas->verb ) fprintf(stderr," [clip(%.1f)]",clip) ;
555 for( ii=0 ; ii < nxyz ; ii++ ) if( wf[ii] < clip ) wf[ii] = 0.0f ;
556
557 /* keep only the largest cluster of nonzero voxels */
558
559 { byte *mmm = (byte *)malloc( sizeof(byte)*nxyz ) ;
560 for( ii=0 ; ii < nxyz ; ii++ ) mmm[ii] = (wf[ii] > 0.0f) ;
561 THD_mask_clust( nx,ny,nz, mmm ) ;
562 THD_mask_erode( nx,ny,nz, mmm, 1, 2 ) ; /* cf. thd_automask.c */
563 THD_mask_clust( nx,ny,nz, mmm ) ;
564 for( ii=0 ; ii < nxyz ; ii++ ) if( !mmm[ii] ) wf[ii] = 0.0f ;
565 free((void *)mmm) ;
566 }
567
568 if( bas->verb ) fprintf(stderr,"\n") ;
569
570 /*-- make integer index map of weight > 0 voxels --*/
571
572 nmap = 0 ;
573 for( ii=0 ; ii < nxyz ; ii++ ) if( wf[ii] > 0.0f ) nmap++ ;
574
575 if( bas->verb )
576 fprintf(stderr,"+ using %d [%.3f%%] voxels\n",nmap,(100.0*nmap)/nxyz);
577
578 if( nmap < 7*nfree+13 ){
579 fprintf(stderr,"** mri_warp3D_align error: weight image too zero-ish!\n") ;
580 mri_warp3D_align_cleanup( bas ) ; mri_free(cim) ;
581 RETURN(1) ;
582 }
583
584 bas->imap = mri_new( nmap , 1 , MRI_int ) ;
585 ima = MRI_INT_PTR(bas->imap) ;
586 bas->imsk = mri_new_conforming( bas->imww , MRI_byte ) ;
587 msk = MRI_BYTE_PTR(bas->imsk) ;
588 for( ii=jj=0 ; ii < nxyz ; ii++ ){
589 if( wf[ii] > 0.0f ){ ima[jj++] = ii; msk[ii] = 1; }
590 }
591
592 /* make copy of sqrt(weight), but only at mapped indexes */
593
594 wtar = (float *)malloc(sizeof(float)*nmap) ;
595 for( ii=0 ; ii < nmap ; ii++ ) wtar[ii] = sqrt(wf[ima[ii]]) ;
596
597 /*-- for parameters that don't come with a step size, find one --*/
598
599 clip = bas->tolfac ; if( clip <= 0.0f ) clip = 0.03f ;
600
601 for( ii=0 ; ii < npar ; ii++ ){
602 if( bas->param[ii].fixed ) continue ; /* don't need this */
603 if( bas->param[ii].delta <= 0.0f )
604 mri_warp3D_get_delta( bas , ii ) ; /* find step size */
605 if( bas->param[ii].toler <= 0.0f ){ /* and set default tolerance */
606 bas->param[ii].toler = clip * bas->param[ii].delta ;
607 if( bas->verb )
608 fprintf(stderr,"+ set toler param#%d [%s] = %f\n",
609 ii+1,bas->param[ii].name,bas->param[ii].toler) ;
610 }
611 }
612
613 /* don't need the computed weight image anymore */
614
615 mri_free(bas->imww) ; bas->imww = NULL ; wf = NULL ;
616
617 /*-- create image containing basis columns, then pseudo-invert it --*/
618
619 if( bas->verb ) fprintf(stderr,"+ Compute Derivatives of Base\n") ;
620 fitim = mri_warp3D_align_fitim( bas , cim , bas->regmode , bas->delfac ) ;
621 if( bas->verb ) fprintf(stderr,"+ calculate pseudo-inverse\n") ;
622 bas->imps = mri_psinv( fitim , wtar ) ;
623 mri_free(fitim) ;
624
625 if( bas->imps == NULL ){ /* bad bad bad */
626 fprintf(stderr,"** mri_warp3D_align error: can't invert Base matrix!\n") ;
627 free((void *)wtar) ; mri_warp3D_align_cleanup( bas ) ; mri_free(cim) ;
628 RETURN(1) ;
629 }
630
631 /*--- twoblur? ---*/
632
633 if( bas->twoblur > 0.0f ){
634 float *car=MRI_FLOAT_PTR(cim) ;
635 float blur = bas->twoblur ;
636 float bfac = blur ;
637 if( bfac < 1.1234f ) bfac = 1.1234f ;
638 else if( bfac > 1.3456f ) bfac = 1.3456f ;
639
640 if( bas->verb ) fprintf(stderr,"+ Compute Derivatives of Blurred Base\n") ;
641 EDIT_blur_volume_3d( nx,ny,nz , 1.0f,1.0f,1.0f ,
642 MRI_float , car, blur,blur,blur ) ;
643 fitim = mri_warp3D_align_fitim( bas , cim , MRI_LINEAR , bfac*bas->delfac ) ;
644 if( bas->verb ) fprintf(stderr,"+ calculate pseudo-inverse\n") ;
645 bas->imps_blur = mri_psinv( fitim , wtar ) ;
646 mri_free(fitim) ;
647 if( bas->imps_blur == NULL ){ /* bad */
648 fprintf(stderr,"** mri_warp3D_align error: can't invert Blur matrix!\n") ;
649 }
650 }
651
652 /*--- done ---*/
653
654 mri_free(cim) ; free((void *)wtar) ;
655
656 if( bas->verb ){
657 double st = (NI_clock_time()-ctstart) * 0.001 ;
658 fprintf(stderr,"++ mri_warp3D_align_setup EXIT: %.2f seconds elapsed\n",st);
659 }
660
661 RETURN(0);
662 }
663
664 /*-----------------------------------------------------------------------
665 Input: basis = MRI_warp3D_align_basis *
666 im = MRI_IMAGE * to align to base image
667
668 Output: Return value is aligned image.
669 Note that returned image is floats.
670 -------------------------------------------------------------------------*/
671
mri_warp3D_align_one(MRI_warp3D_align_basis * bas,MRI_IMAGE * im)672 MRI_IMAGE * mri_warp3D_align_one( MRI_warp3D_align_basis *bas, MRI_IMAGE *im )
673 {
674 float *fit , *dfit , *qfit , *tol ;
675 int iter , good,ngood , ii, pp , skip_first ;
676 MRI_IMAGE *tim , *fim ;
677 float *pmat=MRI_FLOAT_PTR(bas->imps) , /* pseudo inverse: n X m matrix */
678 *tar , tv , sfit ;
679 int n=bas->imps->nx , /* = nfree+1 */
680 m=bas->imps->ny , /* = imap->nx = length of ima */
681 npar=bas->nparam , /* = number of warp parameters */
682 nfree=bas->nfree , /* = number of free warp parameters */
683 *ima=MRI_INT_PTR(bas->imap) , /* = indexes in fim of voxels to use */
684 *pma ; /* = map of free to total params */
685 int ctstart ;
686 int do_twopass=(bas->imps_blur != NULL && bas->twoblur > 0.0f) , passnum=1 ;
687 int blur_pass ;
688 char *save_prefix ;
689 static int save_index=0 ;
690
691 #define AITMAX 3.33
692 #define NMEM 5
693 float *fitmem[NMEM] ;
694 int mm , last_aitken , num_aitken=0 ;
695 float sdif , fitdif[NMEM] ; /* 28 Sep 2005 */
696 int num_bad_diff ;
697 float best_dif , best_par[999] ; /* 09 Jan 2006 */
698 int best_ite=0 ;
699
700 ENTRY("mri_warp3D_align_one") ;
701
702 ctstart = NI_clock_time() ;
703
704 save_prefix = getenv("AFNI_WARPDRIVE_SAVER") ;
705
706 /** pma[k] = external parameter index for the k-th free parameter **/
707
708 pma = (int *)malloc(sizeof(int) * nfree) ;
709 for( pp=ii=0 ; ii < npar ; ii++ )
710 if( !bas->param[ii].fixed ) pma[pp++] = ii ;
711
712 #if 0
713 fprintf(stderr,"pma=") ;
714 for(pp=0;pp<nfree;pp++)fprintf(stderr," %d",pma[pp]);
715 fprintf(stderr,"\n") ;
716 #endif
717
718 fit = (float *)malloc(sizeof(float) * npar ) ;
719 dfit = (float *)malloc(sizeof(float) * npar ) ;
720 qfit = (float *)malloc(sizeof(float) * nfree) ;
721 tol = (float *)malloc(sizeof(float) * npar ) ;
722
723 for( mm=0 ; mm < NMEM ; mm++ ) fitmem[mm] = NULL ;
724 for( mm=0 ; mm < NMEM ; mm++ ) fitdif[mm] = 3.e+33 ;
725
726 /*--- loop back point for two pass alignment ---*/
727
728 bas->num_iter = 0 ;
729 mri_warp3D_set_womask( bas->imsk ) ;
730
731 ReStart:
732
733 best_dif = -666.0f ; /* 09 Jan 2006 */
734 blur_pass = (do_twopass && passnum==1) ;
735 mri_warp3D_method( blur_pass ? MRI_LINEAR : bas->regmode ) ;
736
737 /* load initial fit parameters;
738 if they are all the identity transform value,
739 then skip the first transformation of the fim volume */
740
741 if( passnum == 1 ){
742 skip_first = 1 ;
743 for( pp=0 ; pp < npar ; pp++ ){
744 if( bas->param[pp].fixed ){
745 fit[pp] = bas->param[pp].val_fixed ;
746 } else {
747 fit[pp] = bas->param[pp].val_init ;
748 }
749 skip_first = skip_first && (fit[pp] == bas->param[pp].ident) ;
750 }
751 } else {
752 skip_first = 0 ; /* and fit[] is unchanged */
753 }
754
755 fitmem[0] = (float *)malloc(sizeof(float)*npar) ;
756 memcpy( fitmem[0] , fit , sizeof(float)*npar ) ;
757
758 for( pp=0 ; pp < npar ; pp++ ) tol[pp] = bas->param[pp].toler ;
759
760 if( blur_pass ){
761 float fac = (1.0f+bas->twoblur) ;
762 if( fac < 3.0f ) fac = 3.0f ;
763 for( pp=0 ; pp < npar ; pp++ ) tol[pp] *= fac ;
764 }
765
766 if( bas->verb )
767 fprintf(stderr,"++ mri_warp3D_align_one: START PASS #%d\n",passnum) ;
768
769 /* setup base image for registration into fim,
770 and pseudo-inverse of base+derivative images into pmat */
771
772 if( blur_pass ){ /* first pass ==> registering blurred images */
773 float *far , blur=bas->twoblur ;
774 int nx=im->nx , ny=im->ny , nz=im->nz ;
775 fim = mri_to_float( im ) ; far = MRI_FLOAT_PTR(fim) ;
776 EDIT_blur_volume_3d( nx,ny,nz , 1.0f,1.0f,1.0f ,
777 MRI_float , far, blur,blur,blur ) ;
778 pmat = MRI_FLOAT_PTR(bas->imps_blur) ;
779 } else { /* registering original image */
780 if( im->kind == MRI_float ) fim = im ;
781 else fim = mri_to_float( im ) ;
782 pmat = MRI_FLOAT_PTR(bas->imps) ;
783 }
784
785 /******** iterate fit ********/
786
787 iter = 0 ; good = 1 ; last_aitken = 3 ; num_bad_diff = 0 ;
788 while( good ){
789 if( skip_first ){
790 tim = fim ; skip_first = 0 ;
791 } else {
792 bas->vwset( npar , fit ) ; /**************************/
793 tim = mri_warp3D( fim , 0,0,0 , bas->vwfor ) ; /* warp on current params */
794 } /**************************/
795 tar = MRI_FLOAT_PTR(tim) ;
796
797 sdif = mri_scaled_diff( bas->imbase , tim , bas->imsk ) ;
798 if( bas->verb )
799 fprintf(stderr,"++++++++++ Start iter=%d RMS_diff=%g\n",iter+1,sdif) ;
800
801 if( best_dif < 0.0f || sdif < best_dif ){ /* 09 Jan 2006 */
802 best_dif = sdif ; best_ite = iter ;
803 memcpy( best_par , fit , sizeof(float)*npar ) ;
804 }
805
806 /* find least squares fit of base + derivatives to warped image */
807
808 sfit = 0.0f ;
809 for( pp=0 ; pp < npar ; pp++ ) dfit[pp] = 0.0f ;
810 for( pp=0 ; pp < nfree ; pp++ ) qfit[pp] = 0.0f ;
811 for( ii=0 ; ii < m ; ii++ ){
812 tv = tar[ima[ii]] ; sfit += P(nfree,ii) * tv ;
813 for( pp=0 ; pp < nfree ; pp++ ) qfit[pp] += P(pp,ii) * tv ;
814 }
815 if( tim != fim ) mri_free( tim ) ;
816 #if 0
817 fprintf(stderr,"qfit=");
818 for(pp=0;pp<nfree;pp++)fprintf(stderr," %g",qfit[pp]);
819 fprintf(stderr,"\n") ;
820 #endif
821 for( pp=0 ; pp < nfree ; pp++ ) dfit[pma[pp]] = qfit[pp] ;
822 for( pp=0 ; pp < npar ; pp++ ){
823 fit[pp] += dfit[pp] ;
824 if( fit[pp] > bas->param[pp].max ) fit[pp] = bas->param[pp].max ;
825 else if( fit[pp] < bas->param[pp].min ) fit[pp] = bas->param[pp].min ;
826 }
827
828 if( bas->verb ){
829 fprintf(stderr,"+ Delta:") ;
830 for( pp=0 ; pp < npar ; pp++ ) fprintf(stderr," %13.6g",dfit[pp]) ;
831 fprintf(stderr,"\n") ;
832 fprintf(stderr,"+ Total: scale factor=%g\n"
833 "+ #%5d:",sfit,iter+1) ;
834 for( pp=0 ; pp < npar ; pp++ ) fprintf(stderr," %13.6g", fit[pp]) ;
835 fprintf(stderr,"\n") ;
836 }
837
838 /* save fit results for a while into the past, and then maybe do Aitken */
839
840 if( fitmem[NMEM-1] != NULL ) free((void *)fitmem[NMEM-1]) ;
841 for( mm=NMEM-1 ; mm > 0 ; mm-- ) fitmem[mm] = fitmem[mm-1] ;
842 fitmem[0] = (float *)malloc(sizeof(float)*npar) ;
843 memcpy( fitmem[0] , fit , sizeof(float)*npar ) ;
844
845 /* 28 Sep 2005: if RMS went up, back off the changes! */
846
847 for( mm=NMEM-1 ; mm > 0 ; mm-- ) fitdif[mm] = fitdif[mm-1] ;
848 fitdif[0] = sdif ;
849
850 if( iter > 1 && num_bad_diff < 2 &&
851 fitmem[1] != NULL && fitdif[0] > 1.0666f * fitdif[1] ){
852 for( pp=0 ; pp < npar ; pp++ )
853 fit[pp] = 0.5 * ( fitmem[0][pp] + fitmem[1][pp] ) ;
854 memcpy( fitmem[0] , fit , sizeof(float)*npar ) ;
855 last_aitken = iter+1 ; num_bad_diff++ ;
856 if( bas->verb )
857 fprintf(stderr,"+++ RMS_diff changes too much! Shrinking!\n") ;
858 } else {
859 num_bad_diff = 0 ;
860 }
861
862 iter++ ;
863 if( iter > last_aitken+NMEM && !AFNI_noenv("AFNI_WARPDRIVE_AITKEN") ){
864 double s0,s1,s2 , dd , de,df ;
865 num_aitken = 0 ;
866 for( pp=0 ; pp < npar ; pp++ ){
867 dd = fabs(fitmem[1][pp]-fit[pp]) ;
868 if( dd <= tol[pp] ) continue ; /* done here */
869 de = dd ;
870 for( mm=2 ; mm < NMEM ; mm++ ){
871 df = fabs(fitmem[mm][pp]-fitmem[mm-1][pp]) ;
872 if( df <= de ) break ;
873 de = df ;
874 }
875 if( mm == NMEM ){ /* do Aitken */
876 s2 = fit[pp] ; s1 = fitmem[1][pp] ; s0 = fitmem[2][pp] ;
877 de = ( (s2-s1) - (s1-s0) ) ;
878 if( de != 0.0 ){
879 de = -(s2-s1)*(s2-s1) / de ; dd *= AITMAX ;
880 if( fabs(de) > dd ){ de = (de > 0.0) ? dd : -dd ; }
881 fit[pp] += de ;
882 if( fit[pp] > bas->param[pp].max ) fit[pp] = bas->param[pp].max ;
883 else if( fit[pp] < bas->param[pp].min ) fit[pp] = bas->param[pp].min ;
884 num_aitken++ ;
885 }
886 }
887 }
888 if( num_aitken > 0 ) last_aitken = iter ;
889
890 if( bas->verb && num_aitken > 0 ){
891 fprintf(stderr,"+ Aitken on %d params:\n"
892 "+________:",num_aitken) ;
893 for( pp=0 ; pp < npar ; pp++ ){
894 if( fit[pp] != fitmem[0][pp] ){
895 fprintf(stderr," %13.6g", fit[pp]) ; fitmem[0][pp] = fit[pp] ;
896 } else {
897 fprintf(stderr," _____________") ;
898 }
899 }
900 fprintf(stderr,"\n") ;
901 }
902 }
903
904 /* save intermediate image? */
905
906 if( save_prefix != NULL ){
907 char sname[THD_MAX_NAME] ; FILE *fp ;
908 mri_warp3D_set_womask( NULL ) ; /* must warp the whole thing */
909 bas->vwset( npar , fit ) ;
910 tim = mri_warp3D( fim , 0,0,0 , bas->vwfor ) ;
911 tar = MRI_FLOAT_PTR(tim) ;
912 mri_warp3D_set_womask( bas->imsk ) ;
913 sprintf(sname,"%s_%04d.mmm",save_prefix,save_index++) ;
914 fprintf(stderr,"+ Saving intermediate image to binary file %s\n",sname) ;
915 fp = fopen( sname , "w" ) ;
916 if( fp != NULL ){
917 fwrite( tar, tim->pixel_size, tim->nvox, fp ) ; fclose(fp) ;
918 }
919
920 if( bas->vwdet != NULL ){
921 int i,j,k , nx=tim->nx,ny=tim->ny,nz=tim->nz ;
922 for( k=0 ; k < nz ; k++ ){
923 for( j=0 ; j < ny ; j++ ){
924 for( i=0 ; i < nx ; i++ ){
925 tar[i+(j+k*ny)*nx] = bas->vwdet( (float)i,(float)j,(float)k ) ;
926 }}}
927 sprintf(sname,"%s_%04d.ddd",save_prefix,save_index-1) ;
928 fprintf(stderr,"+ Saving determinant image to binary file %s\n",sname) ;
929 fp = fopen( sname , "w" ) ;
930 if( fp != NULL ){
931 fwrite( tar, tim->pixel_size, tim->nvox, fp ) ; fclose(fp) ;
932 }
933 }
934 mri_free( tim ) ;
935 }
936
937 /* loop back for more iterations? */
938
939 if( last_aitken == iter ) continue ; /* don't test, just loop */
940 if( fitmem[2] == NULL ) continue ;
941
942 ngood = 0 ;
943 for( pp=0 ; pp < npar ; pp++ )
944 if( !bas->param[pp].fixed )
945 ngood += ( ( fabs(fitmem[1][pp]-fitmem[0][pp]) <= tol[pp] ) &&
946 ( fabs(fitmem[2][pp]-fitmem[1][pp]) <= tol[pp] ) ) ;
947
948 good = (ngood < nfree) && (iter < bas->max_iter) ;
949
950 } /******** end while loop for iteration of fitting procedure ********/
951
952 for( mm=0 ; mm < NMEM ; mm++ )
953 if( fitmem[mm] != NULL ){ free((void *)fitmem[mm]); fitmem[mm] = NULL; }
954
955 /*--- 09 Jan 2006: if prior best fit was better than current, replace ---*/
956
957 if( best_dif > 0.0f && 1.0666f*best_dif < sdif ){
958 memcpy( fit , best_par , sizeof(float)*npar ) ;
959 if( bas->verb )
960 fprintf(stderr,"+++++ Replacing final fit with iter #%d's fit +++++\n",
961 best_ite+1) ;
962 }
963
964 /*--- do the second pass? ---*/
965
966 if( blur_pass ){
967 if( bas->verb )
968 fprintf(stderr,"+++++++++++++ Loop back for next pass +++++++++++++\n");
969 mri_free(fim) ; fim = NULL ; passnum++ ; goto ReStart ;
970 } else {
971 if( bas->verb )
972 fprintf(stderr,"+++++++++++++ Convergence test passed +++++++++++++\n");
973 }
974
975 /*--- done! ---*/
976
977 bas->num_iter = iter ;
978
979 for( pp=0 ; pp < npar ; pp++ ) bas->param[pp].val_out = fit[pp] ;
980
981 /*-- do the actual realignment to get the output image --*/
982
983 if( bas->regfinal > 0 ) mri_warp3D_method( bas->regfinal ) ;
984 mri_warp3D_set_womask( NULL ) ;
985 bas->vwset( npar , fit ) ;
986 tim = mri_warp3D( fim , 0,0,0 , bas->vwfor ) ;
987
988 if( fim != im ) mri_free(fim) ; /* if it was a copy, junk it */
989 free((void *)dfit) ; free((void *)fit) ;
990 free((void *)qfit) ; free((void *)pma) ; free((void *)tol) ;
991
992 if( bas->verb ){
993 double st = (NI_clock_time()-ctstart) * 0.001 ;
994 fprintf(stderr,"++ mri_warp3D_align_one EXIT: %.2f seconds elapsed\n",st) ;
995 }
996
997 RETURN( tim ) ;
998 }
999
1000 /*--------------------------------------------------------------------*/
1001
mri_warp3D_align_cleanup(MRI_warp3D_align_basis * bas)1002 void mri_warp3D_align_cleanup( MRI_warp3D_align_basis *bas )
1003 {
1004 if( bas == NULL ) return ;
1005 if( bas->imww != NULL ){ mri_free(bas->imww) ; bas->imww = NULL ; }
1006 if( bas->imap != NULL ){ mri_free(bas->imap) ; bas->imap = NULL ; }
1007 if( bas->imps != NULL ){ mri_free(bas->imps) ; bas->imps = NULL ; }
1008 if( bas->imsk != NULL ){ mri_free(bas->imsk) ; bas->imsk = NULL ; }
1009
1010 if( bas->imps_blur != NULL ){ mri_free(bas->imps_blur) ; bas->imps_blur = NULL ; }
1011 }
1012