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; }