/* Multiply columns of m by v */
void mat_mul_cols(Matrix m, double *v)
{
int xi,yi;
double *p=m.dat;
for (yi=0; yi<m.ny; yi++,p+=m.nx)
vec_mul_scalar(p,m.nx,v[yi]);
}
void next_position(int *steps)
{
double newp[MAX_DIM];
double err[MAX_DIM];
int i;
tim += 1.0;
vec_mul_scalar(newp, incvec, tim);
vec_sum(newp, newp, origin);
vec_diff(err, newp, position);
for (i = 0; i < MAX_DIM; i++) {
steps[i] = (int)err[i];
position[i] += (double)steps[i];
}
}
void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
{
double M;
double log_thr;
int Xs,D,Ys,i,j,ep=0;
int maxentep;
int pxy_size;
double mx;
Matrix phi,psi,phi_tr,psi_tr,phi_exp_d,psi_exp_d,psi_exp_m,phi_exp_m,grad_phi,grad_psi;
double *px0,*py0,*px,*py,*pa,*pb,*grad_a,*grad_b;
double Z,loglik;
int xi,yi,di;
int b_cond; /* Says if the model is conditioned on x, or joint */
double *p0_i,*p0_j,*p0_val;
int Np0;
MatFromMLab(prhs[0],&phi); /* Size NX,D */
MatFromMLab(prhs[1],&psi); /* Size NX,D */
MatFromMLab(prhs[2],&phi_tr); /* Size D,NX */
MatFromMLab(prhs[3],&psi_tr); /* Size D,NY */
px0 = mxGetPr(prhs[4]);
py0 = mxGetPr(prhs[5]);
MatFromMLab(prhs[6],&phi_exp_d);
MatFromMLab(prhs[7],&psi_exp_d);
pa = mxGetPr(prhs[8]);
pb = mxGetPr(prhs[9]);
b_cond = *mxGetPr(prhs[10]);
p0_i = mxGetPr(prhs[11]);
p0_j = mxGetPr(prhs[12]);
p0_val = mxGetPr(prhs[13]);
Xs = mxGetM(prhs[0]);
D = mxGetN(prhs[0]);
Ys = mxGetM(prhs[1]);
Np0 = mxGetNumberOfElements(prhs[13]);
pxy_size = Xs*Ys;
plhs[0] = mxCreateDoubleMatrix(D,Xs,mxREAL);
plhs[1] = mxCreateDoubleMatrix(D,Ys,mxREAL);
plhs[2] = mxCreateDoubleMatrix(1,Xs,mxREAL);
plhs[3] = mxCreateDoubleMatrix(1,Ys,mxREAL);
plhs[4] = mxCreateDoubleMatrix(1,1,mxREAL);
grad_phi.dat = mxGetPr(plhs[0]);
grad_psi.dat = mxGetPr(plhs[1]);
grad_phi.nx = D;
grad_phi.ny = Xs;
grad_phi.nall = Xs*D;
grad_psi.nx = D;
grad_psi.ny = Ys;
grad_psi.nall = Ys*D;
grad_a = mxGetPr(plhs[2]);
grad_b = mxGetPr(plhs[3]);
AllocMat(D,Ys,&phi_exp_m);
AllocMat(D,Xs,&psi_exp_m);
px = malloc(Xs*sizeof(double));
py = malloc(Ys*sizeof(double));
if (px==NULL || py==NULL)
printf("Error in allocation\n");
if (!b_cond)
mat_logemb_model(phi_tr, psi_tr, pa, pb, phi_exp_m, psi_exp_m, px, py, p0_i, p0_j, p0_val, Np0, mxGetPr(plhs[4]),b_cond) ;
else
{
/* Run logemb model one time to get the x partition function in px */
mat_logemb_model(phi_tr, psi_tr, pa, pb, phi_exp_m, psi_exp_m, px, py, p0_i, p0_j, p0_val, Np0, mxGetPr(plhs[4]),b_cond) ;
/* Create new x factor */
for (xi=0; xi<Xs; xi++)
grad_a[xi] = pa[xi]-log(px[xi])+log(px0[xi]);
mat_logemb_model(phi_tr, psi_tr, grad_a, pb, phi_exp_m, psi_exp_m, px, py, p0_i, p0_j, p0_val, Np0, mxGetPr(plhs[4]),0) ;
}
/* Matlab line: diag(px)*phi-pxy*psi - (diag(px0)*phi-pxy0*psi);
/* equivalent to diag(px-px0)*phi+psi_expect_d - psi_expect_m */
vec_subtract(px,px0,Xs);
mat_set(grad_phi,phi_tr);
/* printf("\n Phi1 Max=%g MaxPx=%g\n",mat_max_abs(grad_phi.dat,grad_phi.nall),mat_max_abs(px,Xs)); */
mat_mul_cols(grad_phi,px);
/* printf("\n Phi2 Max=%g MaxPx=%g\n",mat_max_abs(grad_phi.dat,grad_phi.nall),mat_max_abs(px,Xs)); */
mat_subtract(psi_exp_d.dat,psi_exp_m.dat,psi_exp_d.nall);
/* printf("\n Phi3 Max=%g\n",mat_max_abs(grad_phi.dat,grad_phi.nall)); */
mat_add(grad_phi.dat,psi_exp_d.dat,psi_exp_d.nall);
/* printf("\n Phi4 Max=%g\n",mat_max_abs(grad_phi.dat,grad_phi.nall)); */
vec_subtract(py,py0,Ys);
mat_set(grad_psi,psi_tr);
mat_mul_cols(grad_psi,py);
mat_subtract(phi_exp_d.dat,phi_exp_m.dat,phi_exp_d.nall);
mat_add(grad_psi.dat,phi_exp_d.dat,phi_exp_d.nall);
/* Generate gradient for a and b. Remember px already has px subtracted. Just need to flip sign */
memcpy(grad_a,px,Xs*sizeof(double));
vec_mul_scalar(grad_a,-1,Xs);
memcpy(grad_b,py,Ys*sizeof(double));
vec_mul_scalar(grad_b,-1,Ys);
free(psi_exp_m.dat);
free(phi_exp_m.dat);
free(px);
free(py);
}
void mat_logemb_model(Matrix phi, Matrix psi, double *a, double *b, Matrix phi_expect, Matrix psi_expect, double *px, double *py
,double *p0_i, double *p0_j, double *p0_val, int n_p0, double *loglik, int b_cond)
{
int i1,i2,k,di;
double *p1,*p2,*res_ptr;
double *p_phiexp,*p_psiexp;
double tmp,df;
double Z=0;
int D = phi_expect.nx;
double *ap,*bp,*p_phie,*p_psie;
int NX = phi.ny;
int NY = psi.ny;
int p0ind = 0;
double *aend;
(*loglik) = 0;
p2 = psi.dat;
p_phiexp = phi_expect.dat;
p_psiexp = psi_expect.dat;
/* Reset everything */
memset(phi_expect.dat,0,phi_expect.nall*sizeof(double));
memset(psi_expect.dat,0,psi_expect.nall*sizeof(double));
memset(px,0,NX*sizeof(double));
memset(py,0,NY*sizeof(double));
for (i2=0; i2< psi.ny; i2++) {
p1 = phi.dat;
p_psiexp = psi_expect.dat;
for (i1=0; i1< phi.ny; i1++) {
/* Inlining of l2_dist */
tmp=0;
{
aend=&p1[phi.nx];
double d=0;
for (ap=&p1[0],bp=&p2[0]; ap<aend; ap++, bp++) {
d = *ap-*bp;
tmp+=(d*d);
}
}
tmp = -tmp+a[i1]+b[i2];
if ((i1==p0_i[p0ind]-1) && (i2==p0_j[p0ind]-1))
{
(*loglik)+= p0_val[p0ind]*tmp;
p0ind++;
}
tmp = exp(tmp);
if (!b_cond)
{
/* This can be speeded up by avoiding called to GetMatInd */
aend=&p1[D];
for (di=0,ap=&p1[0],bp=&p2[0],p_phie=&p_phiexp[0],p_psie=&p_psiexp[0]; di<D; di++,ap++,bp++,p_phie++,p_psie++)
{
/*
phi_expect.dat[GetMatInd(phi_expect,di,i2)]+=tmp*(*ap);
psi_expect.dat[GetMatInd(psi_expect,i1,di)]+=tmp*(*bp);
*/
/*
phi_expect.dat[GetMatInd(phi_expect,di,i2)]+=tmp*phi.dat[GetMatInd(phi,di,i1)];
psi_expect.dat[GetMatInd(psi_expect,i1,di)]+=tmp*psi.dat[GetMatInd(psi,di,i2)];
*/
(*p_phie)+=tmp*(*ap);
(*p_psie)+=tmp*(*bp);
}
}
px[i1]+=tmp;
py[i2]+=tmp;
Z+= tmp;
p1+= phi.nx;
p_psiexp += D;
}
/* Advance to next column */
p2 += phi.nx;
p_phiexp += D;
}
if (!b_cond)
{
vec_mul_scalar(px,NX,1/Z);
vec_mul_scalar(py,NY,1/Z);
mat_el_mul_scalar(phi_expect.dat, 1/Z, phi_expect.nall);
mat_el_mul_scalar(psi_expect.dat, 1/Z, psi_expect.nall);
(*loglik)-= log(Z);
}
}
void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
{
double M;
double log_thr;
int Xs,D,Ys,i,j,ep=0;
int maxentep;
int pxy_size;
double mx;
Matrix pxy,phi,psi,phi_tr,psi_tr,phi_exp_d,psi_exp_d,psi_exp_m,phi_exp_m,grad_phi,grad_psi;
double *px0,*py0,*px,*py,*pa,*pb,*grad_a,*grad_b;
double Z;
int xi,yi,di;
MatFromMLab(prhs[0],&phi); /* Size NX,D */
MatFromMLab(prhs[1],&psi); /* Size NX,D */
MatFromMLab(prhs[2],&phi_tr); /* Size D,NX */
MatFromMLab(prhs[3],&psi_tr); /* Size D,NY */
px0 = mxGetPr(prhs[4]);
py0 = mxGetPr(prhs[5]);
MatFromMLab(prhs[6],&phi_exp_d);
MatFromMLab(prhs[7],&psi_exp_d);
pa = mxGetPr(prhs[8]);
pb = mxGetPr(prhs[9]);
Xs = mxGetM(prhs[0]);
D = mxGetN(prhs[0]);
Ys = mxGetM(prhs[1]);
pxy_size = Xs*Ys;
plhs[0] = mxCreateDoubleMatrix(Xs,D,mxREAL);
plhs[1] = mxCreateDoubleMatrix(D,Ys,mxREAL);
plhs[2] = mxCreateDoubleMatrix(Xs,Ys,mxREAL);
plhs[3] = mxCreateDoubleMatrix(1,Xs,mxREAL);
plhs[4] = mxCreateDoubleMatrix(1,Ys,mxREAL);
grad_phi.dat = mxGetPr(plhs[0]);
grad_psi.dat = mxGetPr(plhs[1]);
grad_phi.nx = Xs;
grad_phi.ny = D;
grad_phi.nall = Xs*D;
grad_psi.nx = D;
grad_psi.ny = Ys;
grad_psi.nall = Ys*D;
/* Will hold the models distribution */
pxy.dat = mxGetPr(plhs[2]);
pxy.nx = Xs;
pxy.ny = Ys;
pxy.nall = Xs*Ys;
grad_a = mxGetPr(plhs[3]);
grad_b = mxGetPr(plhs[4]);
AllocMat(D,Ys,&phi_exp_m);
AllocMat(Xs,D,&psi_exp_m);
px = malloc(Xs*sizeof(double));
py = malloc(Ys*sizeof(double));
mat_logemb_model(phi_tr,psi_tr,pa,pb,pxy);
Z = mat_el_exp_sum(pxy.dat, pxy_size);
mat_el_mul_scalar(pxy.dat, 1/Z, pxy_size);
mat_sum_rows(pxy,px);
mat_sum_cols(pxy,py);
mat_mul_a_tr_b(phi,pxy,phi_exp_m);
mat_mul(pxy,psi,psi_exp_m);
/* Matlab line: diag(px)*phi-pxy*psi - (diag(px0)*phi-pxy0*psi);
/* equivalent to diag(px-px0)*phi+psi_expect_d - psi_expect_m */
vec_subtract(px,px0,Xs);
mat_set(grad_phi,phi);
mat_mul_rows(grad_phi,px);
mat_subtract(psi_exp_d.dat,psi_exp_m.dat,psi_exp_d.nall);
mat_add(grad_phi.dat,psi_exp_d.dat,psi_exp_d.nall);
vec_subtract(py,py0,Ys);
mat_set(grad_psi,psi_tr);
mat_mul_cols(grad_psi,py);
mat_subtract(phi_exp_d.dat,phi_exp_m.dat,phi_exp_d.nall);
mat_add(grad_psi.dat,phi_exp_d.dat,phi_exp_d.nall);
mat_el_log(pxy.dat,pxy.nall);
/* Generate gradient for a and b. Remember px already has px subtracted. Just need to flip sign */
memcpy(grad_a,px,Xs*sizeof(double));
vec_mul_scalar(grad_a,-1,Xs);
memcpy(grad_b,py,Ys*sizeof(double));
vec_mul_scalar(grad_b,-1,Ys);
free(psi_exp_m.dat);
free(phi_exp_m.dat);
free(px);
free(py);
}
