proj/mag/ident/libs/mexfunctions/mixNprob2d.c

#include "mex.h"  /* must appear first */
#include <stdio.h>
#include <math.h>
#include <memory.h>
#include "mexhead.h" /* my mex defines */
#include "Doc/mixNprob2d_docstring.h"  /* autogenerated from this file */

/**************************************************************

% mixNprob2d: probabilities under normal mixture (2 dimensions)
% 
% p=mixNprob2d(i1,i2,model,mode);
% * This routine has been superseded by mixNprobNd.  But we retain it
% because it can be much faster for 2d models.
% 
% Inputs:
%   real i1[m,n];
%   real i2[m,n];
%   real model[6,k];
%   opt int logmode = 1;
% 
% Outputs:
%   real p[m,n];
% 
% See Also: mixN2mixture2d, mixNprobNd
% 
% implemented as a mex file

% MJT 20 july 1998: tested exhaustively against mixNprob,
% the m-file for finding gaussian mixture probabilities,
% and found agreement to within floating-point accuracy

****************************************************************/
/* Updated by -mex2pymex.py-ver1- on Wed Sep 23 16:55:51 2009 */

#define NARGIN_MIN  3      /* min number of inputs */
#define NARGIN_MAX  4      /* max number of inputs */
#define NARGOUT_MIN 1      /* min number of outputs */
#define NARGOUT_MAX 1      /* max number of outputs */

#define ARG_i1 0
#define ARG_i2 1
#define ARG_model 2
#define ARG_logmode 3

#define ARG_p 0

static const char *progname = "mixNprob2d";
#define PROGNAME mixNprob2d
static const char *in_specs[NARGIN_MAX] = {
  "RM",
  "RM",
  "RM",
  "IS(1)"};
static const char *in_names[NARGIN_MAX] = {
  "i1",
  "i2",
  "model",
  "logmode"};
static const char *out_names[NARGOUT_MAX] = {
  "p"};

// defined just for brevity in a generated #include file
#define SHORTNAME m2d

/************************  Utilities  ************************/

/* indexes into model matrix for various quantities */
#define inx_lambda 0 /* weights */
#define inx_mu1    1 /* mean */
#define inx_mu2    2 /* mean */
#define inx_sig11  3 /* (1,1) entry of covariance */
#define inx_sig22  4 /* (2,2) entry */
#define inx_sig12  5 /* (1,2) entry */

#define ActiveData(x) (!isnan(x)) /* not NaN */
/* square of x */
#define sqr(x) ((x)*(x))

static
void
control(double **p,
    double **i1,
    double **i2,
    double **model,
    int log_mode,
    int K,
    int M,
    int N)

{
   void * mxCalloc();
   int k, m, n;
   double det, rho;
   double *normF, *sig1F, *sig2F, *crosF, *halfF; /* constants */
   double *lprob; /* holds log-probabilities */
   double lprob_max; /* maximum log-probability */
   const double lprob_max_init = -1E64; /* anything < log(minfloat) */
   double NaN; /* not-a-number */
   double j1, j2; /* transformed i1 and i2 */
   double sum; /* accumulate probability */
   const double log2pi = log(2 * M_PI);

   lprob = mxCalloc(K, sizeof(double)); /* for log-probabilities */
   /* 
    * for each model, find some common factors once and for all 
    */
   /* space to put them */
   normF = mxCalloc(K, sizeof(double)); /* normalization in front */
   sig1F = mxCalloc(K, sizeof(double)); /* 1/sigma1 for i1 term */
   sig2F = mxCalloc(K, sizeof(double)); /* 1/sigma2 for i2 term */
   crosF = mxCalloc(K, sizeof(double)); /* for i1 * i2 term in exp */
   halfF = mxCalloc(K, sizeof(double)); /* overall exp factor */
   /* find factors for each class */
   for (k = 0; k < K; k++) {
     /* could do error checking here on determinants, etc */
     /* determinant */
     det = model[k][inx_sig11] * model[k][inx_sig22] - 
       sqr(model[k][inx_sig12]);
     /* log(lambda) - log(2 pi) - log(det(Sigma))/2 */
     normF[k] = log(model[k][inx_lambda]) - log2pi - log(det)/2;
     /* 1/sigma1, 1/sigma2 */
     sig1F[k] = 1/sqrt(model[k][inx_sig11]);
     sig2F[k] = 1/sqrt(model[k][inx_sig22]);
     /* correlation coefficient */
     rho = model[k][inx_sig12] * sig1F[k] * sig2F[k];
     /* -2 * rho */
     crosF[k] = -2*rho;
     /* -(1/2) * (1/(1-rho^2)) */
     halfF[k] = -1/(2 * (1 - sqr(rho)));
   }
   NaN = (rho - rho)/(rho - rho); /* need a NAN */
   for (n = 0; n < N; n++)
     for (m = 0; m < M; m++) {
       /* skip inactive pixels */
       if (!ActiveData(i1[n][m]) || !ActiveData(i2[n][m])) {
     p[n][m] = NaN;
     continue; 
       }
       /* otherwise, find weighted per-class log-density */
       for (lprob_max = lprob_max_init, k = 0; k < K; k++) {
     /* transformed i1 and i2 */
     j1 = sig1F[k] * (i1[n][m] - model[k][inx_mu1]);
     j2 = sig2F[k] * (i2[n][m] - model[k][inx_mu2]);
     /* log-density */
     lprob[k] =
       normF[k] + halfF[k] * (sqr(j1) + sqr(j2) + crosF[k] * j1 * j2);
     if (lprob[k] > lprob_max) lprob_max = lprob[k]; 
       }
       /* sum per-class contributions -- summands are <= 1 */
       for (sum = k = 0; k < K; k++)
     sum += exp(lprob[k] - lprob_max); /* rescale to max */
       /* undo the scaling */
       if (log_mode)
     p[n][m] = log(sum) + lprob_max;
       else
     p[n][m] = sum * exp(lprob_max);
     }
   /* free variables */
   mxFree((char *) lprob);
   mxFree((char *) normF);
   mxFree((char *) sig1F);
   mxFree((char *) sig2F);
   mxFree((char *) crosF);
   mxFree((char *) halfF);
}


/************************ mexFunction ************************/

/*
 * Gateway routine
 */
#ifdef StaticP  /* undefined under mex */
StaticP
#endif
void 
mexFunction(
        int nlhs, 
        mxArray *plhs[], 
        int nrhs,
        const mxArray *prhs[])
{
  int log_mode;
  int m, n;
  int k;
  double **p_2d, **i1_2d, **i2_2d, **m_2d; /* pointers for 2d indexing */
  char errstr[120];

  /* Hook for introspection (function signature, docstring) */
  if (nrhs < 0) { 
    plhs[0] = mxt_PackSignature((mxt_Signature) (-nrhs), 
                NARGIN_MIN, NARGIN_MAX, 
                NARGOUT_MIN, NARGOUT_MAX, 
                in_names, in_specs, out_names, docstring);
    return;
  }
   /*
    * check args
    */
   if ((nrhs < NARGIN_MIN) || (nrhs > NARGIN_MAX))
     mexErrMsgTxt((snprintf(errstr, sizeof(errstr),
                "Expect %d <= input args <= %d",
                NARGIN_MIN, NARGIN_MAX), errstr));
  if ((nlhs < NARGOUT_MIN) || (nlhs > NARGOUT_MAX))
      mexErrMsgTxt((snprintf(errstr, sizeof(errstr),
                 "%s: Expect %d <= output args <= %d",
                 progname, NARGOUT_MIN, NARGOUT_MAX), errstr));
   mexargparse(nrhs, prhs, in_names, in_specs, NULL, progname);
   start_sizechecking();
   sizeinit(prhs[ARG_i1]);
   sizeagree(prhs[ARG_i2]); /* size(i1) == size(i2) */
   sizecheck_msg(progname, in_names, ARG_i1);
   sizeinit(prhs[ARG_model]);
   sizeisM(6); /* 6 rows in model */
   sizecheck_msg(progname, in_names, ARG_model);
   
   /*
    * create space for output
    */
   /* 1: find how big it must be */
   m = (int) mxGetM(prhs[ARG_i1]);
   n = (int) mxGetN(prhs[ARG_i1]);
   k = (int) mxGetN(prhs[ARG_model]);

   /* one more error check */
   if (k <= 0)
     mexErrMsgTxt((snprintf(errstr, sizeof(errstr),
                "%s: Must have a non-empty model", 
                progname), errstr));

   /* 2: make the space */
   /* paradoxically works even if nlhs == 0, in which case strictly
      speaking plhs[0] need not exist (i.e. plhs == NULL in this case)
      MATLAB seems to always have plhs non-null to allow for transmission
      of the result via 'ans' */
   plhs[ARG_p] = mxCreateDoubleMatrix(m, n, mxREAL);
   
   /* 3: fill in arguments */
   if (nrhs > ARG_logmode)
     log_mode = (int) mxGetScalar(prhs[ARG_logmode]) > 0;
   else
     log_mode = 1;
      
   /*
    * do the computation
    */
   /* save the 2d pointers */
   p_2d  = mxt_make_matrix2(plhs[ARG_p],     -1, -1, 0.0);
   i1_2d = mxt_make_matrix2(prhs[ARG_i1],    -1, -1, 0.0);
   i2_2d = mxt_make_matrix2(prhs[ARG_i2],    -1, -1, 0.0);
   m_2d  = mxt_make_matrix2(prhs[ARG_model], -1, -1, 0.0);
   /* the workhorse */
   control(p_2d, i1_2d, i2_2d, m_2d, log_mode, k, m, n);
   /* free 2d pointers */
   mxFree(p_2d);
   mxFree(i1_2d);
   mxFree(i2_2d);
   mxFree(m_2d);
}


/* Hook for generic tail matter */
#ifdef MEX2C_TAIL_HOOK
#include "mex2c_tail.h"
#endif

---