1 #include "mrilib.h"
2
3 #ifdef USE_OMP
4 #include <omp.h>
5 #endif
6
7 #undef DEBUG /* for some extra printouts */
8
9 /******************************************************************************
10 Radial Basis Function (RBF) interpolation in 3D.
11 + These functions are not designed for high efficiency!
12 + They are designed for repeated interpolations from the
13 same set of knots, using different sets of values each time.
14 + The C2 polynomial with compact support (0 < r < 1)
15 f(r) = (1-r)^4*(4r+1) is the RBF used here, with a global linear
16 polynomial.
17 + Setup for a given collection of knots: RBF_setup_knots()
18 + Setup for a given set of values at the knots: RBF_setup_evalues()
19 + Evaluation at an arbitrary set of points: RBF_evaluate()
20 + optional: speedup evaluation a little via: RBF_setup_kranges()
21 *******************************************************************************/
22
23 /*............................................................................*/
24 /* RBF expansion
25 i=nk-1
26 f(x) = sum { alpha_i * RBF(x-q_i) } + b_0 + x*b_x +y*b_y + z*b_z
27 i=0
28
29 where the weights alpha_i for knot location q_i are chosen so that
30 the expansion f(q_i) matches the function f() at that location, for
31 each knot q_i. The (optional) global linear polynomial is available
32 to provide nonzero values of f(x) for x far away from any knots.
33 *//*..........................................................................*/
34
35 #if 0 /* would be needed for thin-plate spline RBF */
36 #undef Rlog
37 #define Rlog(x) (((x)<=0.0f) ? 0.0f : logf(x))
38 #endif
39
40 /*! C2 polynomial: argument is 1-r, which should be between 0 and 1;
41 note that this function is "positive definite": [M] matrix will be p.d. */
42
43 #undef RBF_func
44 #define RBF_func(x) ((x)*(x)*(x)*(x)*(5.0f-4.0f*x))
45
46 /*----------------------------------------------------------------------------*/
47
48 static int verb = 0 ;
RBF_set_verbosity(int ii)49 void RBF_set_verbosity( int ii ){ verb = ii ; }
50
51 /*----------------------------------------------------------------------------*/
52 /*! Evaluate RBF expansion on a set of points in rbg,
53 given the set of knots in rbk, and given the coefficients of the RBF
54 (or the values at the fit points) in rbe, and store results in array val
55 -- which should be rbg->npt points long.
56 *//*--------------------------------------------------------------------------*/
57
RBF_evaluate(RBF_knots * rbk,RBF_evalues * rbe,RBF_evalgrid * rbg,float * val)58 int RBF_evaluate( RBF_knots *rbk, RBF_evalues *rbe,
59 RBF_evalgrid *rbg, float *val )
60 {
61 int npt , nk ;
62 double ct ;
63
64 ENTRY("RBF_evaluate") ;
65
66 if( rbk == NULL || rbe == NULL || rbg == NULL || val == NULL ){
67 ERROR_message("Illegal call to RBF_evaluate?!") ;
68 RETURN(0) ;
69 }
70
71 /* if needed, convert rbe to RBF weights, from function values */
72
73 npt = RBF_setup_evalues( rbk , rbe ) ;
74 if( npt == 0 ){
75 ERROR_message("bad evalues input to RBF_evaluate") ; RETURN(0) ;
76 }
77
78 /* evaluate RBF + linear polynomial at each output point */
79
80 npt = rbg->npt ;
81 nk = rbk->nknot ;
82
83 if( verb )
84 INFO_message("RBF_evaluate: %d points X %d knots",npt,nk) ;
85
86 ct = COX_clock_time() ;
87
88 AFNI_OMP_START ;
89 #pragma omp parallel if(npt*nk > 9999)
90 {
91 int ii , jj , uselin=rbk->uselin ;
92 float rr , xm,ym,zm , xd,yd,zd , rad,rqq , xt,yt,zt , *xx,*yy,*zz ;
93 float xk , yk , zk , sum , *ev ;
94 float b0=0.0,bx=0.0,by=0.0,bz=0.0 , rai ;
95 RBFKINT *kfirst , *klast ; int kbot,ktop ;
96
97 /* load some local variables */
98
99 rad = rbk->rad ; rqq = rbk->rqq ; rai = 1.0f / rad ;
100 xm = rbk->xmid ; ym = rbk->ymid ; zm = rbk->zmid ;
101 xd = rbk->xscl ; yd = rbk->yscl ; zd = rbk->zscl ;
102 xx = rbk->xknot; yy = rbk->yknot; zz = rbk->zknot ;
103 ev = rbe->val ;
104 if( uselin ){
105 b0 = rbe->b0 ; bx = rbe->bx ; by = rbe->by ; bz = rbe->bz ;
106 }
107
108 kfirst = rbg->kfirst ; klast = rbg->klast ; kbot = 0 ; ktop = nk-1 ;
109
110 #pragma omp for
111 for( ii=0 ; ii < npt ; ii++ ){
112 xt = rbg->xpt[ii]; yt = rbg->ypt[ii]; zt = rbg->zpt[ii]; /* output xyz */
113 if( kfirst != NULL ){ kbot = kfirst[ii] ; ktop = klast[ii] ; }
114 for( sum=0.0f,jj=kbot ; jj <= ktop ; jj++ ){
115 xk = xt-xx[jj]; rr =xk*xk; if( rr >= rqq ) continue;
116 yk = yt-yy[jj]; rr+=yk*yk; if( rr >= rqq ) continue;
117 zk = zt-zz[jj]; rr+=zk*zk; if( rr >= rqq ) continue;
118 rr = 1.0f - sqrtf(rr) * rai ; sum += ev[jj] * RBF_func(rr) ;
119 }
120 val[ii] = sum ;
121 if( uselin )
122 val[ii] += b0 + bx*(xt-xm)*xd + by*(yt-ym)*yd + bz*(zt-zm)*zd ;
123 }
124
125 } /* end OpenMP */
126 AFNI_OMP_END ;
127
128 if( verb ) ININFO_message(" Elapsed = %.1f",COX_clock_time()-ct) ;
129 RETURN(1) ;
130 }
131
132 /*----------------------------------------------------------------------------*/
133 /*! Setup the particular values from which to interpolate RBF-wise;
134 that is, convert them from values at each fit point to the weights
135 used for the RBF centered at each knot (plus the linear polynomial).
136 Return value is 0 if bad things happened, 1 if good things happened.
137 *//*--------------------------------------------------------------------------*/
138
RBF_setup_evalues(RBF_knots * rbk,RBF_evalues * rbe)139 int RBF_setup_evalues( RBF_knots *rbk , RBF_evalues *rbe )
140 {
141 int nn , ii , jj ;
142 float *mat , *vv ; double *aa ;
143
144 ENTRY("RBF_setup_evalues") ;
145
146 if( rbk == NULL || rbe == NULL || rbe->val == NULL ){
147 ERROR_message("bad call to RBF_setup_evalues") ; RETURN(0) ;
148 }
149
150 if( rbe->code > 0 ) RETURN(1) ; /* already contains RBF weights */
151
152 if( verb )
153 INFO_message("RBF_setup_evalues: solve for knot weights") ;
154
155 nn = rbk->nknot ;
156 vv = rbe->val ;
157 aa = (double *)calloc(sizeof(double),nn) ;
158 for( ii=0 ; ii < nn ; ii++ ) aa[ii] = (double)vv[ii] ;
159
160 /* compute [aa] = [Minv][vv] using Choleski factors */
161
162 rcmat_lowert_solve( rbk->Lmat , aa ) ; /* [Linv] [vv] */
163 rcmat_uppert_solve( rbk->Lmat , aa ) ; /* [Ltinv] [Linv] [vv] */
164
165 /* adjust [aa] via [Q] matrix, for linear polynomial part of fit */
166
167 if( rbk->uselin ){
168 double q0,qx,qy,qz , b0,bx,by,bz ; dmat44 Q=rbk->Qmat ;
169 float *P0=rbk->P0 , *Px=rbk->Px , *Py=rbk->Py , *Pz=rbk->Pz ;
170 for( q0=qx=qy=qz=0.0,ii=0 ; ii < nn ; ii++ ){ /* compute [Pt][aa] */
171 q0 += P0[ii]*aa[ii] ; qx += Px[ii]*aa[ii] ;
172 qy += Py[ii]*aa[ii] ; qz += Pz[ii]*aa[ii] ;
173 }
174 DMAT44_VEC( Q , q0,qx,qy,qz , b0,bx,by,bz ) ; /* compute [beta] */
175 rbe->b0 = b0 ; rbe->bx = bx ; rbe->by = by ; rbe->bz = bz ;
176 for( ii=0 ; ii < nn ; ii++ ) /* [aa] = [vv] - [P][beta] */
177 aa[ii] = (double)vv[ii] - b0*P0[ii] - bx*Px[ii] - by*Py[ii] - bz*Pz[ii] ;
178
179 rcmat_lowert_solve( rbk->Lmat , aa ) ; /* compute [Minv][aa] */
180 rcmat_uppert_solve( rbk->Lmat , aa ) ; /* as final RBF weights */
181 #ifdef DEBUG
182 INFO_message("RBF_setup_evalues: b0=%g bx=%g by=%g bz=%g",b0,bx,by,bz) ;
183 #endif
184 }
185
186 /* put results back into rbe struct */
187
188 for( ii=0 ; ii < nn ; ii++ ) vv[ii] = (float)aa[ii] ;
189 rbe->code = 1 ; /* code that rbe is converted to RBF weights from values */
190 free(aa) ;
191
192 RETURN(2) ;
193 }
194
195 /*----------------------------------------------------------------------------*/
196 /*! Setup the knots struct for radial basis function evaluation.
197 *//*--------------------------------------------------------------------------*/
198
RBF_setup_knots(int nk,float radius,int uselin,float * xx,float * yy,float * zz)199 RBF_knots * RBF_setup_knots( int nk, float radius, int uselin,
200 float *xx , float *yy , float *zz )
201 {
202 RBF_knots *rbk ;
203 int ii , jj , nn , jtop ;
204 float **mat ;
205 rcmat *rcm ;
206 float *P0=NULL,*Px=NULL,*Py=NULL,*Pz=NULL ;
207 float rr , xm,ym,zm , xd,yd,zd , rad,rqq , xt,yt,zt ;
208 double ct ;
209
210 ENTRY("RBF_setup_knots") ;
211
212 if( nk < 5 || xx == NULL || yy == NULL || zz == NULL ){
213 ERROR_message("Illegal inputs to RBF_setup_knots") ; RETURN(NULL) ;
214 }
215
216 ct = COX_clock_time() ;
217
218 /* Setup middle of knot field and scale radius:
219 xm = middle (median) of the x knot coordinates
220 xd = scale (MAD) of the x knot distances from xm;
221 would be about L/4 if knots are uniformly spaced
222 in 1D over a distance of L
223 ==> 4*xd/nk is about inter-knot x distance in 1D uniform case
224 ==> 4*(xd+yd+zd)/(3*cbrt(nk)) is about mean inter-knot distance in 3D
225 RBF support radius is chosen to be a small multiple of this distance */
226
227 qmedmad_float( nk , xx , &xm , &xd ) ;
228 qmedmad_float( nk , yy , &ym , &yd ) ;
229 qmedmad_float( nk , zz , &zm , &zd ) ;
230
231 if( radius <= 0.0f )
232 rad = 4.321f*(xd+yd+zd) / cbrtf((float)nk) ; /* RBF support radius */
233 else
234 rad = radius ; /* user-selected RBF radius */
235
236 rqq = rad*rad ; /* for testing radius-squared */
237
238 if( verb )
239 INFO_message("RBF_setup_knots: knots=%d radius=%.1f",nk,rad) ;
240
241 xd = 1.0f / xd ; /* scale factors : (x-xm)*xd is */
242 yd = 1.0f / yd ; /* dimensionless x-value relative */
243 zd = 1.0f / zd ; /* to middle at xm */
244
245 /*...................................................................*//*
246 Set up symmetric nk X nk matrix for interpolation:
247 mat[ii][jj] = RBF( knot[ii] - knot[jj] ) for jj=0..ii.
248 Call this matrix M. It is positive definite by the choice
249 of the particular radial basis function being used.
250
251 If we are NOT using linear polynomials, then the system of
252 equations to solve for RBF knot weights [alpha] given knot
253 function values [v] is
254 [M] [alpha] = [v]
255 We Choleski decompose [M] = [L] [Lt] so that we solve via
256 [alpha] = [Ltinv][Linv] [v]
257 This will be done in function RBF_setup_evalues(), which
258 converts values of the function to be interpolated at the
259 knots [v] into weights [alpha].
260
261 If we ARE using a global linear polynomial, then the system
262 of equations to solve is
263 [ M P ] [alpha] = [v]
264 [ Pt 0 ] [beta ] = [0]
265 where [P] is an nk X 4 matrix, with entries
266 P_{i0}=1 P_{i1}=x_i P_{i2}=y_i P_{i3}=z_i (knot coordinates)
267 Pt = P-transpose
268 [beta] = 4-vector of weights for linear polynomial
269 The purpose of this is to make the RBF part of the expansion
270 not contain any linear component -- that will be carried by the
271 global linear polynomial. Without this constraint, the system
272 would be over-determined: nk equations with nk+4 unknowns.
273
274 The first nk equations express the interpolation property:
275 the evaluated RBF sum plus the linear polynomial equals [v]
276 at the knots. The last 4 equations express the property that
277 the [alpha] weights by themselves do NOT have any component of
278 a global linear polynomial.
279
280 The symmetric compound (nk+4)X(nk+4) matrix is obviously not
281 positive definite (since it has 0 values on the diagonal), so
282 cannot be solved directly by Choleski factorization. Instead,
283 we solve it via a bordering technique:
284 [M][alpha] + [P][beta] = [v]
285 ==> [alpha] = [Minv][v] - [Minv][P][beta] (*)
286 [Pt][alpha] = [0]
287 ==> [Pt][Minv][v] = [Pt][Minv][P][beta]
288 ==> [beta] = inv{ [Pt][Minv][P] }[Pt][Minv][v] (**)
289 Once [beta] is known from Eqn(**), [alpha] is found by using Eqn(*).
290 In details, given the Choleski factors of [M]:
291 Compute 4x4 matrix [Q] = inv{ [Pt][Ltinv][Linv][P] } (done below)
292 Given [v], [ahat] = [Ltinv][Linv][v] (==[alpha] if not using [P])
293 [beta] = [Q][Pt][ahat]
294 [alpha] = [Ltinv][Linv]{ [v] - [P][beta] }
295 We then use [alpha] to evaluate the RBF at each output grid point,
296 and use [beta] to evaluate the global linear polynomial.
297 *//*.....................................................................*/
298
299 /* create the [M] matrix rows */
300
301 nn = nk ;
302 mat = (float **)malloc(sizeof(float *)*nn) ;
303 for( ii=0 ; ii < nn ; ii++ )
304 mat[ii] = (float *)calloc((ii+1),sizeof(float)) ;
305
306 /* create the [P] matrix columns as needed for the linear polynomial */
307
308 if( uselin ){
309 P0 = (float *)calloc(nn,sizeof(float)) ;
310 Px = (float *)calloc(nn,sizeof(float)) ;
311 Py = (float *)calloc(nn,sizeof(float)) ;
312 Pz = (float *)calloc(nn,sizeof(float)) ;
313 }
314
315 if( verb > 1 ) ININFO_message(" matrix computation") ;
316
317 /* load the [M] matrix and the [P] matrix */
318
319 for( ii=0 ; ii < nn ; ii++ ){
320 for( jj=0 ; jj < ii ; jj++ ){ /* RBF between knots */
321 xt = xx[ii]-xx[jj] ;
322 yt = yy[ii]-yy[jj] ;
323 zt = zz[ii]-zz[jj] ; rr = xt*xt + yt*yt + zt*zt ;
324 if( rr >= rqq ) continue ;
325 rr = 1.0f - sqrtf(rr)/rad ; mat[ii][jj] = RBF_func(rr) ;
326 }
327 mat[ii][ii] = 1.0000005f ; /* RBF(0) = 1 by definition */
328
329 if( uselin ){ /* [P] matrix for linear polynomial */
330 P0[ii] = 1.0f ;
331 Px[ii] = (xx[ii]-xm)*xd ;
332 Py[ii] = (yy[ii]-ym)*yd ;
333 Pz[ii] = (zz[ii]-zm)*zd ;
334 }
335 }
336
337 if( verb > 1 ) ININFO_message(" matrix factorization") ;
338
339 /* convert mat[][] into an rcmat sparse symmetric matrix struct */
340
341 rcm = rcmat_from_rows( nn , mat ) ;
342
343 for( ii=0 ; ii < nn ; ii++ ) free(mat[ii]) ; /* don't need mat */
344 free(mat) ; /* no more */
345
346 if( rcm == NULL ){
347 ERROR_message("RBF_setup_knots: setup of rcmat fails!?") ;
348 if( uselin ){ free(P0); free(Px); free(Py); free(Pz); }
349 RETURN(NULL) ;
350 }
351
352 /* Choleski decompose M matrix */
353
354 ii = rcmat_choleski( rcm ) ;
355 if( ii > 0 ){
356 ERROR_message("RBF_setup_knots: Choleski of rcmat fails at row %d!",ii) ;
357 if( uselin ){ free(P0); free(Px); free(Py); free(Pz); }
358 RETURN(NULL) ;
359 }
360
361 /* create output struct, save stuff into it */
362
363 rbk = (RBF_knots *)calloc(1,sizeof(RBF_knots)) ;
364 rbk->nknot = nk ;
365 rbk->rad = rad ; rbk->rqq = rqq ;
366 rbk->xmid = xm ; rbk->xscl = xd ;
367 rbk->ymid = ym ; rbk->yscl = yd ;
368 rbk->zmid = zm ; rbk->zscl = zd ;
369 rbk->xknot = (float *)calloc(sizeof(float),nk) ; /* saved knots */
370 rbk->yknot = (float *)calloc(sizeof(float),nk) ;
371 rbk->zknot = (float *)calloc(sizeof(float),nk) ;
372 memcpy(rbk->xknot,xx,sizeof(float)*nk) ;
373 memcpy(rbk->yknot,yy,sizeof(float)*nk) ;
374 memcpy(rbk->zknot,zz,sizeof(float)*nk) ;
375
376 rbk->Lmat = rcm ; /* saved [L] matrix */
377
378 rbk->uselin = uselin ;
379 rbk->P0 = P0 ; rbk->Px = Px ; rbk->Py = Py ; rbk->Pz = Pz ;
380
381 /* compute the Q matrix for the linear polynomial coefficients */
382
383 if( uselin ){
384 double *vv[4],*vi,*vj ; dmat44 Q ; double sum ; register int kk ;
385 #ifdef DEBUG
386 INFO_message(" compute Q matrix") ;
387 #endif
388 vv[0] = (double *)malloc(sizeof(double)*nn) ;
389 vv[1] = (double *)malloc(sizeof(double)*nn) ;
390 vv[2] = (double *)malloc(sizeof(double)*nn) ;
391 vv[3] = (double *)malloc(sizeof(double)*nn) ;
392 for( ii=0 ; ii < nn ; ii++ ){
393 vv[0][ii] = P0[ii] ; vv[1][ii] = Px[ii] ;
394 vv[2][ii] = Py[ii] ; vv[3][ii] = Pz[ii] ;
395 }
396 /* compute [Linv][P] into the 4 columns [vv] */
397 rcmat_lowert_solve(rcm,vv[0]) ; rcmat_lowert_solve(rcm,vv[1]) ;
398 rcmat_lowert_solve(rcm,vv[2]) ; rcmat_lowert_solve(rcm,vv[3]) ;
399 /* compute 4x4 matrix [vv]^T [vv], then invert it to get output [Q] */
400 for( ii=0 ; ii < 4 ; ii++ ){
401 vi = vv[ii] ;
402 for( jj=0 ; jj < 4 ; jj++ ){
403 vj = vv[jj] ;
404 for( sum=0.0,kk=0 ; kk < nn ; kk++ ) sum += vi[kk]*vj[kk] ;
405 Q.m[ii][jj] = sum ;
406 }
407 }
408 free(vv[0]) ; free(vv[1]) ; free(vv[2]) ; free(vv[3]) ;
409 rbk->Qmat = generic_dmat44_inverse(Q) ; /* [Q] matrix to be saved */
410 #ifdef DEBUG
411 DUMP_DMAT44("Q",Q) ;
412 DUMP_DMAT44("Qinv",rbk->Qmat) ;
413 #endif
414 }
415
416 if( verb > 1 ) ININFO_message(" Elapsed = %.1f",COX_clock_time()-ct) ;
417
418 RETURN(rbk) ;
419 }
420
421 /*------------------------------------------------------------------*/
422
423 #if 0
424 #undef ADDTO_intar
425 #define ADDTO_intar(nar,nal,ar,val) \
426 do{ if( (nar) == (nal) ){ \
427 (nal) = 1.2*(nal)+16; (ar) = (int *)realloc((ar),sizeof(int)*(nal)); \
428 } \
429 (ar)[(nar)++] = (val); \
430 } while(0)
431
432 #undef CLIP_intar
433 #define CLIP_intar(nar,nal,ar) \
434 do{ if( (nar) < (nal) && (nar) > 0 ){ \
435 (nal) = (nar); (ar) = (int *)realloc((ar),sizeof(int)*(nal)); \
436 }} while(0)
437 #endif
438
439 /*------------------------------------------------------------------*/
440 /*! For speedup, for each grid point compute the first and last
441 knots that can affect its output value. This will save time
442 when looping over knots to evaluate the RBF expansion.
443 *//*----------------------------------------------------------------*/
444
RBF_setup_kranges(RBF_knots * rbk,RBF_evalgrid * rbg)445 void RBF_setup_kranges( RBF_knots *rbk , RBF_evalgrid *rbg )
446 {
447 int npt , nk ;
448 double ct ;
449
450 ENTRY("RBF_setup_kranges") ;
451
452 if( rbk == NULL || rbg == NULL ) EXRETURN ;
453
454 if( rbg->klast != NULL ){ free(rbg->klast) ; rbg->klast = NULL; }
455 if( rbg->kfirst!= NULL ){ free(rbg->kfirst); rbg->kfirst= NULL; }
456
457 if( rbk->nknot > RBFKINT_MAX ) EXRETURN ; /* too many to store r/
458
459 /* load some local variables */
460
461 npt = rbg->npt ;
462 nk = rbk->nknot ;
463
464 rbg->kfirst = (RBFKINT *)calloc(sizeof(RBFKINT),npt) ;
465 rbg->klast = (RBFKINT *)calloc(sizeof(RBFKINT),npt) ;
466
467 if( verb ){
468 INFO_message("RBF_setup_kranges: %d grid points",npt) ;
469 }
470
471 ct = COX_clock_time() ;
472
473 AFNI_OMP_START ;
474 #pragma omp parallel if(npt*nk > 9999)
475 {
476 int ii,jj , kbot,ktop ;
477 float xt,yt,zt, rqq, xk,yk,zk, rr, *xx,*yy,*zz ;
478 RBFKINT *klast , *kfirst ;
479
480 rqq = rbk->rqq ; xx = rbk->xknot; yy = rbk->yknot; zz = rbk->zknot;
481 kfirst = rbg->kfirst ; klast = rbg->klast ;
482 #pragma omp for
483 for( ii=0 ; ii < npt ; ii++ ){
484 xt = rbg->xpt[ii]; yt = rbg->ypt[ii]; zt = rbg->zpt[ii];
485 kbot = ktop = -1 ;
486 for( jj=0 ; jj < nk ; jj++ ){
487 xk = xt-xx[jj]; rr =xk*xk; if( rr >= rqq ) continue;
488 yk = yt-yy[jj]; rr+=yk*yk; if( rr >= rqq ) continue;
489 zk = zt-zz[jj]; rr+=zk*zk; if( rr >= rqq ) continue;
490 ktop = jj ; if( kbot < 0 ) kbot = jj ;
491 }
492 if( kbot >= 0 ){
493 kfirst[ii] = (RBFKINT)kbot ;
494 klast [ii] = (RBFKINT)ktop ;
495 }
496 }
497
498 } /* end OpenMP */
499 AFNI_OMP_END ;
500
501 if( verb > 1 ){
502 float ntot=0.0f ; int ii ;
503 for( ii=0 ; ii < npt ; ii++ ) ntot += (1.0f+rbg->klast[ii]-rbg->kfirst[ii]) ;
504 ININFO_message(" average krange = %.1f Elapsed = %.1f",
505 ntot/npt , COX_clock_time()-ct ) ;
506 }
507
508 EXRETURN ;
509 }
510