void CommandComputeMatches(int nlhs, mxArray* plhs[], int nrhs, const mxArray* prhs[])
{
// Expected Input:
// cmd
// class handle
// samplePtsXfmd ~ 2D sample points
// sampleNormsXfmd ~ 2D sample orientations
// sigma2 ~ position variances
// k ~ orientation concentrations
// matchDatumsInit ~ initial match datums [optional]
// match_ThetaMax ~ maximum permitted orientation error for a match [optional]
//
// Output:
// MatchPts2D ~ 2D match points (in image coords) (2 x numSamps double)
// MatchNorms2D ~ 2D match norms (in image coords) (2 x numSamps double)
// MatchDatums ~ match datums (numSamps int32) (optional)
// PermittedMatches ~ permitted matches (i.e. permitted if theta < thetaMax) (numSamps logical) (optional)
//
// Check parameters
if ((nlhs < 2 || nlhs > 4) || (nrhs < 6 || nrhs > 8))
{
MEX_ERROR("ComputeMatches: requires 2 to 4 outputs, 6 to 8 inputs.");
}
// Get the class instance from the second input
mexInterface_Alg2D_DirPDTree_vonMises_Edges& obj =
*convertMat2Ptr<mexInterface_Alg2D_DirPDTree_vonMises_Edges>(prhs[1]);
alg2D_DirPDTree_vonMises_Edges& alg = *obj.pAlg;
//--- Inputs ---//
MEX_DEBUG("Parsing Inputs...\n");
// Parse Samples
//vctDynamicVector<vct2> samplePts;
//vctDynamicVector<vct2> sampleNorms;
MEX_DEBUG(" ...samplePts\n");
Parse2DVectorArray_Double(prhs[2], obj.samplePtsXfmd);
MEX_DEBUG(" ...sampleNorms\n");
Parse2DVectorArray_Double(prhs[3], obj.sampleNormsXfmd);
// Parse Noise Model
MEX_DEBUG(" ...sigma2\n");
vctDynamicVector<double> sigma2;
ParseVector_Double(prhs[4], sigma2);
if (sigma2.size() == 1)
{
obj.sigma2.SetSize(obj.samplePtsXfmd.size());
obj.sigma2.SetAll(sigma2[0]);
}
else
{
if (sigma2.size() != obj.samplePtsXfmd.size())
{
MEX_ERROR("Invalid size sigma2");
}
obj.sigma2 = sigma2;
}
MEX_DEBUG(" ...k\n");
vctDynamicVector<double> k;
ParseVector_Double(prhs[5], k);
if (k.size() == 1)
{
obj.k.SetSize(obj.samplePtsXfmd.size());
obj.k.SetAll(k[0]);
}
else
{
if (k.size() != obj.samplePtsXfmd.size())
{
MEX_ERROR("Invalid size k");
}
obj.k = k;
}
// Parse Match Datum Initializers
bool bDatumsInitialized = false;
if (nrhs >= 7)
{
MEX_DEBUG(" ...matchDatumsInit\n");
if (!mxIsEmpty(prhs[6]))
{
ParseVector_Int(prhs[6], obj.matchDatums);
bDatumsInitialized = true;
}
}
// Parse match_MaxTheta
if (nrhs >= 8)
{
MEX_DEBUG(" ...match_MaxTheta\n");
ParseDouble(prhs[7], obj.pAlg->dThetaMax);
//ParseDouble(prhs[7], obj.match_ThetaMax);
}
else
{
obj.pAlg->dThetaMax = cmnPI; // default
//obj.match_ThetaMax = cmnPI; // default
}
MEX_DEBUG(" ...done\n");
//--- Computation ---//
obj.nSamples = obj.samplePtsXfmd.size();
obj.matchPts.SetSize(obj.nSamples);
obj.matchNorms.SetSize(obj.nSamples);
obj.matchDatums.SetSize(obj.nSamples);
obj.matchErrors.SetSize(obj.nSamples);
obj.matchPermitted.SetSize(obj.nSamples);
if (!bDatumsInitialized)
{
// initialize match datums with accelerated approximate search
// Note: since this is a position-only initialization, there
// may be many non-permitted matches in the initialization;
// for these matches, the initialization will not have helped
for (unsigned int i = 0; i < obj.nSamples; i++)
{
obj.matchDatums[i] = obj.pDirTree->FastInitializeProximalDatum(
obj.samplePtsXfmd[i], obj.sampleNormsXfmd[i],
obj.matchPts[i], obj.matchNorms[i]);
}
}
// initialize all permitted matches to false
obj.matchPermitted.SetAll(false);
//// initialize permitted match flag
//obj.matchPermitted.SetAll(false);
//double theta;
//for (unsigned int i = 0; i < obj.nSamples; i++)
//{
// theta = acos( vctDotProduct(obj.sampleNormsXfmd[i], obj.matchNorms[i]) );
// if (theta > obj.pAlg->dThetaMax)
// {
// obj.matchPermitted[i] = false;
// }
// else
// {
// obj.matchPermitted[i] = true;
// }
//}
// compute matches
obj.ComputeMatches();
//--- Outputs ---//
unsigned int nSamps = obj.nSamples;
// outputs 1 & 2: 2D match points and normals
plhs[0] = mxCreateDoubleMatrix(2, nSamps, mxREAL);
plhs[1] = mxCreateDoubleMatrix(2, nSamps, mxREAL);
double *matchPts = mxGetPr(plhs[0]);
double *matchNorms = mxGetPr(plhs[1]);
for (unsigned int i = 0; i < nSamps; i++)
{
(*matchPts) = obj.matchPts[i].Element(0);
matchPts++;
(*matchPts) = obj.matchPts[i].Element(1);
matchPts++;
(*matchNorms) = obj.matchNorms[i].Element(0);
matchNorms++;
(*matchNorms) = obj.matchNorms[i].Element(1);
matchNorms++;
}
// output 3: match datums
if (nlhs >= 3)
{
plhs[2] = mxCreateNumericMatrix(1, nSamps, mxINT32_CLASS, mxREAL);
int *matchDatums = static_cast<int*> (mxGetData(plhs[2]));
for (unsigned int i = 0; i < nSamps; i++)
{
(*matchDatums) = obj.matchDatums[i];
matchDatums++;
}
}
// output 4: permitted matches
if (nlhs >= 4)
{
plhs[3] = mxCreateLogicalMatrix(1, nSamps);
mxLogical *matchPerm = mxGetLogicals(plhs[3]);
//bool *matchDatums = static_cast<bool*> (mxGetData(plhs[4]));
for (unsigned int i = 0; i < nSamps; i++)
{
(*matchPerm) = obj.matchPermitted[i];
matchPerm++;
}
}
}
void
mexFunction (int nout, mxArray * out[], int nin, const mxArray * in[])
{
enum {IN_PARENTS = 0, IN_DATA, IN_OPT} ;
enum {OUT_TREE} ;
vl_uint32 const *parents ;
vl_uint32 *tree ;
double const *data ;
int nnull = 0 ;
int histmode = 0 ;
int i, P, N ;
/* -----------------------------------------------------------------
* Check the arguments
* -------------------------------------------------------------- */
if ((nin < 2) || (nin > 3)) {
mexErrMsgTxt ("Two or three arguments required.") ;
}
if (nout > 1) {
mexErrMsgTxt ("Too many output arguments.") ;
}
if (!uIsRealMatrix(in[IN_DATA], -1, -1)) {
mexErrMsgTxt ("DATA must be a matrix of DOUBLE");
}
if (!uIsVector(in[IN_PARENTS], -1)) {
mexErrMsgTxt ("PARENTS must be a vector") ;
}
if (mxGetClassID(in[IN_PARENTS]) != mxUINT32_CLASS) {
mexErrMsgTxt ("PARENTS must be UINT32") ;
}
N = mxGetNumberOfElements (in[IN_DATA]) ;
data = mxGetPr (in[IN_DATA]) ;
P = mxGetNumberOfElements (in[IN_PARENTS]) ;
parents = mxGetData (in[IN_PARENTS]) ;
if (nin > 2) {
enum {buflen = 32} ;
char buf [buflen] ;
if (!uIsString(in[IN_OPT], -1)) {
mexErrMsgTxt("OPT must be a string") ;
}
mxGetString(in[IN_OPT], buf, buflen) ;
buf [buflen - 1] = 0 ;
if (!uStrICmp("hist", buf)) {
mexErrMsgTxt("OPT must be equal to 'hist'") ;
}
histmode = 1 ;
}
out[OUT_TREE] = mxCreateNumericMatrix(1, P,mxUINT32_CLASS, mxREAL) ;
tree = mxGetData (out[OUT_TREE]) ;
/* -----------------------------------------------------------------
* Do the job
* -------------------------------------------------------------- */
{
char buf [1024] ;
vl_uint32 max_node = 0 ;
vl_uint32 min_node = 0 ;
vl_uint32 last_leaf = 0 ;
vl_uint32 root = 0 ;
/* exhamine parents for errors and informations */
for (i = 0 ; i < P ; ++i) {
vl_uint32 node = parents [i] ;
if ((node != 0) & (node != 1)) {
max_node = VL_MAX (node, max_node) ;
min_node = VL_MIN (node, min_node) ;
}
/* check no node points outside the tree */
if (node > P) {
snprintf(buf, sizeof(buf),
"Out of bounds link PARENTS[%d] = %u > %u", i, node, P) ;
mexErrMsgTxt (buf) ;
}
/* check node points to something above him */
if ((node != 0) & (node != 1) & (node < i)) {
snprintf(buf, sizeof(buf),
"Backward link PARENTS[%d] = %u < %d", i, node, i) ;
mexErrMsgTxt (buf) ;
}
if (node == 0) ++ nnull ;
}
/* now
*
* min_node = first node which is not a leaf
* max_node = root node
* nnull = number of leaves pointing to the null node
*/
last_leaf = min_node - 1 ;
root = max_node ;
/* process data */
for (i = 0 ; i < N ; ++i) {
int w = 1 ;
int x = (int) data [i] ;
if (histmode) {
w = x ;
x = i ;
}
if ((x < 1) | (x > last_leaf)) {
if (histmode) {
snprintf(buf, sizeof(buf),
"DATA length exceeds number of AIB leaves") ;
} else {
snprintf(buf, sizeof(buf),
"DATA [%d] = %d is not a leaf", i, x) ;
}
mexErrMsgTxt (buf) ;
}
while (true) {
int x_ = (int) parents [x -1] ;
/* mexPrintf("%d : x_=%d, x=%d\n", i, x_, x) ; */
++ tree [x - 1] ;
if ((x_ == x) | (x_ == 0) | (x_ == 1)) break ;
x = x_ ;
}
}
}
}
void mexFunction(int nlhs, mxArray *plhs[],
int nrhs, const mxArray *prhs[]){
double *lnQ,*lnqst; /* inputparameters */
double *s; /* output parameter */
int T,N,t,j,k,ss; /* temporary variables */
double Z;
double *lnp0, *lnp1,*lnpp;
double *maxPrev;
mxArray *mxmaxPrev,*mxpp, *mxp1, *mxp0; /* temporary arrays */
/* check number of input/output arguments */
if (nlhs != 1)
mexErrMsgTxt("One output argument required.");
if (nrhs != 2)
mexErrMsgTxt("Two input argument required.");
/* size of input variables */
T = mxGetM(LNQST_IN); /* number of rows = number of time points*/
N = mxGetN(LNQST_IN); /* number of columns = number of states*/
/* check that the other variables have consistent sizes */
if ( mxGetM(LNQ_IN) != N || mxGetN(LNQ_IN) != N)
mexErrMsgTxt("Q is not an N by N matrix (N = # columns in lnqst).");
/* retrieve input data */
lnQ=mxGetPr(LNQ_IN);
lnqst=mxGetPr(LNQST_IN);
/* Create an mxArray for the output data */
S_OUT = mxCreateNumericMatrix(T,1,mxDOUBLE_CLASS, mxREAL);
s=mxGetPr(S_OUT); /* pointer to output array*/
/* allocate temporary arrays */
mxp0=mxCreateDoubleMatrix(1,N, mxREAL);
mxp1=mxCreateDoubleMatrix(1,N, mxREAL);
lnp0=mxGetPr(mxp0);
lnp1=mxGetPr(mxp1);
mxmaxPrev=mxCreateNumericMatrix(T,N,mxDOUBLE_CLASS,mxREAL);
maxPrev=mxGetPr(mxmaxPrev);
mxpp=mxCreateDoubleMatrix(N,1, mxREAL);
lnpp=mxGetPr(mxpp);
/* start of actual algorithm */
/*lnP1=lnqst(1,:)-mean(lnqst(1,:)); % initial probability, not normalized */
Z=0.0;
for(k=0;k<N;k++){
Z=Z+lnqst[k*T]/N;
}
for(k=0;k<N;k++){
lnp1[k]=lnqst[k*T]-Z;
}
for(t=1;t<T;t++){
for(k=0;k<N;k++){/*lnP0=lnP1;lnP1=zeros(1,N);*/
lnp0[k]=lnp1[k];
lnp1[k]=0.0;
}
for(j=0;j<N;j++){
for(k=0;k<N;k++){
/* lnPP(kV)=lnP0(kV)+lnQ(kV,jV)+lnqst(tV,jV);
* % probability of most likely path that ends with kV -> jV*/
lnpp[k]=lnp0[k]+lnQ[k+j*N]+lnqst[t+j*T];
}
/* probability of previous state before ending up at jV.
[lnP1(jV), MaxPrev(tV,jV)]=max(lnPP); */
lnp1[j]=lnpp[0];
maxPrev[t+j*T]=0;
for(k=1;k<N;k++){
if(lnpp[k]>lnp1[j]){
lnp1[j]=lnpp[k];
maxPrev[t+j*T]=k;
}
}
}
Z=0.0; /*lnP1=lnP1-mean(lnP1); % rescale*/
for(k=0;k<N;k++){
Z=Z+lnp1[k]/N;
}
for(k=0;k<N;k++){
lnp1[k]=lnp1[k]-Z;
}
}
s[T-1]=0; /* [~,S(T)]=max(lnP1); */
Z=lnp1[0];
for(k=1;k<N;k++){
if(lnp1[k]>Z){
Z=lnp1[k];
s[T-1]=k;
}
}
/* for tV=T-1:-1:1
* S(tV)=MaxPrev(tV+1,S(tV+1));
* end*/
for(t=T-2;t>=0;t--){
ss=(int)s[t+1];
s[t]=maxPrev[t+1+T*ss];
}
/* finished actual algorithm */
/* transform back to matlab's index convention */
for(t=0;t<T;t++){
s[t]=s[t]+1;
}
/* destroy temporary arrays */
mxDestroyArray(mxp0);
mxDestroyArray(mxp1);
mxDestroyArray(mxpp);
mxDestroyArray(mxmaxPrev);
}
void setField(mxArray** A, const int structIdx, const char* fieldname, const int size, mxClassID mxType, const T* value) {
mxArray *fieldValLo = mxCreateNumericMatrix(1, size, mxType, mxREAL);
T* loVal = (T*)mxGetData(fieldValLo);
memcpy(loVal, value, sizeof(T)*size);
mxSetFieldByNumber(*A, structIdx, mxGetFieldNumber(*A,fieldname), fieldValLo);
}
void mexFunction(int nlhs, mxArray *plhs[],
int nrhs, const mxArray *prhs[])
{
double fs, c, eleSpace, spaSpac;
double alphaT, alphaR;
double eleSpac, sapSpac;
float * rawData;
int m, n;
double *tTot, * delayValueT, * delayValueR;
float * dasData;
double deltaZ, deltaX;
double * steerLabel;
int * delayData;
int i, j, k;
float pi = 3.1415926535897932384626;
double z, x, xi, ttx, trx;
float delay;
int apeSize;
int iIntep;
int i1, j1, k1, apeSize1, iIntep1, delay1;
double z1, x1, xi1, ttx1, trx1;
///////////
double z0, x0;
int m0, m1, n1;
/////////////////////////
fs = 40e6;
c = 1540.0;
//////////////////////////
rawData = mxGetPr(prhs[0]);
m = mxGetM(prhs[0]);
n = mxGetN(prhs[0]);
// The delay between the transmission of the high-voltage pulse
// and the start of the acquisition of the SonixDAQ
tTot = mxGetPr(prhs[1]);
// Note that the unit of transmit delay determined in Texo
// between the adjacent element is 12.5 ns (or 80 MHz sampling rate)
// refer the note in {Apr. 22, 2014}
/////////////////////////
//mexPrintf("%d\n", m);
//mexPrintf("%d\n", n);
//mexPrintf("%f\n", *tTot);
/////////////////////////
// Window functions
//////////////////////////////
// plhs[1] = mxCreateNumericMatrix(n, 1, mxINT32_CLASS, 0);
// delayData = mxGetPr(plhs[1]);
// apeSize = 24;
// deltaZ = eleSpac / 4 / tan(alphaR) / 20;
// deltaX = eleSpac / 4 / 20 * *steerLabel;
// x = 0;
// alphaR = 12/180 * pi;
// Initial parameters:
// z0, the z-axis corrdinate of the begining point
// x0, ...
// m0, the index of centeral position for the begining point
// alphaR, the steered angle for receiving.
// m1, width of the ROI
// n1, height of the ROI
alphaR = *(mxGetPr(prhs[2]));
x0 = *(mxGetPr(prhs[3]));
z0 = *(mxGetPr(prhs[4]));
m0 = (int)*(mxGetPr(prhs[5])); //
m1 = (int)*(mxGetPr(prhs[6]));
n1 = (int)*(mxGetPr(prhs[7]));
///////////////////////////////
// Parameter of the L14-38 Xducer.
eleSpac = 0.3048e-3;
sapSpac = eleSpac/4/sin(fabs(alphaR))/20;
deltaX = sapSpac * sin(alphaR);
deltaZ = sapSpac * cos(alphaR);
plhs[0] = mxCreateNumericMatrix(n1, m1, mxSINGLE_CLASS, 0);
dasData = mxGetPr(plhs[0]);
#pragma omp parallel for shared(dasData, rawData, n, m, m1, n1, m0, deltaZ, deltaX, pi, x0, z0, eleSpac, c, fs, tTot) \
private(i, j, k, iIntep, z, x, xi, ttx, trx, delay, apeSize)
for (i = 0; i < m1; i ++) // x-direction
{
x = x0 + i * eleSpac/2 - 30 * deltaX;
z = z0 - 30 * deltaZ;
iIntep = (int) ((m0 + i)/2);
for (j = 0; j < n1; j ++) // z-direction
{
ttx = z;
// Unit in [m]
apeSize = (int) (z/2.8/0.30480*1e3);
if (apeSize < 4)
apeSize = 4;
if (apeSize > 32)
apeSize = 32;
for (k = -apeSize; k < apeSize + 1; k ++)
{
if ((k + iIntep) < 0 || (k + iIntep) >= n)
continue;
xi = (k + iIntep) * eleSpac;
// Unit in [m]
trx = sqrt( (x - xi) * (x - xi) + z * z);
// Unit in [m]
delay = (ttx + trx)/c * fs + *tTot;
// Uint in [s]
//mexPrintf("%d\n", delay);
if (delay >= (m - 1))
continue;
// dasData[i * n1 + j] = j;
dasData[i * n1 + j] += (0.54f - 0.46f * cosf(pi*(k + apeSize + 0.0f)/apeSize)) \
* ( (delay - (int) (delay)) \
* ( rawData[(k + iIntep) * m + (int) (delay + 1) ] \
- rawData[(k + iIntep) * m + (int) (delay) ]) \
+ rawData[(k + iIntep) * m + (int) (delay) ] );
}
z += deltaZ;
x += deltaX;
}
}
return;
}
void cmex_module(int nlhs, mxArray *plhs[], /**< () */
int nrhs, const mxArray *prhs[] ) /**< (modulename) */
{
if (nrhs>0)
{
char fname[1024];
MODULE *mod;
if (!mxIsChar(prhs[0]))
output_error("Module name is not a string");
else if (nlhs>1)
output_error("Only one return value is possible");
else if (mxGetString(prhs[0],fname,sizeof(fname))!=0)
output_error("Module name too long");
else if ((mod=module_find(fname))==NULL && (mod=module_load(fname,0,NULL))==NULL)
output_error("Module load failed");
else if (nlhs=0)
output_message("Module '%s(%d.%d)' loaded ok", mod->name, mod->major, mod->minor);
else
{
/* set the standard info */
char *fnames[256] = {"handle","name","major","minor"};
mxArray *name = mxCreateString(mod->name);
mxArray *handle = mxCreateNumericMatrix(1,1,sizeof(int32)==sizeof(int)?mxUINT32_CLASS:mxUINT64_CLASS,mxREAL);
mxArray *major = mxCreateNumericMatrix(1,1,mxUINT8_CLASS,mxREAL);
mxArray *minor = mxCreateNumericMatrix(1,1,mxUINT8_CLASS,mxREAL);
mxArray *value[256];
unsigned int64 *pHandle = mxGetData(handle);
unsigned char *pMajor = mxGetData(major);
unsigned char *pMinor = mxGetData(minor);
char32 varname="";
int nFields=4;
char vnames[256][32];
*pHandle = (unsigned int64)mod->hLib;
*pMajor = (unsigned char)mod->major;
*pMinor = (unsigned char)mod->minor;
/* get the module data */
while (module_getvar(mod,varname,NULL,0))
{
char32 buffer;
if (module_getvar(mod,varname,buffer,sizeof(buffer)) && nFields<sizeof(fname)/sizeof(fname[0]))
{
double *pVal;
output_verbose("module variable %s = '%s'", varname, buffer);
value[nFields] = mxCreateDoubleMatrix(1,1,mxREAL);
pVal = mxGetPr(value[nFields]);
*pVal = atof(buffer);
strcpy(vnames[nFields],varname);
fnames[nFields] = vnames[nFields];
nFields++;
}
}
/* construct the result */
plhs[0] = mxCreateStructMatrix(1,1,nFields,fnames);
mxSetFieldByNumber(plhs[0],0,0,handle);
mxSetFieldByNumber(plhs[0],0,1,name);
mxSetFieldByNumber(plhs[0],0,2,major);
mxSetFieldByNumber(plhs[0],0,3,minor);
while (nFields-->4)
mxSetFieldByNumber(plhs[0],0,nFields,value[nFields]);
}
}
else
output_error("Module not specified");
return;
}
void mexFunction( int nlhs, mxArray *plhs[],
int nrhs, const mxArray*prhs[] )
{
LSMLIB_REAL *phi_x_plus, *phi_x_minus;
int ilo_grad_phi_gb, ihi_grad_phi_gb;
LSMLIB_REAL *phi;
int ilo_phi_gb, ihi_phi_gb;
LSMLIB_REAL *D1;
int ilo_D1_gb, ihi_D1_gb;
int ilo_fb, ihi_fb;
LSMLIB_REAL dx;
int ghostcell_width;
int dim_M, dim_N;
/* Check for proper number of arguments */
if (nrhs != 3) {
mexErrMsgTxt("Three required input arguments.");
} else if (nlhs > 2) {
mexErrMsgTxt("Too many output arguments.");
}
/* Parameter Checks */
dim_M = mxGetM(PHI);
dim_N = mxGetN(PHI);
if ( (dim_M > 1) && (dim_N > 1) ) {
mexErrMsgTxt("phi should be a 1 dimensional array.");
}
/* Check that the inputs have the correct floating-point precision */
#ifdef LSMLIB_DOUBLE_PRECISION
if (!mxIsDouble(PHI)) {
mexErrMsgTxt("Incompatible precision: LSMLIB built for double-precision but phi is single-precision");
}
#else
if (!mxIsSingle(PHI)) {
mexErrMsgTxt("Incompatible precision: LSMLIB built for single-precision but phi is double-precision");
}
#endif
/* Get ghostcell_width */
ghostcell_width = mxGetPr(GHOSTCELL_WIDTH)[0];
/* Get dx */
dx = (LSMLIB_REAL) mxGetPr(DX)[0];
/* Assign pointers for phi */
phi = (LSMLIB_REAL*) mxGetPr(PHI);
/* Get length of phi data */
ilo_phi_gb = 1;
ihi_phi_gb = mxGetM(PHI);
if (1 == ihi_phi_gb) {
/* phi is row matrix */
ihi_phi_gb = mxGetN(PHI);
}
/* Create matrices for plus and minus derivatives */
ilo_grad_phi_gb = ilo_phi_gb;
ihi_grad_phi_gb = ihi_phi_gb;
#ifdef LSMLIB_DOUBLE_PRECISION
PHI_X_PLUS = mxCreateDoubleMatrix(
ihi_grad_phi_gb-ilo_grad_phi_gb+1, 1, mxREAL);
PHI_X_MINUS = mxCreateDoubleMatrix(
ihi_grad_phi_gb-ilo_grad_phi_gb+1, 1, mxREAL);
#else
PHI_X_PLUS = mxCreateNumericMatrix(
ihi_grad_phi_gb-ilo_grad_phi_gb+1, 1, mxSINGLE_CLASS, mxREAL);
PHI_X_MINUS = mxCreateNumericMatrix(
ihi_grad_phi_gb-ilo_grad_phi_gb+1, 1, mxSINGLE_CLASS, mxREAL);
#endif
phi_x_plus = (LSMLIB_REAL*) mxGetPr(PHI_X_PLUS);
phi_x_minus = (LSMLIB_REAL*) mxGetPr(PHI_X_MINUS);
/* Allocate scratch memory for undivided differences */
ilo_D1_gb = ilo_phi_gb;
ihi_D1_gb = ihi_phi_gb;
D1 = (LSMLIB_REAL*) mxMalloc( sizeof(LSMLIB_REAL) * (ihi_D1_gb-ilo_D1_gb+1) );
if (!D1) {
mexErrMsgTxt("Unable to allocate memory for scratch data...aborting....");
}
/* Do the actual computations in a Fortran 77 subroutine */
ilo_fb = ilo_phi_gb+ghostcell_width;
ihi_fb = ihi_phi_gb-ghostcell_width;
LSM1D_HJ_WENO5(
phi_x_plus, &ilo_grad_phi_gb, &ihi_grad_phi_gb,
phi_x_minus, &ilo_grad_phi_gb, &ihi_grad_phi_gb,
phi, &ilo_phi_gb, &ihi_phi_gb,
D1, &ilo_D1_gb, &ihi_D1_gb,
&ilo_fb, &ihi_fb,
&dx);
/* Deallocate scratch memory for undivided differences */
mxFree(D1);
return;
}
/** @brief Driver.
**
** @param nount number of output arguments.
** @param out output arguments.
** @param nin number of input arguments.
** @param in input arguments.
**/
void
mexFunction(int nout, mxArray *out[],
int nin, const mxArray *in[])
{
enum { IN_ID, IN_NEXT, IN_K, IN_X } ;
enum { OUT_SEL } ;
vl_uint32 const * next ;
vl_uint32 * sel ;
vl_uint8 const * id ;
vl_uint8 const * x ;
unsigned int K, i, N, res, last, ndims ;
/* -----------------------------------------------------------------
* Check arguments
* -------------------------------------------------------------- */
if( nin != 4 ) {
mexErrMsgTxt("Four arguments required") ;
} else if (nout > 1) {
mexErrMsgTxt("At most one output argument.") ;
}
if(! mxIsNumeric(in[IN_NEXT])|| mxGetClassID(in[IN_NEXT])!= mxUINT32_CLASS) {
mexErrMsgTxt("NEXT must be UINT32.") ;
}
if(! mxIsNumeric(in[IN_X]) || mxGetClassID(in[IN_X]) != mxUINT8_CLASS) {
mexErrMsgTxt("X must be UINT8") ;
}
if (mxGetM(in[IN_NEXT]) != 1) {
mexErrMsgTxt("NEXT must be a row vector") ;
}
if(! mxIsNumeric(in[IN_ID]) || mxGetClassID(in[IN_ID])!= mxUINT8_CLASS) {
mexErrMsgTxt("ID must be UINT8.") ;
}
ndims = mxGetM(in[IN_ID]) ;
res = mxGetN(in[IN_ID]) ;
if(res != mxGetN(in[IN_NEXT])) {
mexErrMsgTxt("ID, NEXT must have the same number of columns") ;
}
if(ndims != mxGetM(in[IN_X])) {
mexErrMsgTxt("ID and X must havethe same number of rows") ;
}
if(! vlmxIsPlainScalar(in[IN_K])) {
mexErrMsgTxt("K must be a scalar") ;
}
K = (unsigned int) *mxGetPr(in[IN_K]) ;
N = mxGetN(in[IN_X]) ;
id = mxGetData(in[IN_ID]) ;
next = mxGetData(in[IN_NEXT]) ;
x = mxGetData(in[IN_X]) ;
out[OUT_SEL] = mxCreateNumericMatrix
(1, N, mxUINT32_CLASS, mxREAL) ;
sel = mxGetData (out[OUT_SEL]) ;
/* search for last occupied slot */
last = res ;
for (i = 0 ; i < res ; ++i) last = VL_MAX(last, next [i]) ;
/* REMARK: last and next are 1 based */
if (K > res) {
mexErrMsgTxt("K cannot be larger then the size of H") ;
}
if (last > res) {
mexErrMsgTxt("An element of NEXT is greater than the size of the table") ;
}
/* -----------------------------------------------------------------
* Do job
* -------------------------------------------------------------- */
for (i = 0 ; i < N ; ++i) {
/* hash */
unsigned int h1, h2 ;
unsigned int j, p = 0 ;
h1 = fnv_hash(x + i * ndims, ndims) % K ;
h2 = h1 | 0x1 ; /* this needs to be odd */
/* search first free or matching position */
p = h1 % K ;
for (j = 0 ; j < K ; ++j) {
if (is_null (id + p * ndims, ndims) ||
is_equal(id + p * ndims, x + i * ndims, ndims)) break ;
h1 += h2 ;
p = h1 % K ;
}
/* handle extended table */
while (! is_null (id + p * ndims, ndims) &&
! is_equal(id + p * ndims, x + i * ndims, ndims)) {
p = next [p] - 1 ;
}
/* found or not ? */
if (is_equal(id + p * ndims, x + i * ndims, ndims)) {
/* found */
*sel++ = p + 1 ;
} else {
/* not found */
*sel++ = 0 ;
}
} /* next guy to search for */
}
mxArray * create_numeric_array(mwSize size, mxClassID classid) {
return mxCreateNumericMatrix(1, size, classid, 0); // 0 = not complex
}
/* entrance point for mex file */
void mexFunction(
int nlhs, // Number of left hand side (output) arguments
mxArray *plhs[], // Array of left hand side arguments
int nrhs, // Number of right hand side (input) arguments
const mxArray *prhs[] // Array of right hand side arguments
)
{
/* no inputs - one output : give data regarding the class */
if ( nlhs == 1 && nrhs == 0 ) {
plhs[0] = mxCreateNumericMatrix(1, 1, mxCOORD_CLASS, mxREAL);
ANNcoord * tmp = (ANNcoord*)mxGetData(plhs[0]);
*tmp = 0;
return;
}
/* regular modes */
if ( nrhs < 2 )
mexErrMsgIdAndTxt("annmex:inputs","too few input arguments");
/* first argument - mode of operation */
ann_mex_mode_t mode;
int tmpi(0);
int * iptr = NULL;
GetScalar(prhs[0], tmpi);
mode = (ann_mex_mode_t)tmpi;
/* special treatment for opening */
if ( mode == OPEN ) {
if ( nlhs != 1 )
mexErrMsgIdAndTxt("annmex:open","wrong number of outputs - must be one");
ann_mex_t * annp = new ann_mex_t(prhs[1]);
/* convert the pointer and return it as pointer_type */
plhs[0] = mxCreateNumericMatrix(1, 1, mxPOINTER_CLASS, mxREAL);
pointer_t * ptr = (pointer_t*)mxGetData(plhs[0]);
*ptr = (pointer_t)annp;
return;
}
/* for all pther modes - second argument is the ann_mex_t pointer */
/* extract pointer */
pointer_t * ptr = (pointer_t*)mxGetData(prhs[1]);
ann_mex_t * annp = (ann_mex_t*)(*ptr);
if ( ! annp->IsGood() )
mexErrMsgIdAndTxt("annmex:pointer","internal data structure is faulty - possible wrong handle");
if ( mode == CLOSE ) {
delete annp;
return;
}
/* for the search operations - extract the parameters */
if ( nrhs < 3 )
mexErrMsgIdAndTxt("annmex:inputs","must have at least 3 inputs");
int k = 1;
if ( nrhs >= 4 )
GetScalar(prhs[3], k);
double eps = 0.0;
if ( nrhs >= 5 )
GetScalar(prhs[4], eps);
ANNdist r2 = 1;
if ( mode == FRSEARCH ) {
if ( nrhs == 6 )
GetScalar(prhs[5], r2);
r2 = r2*r2; // input r is _not_ squared
}
switch (mode) {
case KSEARCH:
if ( nlhs != 2 )
mexErrMsgIdAndTxt("annmex:outputs","must have two output arguments");
annp->annkSearch(prhs[2], k, &plhs[0], &plhs[1], eps);
return;
case PRISEARCH:
if ( nlhs != 2 )
mexErrMsgIdAndTxt("annmex:outputs","must have two output arguments");
annp->annkPriSearch(prhs[2], k, &plhs[0], &plhs[1], eps);
return;
case FRSEARCH:
if ( nlhs != 3 )
mexErrMsgIdAndTxt("annmex:outputs","must have three output arguments");
tmpi = annp->annkFRSearch(prhs[2], r2, k, &plhs[0], &plhs[1], eps);
plhs[2] = mxCreateNumericMatrix(1, 1, mxINT32_CLASS, mxREAL);
iptr = (int*)mxGetData(plhs[2]);
*iptr = tmpi;
return;
default:
mexErrMsgIdAndTxt("annmex:mode","unrecognized mode");
}
}
void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
{
if (nrhs < 2) {
mexPrintf("Usage: [indices,coefs] = barycentricInterpolation(bins,pts)\n");
return;
}
double* pts = mxGetPr(prhs[1]);
int i,j;
int m = static_cast<int>(mxGetM(prhs[1])), n=static_cast<int>(mxGetN(prhs[1])), d=m+1;
if (mxGetNumberOfElements(prhs[0])!=m)
mexErrMsgIdAndTxt("Drake:barycentricInterpolation:BadInput","the number of bins must equal the dimension of the points");
int* binsize = new int[m];
double** bins = new double*[m];
mxArray* pbin;
int* nskip = new int[m]; // offsets for inline sub2ind
for (i=0; i<m; i++) {
pbin = mxGetCell(prhs[0],i);
binsize[i] = static_cast<int>(mxGetNumberOfElements(pbin));
bins[i]=mxGetPr(pbin);
if (i==0) nskip[i]=1;
else nskip[i] = nskip[i-1]*binsize[i-1];
}
plhs[0] = mxCreateNumericMatrix(d,n,mxINT64_CLASS,mxREAL);
int64_t* indices = (int64_t*) mxGetData(plhs[0]);
double *coefs, *dcoefs=NULL;
if (nlhs>1) {
plhs[1] = mxCreateDoubleMatrix(d,n,mxREAL);
coefs = mxGetPr(plhs[1]);
if (nlhs>2) {
// todo: use a sparse matrix here instead
plhs[2] = mxCreateDoubleMatrix(d*n,m,mxREAL);
dcoefs = mxGetPr(plhs[2]);
}
} else {
coefs = new double[d*n];
}
bary* b = new bary[d];
for (j=0; j<n; j++) {
int64_t sidx = 1; // index of the top right corner
for (i=0; i<m; i++) {
double pt = pts[i];
double* current_bin = bins[i];
int current_bin_size=binsize[i];
b[i].dim = i;
// truncate back onto the grid if it's off the edge
if (current_bin_size==1) { // support singleton dimensions
// sidx is unchanged
b[i].fracway = 1.0;
} else if (pt>current_bin[current_bin_size-1]) {
sidx += nskip[i]*(current_bin_size-1);
b[i].fracway=1.0;
b[i].dfracway=0.0;
} else if (pt<current_bin[0]) {
sidx += nskip[i];
b[i].fracway=0.0;
b[i].dfracway=0.0;
} else {
// note: could do smarter search here
int next_bin_index=1;
while (pt>current_bin[next_bin_index]) next_bin_index++;
sidx += nskip[i]*next_bin_index;
b[i].fracway = (pt - current_bin[next_bin_index-1])/(current_bin[next_bin_index]-current_bin[next_bin_index-1]);
b[i].dfracway = 1/(current_bin[next_bin_index]-current_bin[next_bin_index-1]);
}
}
// sort dimensions based on fracway (lowest to highest)
qsort(b,m,sizeof(bary),barycomp);
b[m].fracway = 1.0; // final element, to make the loop below cleaner
b[m].dfracway = 0;
b[m].dim = m-1;
// top right corner
indices[0] = sidx;
coefs[0] = b[0].fracway;
if (dcoefs) dcoefs[0 + (d*n)*b[0].dim] = b[0].dfracway;
// move down the box. sorted list of dimensions tells us which order to descend.
for (i=0; i<m; i++) {
if (binsize[b[i].dim]>1) // support singleton dimension
sidx -= nskip[b[i].dim];
indices[i+1] = sidx;
coefs[i+1] = b[i+1].fracway - b[i].fracway;
if (dcoefs) {
dcoefs[i+1 + (d*n)*b[i+1].dim] = b[i+1].dfracway;
dcoefs[i+1 + (d*n)*b[i].dim] = -b[i].dfracway;
}
}
// move pointers ahead to the next point:
pts += m;
indices += d;
coefs += d;
if (dcoefs) dcoefs += d;
}
// clean up
delete[] binsize;
delete[] bins;
delete[] nskip;
delete[] b;
if (nlhs<2) delete[] coefs;
}
void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
{
nTupleVolume<float> *imgVol, *occVol,
*gradXvol, *gradYvol, *normGradXvol, *normGradYvol, *dispField;
int * imgVolSizes, *dispFieldSizes;
float sigmaColour;
float *dispFieldTemp;
int numDimsImgVol, numDimsDispField;
int useAllPatches, reconstructionType;
parameterStruct *patchMatchParams;
mwSize resultSize[4];
if (nrhs < (SIGMA_COLOUR_INPUT+1))
{
mexErrMsgTxt("Error. A minimum of 10 parameters are needed in the spatio-temporal patch match mexfunction.\n");
return;
}
ASSERT ( (mxGetClassID(prhs[IMG_VOL_INPUT]) == mxSINGLE_CLASS) & (mxGetClassID(prhs[OCC_VOL_INPUT]) == mxSINGLE_CLASS)
& (mxGetClassID(prhs[DISP_FIELD_INPUT]) == mxSINGLE_CLASS));
/* Get sizes of variables*/
imgVolSizes = (int*)mxGetDimensions(prhs[IMG_VOL_INPUT]); /* sizes of imgVol*/
numDimsImgVol = (int)mxGetNumberOfDimensions(prhs[IMG_VOL_INPUT]); /*length of sizes of imgVolA*/
dispFieldSizes = (int*)mxGetDimensions(prhs[DISP_FIELD_INPUT]); /*sizes of imgVolB*/
numDimsDispField = (int)mxGetNumberOfDimensions(prhs[DISP_FIELD_INPUT]); /* length of sizes of dispField*/
if ((numDimsImgVol != 4) || (numDimsDispField != 4))
{
mexPrintf("Number of dimensions :\n imgVol : %d\nimgVolFine : %d\nDispField : %d\n",numDimsImgVol,numDimsDispField);
mexErrMsgTxt("Error, the number of array dimensions must be 4 (x,y,t,multivalue).");
plhs[0] = mxCreateNumericMatrix((mwSize)1,(mwSize)1, mxINT32_CLASS, (mxComplexity)0);
dispFieldTemp = (float*)mxGetPr(plhs[0]);
*dispFieldTemp = -1;
return;
}
//get the patchMatch parameters
ASSERT(mxIsStruct(prhs[PATCH_MATCH_PARAMS_INPUT]) );
patchMatchParams = (parameterStruct*)new parameterStruct;
parse_patch_match_parameters(prhs[PATCH_MATCH_PARAMS_INPUT],patchMatchParams);
/* sigma colour*/
sigmaColour = (float)(*mxGetPr(prhs[SIGMA_COLOUR_INPUT]));
if ( (imgVolSizes[2] < patchMatchParams->patchSizeX) || (imgVolSizes[1] < patchMatchParams->patchSizeY) || (imgVolSizes[3] < patchMatchParams->patchSizeT) )
{
MY_PRINTF("Image sizes :\n x : %d, y : %d, t : %d\n",(int)imgVolSizes[2],(int)imgVolSizes[1],(int)imgVolSizes[3]);
MY_PRINTF("Patch sizes :\n x : %d, y : %d, t : %d\n",(int)patchMatchParams->patchSizeX,(int)patchMatchParams->patchSizeY,(int)patchMatchParams->patchSizeT);
mexErrMsgTxt("Error, the patch sizes are too large for the image.");
plhs[0] = mxCreateNumericMatrix((mwSize)1,(mwSize)1, mxINT32_CLASS, (mxComplexity)0);
dispFieldTemp = (float*)mxGetPr(plhs[0]);
*dispFieldTemp = -1;
return;
}
if (nrhs >= (USE_ALL_PATCHES_INPUT+1))
useAllPatches = (int)(*mxGetPr(prhs[USE_ALL_PATCHES_INPUT])); /* if we want to use all surrounding patches or not*/
else
useAllPatches = 1;
if (nrhs >= (RECONSTRUCTION_TYPE_INPUT+1))
reconstructionType = (int)(*mxGetPr(prhs[RECONSTRUCTION_TYPE_INPUT])); /* the manner in which we want to reconstruct the image*/
else
reconstructionType = 0;
int imageIndexing = 1;
//input image volumes
imgVol = new nTupleVolume<float>(3, (int)imgVolSizes[2], (int)imgVolSizes[1], (int)imgVolSizes[3],
patchMatchParams->patchSizeX, patchMatchParams->patchSizeY, patchMatchParams->patchSizeT,imageIndexing,
(float*)mxGetPr(prhs[IMG_VOL_INPUT]));
occVol = new nTupleVolume<float>(1, (int)imgVolSizes[2], (int)imgVolSizes[1], (int)imgVolSizes[3],
patchMatchParams->patchSizeX, patchMatchParams->patchSizeY, patchMatchParams->patchSizeT,imageIndexing,
(float*)mxGetPr(prhs[OCC_VOL_INPUT]));
//shift volume
dispField = new nTupleVolume<float>(4, (int)imgVolSizes[2], (int)imgVolSizes[1], (int)imgVolSizes[3],
patchMatchParams->patchSizeX, patchMatchParams->patchSizeY, patchMatchParams->patchSizeT,imageIndexing,
(float*)mxGetPr(prhs[DISP_FIELD_INPUT]));
//features
gradXvol = new nTupleVolume<float>(1, (int)imgVolSizes[2], (int)imgVolSizes[1], (int)imgVolSizes[3],
patchMatchParams->patchSizeX, patchMatchParams->patchSizeY, patchMatchParams->patchSizeT,imageIndexing,
(float*)patchMatchParams->gradX);
gradYvol = new nTupleVolume<float>(1, (int)imgVolSizes[2], (int)imgVolSizes[1], (int)imgVolSizes[3],
patchMatchParams->patchSizeX, patchMatchParams->patchSizeY, patchMatchParams->patchSizeT,imageIndexing,
(float*)patchMatchParams->gradY);
normGradXvol = new nTupleVolume<float>(1, (int)imgVolSizes[2], (int)imgVolSizes[1], (int)imgVolSizes[3],
patchMatchParams->patchSizeX, patchMatchParams->patchSizeY, patchMatchParams->patchSizeT,imageIndexing,
(float*)patchMatchParams->normGradX);
normGradYvol = new nTupleVolume<float>(1, (int)imgVolSizes[2], (int)imgVolSizes[1], (int)imgVolSizes[3],
patchMatchParams->patchSizeX, patchMatchParams->patchSizeY, patchMatchParams->patchSizeT,imageIndexing,
(float*)patchMatchParams->normGradY);
/*colour estimation*/
reconstruct_videos(imgVol, occVol,
gradXvol, gradYvol, normGradXvol, normGradYvol,
dispField, (float)sigmaColour, useAllPatches, reconstructionType);
/*OUTPUT*/
/* Create output matrix*/
resultSize[0] = (int)(imgVolSizes[0]);
resultSize[1] = (int)(imgVolSizes[1]);
resultSize[2] = (int)(imgVolSizes[2]);
resultSize[3] = (int)(imgVolSizes[3]);
plhs[0] = mxCreateNumericArray((mwSize)4,(mwSize*)resultSize, mxSINGLE_CLASS, (mxComplexity)0);
float *imgVolOutTemp = (float*)mxGetPr(plhs[0]);
memcpy(imgVolOutTemp,imgVol->get_value_ptr( 0,0,0,0), resultSize[0]*resultSize[1]*resultSize[2]*resultSize[3]*sizeof(float));
//create other output matrices
resultSize[0] = (int)(imgVolSizes[1]);
resultSize[1] = (int)(imgVolSizes[2]);
resultSize[2] = (int)(imgVolSizes[3]);
plhs[1] = mxCreateNumericArray((mwSize)3,(mwSize*)resultSize, mxSINGLE_CLASS, (mxComplexity)0);
float* gradXoutTemp = (float*)mxGetPr(plhs[1]);
memcpy(gradXoutTemp,gradXvol->get_value_ptr( 0,0,0,0), resultSize[0]*resultSize[1]*resultSize[2]*sizeof(float));
plhs[2] = mxCreateNumericArray((mwSize)3,(mwSize*)resultSize, mxSINGLE_CLASS, (mxComplexity)0);
float* gradYoutTemp = (float*)mxGetPr(plhs[2]);
memcpy(gradYoutTemp,gradYvol->get_value_ptr( 0,0,0,0), resultSize[0]*resultSize[1]*resultSize[2]*sizeof(float));
plhs[3] = mxCreateNumericArray((mwSize)3,(mwSize*)resultSize, mxSINGLE_CLASS, (mxComplexity)0);
float* normGradXoutTemp = (float*)mxGetPr(plhs[3]);
memcpy(normGradXoutTemp,normGradXvol->get_value_ptr( 0,0,0,0), resultSize[0]*resultSize[1]*resultSize[2]*sizeof(float));
plhs[4] = mxCreateNumericArray((mwSize)3,(mwSize*)resultSize, mxSINGLE_CLASS, (mxComplexity)0);
float* normGradYoutTemp = (float*)mxGetPr(plhs[4]);
memcpy(normGradYoutTemp,normGradYvol->get_value_ptr( 0,0,0,0), resultSize[0]*resultSize[1]*resultSize[2]*sizeof(float));
delete imgVol;
delete gradXvol;
delete gradYvol;
delete normGradXvol;
delete normGradYvol;
delete patchMatchParams;
return;
}
// Function definitions.
// -----------------------------------------------------------------
void mexFunction (int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
{
//Input Args
double *x0;
//Outputs Args
double *x, *fval, *exitflag, *iter, *feval;
//Internal Vars
size_t ndec;
int printLevel = 0;
//M1QN3 Vars
int m = 5, n, indic, ndz;
int uiparm[2] = {1,0}; //user integer array [citer, printLevel]
double *g, dxmin = 1e-8, df1, epsg = 1e-6, *dz;
char *normtype = "dfn";
int impres = 4, io = 18, omode = 0, reverse = 0;
int imode[3] = {0,0,0}; //DIS, cold start, no SIMUL with indic = 1
int iz[5];
float *rzs = NULL; double *dzs = NULL; //dummy args
//Defaults
int maxfev = 1500;
int maxiter = 1000;
maxtime = 1000;
iterF.enabled = false;
if (nrhs < 1) {
if(nlhs < 1)
mexPrintf("\nThis is M1QN3 v%s MEX Interface\n",M1QN3_VERSION);
else
plhs[0] = mxCreateString(M1QN3_VERSION);
return;
}
//Check user inputs
checkInputs(prhs,nrhs);
//Get Sizes
ndec = mxGetNumberOfElements(pX0);
//Get Objective Function Handle
if (mxIsChar(pFUN)) {
CHECK(mxGetString(pFUN, fun.f, FLEN) == 0,"error reading objective name string");
fun.nrhs = 1;
fun.xrhs = 0;
} else {
fun.prhs[0] = (mxArray*)pFUN;
strcpy(fun.f, "feval");
fun.nrhs = 2;
fun.xrhs = 1;
}
fun.prhs[fun.xrhs] = mxCreateDoubleMatrix(ndec, 1, mxREAL); //x0
//Get Gradient Function Handle
if (mxIsChar(pGRAD)) {
CHECK(mxGetString(pGRAD, fun.g, FLEN) == 0,"error reading gradient name string");
fun.nrhs_g = 1;
fun.xrhs_g = 0;
} else {
fun.prhs_g[0] = (mxArray*)pGRAD;
strcpy(fun.g, "feval");
fun.nrhs_g = 2;
fun.xrhs_g = 1;
}
fun.prhs_g[fun.xrhs_g] = mxCreateDoubleMatrix(ndec, 1, mxREAL); //x0
//Get x0
x0 = mxGetPr(pX0);
//Get Options if specified
if(nrhs > eOPTS) {
if(mxGetField(pOPTS,0,"display"))
printLevel = (int)*mxGetPr(mxGetField(pOPTS,0,"display"));
if(mxGetField(pOPTS,0,"maxfeval"))
maxfev = (int)*mxGetPr(mxGetField(pOPTS,0,"maxfeval"));
if(mxGetField(pOPTS,0,"maxiter"))
maxiter = (int)*mxGetPr(mxGetField(pOPTS,0,"maxiter"));
if(mxGetField(pOPTS,0,"maxtime"))
maxtime = *mxGetPr(mxGetField(pOPTS,0,"maxtime"));
if(mxGetField(pOPTS,0,"tolafun"))
epsg = *mxGetPr(mxGetField(pOPTS,0,"tolafun")); //not function tolerance (gradient)
if(mxGetField(pOPTS,0,"nupdates"))
m = (int)*mxGetPr(mxGetField(pOPTS,0,"nupdates")); //number of l-bfgs updates
if(mxGetField(pOPTS,0,"iterfun") && !mxIsEmpty(mxGetField(pOPTS,0,"iterfun")))
{
iterF.prhs[0] = (mxArray*)mxGetField(pOPTS,0,"iterfun");
strcpy(iterF.f, "feval");
iterF.enabled = true;
iterF.prhs[1] = mxCreateNumericMatrix(1,1,mxINT32_CLASS,mxREAL);
iterF.prhs[2] = mxCreateDoubleMatrix(1,1,mxREAL);
iterF.prhs[3] = mxCreateDoubleMatrix(ndec,1,mxREAL);
}
}
//Create Outputs
plhs[0] = mxCreateDoubleMatrix(ndec,1, mxREAL);
plhs[1] = mxCreateDoubleMatrix(1,1, mxREAL);
plhs[2] = mxCreateDoubleMatrix(1,1, mxREAL);
plhs[3] = mxCreateDoubleMatrix(1,1, mxREAL);
plhs[4] = mxCreateDoubleMatrix(1,1, mxREAL);
x = mxGetPr(plhs[0]);
fval = mxGetPr(plhs[1]);
exitflag = mxGetPr(plhs[2]);
iter = mxGetPr(plhs[3]);
feval = mxGetPr(plhs[4]);
//Copy initial guess to x
memcpy(x,x0,ndec*sizeof(double));
//Print Header
if(printLevel) {
mexPrintf("\n------------------------------------------------------------------\n");
mexPrintf(" This is M1QN3 v%s\n",M1QN3_VERSION);
mexPrintf(" Authors: Jean Charles Gilbert, Claude Lemarechal, INRIA\n MEX Interface J. Currie 2012\n\n");
mexPrintf(" Problem Properties:\n");
mexPrintf(" # Decision Variables: %4d\n",ndec);
mexPrintf("------------------------------------------------------------------\n");
}
//Assign Arguments
n = (int)ndec;
indic = 4;
g = (double*)mxCalloc(n,sizeof(double)); //allocate memory for gradient
ndz = 4*n + m*(2*n + 1);
dz = (double*)mxCalloc(ndz,sizeof(double));
//Start timer
start = clock();
//Initialization Call
SIMUL(&indic, &n, x, fval, g, uiparm, NULL, NULL);
//Set df1 (initial estiamte of f reduction)
df1 = *fval;
//MEX Options
uiparm[0] = 1;
uiparm[1] = printLevel;
// FILE* myFile = fopen("myFile.txt","w");
// fprintf(myFile,"Hello world!\n");
// fclose(myFile);
//Call Algorithm
M1QN3(SIMUL,EUCLID,CTONBE,CTCABE,&n,x,fval,g,&dxmin,&df1,&epsg,normtype,
&impres,&io,imode,&omode,&maxiter,&maxfev,iz,dz,&ndz,&reverse,&indic,
uiparm,rzs,dzs);
//Save Status & Iterations
*exitflag = (double)omode;
*iter = maxiter;
*feval = maxfev;
//Check if maxtime exceeded
if(((double)(end-start))/CLOCKS_PER_SEC > maxtime)
*exitflag = 8;
//Print Header
if(printLevel){
//Termination Detected
switch((int)*exitflag)
{
//Success
case 1:
mexPrintf("\n *** SUCCESSFUL TERMINATION ***\n *** gradient convergence |gk|/|g1| < epsg ***\n"); break;
//Error
case 5:
mexPrintf("\n *** MAXIMUM FUNCTION EVALUATIONS REACHED ***\n"); break;
case 4:
mexPrintf("\n *** MAXIMUM ITERATIONS REACHED ***\n"); break;
case 8:
mexPrintf("\n *** MAXIMUM TIME REACHED ***\n"); break;
case 2:
mexPrintf("\n *** ERROR: one of the input arguments is not well initialized ***\n"); break;
case 3:
mexPrintf("\n *** ERROR: the line-search is blocked on tmax = 10^20 ***\n"); break;
case 6:
mexPrintf("\n *** ERROR: stop dxmin during the line-search ***\n"); break;
case 7:
mexPrintf("\n *** ERROR: either (g,d) is nonnegative or (y,s) is nonpositive ***\n"); break;
//Early Exit
case 0:
mexPrintf("\n *** TERMINATION: USER EXITED ***\n"); break;
//Other Error
default:
mexPrintf("\n *** ERROR: internal error code %d ***\n",omode); break;
}
if(*exitflag==1)
mexPrintf("\n Final fval: %12.5g\n In %3.0f iterations\n",*fval,*iter);
mexPrintf("------------------------------------------------------------------\n\n");
}
//Free Memory
mxFree(g);
mxFree(dz);
}
static mxArray *get_object_data(OBJECT *obj)
{
mxArray *plhs[1];
/* set the standard info */
#define ERROR "(error)"
#define NONE "(none)"
char *fnames[1024] = {"id","class","parent","rank","clock","latitude","longitude","in_svc","out_svc","flags",NULL}; // };
int nFields = 0;
int nData = 0;
char value[1024];
PROPERTY *prop;
mxArray *pId = mxCreateString(convert_from_object(value,sizeof(value),&obj,NULL)?value:ERROR);
mxArray *pClass = mxCreateString(obj->oclass->name);
mxArray *pParent = mxCreateString(obj->parent!=NULL&&convert_from_object(value,sizeof(value),&(obj->parent),NULL)?value:NONE);
mxArray *pRank = mxCreateNumericMatrix(1,1,mxUINT32_CLASS,mxREAL);
mxArray *pClock = mxCreateString(convert_from_timestamp(obj->clock,value,sizeof(value))?value:ERROR);
mxArray *pLatitude = mxCreateString(convert_from_latitude(obj->latitude,value,sizeof(value))?value:NONE);
mxArray *pLongitude = mxCreateString(convert_from_longitude(obj->longitude,value,sizeof(value))?value:NONE);
mxArray *pInSvc = mxCreateString(convert_from_timestamp(obj->in_svc,value,sizeof(value))?value:ERROR);
mxArray *pOutSvc = mxCreateString(convert_from_timestamp(obj->out_svc,value,sizeof(value))?value:ERROR);
mxArray *pFlags = mxCreateString(convert_from_set(value,sizeof(value),(void*)&obj->flags,object_flag_property())?value:ERROR);
*(OBJECTRANK*)mxGetPr(pRank) = obj->rank;
/* count the number of header items */
while (fnames[nFields]!=NULL) nFields++;
/* count the number of object properties and assign the field names */
for (prop=class_get_first_property(obj->oclass); prop!=NULL; prop=class_get_next_property(prop))
/** @todo don't damage the original fieldname when making it safe for Matlab */
fnames[nFields+nData++] = make_fieldname(prop->name);
/* construct the return value */
plhs[0] = mxCreateStructMatrix(1,1,nFields+nData,fnames);
/* construct the header fields */
mxSetFieldByNumber(plhs[0],0,0,pId);
mxSetFieldByNumber(plhs[0],0,1,pClass);
mxSetFieldByNumber(plhs[0],0,2,pParent);
mxSetFieldByNumber(plhs[0],0,3,pRank);
mxSetFieldByNumber(plhs[0],0,4,pClock);
mxSetFieldByNumber(plhs[0],0,5,pLatitude);
mxSetFieldByNumber(plhs[0],0,6,pLongitude);
mxSetFieldByNumber(plhs[0],0,7,pInSvc);
mxSetFieldByNumber(plhs[0],0,8,pOutSvc);
mxSetFieldByNumber(plhs[0],0,9,pFlags);
/* construct the data fields */
for (prop=class_get_first_property(obj->oclass); prop!=NULL; nFields++,prop=class_get_next_property(prop))
{
mxArray *pValue;
if (prop->ptype==PT_double)
{
pValue = mxCreateDoubleMatrix(1,1,mxREAL);
*(double*)mxGetPr(pValue) = *object_get_double(obj,prop);
}
else if (prop->ptype==PT_int32)
{
pValue = mxCreateDoubleMatrix(1,1,mxREAL);
*(double*)mxGetPr(pValue) = (double)*object_get_int32(obj,prop);
}
else if (prop->ptype==PT_complex)
{
complex *pData = object_get_complex(obj,prop);
pValue = mxCreateDoubleMatrix(1,1,mxCOMPLEX);
*(double*)mxGetPr(pValue) = pData->r;
*(double*)mxGetPi(pValue) = pData->i;
}
else
{
pValue = mxCreateString(object_get_value_by_name(obj,prop->name,value,sizeof(value))?value:ERROR);
}
mxSetFieldByNumber(plhs[0],0,nFields,pValue);
}
return plhs[0];
}
void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]) {
//check inputs
if (nrhs != 12 || (nlhs != 1 && nlhs != 0)) {
mexErrMsgIdAndTxt("CPROB:BadNArgs",
"Need 12 inputs and 1 output.");
}
//gets size of variables
const int* length = mxGetDimensions(prhs[2]);
const size_t n = length[1];
//get value of input variables
double * RIn = mxGetPr(prhs[0]);
double * vRIn = mxGetPr(prhs[1]);
double * tAIn = mxGetPr(prhs[2]);
double * vtAIn = mxGetPr(prhs[3]);
double * RAIn = mxGetPr(prhs[4]);
double * vRAIn = mxGetPr(prhs[5]);
double * tBIn = mxGetPr(prhs[6]);
double * vtBIn = mxGetPr(prhs[7]);
double * RBIn = mxGetPr(prhs[8]);
double * vRBIn = mxGetPr(prhs[9]);
double * t = mxGetPr(prhs[10]);
double * s = mxGetPr(prhs[11]);
//setup outputs
plhs[0] = mxCreateNumericMatrix(n, 1, mxDOUBLE_CLASS, mxREAL);
double *logl = mxGetPr(plhs[0]);
for(size_t i = 0; i < n; i++){
double* tA = &tAIn[3*i];
double* tB = &tBIn[3*i];
double* RA = &RAIn[3*i];
double* RB = &RBIn[3*i];
double* vtA = &vtAIn[3*i];
double* vtB = &vtBIn[3*i];
double* vRA = &vRAIn[3*i];
double* vRB = &vRBIn[3*i];
double* Rs = &RIn[0];
double* Re = &RIn[3];
double* vRs = &vRIn[0];
double* vRe = &vRIn[3];
double stB[3];
double svtB[3];
double err[6];
double verr[6];
if(s[0] != 0){
combineScaleB(stB,svtB,tA,vtA,tB,vtB,RA,vRA,RB,vRB,Rs,vRs,Re,vRe,t);
findErrVar(err,verr,tA,vtA,stB,svtB,RA,vRA,RB,vRB,Rs,vRs,Re,vRe,t);
}
else{
findErrVar(err,verr,tA,vtA,tB,vtB,RA,vRA,RB,vRB,Rs,vRs,Re,vRe,t);
}
/*err[0] = err[0] + err[3];
err[1] = err[1] + err[4];
err[2] = err[2] + err[5];
verr[0] = verr[0] + verr[3];
verr[1] = verr[1] + verr[4];
verr[2] = verr[2] + verr[5];
//find exponential exponent
double eExp = -0.5*(err[0]*err[0]/verr[0] + err[1]*err[1]/verr[1] + err[2]*err[2]/verr[2]);
//find part before exponential
double bExp = -log(sqrt(8*M_PI*M_PI*M_PI*verr[0]*verr[1]*verr[2]));
//finding log likelihood
logl[i] = bExp + eExp;*/
//find exponential exponent
double eExp1 = -0.5*(err[0]*err[0]/verr[0] + err[1]*err[1]/verr[1] + err[2]*err[2]/verr[2]);
//find part before exponential
double bExp1 = -log(sqrt(8*M_PI*M_PI*M_PI*verr[0]*verr[1]*verr[2]));
//find exponential exponent
double eExp2 = -0.5*(err[3]*err[3]/verr[3] + err[4]*err[4]/verr[4] + err[5]*err[5]/verr[5]);
//find part before exponential
double bExp2 = -log(sqrt(8*M_PI*M_PI*M_PI*verr[3]*verr[4]*verr[5]));
//finding log likelihood
logl[i] = (bExp1 + eExp1 + bExp2 + eExp2)/2;
//logl[i] = (bExp1 + eExp1);
}
}
void mexFunction(
int nargout,
mxArray *out[],
int nargin,
const mxArray *in[]
)
{
/* declare variables */
int nr, nc, np, nb, total;
double *dim, sample_rate;
int r_i, r_j, a1, a2, b1, b2, self, neighbor;
int i, j, k, s, t, nsamp, th_rand, no_sample;
unsigned long *p;
/* check argument */
if (nargin < 2) {
mexErrMsgTxt("Two input arguments required");
}
if (nargout> 2) {
mexErrMsgTxt("Too many output arguments.");
}
/* get image size */
i = mxGetM(in[0]);
j = mxGetN(in[0]);
dim = (double *)mxGetData(in[0]);
nr = (int)dim[0];
if (j>1 || i>1) {
nc = (int)dim[1];
} else {
nc = nr;
}
np = nr * nc;
/* get neighbourhood size */
i = mxGetM(in[1]);
j = mxGetN(in[1]);
dim = (double*)mxGetData(in[1]);
r_i = (int)dim[0];
if (j>1 || i>1) {
r_j = (int)dim[1];
} else {
r_j = r_i;
}
if (r_i<0) { r_i = 0; }
if (r_j<0) { r_j = 0; }
/* get sample rate */
if (nargin==3) {
sample_rate = (mxGetM(in[2])==0) ? 1: mxGetScalar(in[2]);
} else {
sample_rate = 1;
}
/* prepare for random number generator */
if (sample_rate<1) {
srand( (unsigned)time( NULL ) );
th_rand = (int)ceil((double)RAND_MAX * sample_rate);
no_sample = 0;
} else {
sample_rate = 1;
th_rand = RAND_MAX;
no_sample = 1;
}
/* figure out neighbourhood size */
nb = (r_i + r_i + 1) * (r_j + r_j + 1);
if (nb>np) {
nb = np;
}
nb = (int)ceil((double)nb * sample_rate);
/* intermediate data structure */
p = (unsigned long *)mxCalloc(np * (nb+1), sizeof(unsigned long));
if (p==NULL) {
mexErrMsgTxt("Not enough space for my computation.");
}
/* computation */
total = 0;
for (j=0; j<nc; j++) {
for (i=0; i<nr; i++) {
self = i + j * nr;
/* put self in, otherwise the index is not ordered */
p[self] = p[self] + 1;
p[self+p[self]*np] = self;
/* j range */
b1 = j;
b2 = j + r_j;
if (b2>=nc) { b2 = nc-1; }
/* i range */
a1 = i - r_i;
if (a1<0) { a1 = 0; }
a2 = i + r_i;
if (a2>=nr) { a2 = nr-1; }
/* number of more samples needed */
nsamp = nb - p[self];
k = 0;
t = b1;
s = i + 1;
if (s>a2) {
s = a1;
t = t + 1;
}
while (k<nsamp && t<=b2) {
if (no_sample || (rand()<th_rand)) {
k = k + 1;
neighbor = s + t * nr;
p[self] = p[self] + 1;
p[self+p[self]*np] = neighbor;
p[neighbor] = p[neighbor] + 1;
p[neighbor+p[neighbor]*np] = self;
}
s = s + 1;
if (s>a2) {
s = a1;
t = t + 1;
}
} /* k */
total = total + p[self];
} /* i */
} /* j */
/* i, j */
out[0] = mxCreateNumericMatrix(total, 1, mxUINT32_CLASS, mxREAL);
out[1] = mxCreateNumericMatrix(np+1, 1, mxUINT32_CLASS, mxREAL);
unsigned int *qi = (unsigned int *)mxGetData(out[0]);
unsigned int *qj = (unsigned int *)mxGetData(out[1]);
if (out[0]==NULL || out[1]==NULL) {
mexErrMsgTxt("Not enough space for the output matrix.");
}
total = 0;
for (j=0; j<np; j++) {
qj[j] = total;
s = j + np;
for (t=0; t<p[j]; t++) {
qi[total] = p[s];
total = total + 1;
s = s + np;
}
}
qj[np] = total;
mxFree(p);
}
/* The main mex function */
void mexFunction(int nlhs, mxArray *plhs[],
int nrhs, const mxArray *prhs[])
{
char *action;
/* Check for an input */
if (nrhs == 0)
mexErrMsgIdAndTxt("libusb:libusb", "No input! What shall I do?!");
/* Check for a string */
if (!mxIsChar(prhs[0]))
mexErrMsgIdAndTxt("libusb:libusb", "Expected a string as the first argument");
/* Find out what the action required is */
action = mxArrayToString(prhs[0]);
/* Initialise the USB interface */
if (strcmp(action, "init") == 0) {
/* Check inputs/outputs */
if (nrhs != 1)
mexErrMsgIdAndTxt("libusb:init", "Expected no inputs to init");
if (nlhs != 0)
mexErrMsgIdAndTxt("libusb:init", "Expected no outputs for init");
/* Clear-up as necessary and (re)initialise the library */
clearup();
libusb_init(NULL);
/* Open the USB device */
device = libusb_open_device_with_vid_pid(NULL, 0x0123, 0x4567);
if (device == NULL) {
libusb_exit(NULL);
mexErrMsgIdAndTxt("libusb:init", "Unable to open USB device - is it plugged in?");
}
else {
/* Claim the USB interface */
if (libusb_claim_interface(device, 0) < 0) {
libusb_close(device);
libusb_exit(NULL);
device = NULL;
mexErrMsgIdAndTxt("libusb:init", "Failed to claim the USB device - is something else using it?");
}
else
mexAtExit(clearup);
}
}
/* Write to the USB interface */
else if (strcmp(action, "write") == 0) {
int sent, ret;
/* Check inputs/outputs */
if (nrhs != 2)
mexErrMsgIdAndTxt("libusb:write", "Expected one input to write (the data)");
if (nlhs > 1)
mexErrMsgIdAndTxt("libusb:write", "Expected one output for write (the size of the data written)");
if (!mxIsUint8(prhs[1]))
mexErrMsgIdAndTxt("libusb:write", "Expected the input data to be a uint8 array");
/* Check that device isn't NULL */
if (device == NULL)
mexErrMsgIdAndTxt("libusb:write", "libusb must be initialised first!");
/* Do the data transfer */
ret = libusb_bulk_transfer(device, 1, (unsigned char *)mxGetData(prhs[1]), (int)mxGetNumberOfElements(prhs[1]), &sent, timeout);
if (ret != 0)
mexErrMsgIdAndTxt("libusb:write", "Error in bulk transfer - %d", ret);
/* Return the number of bytes sent */
plhs[0] = mxCreateDoubleScalar(sent);
}
else if (strcmp(action, "read") == 0) {
int received, ret;
/* Check inputs/outputs */
if (nrhs != 2)
mexErrMsgIdAndTxt("libusb:read", "Expected one input to read (the data size)");
if (nlhs > 1)
mexErrMsgIdAndTxt("libusb:read", "Expected one output for read (the data read)");
if (mxGetNumberOfElements(prhs[1]) != 1)
mexErrMsgIdAndTxt("libusb:read", "Expected the input data to be a scalar");
/* Check that device isn't NULL */
if (device == NULL)
mexErrMsgIdAndTxt("libusb:read", "libusb must be initialised first!");
/* Create the return data structure */
plhs[0] = mxCreateNumericMatrix(1, (int)mxGetScalar(prhs[1]), mxUINT8_CLASS, mxREAL);
/* Do the data transfer */
ret = libusb_bulk_transfer(device, 129, (unsigned char *)mxGetData(plhs[0]), (int)mxGetNumberOfElements(plhs[0]), &received, timeout);
if (ret != 0)
mexErrMsgIdAndTxt("libusb:read", "Error in bulk transfer - %d", ret);
/* Update the size of the return data structure */
mxSetN(plhs[0], received);
}
else if (strcmp(action, "exit") == 0) {
/* Check inputs/outputs */
if (nrhs != 1)
mexErrMsgIdAndTxt("libusb:exit", "Expected no inputs to exit");
if (nlhs != 0)
mexErrMsgIdAndTxt("libusb:exit", "Expected no outputs for exit");
/* Clear-up */
clearup();
}
else {
mexErrMsgIdAndTxt("libusb:libusb", "Unknown command; available commands are: init, write, read and exit");
}
}
// [states] = grante_sample(model, fg, method, sample_count, options);
void mexFunction(int nlhs, mxArray* plhs[], int nrhs, const mxArray* prhs[]) {
// Option structure
const mxArray* opt_s = 0;
if (nrhs >= 4 && mxIsEmpty(prhs[4]) == false)
opt_s = prhs[4];
MatlabCPPInitialize(GetScalarDefaultOption(opt_s, "verbose", 0) > 0);
if (nrhs < 4 || nrhs > 5 || nlhs != 1) {
mexErrMsgTxt("Wrong number of arguments.\n");
MatlabCPPExit();
return;
}
// Master model
Grante::FactorGraphModel model;
if (matlab_parse_factorgraphmodel(prhs[0], model) == false) {
MatlabCPPExit();
return;
}
// Parse factor graph
std::vector<Grante::FactorGraph*> FG;
bool fgs_parsed = matlab_parse_factorgraphs(model, prhs[1], FG);
if (fgs_parsed == false) {
MatlabCPPExit();
return;
}
size_t num_fgs = FG.size();
if (num_fgs != 1) {
mexErrMsgTxt("mex_grante_sample supports only one "
"factor graph at a time.\n");
for (unsigned int fgi = 0; fgi < FG.size(); ++fgi)
delete (FG[fgi]);
MatlabCPPExit();
return;
}
// Compute energies
Grante::FactorGraph* fg = FG[0];
fg->ForwardMap();
// Parse sampling method
std::string method_name = GetMatlabString(prhs[2]);
Grante::InferenceMethod* inf = 0;
if (method_name == "treeinf") {
if (Grante::FactorGraphStructurizer::IsForestStructured(fg) == false) {
mexErrMsgTxt("Exact sampling is currently only "
"possible for tree-structured factor graphs.\n");
MatlabCPPExit();
return;
}
inf = new Grante::TreeInference(fg);
} else if (method_name == "gibbs") {
Grante::GibbsInference* ginf = new Grante::GibbsInference(fg);
ginf->SetSamplingParameters(
GetIntegerDefaultOption(opt_s, "gibbs_burnin", 100),
GetIntegerDefaultOption(opt_s, "gibbs_spacing", 0),
GetIntegerDefaultOption(opt_s, "gibbs_samples", 1));
inf = ginf;
} else if (method_name == "mcgibbs") {
Grante::MultichainGibbsInference* mcginf =
new Grante::MultichainGibbsInference(fg);
mcginf->SetSamplingParameters(
GetIntegerDefaultOption(opt_s, "mcgibbs_chains", 5),
GetScalarDefaultOption(opt_s, "mcgibbs_maxpsrf", 1.01),
GetIntegerDefaultOption(opt_s, "mcgibbs_spacing", 0),
GetIntegerDefaultOption(opt_s, "mcgibbs_samples", 1000));
inf = mcginf;
} else {
mexErrMsgTxt("Unknown sampling method. Use 'treeinf', "
"'gibbs', or 'mcgibbs'.\n");
MatlabCPPExit();
return;
}
// Parse sample_count
if (mxIsDouble(prhs[3]) == false || mxGetNumberOfElements(prhs[3]) != 1) {
mexErrMsgTxt("sample_count must be a (1,1) double array.\n");
MatlabCPPExit();
return;
}
unsigned int sample_count = mxGetScalar(prhs[3]);
assert(sample_count > 0);
// Perform inference
mexPrintf("[Grante] performing sampling using method: '%s'\n",
method_name.c_str());
unsigned int var_count = fg->Cardinalities().size();
plhs[0] = mxCreateNumericMatrix(var_count, sample_count,
mxDOUBLE_CLASS, mxREAL);
double* sample_p = mxGetPr(plhs[0]);
// Sample
std::vector<std::vector<unsigned int> > states;
inf->Sample(states, sample_count);
assert(states.size() == sample_count);
for (unsigned int si = 0; si < states.size(); ++si) {
// Add 1 for Matlab indexing
std::transform(states[si].begin(), states[si].end(),
&sample_p[si * var_count],
std::bind2nd(std::plus<double>(), 1.0));
}
delete (inf);
delete (fg);
MatlabCPPExit();
}
