void compute_P(const vnl_matrix<double>& x,const vnl_matrix<double>& y, vnl_matrix<double>& P, double &E, double sigma, int outliers) {
double k;
k = -2*sigma*sigma;
//P.set_size(m,n); P.fill(0);
//vnl_vector<double> v_ij;
vnl_vector<double> column_sum;
int m = x.rows();
int s = y.rows();
int d = x.cols();
column_sum.set_size(s);
column_sum.fill(0);
double outlier_term = outliers*pow((2*sigma*sigma*3.1415926),0.5*d);
for (int i = 0; i < m; ++i) {
for (int j = 0; j < s; ++j) {
double r = 0;
for (int t = 0; t < d; ++t) {
r += (x(i,t) - y(j,t))*(x(i,t) - y(j,t));
}
P(i,j) = exp(r/k);
column_sum[j]+=P(i,j);
}
}
if (outliers!=0) {
for (int i = 0; i < s; ++i)
column_sum[i] += outlier_term;
}
if (column_sum.min_value()>(1e-12)) {
E = 0;
for (int i = 0; i < s; ++i) {
for (int j = 0; j < m; ++j){
P(j,i) = P(j,i)/column_sum[i];
}
E -= log(column_sum[i]);
}
//vcl_cerr< < s;
//vcl_cerr<<P.get_column(10);
}
else {
P.empty();
}
}
void RegPowell::computeIterativeRegistration( int nmax,double epsit,MRI * mriS, MRI* mriT, const vnl_matrix < double > & m , double iscaleinit)
// retruns 4x4 matrix and iscale value
{
if (!mriS) mriS = mri_source;
if (!mriT) mriT = mri_target;
assert (mriS && mriT);
tocurrent = this; // so that we can access this from static cost function
pair < vnl_matrix_fixed < double, 4, 4> , double > fmd(vnl_matrix_fixed < double, 4 , 4> () ,iscaleinit);
// check if mi (inital transform) is passed
if (!m.empty()) fmd.first = m;
else if (!Minit.empty()) fmd.first = Minit;
else fmd.first = initializeTransform(mriS,mriT) ;
if (debug > 0)
{
cout << " - initial transform:\n" ;
//MatrixPrintFmt(stdout,"% 2.8f",fmd.first);
cout << fmd.first << endl;
}
// here maybe better to symmetrically warp both images SQRT(M)
// this keeps the problem symmetric
cout << " - warping source and target (sqrt)" << endl;
//if (mh1) MatrixFree(&mh1);
mh1 = MyMatrix::MatrixSqrt(fmd.first);
// do not just assume m = mh*mh, rather m = mh2 * mh
// for transforming target we need mh2^-1 = mh * m^-1
//MATRIX * mi = MatrixInverse(fmd.first,NULL);
//MATRIX * mhi = MatrixMultiply(mh1,mi,NULL);
vnl_matrix_fixed < double, 4, 4 > mhi = mh1 * vnl_inverse(fmd.first);
//set static
//if (mh2) MatrixFree(&mh2);
//mh2 = MatrixInverse(mhi,NULL); // M = mh2 * mh1
mh2 = vnl_inverse(mhi); // M = mh2 * mh1
//if (mri_Swarp) MRIfree(&mri_Swarp);
//mri_Swarp = MRIclone(mriS,NULL);
//mri_Swarp = MRIlinearTransform(mriS,mri_Swarp, mh);
//if (mri_Twarp) MRIfree(&mri_Twarp);
//mri_Twarp = MRIclone(mriS,NULL); // bring them to same space (just use src geometry)
//mri_Twarp = MRIlinearTransform(mriT,mri_Twarp, mhi);
// //MatrixFree(&mh);
//MatrixFree(&mhi);
// MatrixFree(&mi);
// adjust intensity later in first powell call
scf = mriS;
tcf = mriT;
// create parameter vector:
pcount = 3; // transolny
if (rigid) pcount = 6;
else pcount = 12;
if (pcount==3) assert(transonly);
if (iscale) pcount++;
// compute Registration
cout << " - compute new registration ( " << pcount << " params )" << endl;
float fret, fstart, min_sse;
float* p = ::vector(1, pcount+1) ;
float** xi = ::matrix(1, pcount+1, 1, pcount+1) ;
float tol = 1e-5; //-8
int maxiter = 36;
int iter;
for (int i = 1;i<=pcount;i++)
{
p[i] = 0.0;
for (int j = i;j<=pcount;j++)
{
xi[i][j] = 0.0;
xi[j][i] = 0.0;
}
xi[i][i] = 1.0;
}
if (iscale) p[pcount] = iscaleinit;
min_sse = costFunction(p) ;
cout << " min_sse: " << min_sse << endl;
icount = 0;
//OpenPowell(p, xi, pcount, tol, &iter, &fret, costFunction);
OpenPowell2(p, xi, pcount, tol,tol,maxiter, &iter, &fret, costFunction);
cout << endl << "best alignment initial powell: " << fret << " (" << iter << " steps)" << endl;
int count = 0;
do
{
count++;
// reinitialize powell directions
for (int r = 1 ; r <= pcount ; r++)
{
for (int c = 1 ; c <= pcount ; c++)
{
xi[r][c] = r == c ? 1 : 0 ;
}
}
fstart = fret ;
icount = 0;
OpenPowell(p, xi, pcount, tol, &iter, &fret, costFunction);
cout << endl << " return from " << count << " powell!!!" << endl;
// if (Gdiag & DIAG_SHOW && DIAG_VERBOSE_ON)
// printf("best alignment after powell: %2.3f (%d steps)\n", fret, iter) ;
cout << "best alignment after powell: " << fret << " (" << iter << " steps)" << endl;
if ((fstart-fret)/fstart < tol)
break ;
}
while (fret < fstart) ;
vnl_vector < double > v(RegPowell::pcount);
for (int i = 1;i<=RegPowell::pcount; i++)
v[i-1] = p[i];
// MatrixPrintFmt(stdout,"% 2.8f",v);
cout << v << endl;
//if (fmd.first) MatrixFree(&fmd.first);
fmd = RegistrationStep<double>::convertP2Md(v,rtype);
//MatrixFree(&v);
//MatrixPrintFmt(stdout,"% 2.8f",fmd.first);
cout << fmd.first << endl;
free_matrix(xi, 1, pcount+1, 1, pcount+1) ;
free_vector(p, 1, pcount+1) ;
// new full M = mh2 * cm * mh1
//fmd.first = MatrixMultiply(mh2,fmd.first,fmd.first);
//fmd.first = MatrixMultiply(fmd.first,mh1,fmd.first);
fmd.first = mh2 * fmd.first * mh1;
Mfinal = fmd.first;
iscalefinal = fmd.second;
cout << endl << " DONE " << endl;
cout << endl << "Final Transform:" << endl;
//MatrixPrintFmt(stdout,"% 2.8f",Mfinal);
cout << Mfinal << endl;
MRI* mri_Swarp = MRIclone(mriT,NULL);
mri_Swarp = MyMRI::MRIlinearTransform(mriS,mri_Swarp,Mfinal);
MRIwrite(mriS,"mriS.mgz");
MRIwrite(mri_Swarp,"mriSwarp.mgz");
MRIwrite(mriT,"mriT.mgz");
// exit(1);
// return fmd ;
//
// //p = computeRegistrationStepP(mri_Swarp,mri_Twarp);
// if (pw.second) MRIfree(&pw.second);
// pw = computeRegistrationStepW(mri_Swarp,mri_Twarp);
//
// if (cmd.first != NULL) MatrixFree(&cmd.first);
// cmd = convertP2MATRIXd(pw.first);
// if (lastp) MatrixFree(&lastp);
// lastp = pw.first;
// pw.first = NULL;
//
//
// // store M and d
// cout << " - store transform" << endl;
// //cout << endl << " current : Matrix: " << endl;
// //MatrixPrintFmt(stdout,"% 2.8f",cmd.first);
// //cout << " intens: " << cmd.second << endl;
// MATRIX* fmdtmp = MatrixCopy(fmd.first,NULL);
// //fmd.first = MatrixMultiply(cmd.first,fmd.first,fmd.first); //old
// if(mh2) MatrixFree(&mh2);
// mh2 = MatrixInverse(mhi,NULL); // M = mh2 * mh
// // new M = mh2 * cm * mh
// fmd.first = MatrixMultiply(mh2,cmd.first,fmd.first);
// fmd.first = MatrixMultiply(fmd.first,mh,fmd.first);
//
// fmd.second *= cmd.second;
// //cout << endl << " Matrix: " << endl;
// //MatrixPrintFmt(stdout,"% 2.8f",fmd.first);
// if (!rigid) diff = getFrobeniusDiff(fmd.first, fmdtmp);
// else diff = sqrt(RigidTransDistSq(fmd.first, fmdtmp));
// cout << " -- old difference to prev. transform: " << diff << endl;
// diff = sqrt(AffineTransDistSq(fmd.first, fmdtmp, 100));
// cout << " -- difference to prev. transform: " << diff << endl;
// //cout << " intens: " << fmd.second << endl;
//
// MatrixFree(&fmdtmp);
// MatrixFree(&mi);
// //MatrixFree(&mh);
// //MatrixFree(&mhi);
// //MatrixFree(&mh2);
//
// }
//
// // DEBUG OUTPUT
// if (debug > 0)
// {
// // write weights and warped images after last step:
//
// MRIwrite(mri_Swarp,(name+"-mriS-warp.mgz").c_str());
// MRIwrite(mri_Twarp,(name+"-mriT-warp.mgz").c_str());
// MRI* salign = MRIclone(mriS,NULL);
// salign = MRIlinearTransform(mri_Swarp, salign,cmd.first);
// MRIwrite(salign,(name+"-mriS-align.mgz").c_str());
// MRIfree(&salign);
// if (pw.second)
// {
// // in the half-way space:
// string n = name+string("-mriS-weights.mgz");
// MRIwrite(pw.second,n.c_str());
// }
// }
//
// // store weights (mapped to target space):
// if (pw.second)
// {
// // remove negative weights (markers) set to 1
// int x,y,z;
// for (z = 0 ; z < pw.second->depth ; z++)
// for (x = 0 ; x < pw.second->width ; x++)
// for (y = 0 ; y < pw.second->height ; y++)
// {
// if (MRIFvox(pw.second,x,y,z) < 0) MRIFvox(pw.second,x,y,z) = 1;
// }
// MRI * mtmp = MRIalloc(mriT->width,mriT->height,mriT->depth,MRI_FLOAT);
// MRIcopyHeader(mriT,mtmp);
// mtmp->type = MRI_FLOAT;
// mtmp = MRIlinearTransform(pw.second,mtmp,mh2);
// //MRIwrite(mtmp,weightsname.c_str());
// MRIfree(&pw.second);
// pw.second = mtmp;
// }
// if (mri_weights) MRIfree(&mri_weights);
// mri_weights = pw.second;
// if (mov2weights) MatrixFree(&mov2weights);
// if (dst2weights) MatrixFree(&dst2weights);
// mov2weights = mh; // no freeing needed
// dst2weights = mhi;
//
// if (diff > epsit) // adjust mh and mhi to new midpoint
// {
// cout << " -- adjusting half-way maps " << endl;
// MATRIX * ch = MatrixSqrt(fmd.first);
// // do not just assume c = ch*ch, rather c = ch2 * ch
// // for transforming target we need ch2^-1 = ch * c^-1
// MATRIX * ci = MatrixInverse(cmd.first,NULL);
// MATRIX * chi = MatrixMultiply(ch,ci,NULL);
// // append ch or chi to mh mhi
// mov2weights = MatrixMultiply(ch,mh,NULL);
// dst2weights = MatrixMultiply(chi,mhi,NULL);
// MatrixFree(&mh);
// MatrixFree(&mhi);
// }
//
// MRIfree(&mri_Twarp);
// MRIfree(&mri_Swarp);
// MatrixFree(&mh2);
// if (cmd.first != NULL) MatrixFree(&cmd.first);
//
//
// Mfinal = MatrixCopy(fmd.first, Mfinal);
// iscalefinal = fmd.second;
//
// return fmd;
}
