t_max_err poki_recLevel_set(t_poki *x, void *attr, long ac, t_atom *av)
{
if (ac && av)
{
const double target = atom_getfloat(av);
if (absDif(target, x->p_recLevelTarget) > 0.0001)
{
x->p_recLevelTarget = target;
if (absDif(target, x->p_recLevel) > 0.0001)
{
x->p_recFadeInc = (x->p_recLevelTarget - x->p_recLevel) / x->p_fadeSamps;
x->p_recFadeFlag = 1;
}
else
{
x->p_recFadeFlag = 0;
}
}
else
{
x->p_recLevelTarget = target;
x->p_recLevel = target;
x->p_recFadeInc = 0.0;
x->p_recFadeFlag = 0;
}
}
return MAX_ERR_NONE;
}
void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]) {
/* Variables */
int n, s, e, e2, n1, n2, neigh, Vind, Vind2, s1, s2,
nNodes, nEdges, maxState,
iter, maxIter,
*edgeEnds, *nStates, *V, *E, *y;
double *nodePot, *edgePot, *nodeBel,
z, *prodMsgs, *oldMsgs, *newMsgs, *tmp, *tmp1, *tmp2, *mu,nNbrs;
/* Input */
nodePot = mxGetPr(prhs[0]);
edgePot = mxGetPr(prhs[1]);
edgeEnds = (int*)mxGetPr(prhs[2]);
nStates = (int*)mxGetPr(prhs[3]);
V = (int*)mxGetPr(prhs[4]);
E = (int*)mxGetPr(prhs[5]);
maxIter = ((int*)mxGetPr(prhs[6]))[0];
mu = mxGetPr(prhs[7]);
if (!mxIsClass(prhs[2],"int32")||!mxIsClass(prhs[3],"int32")||!mxIsClass(prhs[4],"int32")||!mxIsClass(prhs[5],"int32")||!mxIsClass(prhs[6],"int32"))
mexErrMsgTxt("edgeEnds, nStates, V, E, maxIter must be int32");
/* Compute Sizes */
nNodes = mxGetDimensions(prhs[0])[0];
maxState = mxGetDimensions(prhs[0])[1];
nEdges = mxGetDimensions(prhs[2])[0];
/* Output */
plhs[0] = mxCreateDoubleMatrix(nNodes,maxState,mxREAL);
nodeBel = mxGetPr(plhs[0]);
prodMsgs = mxCalloc(maxState*nNodes, sizeof(double));
oldMsgs = mxCalloc(maxState*nEdges*2, sizeof(double));
newMsgs = mxCalloc(maxState*nEdges*2, sizeof(double));
tmp = mxCalloc(maxState, sizeof(double));
tmp1 = mxCalloc(maxState, sizeof(double));
tmp2 = mxCalloc(maxState, sizeof(double));
/* Initialize */
for(e = 0; e < nEdges; e++) {
n1 = edgeEnds[e]-1;
n2 = edgeEnds[e+nEdges]-1;
for(s = 0; s < nStates[n2]; s++)
newMsgs[s+maxState*e] = 1./nStates[n2];
for(s = 0; s < nStates[n1]; s++)
newMsgs[s+maxState*(e+nEdges)] = 1./nStates[n1];
}
for(iter = 0; iter < maxIter; iter++) {
for(n=0;n<nNodes;n++) {
/* Update Messages */
for(Vind = V[n]-1; Vind < V[n+1]-1; Vind++) {
e = E[Vind]-1;
n1 = edgeEnds[e]-1;
n2 = edgeEnds[e+nEdges]-1;
/* First part of message is nodePot*/
for(s = 0; s < nStates[n]; s++)
tmp[s] = nodePot[n + nNodes*s];
/* Multiply by messages from neighbors except j */
for(Vind2 = V[n]-1; Vind2 < V[n+1]-1; Vind2++) {
e2 = E[Vind2]-1;
if (e != e2) {
if (n == edgeEnds[e2+nEdges]-1) {
for(s = 0; s < nStates[n]; s++) {
tmp[s] *= pow(newMsgs[s+maxState*e2], mu[e2]);
}
}
else {
for(s = 0; s < nStates[n]; s++) {
tmp[s] *= pow(newMsgs[s+maxState*(e2+nEdges)], mu[e2]);
}
}
}
else {
if (n == edgeEnds[e2+nEdges]-1) {
for(s = 0; s < nStates[n]; s++) {
tmp[s] /= pow(newMsgs[s+maxState*e2], 1.-mu[e2]);
}
}
else {
for(s = 0; s < nStates[n]; s++) {
tmp[s] /= pow(newMsgs[s+maxState*(e2+nEdges)], 1.-mu[e2]);
}
}
}
}
/* Now multiply by edge potential to get new message */
if (n == n2) {
for(s1 = 0; s1 < nStates[n1]; s1++) {
newMsgs[s1+maxState*(e+nEdges)] = tmp[0]*pow(edgePot[s1+maxState*(0+maxState*e)], 1./mu[e]);
for(s2 = 1; s2 < nStates[n2]; s2++) {
if(tmp[s2]*pow(edgePot[s1+maxState*(s2+maxState*e)], 1./mu[e]) > newMsgs[s1+maxState*(e+nEdges)])
newMsgs[s1+maxState*(e+nEdges)] = tmp[s2]*pow(edgePot[s1+maxState*(s2+maxState*e)], 1./mu[e]);
}
/* Might get NaN or Inf values due to 0*log(0) */
newMsgs[s1+maxState*(e+nEdges)] = zeroIfNaN(newMsgs[s1+maxState*(e+nEdges)]);
}
/* Safe normalize */
z = 0.0;
for(s = 0; s < nStates[n1]; s++)
z += newMsgs[s+maxState*(e+nEdges)];
if (z > 0.0) {
for(s = 0; s < nStates[n1]; s++)
newMsgs[s+maxState*(e+nEdges)] /= z;
}
}
else {
for(s2 = 0; s2 < nStates[n2]; s2++) {
newMsgs[s2+maxState*e] = tmp[0]*pow(edgePot[0+maxState*(s2+maxState*e)], 1./mu[e]);
for(s1 = 1; s1 < nStates[n1]; s1++) {
if(tmp[s1]*pow(edgePot[s1+maxState*(s2+maxState*e)], 1./mu[e]) > newMsgs[s2+maxState*e])
newMsgs[s2+maxState*e] = tmp[s1]*pow(edgePot[s1+maxState*(s2+maxState*e)], 1./mu[e]);
}
/* Might get NaN or Inf values due to 0*log(0) */
newMsgs[s2+maxState*e] = zeroIfNaN(newMsgs[s2+maxState*e]);
}
/* Safe normalize */
z = 0.0;
for(s = 0; s < nStates[n2]; s++)
z += newMsgs[s+maxState*e];
if (z > 0.0) {
for(s = 0; s < nStates[n2]; s++)
newMsgs[s+maxState*e] /= z;
}
}
}
}
/* Print out messages */
/*
* printf("\n\nIter = %d\n", iter);
* for(s=0;s<maxState;s++) {
* for(e=0;e<nEdges*2;e++) {
* printf("newMsgs(%d,%d) = %f\n", s, e, newMsgs[s+maxState*e]);
* }
* }
*/
/* oldMsgs = newMsgs */
z = 0;
for(s=0;s<maxState;s++) {
for(e=0;e<nEdges*2;e++) {
z += absDif(newMsgs[s+maxState*e], oldMsgs[s+maxState*e]);
oldMsgs[s+maxState*e] = newMsgs[s+maxState*e];
}
}
/* if sum(abs(newMsgs(:)-oldMsgs(:))) < 1e-4; break; */
if(z < 1e-4) {
break;
}
}
/* ******************* DONE MESSAGE PASSING ********************** */
/*if(iter == maxIter)
* {
* printf("LBP reached maxIter of %d iterations\n",maxIter);
* }
* printf("Stopped after %d iterations\n",iter); */
/* compute nodeBel */
for(n = 0; n < nNodes; n++) {
/* Init to nodePot */
for(s = 0; s < nStates[n]; s++)
prodMsgs[s+maxState*n] = nodePot[n+nNodes*s];
/* Multiply by product of messages */
for(Vind = V[n]-1; Vind < V[n+1]-1; Vind++) {
e = E[Vind]-1;
n1 = edgeEnds[e]-1;
n2 = edgeEnds[e+nEdges]-1;
if (n == n2) {
for(s = 0; s < nStates[n]; s++) {
prodMsgs[s+maxState*n] *= pow(newMsgs[s+maxState*e], mu[e]);
}
}
else {
for(s = 0; s < nStates[n]; s++) {
prodMsgs[s+maxState*n] *= pow(newMsgs[s+maxState*(e+nEdges)], mu[e]);
}
}
}
/* Normalize */
z = 0.0;
for(s = 0; s < nStates[n]; s++)
z += prodMsgs[s+maxState*n];
for(s = 0; s < nStates[n]; s++) {
/* Note: unsafe normalize! */
nodeBel[n+nNodes*s] = prodMsgs[s+maxState*n] / z;
/* Clamp to [0,1] (just in case) */
if (nodeBel[n+nNodes*s] < 0.0)
nodeBel[n+nNodes*s] = 0.0;
else if (nodeBel[n+nNodes*s] > 1.0)
nodeBel[n+nNodes*s] = 1.0;
}
}
/* Free memory */
mxFree(prodMsgs);
mxFree(oldMsgs);
mxFree(newMsgs);
mxFree(tmp);
mxFree(tmp1);
mxFree(tmp2);
}
void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
{
/* Variables */
int n, s,e,e2,n1,n2,neigh,Vind,Vind2,s1,s2,
nNodes, nEdges, maxState,
iter,maxIter,nNbrs,
*edgeEnds, *nStates, *V, *E,*y;
double *nodePot, *edgePot, *nodeBel,
z,*prodMsgs,*oldMsgs,*newMsgs,*tmp;
/* Input */
nodePot = mxGetPr(prhs[0]);
edgePot = mxGetPr(prhs[1]);
edgeEnds = (int*)mxGetPr(prhs[2]);
nStates = (int*)mxGetPr(prhs[3]);
V = (int*)mxGetPr(prhs[4]);
E = (int*)mxGetPr(prhs[5]);
maxIter = ((int*)mxGetPr(prhs[6]))[0];
if (!mxIsClass(prhs[2],"int32")||!mxIsClass(prhs[3],"int32")||!mxIsClass(prhs[4],"int32")||!mxIsClass(prhs[5],"int32")||!mxIsClass(prhs[6],"int32"))
mexErrMsgTxt("edgeEnds, nStates, V, E, maxIter must be int32");
/* Compute Sizes */
nNodes = mxGetDimensions(prhs[0])[0];
maxState = mxGetDimensions(prhs[0])[1];
nEdges = mxGetDimensions(prhs[2])[0];
/* Output */
plhs[0] = mxCreateDoubleMatrix(nNodes,maxState,mxREAL);
nodeBel = mxGetPr(plhs[0]);
prodMsgs = mxCalloc(maxState*nNodes,sizeof(double));
oldMsgs = mxCalloc(maxState*nEdges*2,sizeof(double));
newMsgs = mxCalloc(maxState*nEdges*2,sizeof(double));
tmp = mxCalloc(maxState,sizeof(double));
/* Initialize */
for(e = 0; e < nEdges; e++)
{
n1 = edgeEnds[e]-1;
n2 = edgeEnds[e+nEdges]-1;
for(s = 0; s < nStates[n2]; s++)
newMsgs[s+maxState*e] = 1./nStates[n2];
for(s = 0; s < nStates[n1]; s++)
newMsgs[s+maxState*(e+nEdges)] = 1./nStates[n1];
}
for(iter = 0; iter < maxIter; iter++)
{
for(n=0;n<nNodes;n++)
{
/* Update Messages */
for(Vind = V[n]-1; Vind < V[n+1]-1; Vind++)
{
e = E[Vind]-1;
n1 = edgeEnds[e]-1;
n2 = edgeEnds[e+nEdges]-1;
/* First part of message is nodePot*/
for(s = 0; s < nStates[n]; s++)
tmp[s] = nodePot[n + nNodes*s];
/* Multiply by messages from neighbors except j */
for(Vind2 = V[n]-1; Vind2 < V[n+1]-1; Vind2++)
{
e2 = E[Vind2]-1;
if (e != e2)
{
if (n == edgeEnds[e2+nEdges]-1)
{
for(s = 0; s < nStates[n]; s++)
{
tmp[s] *= newMsgs[s+maxState*e2];
}
}
else
{
for(s = 0; s < nStates[n]; s++)
{
tmp[s] *= newMsgs[s+maxState*(e2+nEdges)];
}
}
}
}
/* Now multiply by edge potential to get new message */
if (n == n2)
{
for(s1 = 0; s1 < nStates[n1]; s1++)
{
newMsgs[s1+maxState*(e+nEdges)] = tmp[0]*edgePot[s1+maxState*(0+maxState*e)];
for(s2 = 1; s2 < nStates[n2]; s2++)
{
if(tmp[s2]*edgePot[s1+maxState*(s2+maxState*e)] > newMsgs[s1+maxState*(e+nEdges)])
newMsgs[s1+maxState*(e+nEdges)] = tmp[s2]*edgePot[s1+maxState*(s2+maxState*e)];
}
}
z = 0.0;
for(s = 0; s < nStates[n1]; s++)
z += newMsgs[s+maxState*(e+nEdges)];
for(s = 0; s < nStates[n1]; s++)
newMsgs[s+maxState*(e+nEdges)] /= z;
}
else
{
for(s2 = 0; s2 < nStates[n2]; s2++)
{
newMsgs[s2+maxState*e] = tmp[0]*edgePot[0+maxState*(s2+maxState*e)];
for(s1 = 1; s1 < nStates[n1]; s1++)
{
if(tmp[s1]*edgePot[s1+maxState*(s2+maxState*e)] > newMsgs[s2+maxState*e])
newMsgs[s2+maxState*e] = tmp[s1]*edgePot[s1+maxState*(s2+maxState*e)];
}
}
z = 0.0;
for(s = 0; s < nStates[n2]; s++)
z += newMsgs[s+maxState*e];
for(s = 0; s < nStates[n2]; s++)
newMsgs[s+maxState*e] /= z;
}
}
}
/* oldMsgs = newMsgs */
z = 0;
for(s=0;s<maxState;s++)
{
for(e=0;e<nEdges*2;e++)
{
z += absDif(newMsgs[s+maxState*e],oldMsgs[s+maxState*e]);
oldMsgs[s+maxState*e] = newMsgs[s+maxState*e];
}
}
/* if sum(abs(newMsgs(:)-oldMsgs(:))) < 1e-4; break; */
/*printf("z = %f\n",z);*/
if(z < 1e-4)
{
break;
}
}
/*if(iter == maxIter)
{
printf("LBP reached maxIter of %d iterations\n",maxIter);
}
printf("Stopped after %d iterations\n",iter); */
/* compute nodeBel */
for(n = 0; n < nNodes; n++)
{
for(s = 0; s < nStates[n]; s++)
prodMsgs[s+maxState*n] = nodePot[n+nNodes*s];
for(Vind = V[n]-1; Vind < V[n+1]-1; Vind++)
{
e = E[Vind]-1;
n1 = edgeEnds[e]-1;
n2 = edgeEnds[e+nEdges]-1;
if (n == n2)
{
for(s = 0; s < nStates[n]; s++)
{
prodMsgs[s+maxState*n] *= newMsgs[s+maxState*e];
}
}
else
{
for(s = 0; s < nStates[n]; s++)
{
prodMsgs[s+maxState*n] *= newMsgs[s+maxState*(e+nEdges)];
}
}
}
z = 0;
for(s = 0; s < nStates[n]; s++)
{
nodeBel[n + nNodes*s] = prodMsgs[s+maxState*n];
z = z + nodeBel[n+nNodes*s];
}
for(s = 0; s < nStates[n]; s++)
nodeBel[n + nNodes*s] /= z;
}
/* Free memory */
mxFree(prodMsgs);
mxFree(oldMsgs);
mxFree(newMsgs);
mxFree(tmp);
}
t_int *poki_perform(t_int *w)
{
t_float * in = (t_float *)(w[1]);
t_float * out = (t_float *)(w[2]);
t_poki * x = (t_poki *)(w[3]);
t_float * index = x->p_connected ? (t_float *)(w[4]) : NULL;
int n = (int)(w[5]);
if (index == NULL)
goto out;
t_buffer *b = x->p_buf;
float *tab;
long frames,nc;
if (x->p_obj.z_disabled)
goto out;
if (!b)
goto out;
// buffer data structure exists
ATOMIC_INCREMENT(&b->b_inuse);
if (!b->b_valid) {
ATOMIC_DECREMENT(&b->b_inuse);
goto out;
}
tab = b->b_samples;
frames = b->b_frames;
nc = b->b_nchans;
if (nc != 1)
{
ATOMIC_DECREMENT(&b->b_inuse);
goto zero;
}
else
{
while (n--)
{
const long idx = (long)(*index + 0.5f) % frames;
const int step = (idx - x->p_idx0) % frames;
int interpCount = step-1;
float input = *in;
if (x->p_preFadeFlag)
{
x->p_preLevel += x->p_preFadeInc;
if (absDif(x->p_preLevel, x->p_preLevelTarget) < 0.001)
{
x->p_preLevel = x->p_preLevelTarget;
x->p_preFadeFlag = 0;
}
}
if (x->p_recFadeFlag)
{
x->p_recLevel += x->p_recFadeInc;
if (absDif(x->p_recLevel, x->p_recLevelTarget) < 0.001)
{
x->p_recLevel = x->p_recLevelTarget;
x->p_recFadeFlag = 0;
}
}
// these macros are weirdly undefined in deployment builds for some target architectures
//if (FIX_DENORM_DOUBLE(x->p_recLevel) > 0.00001)
if (absDif(x->p_recLevel, 0.0) > 1e-6) // -120dB
{ // recording level is non-zero
input *= x->p_recLevel;
//if (FIX_DENORM_DOUBLE(x->p_preLevel) > 0.0)
if (absDif(x->p_preLevel, 0.0) > 1e-6)
{
if (absDif(x->p_preLevel, 1.0) < 0.001)
{
input += tab[idx];
}
else
{ // pre-level is non-unity
input += (tab[idx] * x->p_preLevel);
}
}
}
else
{
/* // FIXME: this behavior is potentially useful, should be an option
// no recording, use overdub level only
input = 0.0;
if (absDif(x->p_preLevel, 1.0) < 0.001)
{ // pre level is unity
input = tab[idx]; // TODO: should just skip this sample
}
else
{ // pre level is non-unity
input = tab[idx] * FIX_DENORM_DOUBLE(x->p_preLevel);
}
*/
// with no recording, we don't change the buffer
input = tab[idx];
interpCount = x->p_interpThresholdSamps + 1; // this should cause the interp steps to be skipped
}
// perform interpolation
if (interpCount <= x->p_interpThresholdSamps)
{
// FIXME: get higher-order interpolation working and add option to switch.
// usually there is not really an audible improvement.
// but i can imaginge someone wanting it for some extreme purpose. -emb
/*
const float y3 = x->p_y0;
const float y2 = tab[(x->p_idx0 - step) % frames];
const float y1 = tab[(idx - (step*2)) % frames];
const float y0 = tab[(idx - (step*3)) % frames];
*/
const float phaseInc = 1.f / ((float)step);
float phase=phaseInc;
int interpIdx = (x->p_idx0 + 1) % frames;
while (interpCount > 0)
{
// 3rd-order:
// tab[interpIdx] = hermite_interp(phase, y0, y1, y2, y3);
// linear:
tab[interpIdx] = x->p_y0 + (phase * (input - x->p_y0));
phase += phaseInc;
interpIdx = ((interpIdx + 1) % frames);
interpCount--;
}
} // interp count < thresh
// no interpolation
//*out = tab[idx];
// location and float offset
const long iIdx = (long)(*index);
const float fIdx = *index - (long)(*index);
// 3rd-order:
// *out = hermite_interp(fIdx,
// tab[(iIdx+1) % frames],
// tab[(iIdx+2) % frames],
// tab[(iIdx+3) % frames],
// tab[(iIdx+4) % frames]
// );
// linear:
const float tab0 = tab[(iIdx+1) % frames];
*out = tab0 + fIdx * (tab[(iIdx + 2) % frames] - tab0);
tab[idx] = input;
x->p_y0 = input;
x->p_idx0 = idx;
out++;
in++;
index++;
} // sample loop
object_method(b, gensym("dirty"));
ATOMIC_DECREMENT(&b->b_inuse);
} // test for mono
return w + 6;
zero:
while(n--) *out++ = 0.0;
out:
return w + 6;
}
void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]) {
/* Variables */
int n, s, e, e2, n1, n2, neigh, Vind, Vind2, s1, s2,
nNodes, nEdges, maxState, dims[3],
iter, maxIter,
*edgeEnds, *nStates, *V, *E, *y;
double *nodePot, *edgePot, *nodeBel, *edgeBel, *logZ,
z, energy1, energy2, entropy1, entropy2, *prodMsgs, *oldMsgs, *newMsgs, *tmp, *tmp1, *tmp2, *mu,nNbrs;
/* Input */
nodePot = mxGetPr(prhs[0]);
edgePot = mxGetPr(prhs[1]);
edgeEnds = (int*)mxGetPr(prhs[2]);
nStates = (int*)mxGetPr(prhs[3]);
V = (int*)mxGetPr(prhs[4]);
E = (int*)mxGetPr(prhs[5]);
maxIter = ((int*)mxGetPr(prhs[6]))[0];
mu = mxGetPr(prhs[7]);
if (!mxIsClass(prhs[2],"int32")||!mxIsClass(prhs[3],"int32")||!mxIsClass(prhs[4],"int32")||!mxIsClass(prhs[5],"int32")||!mxIsClass(prhs[6],"int32"))
mexErrMsgTxt("edgeEnds, nStates, V, E, maxIter must be int32");
/* Compute Sizes */
nNodes = mxGetDimensions(prhs[0])[0];
maxState = mxGetDimensions(prhs[0])[1];
nEdges = mxGetDimensions(prhs[2])[0];
/* Output */
plhs[0] = mxCreateDoubleMatrix(nNodes,maxState,mxREAL);
dims[0] = maxState;
dims[1] = maxState;
dims[2] = nEdges;
plhs[1] = mxCreateNumericArray(3,dims,mxDOUBLE_CLASS,mxREAL);
plhs[2] = mxCreateDoubleMatrix(1,1,mxREAL);
nodeBel = mxGetPr(plhs[0]);
edgeBel = mxGetPr(plhs[1]);
logZ = mxGetPr(plhs[2]);
prodMsgs = mxCalloc(maxState*nNodes, sizeof(double));
oldMsgs = mxCalloc(maxState*nEdges*2, sizeof(double));
newMsgs = mxCalloc(maxState*nEdges*2, sizeof(double));
tmp = mxCalloc(maxState, sizeof(double));
tmp1 = mxCalloc(maxState, sizeof(double));
tmp2 = mxCalloc(maxState, sizeof(double));
/* Initialize */
for(e = 0; e < nEdges; e++) {
n1 = edgeEnds[e]-1;
n2 = edgeEnds[e+nEdges]-1;
for(s = 0; s < nStates[n2]; s++)
newMsgs[s+maxState*e] = 1./nStates[n2];
for(s = 0; s < nStates[n1]; s++)
newMsgs[s+maxState*(e+nEdges)] = 1./nStates[n1];
}
for(iter = 0; iter < maxIter; iter++) {
for(n=0;n<nNodes;n++) {
/* Update Messages */
for(Vind = V[n]-1; Vind < V[n+1]-1; Vind++) {
e = E[Vind]-1;
n1 = edgeEnds[e]-1;
n2 = edgeEnds[e+nEdges]-1;
/* First part of message is nodePot*/
for(s = 0; s < nStates[n]; s++)
tmp[s] = nodePot[n + nNodes*s];
/* Multiply by messages from neighbors except j */
for(Vind2 = V[n]-1; Vind2 < V[n+1]-1; Vind2++) {
e2 = E[Vind2]-1;
if (e != e2) {
if (n == edgeEnds[e2+nEdges]-1) {
for(s = 0; s < nStates[n]; s++) {
tmp[s] *= pow(newMsgs[s+maxState*e2], mu[e2]);
}
}
else {
for(s = 0; s < nStates[n]; s++) {
tmp[s] *= pow(newMsgs[s+maxState*(e2+nEdges)], mu[e2]);
}
}
}
else {
if (n == edgeEnds[e2+nEdges]-1) {
for(s = 0; s < nStates[n]; s++) {
tmp[s] /= pow(newMsgs[s+maxState*e2], 1.-mu[e2]);
}
}
else {
for(s = 0; s < nStates[n]; s++) {
tmp[s] /= pow(newMsgs[s+maxState*(e2+nEdges)], 1.-mu[e2]);
}
}
}
}
/* Now multiply by edge potential to get new message */
if (n == n2) {
for(s1 = 0; s1 < nStates[n1]; s1++) {
newMsgs[s1+maxState*(e+nEdges)] = 0.0;
for(s2 = 0; s2 < nStates[n2]; s2++) {
newMsgs[s1+maxState*(e+nEdges)] += tmp[s2]*pow(edgePot[s1+maxState*(s2+maxState*e)], 1./mu[e]);
}
}
/* Normalize */
z = 0.0;
for(s = 0; s < nStates[n1]; s++)
z += newMsgs[s+maxState*(e+nEdges)];
for(s = 0; s < nStates[n1]; s++)
newMsgs[s+maxState*(e+nEdges)] /= z;
}
else {
for(s2 = 0; s2 < nStates[n2]; s2++) {
newMsgs[s2+maxState*e] = 0.0;
for(s1 = 0; s1 < nStates[n1]; s1++) {
newMsgs[s2+maxState*e] += tmp[s1]*pow(edgePot[s1+maxState*(s2+maxState*e)], 1./mu[e]);
}
}
/* Normalize */
z = 0.0;
for(s = 0; s < nStates[n2]; s++)
z += newMsgs[s+maxState*e];
for(s = 0; s < nStates[n2]; s++)
newMsgs[s+maxState*e] /= z;
}
}
}
/* Print out messages */
/*
* printf("\n\nIter = %d\n", iter);
* for(s=0;s<maxState;s++) {
* for(e=0;e<nEdges*2;e++) {
* printf("newMsgs(%d,%d) = %f\n", s, e, newMsgs[s+maxState*e]);
* }
* }
*/
/* oldMsgs = newMsgs */
z = 0;
for(s=0;s<maxState;s++) {
for(e=0;e<nEdges*2;e++) {
z += absDif(newMsgs[s+maxState*e], oldMsgs[s+maxState*e]);
oldMsgs[s+maxState*e] = newMsgs[s+maxState*e];
}
}
/* if sum(abs(newMsgs(:)-oldMsgs(:))) < 1e-4; break; */
if(z < 1e-4) {
break;
}
}
/* ******************* DONE MESSAGE PASSING ********************** */
/*if(iter == maxIter)
* {
* printf("LBP reached maxIter of %d iterations\n",maxIter);
* }
* printf("Stopped after %d iterations\n",iter); */
/* compute nodeBel */
for(n = 0; n < nNodes; n++) {
for(s = 0; s < nStates[n]; s++)
prodMsgs[s+maxState*n] = nodePot[n+nNodes*s];
for(Vind = V[n]-1; Vind < V[n+1]-1; Vind++) {
e = E[Vind]-1;
n1 = edgeEnds[e]-1;
n2 = edgeEnds[e+nEdges]-1;
if (n == n2) {
for(s = 0; s < nStates[n]; s++) {
prodMsgs[s+maxState*n] *= pow(newMsgs[s+maxState*e], mu[e]);
}
}
else {
for(s = 0; s < nStates[n]; s++) {
prodMsgs[s+maxState*n] *= pow(newMsgs[s+maxState*(e+nEdges)], mu[e]);
}
}
}
z = 0;
for(s = 0; s < nStates[n]; s++) {
nodeBel[n + nNodes*s] = prodMsgs[s+maxState*n];
z = z + nodeBel[n+nNodes*s];
}
for(s = 0; s < nStates[n]; s++)
nodeBel[n + nNodes*s] /= z;
}
/* Compute edgeBel */
for(e = 0; e < nEdges; e++) {
n1 = edgeEnds[e]-1;
n2 = edgeEnds[e+nEdges]-1;
/* temp1 = nodePot by all messages to n1 except from n2 */
for(s = 0; s < nStates[n1]; s++)
tmp1[s] = nodePot[n1 + nNodes*s];
for(Vind = V[n1]-1; Vind < V[n1+1]-1; Vind++) {
e2 = E[Vind]-1;
if (e != e2) {
if (n1 == edgeEnds[e2+nEdges]-1) {
for(s = 0; s < nStates[n1]; s++) {
tmp1[s] *= pow(newMsgs[s+maxState*e2],mu[e2]);
}
}
else {
for(s = 0; s < nStates[n1]; s++) {
tmp1[s] *= pow(newMsgs[s+maxState*(e2+nEdges)],mu[e2]);
}
}
}
else {
if (n1 == edgeEnds[e2+nEdges]-1) {
for(s = 0; s < nStates[n1]; s++) {
tmp1[s] /= pow(newMsgs[s+maxState*e2],1.-mu[e2]);
}
}
else {
for(s = 0; s < nStates[n1]; s++) {
tmp1[s] /= pow(newMsgs[s+maxState*(e2+nEdges)],1.-mu[e2]);
}
}
}
}
/* temp2 = nodePot by all messages to n2 except from n1 */
for(s = 0; s < nStates[n2]; s++)
tmp2[s] = nodePot[n2 + nNodes*s];
for(Vind = V[n2]-1; Vind < V[n2+1]-1; Vind++) {
e2 = E[Vind]-1;
if (e != e2) {
if (n2 == edgeEnds[e2+nEdges]-1) {
for(s = 0; s < nStates[n2]; s++) {
tmp2[s] *= pow(newMsgs[s+maxState*e2],mu[e2]);
}
}
else {
for(s = 0; s < nStates[n2]; s++) {
tmp2[s] *= pow(newMsgs[s+maxState*(e2+nEdges)],mu[e2]);
}
}
}
else {
if (n2 == edgeEnds[e2+nEdges]-1) {
for(s = 0; s < nStates[n2]; s++) {
tmp2[s] /= pow(newMsgs[s+maxState*e2],1.-mu[e2]);
}
}
else {
for(s = 0; s < nStates[n2]; s++) {
tmp2[s] /= pow(newMsgs[s+maxState*(e2+nEdges)],1.-mu[e2]);
}
}
}
}
z = 0;
for(s1 = 0; s1 < nStates[n1]; s1++) {
for(s2 = 0; s2 < nStates[n2]; s2++) {
edgeBel[s1 + maxState*(s2 + maxState*e)] = tmp1[s1]*tmp2[s2]*pow(edgePot[s1+maxState*(s2+maxState*e)],1./mu[e]);
z += edgeBel[s1+maxState*(s2+maxState*e)];
}
}
for(s1 = 0; s1 < nStates[n1]; s1++) {
for(s2 = 0; s2 < nStates[n2]; s2++)
edgeBel[s1+maxState*(s2+maxState*e)] /= z;
}
}
/* Compute Bethe Free Energy */
energy1 = 0;
energy2 = 0;
entropy1 = 0;
entropy2 = 0;
for(n = 0; n < nNodes; n++) {
nNbrs = 0;
for(Vind = V[n]-1; Vind < V[n+1]-1; Vind++)
nNbrs += mu[E[Vind]-1];
/*printf("%f\n",nNbrs);*/
for(s = 0; s < nStates[n]; s++) {
if(nodeBel[n+nNodes*s] > 1e-10)
entropy1 += (nNbrs-1)*nodeBel[n+nNodes*s]*log(nodeBel[n+nNodes*s]);
energy1 -= nodeBel[n+nNodes*s]*log(nodePot[n+nNodes*s]);
}
}
for(e = 0; e < nEdges; e++) {
n1 = edgeEnds[e]-1;
n2 = edgeEnds[e+nEdges]-1;
for(s1 = 0; s1 < nStates[n1];s1++) {
for(s2 = 0; s2 < nStates[n2]; s2++) {
if(edgeBel[s1+maxState*(s2+maxState*e)] > 1e-10) {
entropy2 -= mu[e]*edgeBel[s1+maxState*(s2+maxState*e)]*log(edgeBel[s1+maxState*(s2+maxState*e)]);
}
energy2 -= edgeBel[s1+maxState*(s2+maxState*e)]*log(edgePot[s1+maxState*(s2+maxState*e)]);
}
}
}
/*printf("%f,%f,%f,%f\n",energy1,energy2,entropy1,entropy2);*/
logZ[0] = -energy1-energy2+entropy1+entropy2;
/* Free memory */
mxFree(prodMsgs);
mxFree(oldMsgs);
mxFree(newMsgs);
mxFree(tmp);
mxFree(tmp1);
mxFree(tmp2);
}
void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
{
/* Variables */
int n, s, e, e2, n1, n2, neigh, Vind, Vind2, s1, s2, di, L, foundIt,
nNodes, nEdges, maxState, dims[3],
iter,maxIter,nNbrs,
*edgeEnds, *nStates, *V, *E,*y,
calcMargs, useDamping, *nNeighbors;
double *msgFromNode, *edgePot, *nodeBel, *edgeBel, *logZ,
z,energy1,energy2,entropy1,entropy2,*prodMsgs,*oldMsgs,*newMsgs,*tmp,
*updatedMsg, *nodeBelUptree, *msgFromNodeForUptree, *prodMsgForUptree, *prodMsgForFactorMargs, z1, z2, z3, *nodePot, *msgDownTree, *factorMarginals,
nodeTerm, factorTerm, zf, *messagesFromLastLBP, alpha, M1, temp;
/* Input */
msgFromNode = mxGetPr(prhs[0]);
msgFromNodeForUptree = mxGetPr(prhs[1]);
nodePot = mxGetPr(prhs[2]);
msgDownTree = mxGetPr(prhs[3]);
nNeighbors = (int*)mxGetPr(prhs[4]);
edgePot = mxGetPr(prhs[5]);
edgeEnds = (int*)mxGetPr(prhs[6]);
nStates = (int*)mxGetPr(prhs[7]);
V = (int*)mxGetPr(prhs[8]);
E = (int*)mxGetPr(prhs[9]);
maxIter = ((int*)mxGetPr(prhs[10]))[0];
calcMargs = ((int*)mxGetPr(prhs[11]))[0];
useDamping = ((int*)mxGetPr(prhs[12]))[0];
messagesFromLastLBP = mxGetPr(prhs[13]);
alpha = ((double*)mxGetPr(prhs[14]))[0];
if (!mxIsClass(prhs[6],"int32")||!mxIsClass(prhs[7],"int32")||!mxIsClass(prhs[8],"int32")||!mxIsClass(prhs[9],"int32")||!mxIsClass(prhs[10],"int32"))
mexErrMsgTxt("edgeEnds, nStates, V, E, maxIter must be int32");
/* Compute Sizes */
nNodes = mxGetDimensions(prhs[0])[0];
maxState = mxGetDimensions(prhs[0])[1];
nEdges = mxGetDimensions(prhs[6])[0];
/* Output */
plhs[0] = mxCreateDoubleMatrix(nNodes,maxState,mxREAL);
dims[0] = maxState;
dims[1] = maxState;
dims[2] = nEdges;
plhs[1] = mxCreateNumericArray(3,dims,mxDOUBLE_CLASS,mxREAL);
plhs[2] = mxCreateDoubleMatrix(1,1,mxREAL);
plhs[3] = mxCreateDoubleMatrix(nEdges*maxState*2,1,mxREAL);
plhs[4] = mxCreateDoubleMatrix(nNodes, maxState, mxREAL);
nodeBel = mxGetPr(plhs[0]);
edgeBel = mxGetPr(plhs[1]);
logZ = mxGetPr(plhs[2]);
updatedMsg = mxGetPr(plhs[3]);
nodeBelUptree = mxGetPr(plhs[4]);
prodMsgs = mxCalloc(maxState*nNodes,sizeof(double));
prodMsgForUptree = mxCalloc(maxState*nNodes,sizeof(double));
prodMsgForFactorMargs = mxCalloc(maxState*nNodes,sizeof(double));
oldMsgs = mxCalloc(maxState*nEdges*2,sizeof(double));
newMsgs = mxCalloc(maxState*nEdges*2,sizeof(double));
tmp = mxCalloc(maxState,sizeof(double));
factorMarginals = mxCalloc(maxState*nNodes,sizeof(double));
/* Initialize */
for(e = 0; e < nEdges; e++)
{
n1 = edgeEnds[e]-1;
n2 = edgeEnds[e+nEdges]-1;
for(s = 0; s < nStates[n2]; s++)
newMsgs[s+maxState*e] = myLog(1./nStates[n2]);
for(s = 0; s < nStates[n1]; s++)
newMsgs[s+maxState*(e+nEdges)] = myLog(1./nStates[n1]);
}
for(iter = 0; iter < maxIter; iter++)
{
for(n = 0; n < nNodes; n++)
{
/* Update Messages */
for(Vind = V[n]-1; Vind < V[n+1]-1; Vind++)
{
e = E[Vind]-1;
n1 = edgeEnds[e]-1;
n2 = edgeEnds[e+nEdges]-1;
/* First part of message is msgFromNode*/
for(s = 0; s < nStates[n]; s++)
tmp[s] = msgFromNode[n + nNodes*s];
/* Multiply by messages from neighbors except j */
for(Vind2 = V[n]-1; Vind2 < V[n+1]-1; Vind2++)
{
e2 = E[Vind2]-1;
if (e != e2)
{
if (n == edgeEnds[e2+nEdges]-1)
{
for(s = 0; s < nStates[n]; s++)
{
tmp[s] += newMsgs[s+maxState*e2];
}
}
else
{
for(s = 0; s < nStates[n]; s++)
{
tmp[s] += newMsgs[s+maxState*(e2+nEdges)];
}
}
}
}
/* Now multiply by edge potential to get new message */
if (n == n2)
{
for(s1 = 0; s1 < nStates[n1]; s1++)
{
temp = 0.0;
M1 = tmp[0] + edgePot[s1+maxState*(0+maxState*e)];
for(s2 = 0; s2 < nStates[n2]; s2++)
{
if ((tmp[s2] + edgePot[s1+maxState*(s2+maxState*e)]) > M1)
M1 = tmp[s2] + edgePot[s1+maxState*(s2+maxState*e)];
}
for(s2 = 0; s2 < nStates[n2]; s2++)
{
temp += exp(tmp[s2] + edgePot[s1+maxState*(s2+maxState*e)] - M1);
}
newMsgs[s1+maxState*(e+nEdges)] = M1 + myLog(temp);
}
/* Normalize */
M1 = newMsgs[0+maxState*(e+nEdges)];
for(s = 0; s < nStates[n1]; s++)
if (newMsgs[s+maxState*(e+nEdges)] > M1)
M1 = newMsgs[s+maxState*(e+nEdges)];
temp = 0.0;
z = M1;
for(s = 0; s < nStates[n1]; s++)
{
temp += exp(newMsgs[s+maxState*(e+nEdges)] - M1);
}
z += myLog(temp);
for(s = 0; s < nStates[n1]; s++)
{
newMsgs[s+maxState*(e+nEdges)] -= z;
}
if(useDamping == 1)
{
for(s = 0; s < nStates[n1]; s++)
{
newMsgs[s+maxState*(e+nEdges)] = myLog(alpha*exp(newMsgs[s+maxState*(e+nEdges)]) + (1-alpha)*exp(messagesFromLastLBP[s+maxState*(e+nEdges)]));
}
}
}
else
{
for(s2 = 0; s2 < nStates[n2]; s2++)
{
temp = 0.0;
M1 = tmp[0] + edgePot[0+maxState*(s2+maxState*e)];
for(s1 = 0; s1 < nStates[n1]; s1++)
if ((tmp[s1] + edgePot[s1+maxState*(s2+maxState*e)]) > M1)
M1 = tmp[s1] + edgePot[s1+maxState*(s2+maxState*e)];
for(s1 = 0; s1 < nStates[n1]; s1++)
temp += exp(tmp[s1] + edgePot[s1+maxState*(s2+maxState*e)] - M1);
newMsgs[s2+maxState*e] = M1 + myLog(temp);
}
/* Normalize */
M1 = newMsgs[0+maxState*e];
for (s = 0; s < nStates[n2]; s++)
if (newMsgs[s+maxState*e] > M1)
M1 = newMsgs[s+maxState*e];
z = M1;
temp = 0.0;
for (s = 0; s < nStates[n2]; s++)
temp += exp(newMsgs[s+maxState*e] - M1);
z += myLog(temp);
for (s = 0; s < nStates[n2]; s++)
newMsgs[s+maxState*e] -= z;
if(useDamping == 1)
{
for(s = 0; s < nStates[n2]; s++)
{
newMsgs[s+maxState*e] = myLog(alpha*exp(newMsgs[s+maxState*e]) + (1-alpha)*exp(messagesFromLastLBP[s+maxState*e]));
}
}
}
}
}
/* oldMsgs = newMsgs */
z = 0;
for(s=0;s<maxState;s++)
{
for(e=0;e<nEdges*2;e++)
{
z += absDif(newMsgs[s+maxState*e],oldMsgs[s+maxState*e]);
oldMsgs[s+maxState*e] = newMsgs[s+maxState*e];
updatedMsg[s+maxState*e] = newMsgs[s+maxState*e];
}
}
/* if sum(abs(newMsgs(:)-oldMsgs(:))) < 1e-4; break; */
if(z < 1e-3)
{
break;
}
}
/* compute nodeBel */
for(n = 0; n < nNodes; n++)
{
for(s = 0; s < nStates[n]; s++)
{
prodMsgs[s+maxState*n] = msgFromNode[n+nNodes*s];
prodMsgForUptree[s+maxState*n] = msgFromNodeForUptree[n+nNodes*s];
prodMsgForFactorMargs[s+maxState*n] = msgDownTree[n+nNodes*s];
}
for(Vind = V[n]-1; Vind < V[n+1]-1; Vind++)
{
e = E[Vind]-1;
n1 = edgeEnds[e]-1;
n2 = edgeEnds[e+nEdges]-1;
if (n == n2)
{
for(s = 0; s < nStates[n]; s++)
{
prodMsgs[s+maxState*n] += newMsgs[s+maxState*e];
prodMsgForUptree[s+maxState*n] += newMsgs[s+maxState*e];
prodMsgForFactorMargs[s+maxState*n] += newMsgs[s+maxState*e];
}
}
else
{
for(s = 0; s < nStates[n]; s++)
{
prodMsgs[s+maxState*n] += newMsgs[s+maxState*(e+nEdges)];
prodMsgForUptree[s+maxState*n] += newMsgs[s+maxState*(e+nEdges)];
prodMsgForFactorMargs[s+maxState*n] += newMsgs[s+maxState*(e+nEdges)];
}
}
}
/* normalize */
M1 = prodMsgs[0+maxState*n];
for (s = 0; s < nStates[n]; s++)
{
nodeBel[n+nNodes*s] = prodMsgs[s+maxState*n];
if (prodMsgs[s+maxState*n] > M1)
M1 = prodMsgs[s+maxState*n];
}
z = M1;
temp = 0.0;
for (s = 0; s < nStates[n]; s++)
temp += exp(nodeBel[n+nNodes*s] - M1);
z += myLog(temp);
for (s = 0; s < nStates[n]; s++)
nodeBel[n+nNodes*s] -= z;
M1 = prodMsgForUptree[0+maxState*n];
for (s = 0; s < nStates[n]; s++)
{
nodeBelUptree[n+nNodes*s] = prodMsgForUptree[s+maxState*n];
if (prodMsgForUptree[s+maxState*n] > M1)
M1 = prodMsgForUptree[s+maxState*n];
}
z = M1;
temp = 0.0;
for (s = 0; s < nStates[n]; s++)
temp += exp(nodeBelUptree[n+nNodes*s] - M1);
z += myLog(temp);
for (s = 0; s < nStates[n]; s++)
nodeBelUptree[n+nNodes*s] -= z;
M1 = prodMsgForFactorMargs[0+maxState*n];
for (s = 0; s < nStates[n]; s++)
{
if (prodMsgForFactorMargs[s+maxState*n] > M1)
M1 = prodMsgForFactorMargs[s+maxState*n];
}
z = M1;
temp = 0.0;
for (s = 0; s < nStates[n]; s++)
temp += exp(prodMsgForFactorMargs[s+maxState*n] - M1);
z += myLog(temp);
for (s = 0; s < nStates[n]; s++)
{
prodMsgForFactorMargs[s+maxState*n] -= z;
}
}
/* Compute edgeBel */
for(e = 0; e < nEdges; e++)
{
n1 = edgeEnds[e]-1;
n2 = edgeEnds[e+nEdges]-1;
for(s1 = 0; s1 < nStates[n1]; s1++)
{
for(s2 = 0; s2 < nStates[n2]; s2++)
{
edgeBel[s1+maxState*(s2+maxState*e)] = nodeBel[n1+nNodes*s1] - newMsgs[s1+maxState*(e+nEdges)];
edgeBel[s1+maxState*(s2+maxState*e)] += nodeBel[n2+nNodes*s2] - newMsgs[s2+maxState*e];
edgeBel[s1+maxState*(s2+maxState*e)] += edgePot[s1+maxState*(s2+maxState*e)];
}
}
M1 = edgeBel[maxState*(maxState*e)];
for(s1 = 0; s1 < nStates[n1]; s1++)
for(s2 = 0; s2 < nStates[n2]; s2++)
if (edgeBel[s1+maxState*(s2+maxState*e)] > M1)
M1 = edgeBel[s1+maxState*(s2+maxState*e)];
z = M1;
temp = 0.0;
for(s1 = 0; s1 < nStates[n1]; s1++)
for(s2 = 0; s2 < nStates[n2]; s2++)
temp += exp(edgeBel[s1+maxState*(s2+maxState*e)] - M1);
z += myLog(temp);
for(s1 = 0; s1 < nStates[n1]; s1++)
for(s2 = 0; s2 < nStates[n2]; s2++)
edgeBel[s1+maxState*(s2+maxState*e)] -= z;
}
/* Compute Bethe Free Energy */
nodeTerm = 0.0;
factorTerm = 0.0;
for(n = 0; n < nNodes; n++)
{
nNbrs = V[n+1]-V[n];
M1 = nodePot[n] + prodMsgForFactorMargs[maxState*n];
for(s = 0; s < nStates[n]; s++)
{
nodeTerm += (nNbrs+nNeighbors[n]+1)*exp(nodeBel[n+nNodes*s])*nodeBel[n+nNodes*s];
factorMarginals[s+maxState*n] = nodePot[n+nNodes*s] + prodMsgForFactorMargs[s+maxState*n];
if (factorMarginals[s+maxState*n] > M1)
M1 = factorMarginals[s+maxState*n];
}
zf = M1;
temp = 0.0;
for (s = 0; s < nStates[n]; s++)
temp += exp(factorMarginals[s+maxState*n] - M1);
zf += myLog(temp);
for(s = 0; s < nStates[n]; s++)
{
factorMarginals[s+maxState*n] -= zf;
temp = exp(factorMarginals[s+maxState*n]) * (-nodePot[n+nNodes*s] + factorMarginals[s+maxState*n]);
factorTerm += temp;
}
}
for(e = 0; e < nEdges; e++)
{
n1 = edgeEnds[e]-1;
n2 = edgeEnds[e+nEdges]-1;
for(s1 = 0; s1 < nStates[n1];s1++)
{
for(s2 = 0; s2 < nStates[n2]; s2++)
{
factorTerm += exp(edgeBel[s1+maxState*(s2+maxState*e)]) * (-edgePot[s1+maxState*(s2+maxState*e)] + edgeBel[s1+maxState*(s2+maxState*e)]);
}
}
}
logZ[0] = nodeTerm - factorTerm;
/* Free memory */
mxFree(prodMsgs);
mxFree(oldMsgs);
mxFree(newMsgs);
mxFree(tmp);
mxFree(prodMsgForUptree);
mxFree(prodMsgForFactorMargs);
mxFree(factorMarginals);
}
