MatrixXf CharacterController::GenerateXapv(const std::vector<int> &activeParts)
{
// pvDim without
auto& allClipinfo = m_cpxClipinfo;
auto& pvFacade = allClipinfo.PvFacade;
int pvDim = pvFacade.GetAllPartDimension();
assert(pvDim > 0);
MatrixXf Xabpv(allClipinfo.ClipFrames(), size(activeParts) * pvDim);
ArrayXi incX(pvDim);
incX.setLinSpaced(0, pvDim - 1);
MatrixXi apMask = VectorXi::Map(activeParts.data(), activeParts.size()).replicate(1, pvDim).transpose();
apMask.array() = apMask.array() * pvDim + incX.replicate(1, apMask.cols());
auto maskVec = VectorXi::Map(apMask.data(), apMask.size());
selectCols(pvFacade.GetAllPartsSequence(), maskVec, &Xabpv);
Pca<MatrixXf> pcaXabpv(Xabpv);
int dXabpv = pcaXabpv.reducedRank(g_CharacterPcaCutoff);
Xabpv = pcaXabpv.coordinates(dXabpv);
XabpvT = pcaXabpv.components(dXabpv);
uXabpv = pcaXabpv.mean();
if (g_EnableDebugLogging)
{
ofstream fout(g_CharacterAnalyzeDir / (m_pCharacter->Name + "_Xabpv.pd.csv"));
fout << Xabpv.format(CSVFormat);
fout.close();
}
return Xabpv;
}
void mexFunction(int nlhs, mxArray *plhs[],
int nrhs, const mxArray *prhs[])
{
// This is useful for debugging whether Matlab is caching the mex binary
//mexPrintf("%s %s\n",__TIME__,__DATE__);
igl::matlab::MexStream mout;
std::streambuf *outbuf = std::cout.rdbuf(&mout);
using namespace std;
using namespace Eigen;
using namespace igl;
using namespace igl::matlab;
using namespace igl::copyleft::cgal;
MatrixXd V;
MatrixXi F;
igl::copyleft::cgal::RemeshSelfIntersectionsParam params;
string prefix;
bool use_obj_format = false;
if(nrhs < 2)
{
mexErrMsgTxt("nrhs < 2");
}
parse_rhs_double(prhs,V);
parse_rhs_index(prhs+1,F);
mexErrMsgTxt(V.cols()==3,"V must be #V by 3");
mexErrMsgTxt(F.cols()==3,"F must be #F by 3");
if(nrhs>2)
{
int i = 2;
while(i<nrhs)
{
if(!mxIsChar(prhs[i]))
{
mexErrMsgTxt("Parameter names should be char strings");
}
// Cast to char
const char * name = mxArrayToString(prhs[i]);
if(strcmp("DetectOnly",name) == 0)
{
validate_arg_scalar(i,nrhs,prhs,name);
validate_arg_logical(i,nrhs,prhs,name);
mxLogical * v = (mxLogical *)mxGetData(prhs[++i]);
params.detect_only = *v;
}else if(strcmp("FirstOnly",name) == 0)
{
validate_arg_scalar(i,nrhs,prhs,name);
validate_arg_logical(i,nrhs,prhs,name);
mxLogical * v = (mxLogical *)mxGetData(prhs[++i]);
params.first_only = *v;
}else if(strcmp("StitchAll",name) == 0)
{
validate_arg_scalar(i,nrhs,prhs,name);
validate_arg_logical(i,nrhs,prhs,name);
mxLogical * v = (mxLogical *)mxGetData(prhs[++i]);
params.stitch_all = *v;
}else
{
mexErrMsgTxt(C_STR("Unsupported parameter: "<<name));
}
i++;
}
}
MatrixXi IF;
VectorXi J,IM;
if(F.rows()>0)
{
// Check that there aren't any combinatorially or geometrically degenerate triangles
VectorXd A;
doublearea(V,F,A);
if(A.minCoeff()<=0)
{
mexErrMsgTxt("Geometrically degenerate face found.");
}
if(
(F.array().col(0) == F.array().col(1)).any() ||
(F.array().col(1) == F.array().col(2)).any() ||
(F.array().col(2) == F.array().col(0)).any())
{
mexErrMsgTxt("Combinatorially degenerate face found.");
}
// Now mesh self intersections
{
MatrixXd tempV;
MatrixXi tempF;
remesh_self_intersections(V,F,params,tempV,tempF,IF,J,IM);
//cout<<BLUEGIN("Found and meshed "<<IF.rows()<<" pair"<<(IF.rows()==1?"":"s")
// <<" of self-intersecting triangles.")<<endl;
V=tempV;
F=tempF;
}
// Double-check output
#ifdef DEBUG
// There should be *no* combinatorial duplicates
{
MatrixXi tempF;
unique_simplices(F,tempF);
if(tempF.rows() < F.rows())
{
cout<<REDRUM("Error: selfintersect created "<<
F.rows()-tempF.rows()<<" combinatorially duplicate faces")<<endl;
}else
{
assert(tempF.rows() == F.rows());
cout<<GREENGIN("selfintersect created no duplicate faces")<<endl;
}
F = tempF;
}
#endif
}
// Prepare left-hand side
switch(nlhs)
{
case 5:
{
// Treat indices as reals
plhs[4] = mxCreateDoubleMatrix(IM.rows(),IM.cols(), mxREAL);
double * IMp = mxGetPr(plhs[4]);
VectorXd IMd = (IM.cast<double>().array()+1).matrix();
copy(&IMd.data()[0],&IMd.data()[0]+IMd.size(),IMp);
// Fallthrough
}
case 4:
{
// Treat indices as reals
plhs[3] = mxCreateDoubleMatrix(J.rows(),J.cols(), mxREAL);
double * Jp = mxGetPr(plhs[3]);
VectorXd Jd = (J.cast<double>().array()+1).matrix();
copy(&Jd.data()[0],&Jd.data()[0]+Jd.size(),Jp);
// Fallthrough
}
case 3:
{
// Treat indices as reals
plhs[2] = mxCreateDoubleMatrix(IF.rows(),IF.cols(), mxREAL);
double * IFp = mxGetPr(plhs[2]);
MatrixXd IFd = (IF.cast<double>().array()+1).matrix();
copy(&IFd.data()[0],&IFd.data()[0]+IFd.size(),IFp);
// Fallthrough
}
case 2:
{
// Treat indices as reals
plhs[1] = mxCreateDoubleMatrix(F.rows(),F.cols(), mxREAL);
double * Fp = mxGetPr(plhs[1]);
MatrixXd Fd = (F.cast<double>().array()+1).matrix();
copy(&Fd.data()[0],&Fd.data()[0]+Fd.size(),Fp);
// Fallthrough
}
case 1:
{
plhs[0] = mxCreateDoubleMatrix(V.rows(),V.cols(), mxREAL);
double * Vp = mxGetPr(plhs[0]);
copy(&V.data()[0],&V.data()[0]+V.size(),Vp);
break;
}
default:break;
}
// Restore the std stream buffer Important!
std::cout.rdbuf(outbuf);
}
MatrixXi m(3, 3);
m << 1, 2, 3,
4, 5, 6,
7, 8, 9;
m = (m.array() >= 5).select(-m, m);
cout << m << endl;
// mcmc_helper(im,im_prev,buf,int(xwho),int(xwho2));
void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]){
// trw_bprop_helper2(model, psi_ij_rho, psi_i, rho, maxiter, n, m1, m2, b_i,...
// dm1, dm2, dn, dpsi_i, dpsi_ij);
int i=0;
int nnodes = mapdouble(mxGetField(prhs[i],0,"nnodes"))(0);
int ncliques = mapdouble(mxGetField(prhs[i],0,"ncliques"))(0);
int nvals = mapdouble(mxGetField(prhs[i],0,"nvals"))(0);
MatrixXi pairs = mapdouble(mxGetField(prhs[i],0,"pairs")).cast<int>();
pairs.array() -= 1;
MatrixXi N1 = mapdouble(mxGetField(prhs[i],0,"N1")).cast<int>();
N1.array() -= 1;
MatrixXi N2 = mapdouble(mxGetField(prhs[i],0,"N2")).cast<int>();
N2.array() -= 1;
i++;
MatrixMd psi_ij = mapdouble(prhs[i++]);
MatrixMd psi_i = mapdouble(prhs[i++]);
double rho = mapdouble(prhs[i++])(0);
int maxiter = mapdouble(prhs[i++])(0);
MatrixMd n = mapdouble(prhs[i++]);
MatrixMd m1 = mapdouble(prhs[i++]);
MatrixMd m2 = mapdouble(prhs[i++]);
MatrixMd b_i = mapdouble(prhs[i++]);
MatrixMd mstor = mapdouble(prhs[i++]);
MatrixMd dm1 = mapdouble(prhs[i++]);
MatrixMd dm2 = mapdouble(prhs[i++]);
MatrixMd dn = mapdouble(prhs[i++]);
MatrixMd dpsi_i = mapdouble(prhs[i++]);
MatrixMd dpsi_ij = mapdouble(prhs[i++]);
MatrixMd b_ij0 = mapdouble(prhs[i++]);
MatrixMd db_ij0 = mapdouble(prhs[i++]);
int dorec = mapdouble(prhs[i++]).cast<int>()(0);
int w = mstor.rows()-1;
MatrixXd S (nvals,nvals);
MatrixXd m0(nvals,1);
for(int c=0; c<ncliques; c++){
int i = pairs(c, 0);
int j = pairs(c, 1);
for(int yi=0; yi<nvals; yi++){
for(int yj=0; yj<nvals; yj++){
int index = yi + yj*nvals;
//b_ij0(index,c) = b_ij0(index,c)*psi_i(yi,i)*psi_i(yj,j)*n(yi,i)*n(yj,j)/m1(yi,c)/m2(yj,c);
dpsi_i(yi, i) = dpsi_i(yi, i) + db_ij0(index, c)*b_ij0(index, c)/psi_i(yi, i);
dpsi_i(yj, j) = dpsi_i(yj, j) + db_ij0(index, c)*b_ij0(index, c)/psi_i(yj, j);
dn(yi, i) = dn(yi, i) + db_ij0(index, c)*b_ij0(index, c)/n(yi, i);
dn(yj, j) = dn(yj, j) + db_ij0(index, c)*b_ij0(index, c)/n(yj, j);
dm1(yi, c) = dm1(yi, c) - db_ij0(index, c)*b_ij0(index, c)/m1(yi, c);
dm2(yj, c) = dm2(yj, c) - db_ij0(index, c)*b_ij0(index, c)/m2(yj, c);
}
}
}
for(int i=0; i<b_i.cols(); i++){
for(int yi=0; yi<nvals; yi++){
for(int k=0; k<N1.cols(); k++){
int d = N1(i, k);
if(d==-2) continue;
//n(yi, i) *= m1(yi, d);
dm1(yi,d) += rho * dn(yi,i)*n(yi,i)/m1(yi,d);
}
for(int k=0; k<N2.cols(); k++){
int d = N2(i, k);
if(d==-2) continue;
//n(yi, i) *= m2(yi, d);
dm2(yi,d) += rho * dn(yi,i)*n(yi,i)/m2(yi,d);
}
}
}
int reps;
double conv;
for(reps=0; reps<maxiter; reps++){
for(int c0=2*ncliques-1; c0>=0; c0--){
int c, mode;
if(c0<ncliques){
c = c0;
mode = 1;
} else{
c = ncliques - 1 - (c0-ncliques);
mode = 2;
}
int i = pairs(c,0);
int j = pairs(c,1);
if( mode==1 ){
for(int yi=0; yi<nvals; yi++)
n(yi, i) = 1;
for(int k=0; k<N1.cols(); k++){
int d = N1(i, k);
if(d==-2) continue;
for(int yi=0; yi<nvals; yi++)
n(yi, i) *= m1(yi, d);
}
for(int k=0; k<N2.cols(); k++){
int d = N2(i, k);
if(d==-2) continue;
for(int yi=0; yi<nvals; yi++)
n(yi, i) *= m2(yi, d);
}
// n.col(i) = n.col(i).array().pow(rho);
if(rho==1){
//nothing
} else if(rho==.5){
n.col(i) = n.col(i).array().sqrt();
} else
n.col(i) = n.col(i).array().pow(rho);
// compute m(y_j)
for(int yj=0; yj<nvals; yj++){
m0(yj) = 0;
for(int yi=0; yi<nvals; yi++){
int index = yi + yj*nvals;
S(yi,yj) = psi_ij(index, c)*psi_i(yi, i)*n(yi, i)/m1(yi, c);
m0(yj) += S(yi,yj);
}
}
double k = S.sum();
MatrixXd dm0 = dm2.col(c)/k;
dm0.array() -= (dm2.col(c).array()*m0.array()).sum()/(k*k);
MatrixXd dn = 0*n.col(i);
//#pragma omp parallel for num_threads(2)
for(int yj=0; yj<nvals; yj++){
for(int yi=0; yi<nvals; yi++){
int index = yi + yj*nvals;
dpsi_ij(index,c) += dm0(yj)*S(yi,yj)/psi_ij(index,c);
dpsi_i(yi,i) += dm0(yj)*S(yi,yj)/psi_i(yi,i);
dn(yi) += dm0(yj)*S(yi,yj)/n(yi,i);
dm1(yi,c) -= dm0(yj)*S(yi,yj)/m1(yi,c);
}
}
for(int yi=0; yi<nvals; yi++){
for(int k=0; k<N1.cols(); k++){
int d = N1(i, k);
if(d==-2) continue;
dm1(yi,d) += rho*dn(yi)*n(yi,i)/m1(yi,d);
}
for(int k=0; k<N2.cols(); k++){
int d = N2(i, k);
if(d==-2) continue;
dm2(yi,d) += rho*dn(yi)*n(yi,i)/m2(yi,d);
}
}
dm2.col(c) *= 0.0;
if( dorec )
for(int yj=nvals-1; yj>=0; yj--){ m2(yj,c)=mstor(w); w--;}
} else{
for( int yj=0; yj<nvals; yj++)
n(yj, j) = 1;
for(int k=0; k<N1.cols(); k++){
int d = N1(j, k);
if(d==-2) continue;
for( int yj=0; yj<nvals; yj++)
n(yj, j) *= m1(yj, d);
}
for(int k=0; k<N2.cols(); k++){
int d = N2(j, k);
if(d==-2) continue;
for( int yj=0; yj<nvals; yj++)
n(yj, j) *= m2(yj, d);
}
// n.col(j) = n.col(j).array().pow(rho);
if(rho==1){
//nothing
} else if(rho==.5){
n.col(j) = n.col(j).array().sqrt();
} else
n.col(j) = n.col(j).array().pow(rho);
for(int yi=0; yi<nvals; yi++){
m0(yi) = 0;
for(int yj=0; yj<nvals; yj++){
int index = yi + yj*nvals;
S(yi,yj) = psi_ij(index, c)*psi_i(yj, j)*n(yj, j)/m2(yj, c);
m0(yi) += S(yi,yj);
}
}
double k = S.sum();
MatrixXd dm0 = dm1.col(c)/k;
dm0.array() -= (dm1.col(c).array()*m0.array()).sum()/(k*k);
MatrixXd dn = 0*n.col(i);
for(int yj=0; yj<nvals; yj++){
for(int yi=0; yi<nvals; yi++){
int index = yi + yj*nvals;
dpsi_ij(index,c) += dm0(yi)*S(yi,yj)/psi_ij(index,c);
dpsi_i(yj,j) += dm0(yi)*S(yi,yj)/psi_i(yj,j);
dn(yj) += dm0(yi)*S(yi,yj)/n(yj,j);
dm2(yj,c) -= dm0(yi)*S(yi,yj)/m2(yj,c);
}
}
for(int yj=0; yj<nvals; yj++){
for(int k=0; k<N1.cols(); k++){
int d = N1(j, k);
if(d==-2) continue;
dm1(yj, d) += rho*dn(yj)*n(yj, j)/m1(yj, d);
}
for(int k=0; k<N2.cols(); k++){
int d = N2(j, k);
if(d==-2) continue;
dm2(yj, d) += rho*dn(yj)*n(yj, j)/m2(yj, d);
}
}
dm1.col(c) *= 0.0;
if( dorec )
for(int yi=nvals-1; yi>=0; yi--){ m1(yi,c)=mstor(w); w--;}
}
}
}
}
void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]) {
// trw_bprop_helper2(model, psi_ij_rho, psi_i, rho, maxiter, n, m1, m2, b_i,...
// dm1, dm2, dn, dpsi_i, dpsi_ij);
int i=0;
int nnodes = mapdouble(mxGetField(prhs[i],0,"nnodes"))(0);
int ncliques = mapdouble(mxGetField(prhs[i],0,"ncliques"))(0);
int nvals = mapdouble(mxGetField(prhs[i],0,"nvals"))(0);
MatrixXi pairs = mapdouble(mxGetField(prhs[i],0,"pairs")).cast<int>();
pairs.array() -= 1;
MatrixXi N1 = mapdouble(mxGetField(prhs[i],0,"N1")).cast<int>();
N1.array() -= 1;
MatrixXi N2 = mapdouble(mxGetField(prhs[i],0,"N2")).cast<int>();
N2.array() -= 1;
MatrixXi tree2clique = mapdouble(mxGetField(prhs[i],0,"tree2clique")).cast<int>();
tree2clique.array() -= 1;
MatrixXi treeschedule = mapdouble(mxGetField(prhs[i],0,"treeschedule")).cast<int>();
treeschedule.array() -= 1;
i++;
MatrixMd psi_ij = mapdouble(prhs[i++]);
MatrixMd psi_i = mapdouble(prhs[i++]);
double rho = mapdouble(prhs[i++])(0);
int maxiter = mapdouble(prhs[i++])(0);
MatrixMd n = mapdouble(prhs[i++]);
MatrixMd m1 = mapdouble(prhs[i++]);
MatrixMd m2 = mapdouble(prhs[i++]);
MatrixMd b_i = mapdouble(prhs[i++]);
MatrixMd mstor = mapdouble(prhs[i++]);
MatrixMd dm1 = mapdouble(prhs[i++]);
MatrixMd dm2 = mapdouble(prhs[i++]);
MatrixMd dn = mapdouble(prhs[i++]);
MatrixMd dpsi_i = mapdouble(prhs[i++]);
MatrixMd dpsi_ij = mapdouble(prhs[i++]);
MatrixMd b_ij0 = mapdouble(prhs[i++]);
MatrixMd db_ij0 = mapdouble(prhs[i++]);
int dorec = mapdouble(prhs[i++]).cast<int>()(0);
MatrixMi w = mapint32(prhs[i++]);
#pragma omp parallel for num_threads(NTHREAD)
for(int c=0; c<ncliques; c++) {
int i = pairs(c, 0);
int j = pairs(c, 1);
for(int yi=0; yi<nvals; yi++) {
for(int yj=0; yj<nvals; yj++) {
int index = yi + yj*nvals;
//b_ij0(index,c) = b_ij0(index,c)*psi_i(yi,i)*psi_i(yj,j)*n(yi,i)*n(yj,j)/m1(yi,c)/m2(yj,c);
dpsi_i(yi, i) = dpsi_i(yi, i) + db_ij0(index, c)*b_ij0(index, c)/psi_i(yi, i);
dpsi_i(yj, j) = dpsi_i(yj, j) + db_ij0(index, c)*b_ij0(index, c)/psi_i(yj, j);
dn(yi, i) = dn(yi, i) + db_ij0(index, c)*b_ij0(index, c)/n(yi, i);
dn(yj, j) = dn(yj, j) + db_ij0(index, c)*b_ij0(index, c)/n(yj, j);
dm1(yi, c) = dm1(yi, c) - db_ij0(index, c)*b_ij0(index, c)/m1(yi, c);
dm2(yj, c) = dm2(yj, c) - db_ij0(index, c)*b_ij0(index, c)/m2(yj, c);
}
}
}
#pragma omp parallel for num_threads(NTHREAD)
for(int i=0; i<b_i.cols(); i++) {
for(int yi=0; yi<nvals; yi++) {
for(int k=0; k<N1.cols(); k++) {
int d = N1(i, k);
if(d==-2) continue;
//n(yi, i) *= m1(yi, d);
dm1(yi,d) += rho * dn(yi,i)*n(yi,i)/m1(yi,d);
}
for(int k=0; k<N2.cols(); k++) {
int d = N2(i, k);
if(d==-2) continue;
//n(yi, i) *= m2(yi, d);
dm2(yi,d) += rho * dn(yi,i)*n(yi,i)/m2(yi,d);
}
}
}
//tree_ncliques = sum(double(model.tree2clique>0),1);
//ntree = length(tree_ncliques);
int ntree = tree2clique.cols();
MatrixXi tree_ncliques = MatrixXi::Zero(ntree,1);
for(int tree=0; tree<tree2clique.cols(); tree++)
for(i=0; i<tree2clique.rows(); i++)
if(tree2clique(i,tree) != -1)
tree_ncliques(tree)++;
int reps;
double conv;
for(reps=0; reps<maxiter; reps++) {
// must re-order blocks
//for(int block=0; block<treeschedule.cols(); block++){
for(int block=treeschedule.cols()-1; block>=0; block--) {
// need not re-order trees, but why not...
// helps if someone specifies not parallel trees
// for(int treenum=treeschedule.rows()-1; treenum>=0; treenum--){
#pragma omp parallel for schedule(dynamic) num_threads(NTHREAD)
for(int treenum=0; treenum<treeschedule.rows(); treenum++) {
MatrixXd S (nvals,nvals);
MatrixXd m0(nvals,1);
int tree = treeschedule(treenum,block);
if(tree==-1)
continue;
int ncliques = tree_ncliques(tree);
for(int c0=2*ncliques-1; c0>=0; c0--) {
int c, mode;
if(c0<ncliques) {
c = tree2clique(c0,tree);
mode = 1;
} else {
c = tree2clique(ncliques - 1 - (c0-ncliques),tree);
mode = 2;
}
//cout << "BW c " << c << " mode " << mode << endl;
int i = pairs(c,0);
int j = pairs(c,1);
if( mode==1 ) {
for(int yi=0; yi<nvals; yi++)
n(yi, i) = 1;
for(int k=0; k<N1.cols(); k++) {
int d = N1(i, k);
if(d==-2) continue;
for(int yi=0; yi<nvals; yi++)
n(yi, i) *= m1(yi, d);
}
for(int k=0; k<N2.cols(); k++) {
int d = N2(i, k);
if(d==-2) continue;
for(int yi=0; yi<nvals; yi++)
n(yi, i) *= m2(yi, d);
}
// n.col(i) = n.col(i).array().pow(rho);
if(rho==1) {
//nothing
} else if(rho==.5) {
n.col(i) = n.col(i).array().sqrt();
} else
n.col(i) = n.col(i).array().pow(rho);
// compute m(y_j)
for(int yj=0; yj<nvals; yj++) {
m0(yj) = 0;
for(int yi=0; yi<nvals; yi++) {
int index = yi + yj*nvals;
S(yi,yj) = psi_ij(index, c)*psi_i(yi, i)*n(yi, i)/m1(yi, c);
m0(yj) += S(yi,yj);
}
}
double k = S.sum();
MatrixXd dm0 = dm2.col(c)/k;
dm0.array() -= (dm2.col(c).array()*m0.array()).sum()/(k*k);
MatrixXd dn = 0*n.col(i);
for(int yj=0; yj<nvals; yj++) {
for(int yi=0; yi<nvals; yi++) {
int index = yi + yj*nvals;
dpsi_ij(index,c) += dm0(yj)*S(yi,yj)/psi_ij(index,c);
dpsi_i(yi,i) += dm0(yj)*S(yi,yj)/psi_i(yi,i);
dn(yi) += dm0(yj)*S(yi,yj)/n(yi,i);
dm1(yi,c) -= dm0(yj)*S(yi,yj)/m1(yi,c);
}
}
for(int yi=0; yi<nvals; yi++) {
for(int k=0; k<N1.cols(); k++) {
int d = N1(i, k);
if(d==-2) continue;
dm1(yi,d) += rho*dn(yi)*n(yi,i)/m1(yi,d);
}
for(int k=0; k<N2.cols(); k++) {
int d = N2(i, k);
if(d==-2) continue;
dm2(yi,d) += rho*dn(yi)*n(yi,i)/m2(yi,d);
}
}
dm2.col(c) *= 0.0;
if( dorec )
for(int yj=nvals-1; yj>=0; yj--) {
w(tree)=w(tree)-1;
m2(yj,c)=mstor(w(tree),tree);
}
} else if( mode==2 ) {
for( int yj=0; yj<nvals; yj++)
n(yj, j) = 1;
for(int k=0; k<N1.cols(); k++) {
int d = N1(j, k);
if(d==-2) continue;
for( int yj=0; yj<nvals; yj++)
n(yj, j) *= m1(yj, d);
}
for(int k=0; k<N2.cols(); k++) {
int d = N2(j, k);
if(d==-2) continue;
for( int yj=0; yj<nvals; yj++)
n(yj, j) *= m2(yj, d);
}
// n.col(j) = n.col(j).array().pow(rho);
if(rho==1) {
//nothing
} else if(rho==.5) {
n.col(j) = n.col(j).array().sqrt();
} else
n.col(j) = n.col(j).array().pow(rho);
for(int yi=0; yi<nvals; yi++) {
m0(yi) = 0;
for(int yj=0; yj<nvals; yj++) {
int index = yi + yj*nvals;
S(yi,yj) = psi_ij(index, c)*psi_i(yj, j)*n(yj, j)/m2(yj, c);
m0(yi) += S(yi,yj);
}
}
double k = S.sum();
MatrixXd dm0 = dm1.col(c)/k;
dm0.array() -= (dm1.col(c).array()*m0.array()).sum()/(k*k);
MatrixXd dn = 0*n.col(i);
for(int yj=0; yj<nvals; yj++) {
for(int yi=0; yi<nvals; yi++) {
int index = yi + yj*nvals;
dpsi_ij(index,c) += dm0(yi)*S(yi,yj)/psi_ij(index,c);
dpsi_i(yj,j) += dm0(yi)*S(yi,yj)/psi_i(yj,j);
dn(yj) += dm0(yi)*S(yi,yj)/n(yj,j);
dm2(yj,c) -= dm0(yi)*S(yi,yj)/m2(yj,c);
}
}
for(int yj=0; yj<nvals; yj++) {
for(int k=0; k<N1.cols(); k++) {
int d = N1(j, k);
if(d==-2) continue;
dm1(yj, d) += rho*dn(yj)*n(yj, j)/m1(yj, d);
}
for(int k=0; k<N2.cols(); k++) {
int d = N2(j, k);
if(d==-2) continue;
dm2(yj, d) += rho*dn(yj)*n(yj, j)/m2(yj, d);
}
}
dm1.col(c) *= 0.0;
if( dorec )
for(int yi=nvals-1; yi>=0; yi--) {
w(tree)=w(tree)-1;
m1(yi,c)=mstor(w(tree),tree);
}
}
}
}
}
}
}
// mcmc_helper(im,im_prev,buf,int(xwho),int(xwho2));
void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]){
// trw_simple_helper(model,c_ij, psi_i, rho, n, m1, m2);
// need from model struct:
//model =
//
// nnodes: 10000
//ncliques: 20000
// nvals: 2
// pairs: [20000x2 double]
// N1: {10000x1 cell}
// N2: {10000x1 cell}
int i=0;
int nnodes = mapdouble(mxGetField(prhs[i],0,"nnodes"))(0);
int ncliques = mapdouble(mxGetField(prhs[i],0,"ncliques"))(0);
int nvals = mapdouble(mxGetField(prhs[i],0,"nvals"))(0);
MatrixXi pairs = mapdouble(mxGetField(prhs[i],0,"pairs")).cast<int>();
pairs.array() -= 1;
MatrixXi N1 = mapdouble(mxGetField(prhs[i],0,"N1")).cast<int>();
N1.array() -= 1;
MatrixXi N2 = mapdouble(mxGetField(prhs[i],0,"N2")).cast<int>();
N2.array() -= 1;
//vector<MatrixXi> N1;
//vector<MatrixXi> N2;
//mxArray *mxN1 = mxGetField(prhs[i],0,"N1");
//mxArray *mxN2 = mxGetField(prhs[i],0,"N2");
//for(int node=0; node<nnodes; node++){
// N1.push_back( mapdouble(mxGetCell(mxN1,node)).cast<int>() );
// N2.push_back( mapdouble(mxGetCell(mxN2,node)).cast<int>() );
// N1[node].cwise() -= 1;
// N2[node].cwise() -= 1;
//}
i++;
MatrixMd psi_ij = mapdouble(prhs[i++]);
MatrixMd psi_i = mapdouble(prhs[i++]);
double rho = mapdouble(prhs[i++])(0);
int maxiter = mapdouble(prhs[i++])(0);
double damp = mapdouble(prhs[i++])(0);
double convthresh = mapdouble(prhs[i++])(0);
MatrixMd n = mapdouble(prhs[i++]);
MatrixMd m1 = mapdouble(prhs[i++]);
MatrixMd m2 = mapdouble(prhs[i++]);
MatrixMd b_i = mapdouble(prhs[i++]);
MatrixMd b_ij = mapdouble(prhs[i++]);
MatrixMd Z_ij = mapdouble(prhs[i++]);
MatrixXd b_i_save = b_i;
MatrixXd S (nvals,nvals);
MatrixXd m0(nvals,1);
int reps;
double conv;
for(reps=0; reps<maxiter; reps++){
for(int c0=0; c0<ncliques*2; c0++){
int c, mode;
if(c0<ncliques){
c = c0;
mode = 1;
} else{
c = ncliques - 1 - (c0-ncliques);
mode = 2;
}
int i = pairs(c,0);
int j = pairs(c,1);
if( mode==1 ){
for(int yi=0; yi<nvals; yi++)
n(yi, i) = 1;
for(int k=0; k<N1.cols(); k++){
int d = N1(i, k);
if(d==-2) continue;
for(int yi=0; yi<nvals; yi++)
n(yi, i) *= m1(yi, d);
}
for(int k=0; k<N2.cols(); k++){
int d = N2(i, k);
if(d==-2) continue;
for(int yi=0; yi<nvals; yi++)
n(yi, i) *= m2(yi, d);
}
for(int yi=0; yi<nvals; yi++)
n(yi, i) = pow(n(yi, i), rho);
// compute m(y_j)
for(int yj=0; yj<nvals; yj++){
m0(yj) = 0;
for(int yi=0; yi<nvals; yi++){
int index = yi + yj*nvals;
S(yi,yj) = psi_ij(index, c)*psi_i(yi, i)*n(yi, i)/m1(yi, c);
m0(yj) += S(yi,yj);
}
}
double k = S.sum();
m2.col(c) = m0/k;
} else{
for( int yj=0; yj<nvals; yj++)
n(yj, j) = 1;
for(int k=0; k<N1.cols(); k++){
int d = N1(j, k);
if(d==-2) continue;
for( int yj=0; yj<nvals; yj++)
n(yj, j) *= m1(yj, d);
}
for(int k=0; k<N2.cols(); k++){
int d = N2(j, k);
if(d==-2) continue;
for( int yj=0; yj<nvals; yj++)
n(yj, j) *= m2(yj, d);
}
for( int yj=0; yj<nvals; yj++)
n(yj, j) = pow(n(yj, j), rho);
for(int yi=0; yi<nvals; yi++){
m0(yi) = 0;
for(int yj=0; yj<nvals; yj++){
int index = yi + yj*nvals;
S(yi,yj) = psi_ij(index, c)*psi_i(yj, j)*n(yj, j)/m2(yj, c);
m0(yi) += S(yi,yj);
}
}
double k = S.sum();
m1.col(c) = m0/k;
}
}
b_i = n.array()*psi_i.array();
norm_cols(b_i);
conv = (b_i-b_i_save).array().abs().maxCoeff();
//cout << "reps: " << reps << " conv: " << conv << endl;
if(conv < convthresh)
break;
b_i_save = b_i;
}
//cout << "reps: " << reps << " conv: " << conv << endl;
b_ij = psi_ij;
for(int c=0; c<ncliques; c++){
int i = pairs(c,0);
int j = pairs(c,1);
for(int yi=0; yi<nvals; yi++){
for(int yj=0; yj<nvals; yj++){
int index = yi + yj*nvals;
b_ij(index,c) = b_ij(index,c)*psi_i(yi,i)*psi_i(yj,j)*n(yi,i)*n(yj,j)/m1(yi,c)/m2(yj,c);
}
}
}
Z_ij = b_ij.colwise().sum();
for(int c=0; c<ncliques; c++)
b_ij.col(c) = b_ij.col(c)/Z_ij(c);
plhs[0] = mxCreateDoubleMatrix(1, 1, mxREAL);
double *outArray;
outArray = mxGetPr(plhs[0]);
outArray[0] = reps;
}
