void pdpmbm_add( pdpm_t * obj, unsigned int grp, unsigned int cls ) {
pdpmbm_t * mdl = (pdpmbm_t *) obj->model;
unsigned int i;
if( grp >= obj->ngr || cls >= obj->ngr )
error( "pdpmb_add: invalid argument: grp = %u cls = %u", grp, cls );
//set vcl, recompute gcl, and possibly ncl
obj->vcl[ grp ] = cls;
if( obj->gcl[ cls ] == 0 ) obj->ncl++;
obj->gcl[ cls ] += 1;
//(re)compute gqcl
for( i = 0; i < mdl->q; i++ )
mdl->gqcl[ FMAT(cls, i, obj->ngr) ] += mdl->y[ FMAT(grp, i, obj->ngr) ];
}
void pdpmbm_sub( pdpm_t * obj, unsigned grp, unsigned int cls ) {
pdpmbm_t * mdl = (pdpmbm_t *) obj->model;
unsigned int i;
if( grp >= obj->ngr || cls >= obj->ngr )
error( "pdpmb_sub: invalid argument: grp = %u", grp );
//set vcl, recompute gcl, and possibly ncl
obj->vcl[ grp ] = BAD_VCL;
obj->gcl[ cls ] -= 1;
if( obj->gcl[ cls ] == 0 ) { obj->ncl--; }
//recompute gqcl
for( i = 0; i < mdl->q; i++ )
mdl->gqcl[ FMAT(cls, i, obj->ngr) ] -= mdl->y[ FMAT(grp, i, obj->ngr) ];
}
double pdpmbm_logpcls( pdpm_t * obj, unsigned int cls ) {
pdpmbm_t * mdl = (pdpmbm_t *) obj->model;
unsigned int i;
double logp = 0.0;
if( obj->gcl[ cls ] == 0 ) return logp;
//compute posterior mass
for( i = 0; i < mdl->q; i++ ) {
logp += lgamma( mdl->a0 + (double) mdl->gqcl[ FMAT(cls, i, obj->ngr) ] ) +\
lgamma( mdl->b0 + (double) obj->gcl[ cls ] -\
(double) mdl->gqcl[ FMAT(cls, i, obj->ngr) ] ) -\
lgamma( (double) obj->gcl[ cls ] + mdl->a0 + mdl->b0 );
}
if( obj->flags & FLAG_DIRICHL ) { logp += lgamma( obj->gcl[ cls ] ); }
else { logp += obj->lam * lgamma( obj->gcl[ cls ] + 1 ); }
return logp;
}
static int
qr_companion (double *h, size_t nc, gsl_complex_packed_ptr zroot)
{
double t = 0.0;
size_t iterations, e, i, j, k, m;
double w, x, y, s, z;
double p = 0, q = 0, r = 0;
/* FIXME: if p,q,r, are not set to zero then the compiler complains
that they ``might be used uninitialized in this
function''. Looking at the code this does seem possible, so this
should be checked. */
int notlast;
size_t n = nc;
next_root:
if (n == 0)
return GSL_SUCCESS ;
iterations = 0;
next_iteration:
for (e = n; e >= 2; e--)
{
double a1 = fabs (FMAT (h, e, e - 1, nc));
double a2 = fabs (FMAT (h, e - 1, e - 1, nc));
double a3 = fabs (FMAT (h, e, e, nc));
if (a1 <= GSL_DBL_EPSILON * (a2 + a3))
break;
}
x = FMAT (h, n, n, nc);
if (e == n)
{
GSL_SET_COMPLEX_PACKED (zroot, n-1, x + t, 0); /* one real root */
n--;
goto next_root;
/*continue;*/
}
y = FMAT (h, n - 1, n - 1, nc);
w = FMAT (h, n - 1, n, nc) * FMAT (h, n, n - 1, nc);
if (e == n - 1)
{
p = (y - x) / 2;
q = p * p + w;
y = sqrt (fabs (q));
x += t;
if (q > 0) /* two real roots */
{
if (p < 0)
y = -y;
y += p;
GSL_SET_COMPLEX_PACKED (zroot, n-1, x - w / y, 0);
GSL_SET_COMPLEX_PACKED (zroot, n-2, x + y, 0);
}
else
{
GSL_SET_COMPLEX_PACKED (zroot, n-1, x + p, -y);
GSL_SET_COMPLEX_PACKED (zroot, n-2, x + p, y);
}
n -= 2;
goto next_root;
/*continue;*/
}
/* No more roots found yet, do another iteration */
if (iterations == 60) /* increased from 30 to 60 */
{
/* too many iterations - give up! */
return GSL_FAILURE ;
}
if (iterations % 10 == 0 && iterations > 0)
{
/* use an exceptional shift */
t += x;
for (i = 1; i <= n; i++)
{
FMAT (h, i, i, nc) -= x;
}
s = fabs (FMAT (h, n, n - 1, nc)) + fabs (FMAT (h, n - 1, n - 2, nc));
y = 0.75 * s;
x = y;
w = -0.4375 * s * s;
}
iterations++;
for (m = n - 2; m >= e; m--)
{
double a1, a2, a3;
z = FMAT (h, m, m, nc);
r = x - z;
s = y - z;
p = FMAT (h, m, m + 1, nc) + (r * s - w) / FMAT (h, m + 1, m, nc);
q = FMAT (h, m + 1, m + 1, nc) - z - r - s;
r = FMAT (h, m + 2, m + 1, nc);
s = fabs (p) + fabs (q) + fabs (r);
p /= s;
q /= s;
r /= s;
if (m == e)
break;
a1 = fabs (FMAT (h, m, m - 1, nc));
a2 = fabs (FMAT (h, m - 1, m - 1, nc));
a3 = fabs (FMAT (h, m + 1, m + 1, nc));
if (a1 * (fabs (q) + fabs (r)) <= GSL_DBL_EPSILON * fabs (p) * (a2 + a3))
break;
}
for (i = m + 2; i <= n; i++)
{
FMAT (h, i, i - 2, nc) = 0;
}
for (i = m + 3; i <= n; i++)
{
FMAT (h, i, i - 3, nc) = 0;
}
/* double QR step */
for (k = m; k <= n - 1; k++)
{
notlast = (k != n - 1);
if (k != m)
{
p = FMAT (h, k, k - 1, nc);
q = FMAT (h, k + 1, k - 1, nc);
r = notlast ? FMAT (h, k + 2, k - 1, nc) : 0.0;
x = fabs (p) + fabs (q) + fabs (r);
if (x == 0)
continue; /* FIXME????? */
p /= x;
q /= x;
r /= x;
}
s = sqrt (p * p + q * q + r * r);
if (p < 0)
s = -s;
if (k != m)
{
FMAT (h, k, k - 1, nc) = -s * x;
}
else if (e != m)
{
FMAT (h, k, k - 1, nc) *= -1;
}
p += s;
x = p / s;
y = q / s;
z = r / s;
q /= p;
r /= p;
/* do row modifications */
for (j = k; j <= n; j++)
{
p = FMAT (h, k, j, nc) + q * FMAT (h, k + 1, j, nc);
if (notlast)
{
p += r * FMAT (h, k + 2, j, nc);
FMAT (h, k + 2, j, nc) -= p * z;
}
FMAT (h, k + 1, j, nc) -= p * y;
FMAT (h, k, j, nc) -= p * x;
}
j = (k + 3 < n) ? (k + 3) : n;
/* do column modifications */
for (i = e; i <= j; i++)
{
p = x * FMAT (h, i, k, nc) + y * FMAT (h, i, k + 1, nc);
if (notlast)
{
p += z * FMAT (h, i, k + 2, nc);
FMAT (h, i, k + 2, nc) -= p * r;
}
FMAT (h, i, k + 1, nc) -= p * q;
FMAT (h, i, k, nc) -= p;
}
}
goto next_iteration;
}
static MRI_IMAGE * mri_warp3D_align_fitim( MRI_warp3D_align_basis *bas ,
MRI_IMAGE *cim ,
int warp_mode , float delfac )
{
MRI_IMAGE *fitim , *pim , *mim ;
float *fitar , *car=MRI_FLOAT_PTR(cim) ;
int nfree=bas->nfree , *ima=MRI_INT_PTR(bas->imap) , nmap=bas->imap->nx ;
int npar =bas->nparam ;
float *pvec , *par , *mar ;
int ii , pp , cc ;
float dpar , delta ;
/*-- create image containing basis columns --*/
fitim = mri_new( nmap , nfree+1 , MRI_float ) ;
fitar = MRI_FLOAT_PTR(fitim) ;
pvec = (float *)malloc(sizeof(float) * npar) ;
#undef FMAT
#define FMAT(i,j) fitar[(i)+(j)*nmap] /* col dim=nmap, row dim=nfree+1 */
/* column #nfree = base image itself */
for( ii=0 ; ii < nmap ; ii++ ) FMAT(ii,nfree) = car[ima[ii]] ;
pvec = (float *)malloc(sizeof(float) * npar) ;
/* for each free parameter:
apply inverse transform to base image with param value up and down
compute central difference to approximate derivative of base
image wrt parameter
store as a column in the fitim matrix */
mri_warp3D_method( warp_mode ) ; /* set interpolation mode */
mri_warp3D_set_womask( bas->imsk ) ;
for( pp=0,cc=0 ; pp < npar ; pp++ ){
if( bas->param[pp].fixed ) continue ; /* don't do this one! */
/* init all params to their identity transform value */
for( ii=0 ; ii < npar ; ii++ )
pvec[ii] = (bas->param[ii].fixed) ? bas->param[ii].val_fixed
: bas->param[ii].ident ;
/* change in the pp-th parameter to use for derivative */
dpar = delfac * bas->param[pp].delta ;
if( bas->verb )
fprintf(stderr,"+ difference base by %f in param#%d [%s]\n",
dpar , pp+1 , bas->param[pp].name ) ;
pvec[pp] = bas->param[pp].ident + dpar ; /* set positive change */
bas->vwset( npar , pvec ) ; /* put into transform */
pim = mri_warp3D( cim , 0,0,0 , bas->vwinv ) ; /* warp image */
pvec[pp] = bas->param[pp].ident - dpar ; /* set negative change */
bas->vwset( npar , pvec ) ;
mim = mri_warp3D( cim , 0,0,0 , bas->vwinv ) ;
/* compute derivative */
delta = bas->scale_init / ( 2.0f * dpar ) ;
par = MRI_FLOAT_PTR(pim) ; mar = MRI_FLOAT_PTR(mim) ;
for( ii=0 ; ii < nmap ; ii++ )
FMAT(ii,cc) = delta * ( par[ima[ii]] - mar[ima[ii]] ) ;
#if 0
{ float psum=0.0f,msum=0.0f,dsum=0.0f;
for( ii=0 ; ii < nmap ; ii++ ){
psum += fabsf(par[ima[ii]]) ;
msum += fabsf(mar[ima[ii]]) ;
dsum += fabsf(FMAT(ii,cc)) ;
}
fprintf(stderr," pp=%d psum=%g msum=%g dsum=%g\n",pp,psum,msum,dsum) ;
}
#endif
mri_free(pim) ; mri_free(mim) ; /* no longer needed */
cc++ ; /* oopsie */
}
mri_warp3D_set_womask( NULL ) ;
free((void *)pvec) ;
#if 0
{ int zz , jj ;
for( jj=0 ; jj <= nfree ; jj++ ){
zz = 0 ;
for( ii=0 ; ii < nmap ; ii++ ) if( FMAT(ii,jj) == 0.0 ) zz++ ;
fprintf(stderr," fitim: col#%d has %d zeros out of %d\n",jj,zz,nmap) ;
}
}
#endif
return(fitim) ;
}
