/**
* @brief Write the option possible_words.
* Writes the option possible_words, converting from certain numerical
* representations to their corresponding strings.
*/
void WriteOptionPossibleWords(mxArray *out,const char *field_name,double member,int flag)
{
if(flag)
switch((int)member) /* use value to set string */
{
case 0:
mxAddAndSetField(out,0,field_name,mxCreateDoubleScalar(mxGetInf()));
break;
case -1:
mxAddAndSetField(out,0,field_name,mxCreateString("recommended"));
break;
case -2:
mxAddAndSetField(out,0,field_name,mxCreateString("unique"));
break;
case -3:
mxAddAndSetField(out,0,field_name,mxCreateString("total"));
break;
case -4:
mxAddAndSetField(out,0,field_name,mxCreateString("possible"));
break;
case -5:
mxAddAndSetField(out,0,field_name,mxCreateString("min_tot_pos"));
break;
case -6:
mxAddAndSetField(out,0,field_name,mxCreateString("min_lim_tot_pos"));
break;
default:
mxAddAndSetField(out,0,field_name,mxCreateDoubleScalar(member));
}
}
void LOCALengineHR(int n, int L,
double *M[], int *Aidx[], int Adim[],
double *alpha, double *beta,
double *x, double *v,
double *xproj, double *vproj, double *xwork)
{
int i, nactive, k, nsingle, *pidx;
double scalarquad[3], Isingle[4];
double I1, I2;
double *Icurrent, *Inew;
int ncurrent, nnew;
int Ichoose;
double tfac, BN, gamma, *tBox;
double dummy;
BN = mxGetInf();
/* Handle the box contraints to get initial interval */
/* tBox is unused in Hit&Run, but is used in GasDynamics */
tBox = (double *) calloc(n, sizeof(double));
box2initint(alpha,beta,x,v,n,&I1,&I2,BN,tBox);
free(tBox);
Icurrent = (double *) calloc(2*(L+1), sizeof(double));
Icurrent[0] = I1; Icurrent[1] = I2;
ncurrent = 1;
Inew = (double *) calloc(2*(L+1), sizeof(double));
for (i=0;i<L;i++) {
/* i'th Model */
nactive = Adim[i];
pidx = Aidx[i];
/* Project down to active variables */
for (k=0;k<nactive;k++) {
xproj[k] = x[*pidx-1];
vproj[k] = v[*pidx-1];
pidx++;
}
LOCALquadG2quadS(M[i], scalarquad, xproj, vproj, nactive);
nsingle = LOCALquadS2int(scalarquad, Isingle, &dummy);
nnew = LOCALIntersect(Icurrent, ncurrent, Isingle, nsingle, Inew);
/* Replace Current with New */
ncurrent = nnew;
for (k=0;k<2*ncurrent;k++) {
Icurrent[k] = Inew[k];
}
}
/* Choose an interval */
Ichoose = (int) floor(0.9999*((double) rand())/((double) RAND_MAX)*ncurrent);
gamma = 0.0001 + 0.9998*((double) rand())/((double) RAND_MAX);
tfac = gamma*Icurrent[2*Ichoose] + (1-gamma)*Icurrent[2*Ichoose+1];
/* Update Xwork */
for (i=0;i<n;i++) {
xwork[i] = x[i] + tfac*v[i];
}
/* Free memory */
free(Icurrent);
free(Inew);
}
void step5(double *assignment, double *distMatrix, bool *starMatrix, bool *newStarMatrix, bool *primeMatrix, bool *coveredColumns, bool *coveredRows, int nOfRows, int nOfColumns, int minDim)
{
double h, value;
int row, col;
/* find smallest uncovered element h */
h = mxGetInf();
for(row=0; row<nOfRows; row++)
if(!coveredRows[row])
for(col=0; col<nOfColumns; col++)
if(!coveredColumns[col])
{
value = distMatrix[row + nOfRows*col];
if(value < h)
h = value;
}
/* add h to each covered row */
for(row=0; row<nOfRows; row++)
if(coveredRows[row])
for(col=0; col<nOfColumns; col++)
distMatrix[row + nOfRows*col] += h;
/* subtract h from each uncovered column */
for(col=0; col<nOfColumns; col++)
if(!coveredColumns[col])
for(row=0; row<nOfRows; row++)
distMatrix[row + nOfRows*col] -= h;
/* move to step 3 */
step3(assignment, distMatrix, starMatrix, newStarMatrix, primeMatrix, coveredColumns, coveredRows, nOfRows, nOfColumns, minDim);
}
void prior_t_garch_hl_hyper(double *theta, double *hyper, double *PL_hp, double *VaR,
mwSignedIndex N, mwSignedIndex hp, mwSignedIndex *r1, double *r2)
{
mwSignedIndex i, j;
/* Variable size arrays */
/* c1 = malloc((N)*sizeof(double)); */
for (i=0; i<N; i++)
{
r1[i] = 1;
r2[i] = -mxGetInf();
if (theta[i] <= 0) // omega>0
{
r1[i] = 0;
}
if ((theta[i+N] <0 ) || (theta[i+N] >= 1)) // 0<=alpha<1
{
r1[i] = 0;
}
// mexPrintf("r1[%i] = %i\n",i,r1[i]);
if ((theta[i+2*N] < 0) || (theta[i+2*N] >= 1)) // 0<=beta<1
{
r1[i] = 0;
}
// mexPrintf("r1[%i] = %i\n",i,r1[i]);
if (theta[i+N] + theta[i+2*N] >= 1) //alpha+beta<1
{
r1[i] = 0;
}
// mexPrintf("r1[%i] = %i\n",i,r1[i]);
if (theta[i+4*N] <= 2) //nu>2
{
r1[i] = 0;
}
// mexPrintf("r1[%i] = %i\n",i,r1[i]);
if (PL_hp[i] > VaR[0])
{
r1[i] = 0;
}
// mexPrintf("r1[%i] = %i\n",i,r1[i]);
for (j=1; j<(hp+1); j++)
{
if (abs(theta[N*(j+4)+i]) > 10) /* for numerical stability */
{
r1[i] = 0;
}
}
if (r1[i] == 1)
{
r2[i] = log(hyper[0]) - hyper[0]*(theta[i+4*N] - 2);
}
}
}
/* Function: mdlInitializeSampleTimes =========================================
* Abstract:
* This function is used to specify the sample time(s) for your
* S-function. You must register the same number of sample times as
* specified in ssSetNumSampleTimes.
*/
static void mdlInitializeSampleTimes(SimStruct *S)
{
int idx;
// Just set this to avoid Simulink whinging
ssSetSampleTime(S, 0, mxGetInf());
ssSetOffsetTime(S, 0, 0.0);
}
void assignmentsuboptimal2(double *assignment, double *cost, double *distMatrixIn, int nOfRows, int nOfColumns)
{
int n, row, col, tmpRow, tmpCol, nOfElements;
double value, minValue, *distMatrix, inf;
inf = mxGetInf();
/* make working copy of distance Matrix */
nOfElements = nOfRows * nOfColumns;
distMatrix = (double *)mxMalloc(nOfElements * sizeof(double));
for(n=0; n<nOfElements; n++)
distMatrix[n] = distMatrixIn[n];
/* initialization */
*cost = 0;
for(row=0; row<nOfRows; row++)
#ifdef ONE_INDEXING
assignment[row] = 0.0;
#else
assignment[row] = -1.0;
#endif
/* recursively search for the minimum element and do the assignment */
while(true)
{
/* find minimum distance observation-to-track pair */
minValue = inf;
for(row=0; row<nOfRows; row++)
for(col=0; col<nOfColumns; col++)
{
value = distMatrix[row + nOfRows*col];
if(mxIsFinite(value) && (value < minValue))
{
minValue = value;
tmpRow = row;
tmpCol = col;
}
}
if(mxIsFinite(minValue))
{
#ifdef ONE_INDEXING
assignment[tmpRow] = tmpCol+ 1;
#else
assignment[tmpRow] = tmpCol;
#endif
*cost += minValue;
for(n=0; n<nOfRows; n++)
distMatrix[n + nOfRows*tmpCol] = inf;
for(n=0; n<nOfColumns; n++)
distMatrix[tmpRow + nOfRows*n] = inf;
}
else
break;
} /* while(true) */
}
/**
* @brief Read in the option possible_words.
* Reads the option possible_words, converting string data into its numerical
* representation, and returning a flag to indicate the status.
*/
int ReadOptionPossibleWords(const mxArray *in,const char *field_name,double *member)
{
/* declare local variables */
mxArray *tmp;
double num;
int stringLength, flag, i;
char *str;
tmp = mxGetField(in,0,field_name);
if((tmp==NULL) || mxIsEmpty(tmp)) /* field is empty */
flag = 0;
else
{
if(mxIsChar(tmp)) /* field is string */
{
/* copy string and set to lowercase */
stringLength = mxGetNumberOfElements(tmp) + 1;
str = mxCalloc(stringLength,sizeof(char));
if(mxGetString(tmp,str,stringLength)!=0)
mexErrMsgIdAndTxt("STAToolkit:ReadOptionPossibleWords:invalidValue","Could not convert string data.");
for(i=0;str[i];i++)
str[i] = tolower(str[i]);
/* use string to set member value */
flag = 1;
if(strcmp(str,"recommended")==0)
*member = (double)(-1.0);
else if(strcmp(str,"unique")==0)
*member = (double)(-2.0);
else if(strcmp(str,"total")==0)
*member = (double)(-3.0);
else if(strcmp(str,"possible")==0)
*member = (double)(-4.0);
else if(strcmp(str,"min_tot_pos")==0)
*member = (double)(-5.0);
else if(strcmp(str,"min_lim_tot_pos")==0)
*member = (double)(-6.0);
else
{
mexWarnMsgIdAndTxt("STAToolkit:ReadOptionPossibleWords:invalidValue","Unrecognized option \"%s\" for possible_words. Using default \"recommended\".",str);
*member = (double)(-1.0);
}
}
else /* field is scalar */
{
flag = 2;
num = mxGetScalar(tmp);
if(num==mxGetInf())
*member = (double)(0.0);
else if((num<1.0) || (fmod(num,1.0)>mxGetEps()))
mexErrMsgIdAndTxt("STAToolkit:ReadOptionPossibleWords:invalidValue","possible_words must be a positive integer. Current value is %f.",num);
else
*member = num;
}
}
return flag;
}
/*************************************************************************
* main
*************************************************************************/
void dijkstra1( long int n, long int s, double *D1, double *P1, double *Gpr, int *Gir, int *Gjc) {
int finished;
long int i, startInd, endInd, whichNeigh, nDone, closest;
double closestD, arcLength, INF, SMALL, oldDist;
HeapNode *A, *hnMin, hnTmp; FibHeap *heap;
INF=mxGetInf(); SMALL=mxGetEps();
// setup heap
if ((heap = new FibHeap) == NULL || (A = new HeapNode[n+1]) == NULL )
mexErrMsgTxt( "Memory allocation failed-- ABORTING.\n" );
heap->ClearHeapOwnership();
// initialize
for (i=0; i<n; i++) {
if (i!=s) A[ i ] = (double) INF; else A[ i ] = (double) SMALL;
if (i!=s) D1[ i ] = (double) INF; else D1[ i ] = (double) SMALL;
P1[ i ] = -1;
heap->Insert( &A[i] );
A[ i ].SetIndexValue( (long int) i );
}
// Insert 0 then extract it, which will balance heap
heap->Insert(&hnTmp); heap->ExtractMin();
// loop over nonreached nodes
finished = nDone = 0;
while ((finished==0) && (nDone < n)) {
hnMin = (HeapNode *) heap->ExtractMin();
closest = hnMin->GetIndexValue();
closestD = hnMin->GetKeyValue();
if ((closest<0) || (closest>=n))
mexErrMsgTxt( "Minimum Index out of bound..." );
D1[ closest ] = closestD;
if (closestD == INF) finished=1; else {
// relax all nodes adjacent to closest
nDone++;
startInd = Gjc[ closest ];
endInd = Gjc[ closest+1 ] - 1;
if( startInd!=endInd+1 )
for( i=startInd; i<=endInd; i++ ) {
whichNeigh = Gir[ i ];
arcLength = Gpr[ i ];
oldDist = D1[ whichNeigh ];
if ( oldDist > ( closestD + arcLength )) {
D1[ whichNeigh ] = closestD + arcLength;
P1[ whichNeigh ] = closest + 1;
hnTmp = A[ whichNeigh ];
hnTmp.SetKeyValue( closestD + arcLength );
heap->DecreaseKey( &A[ whichNeigh ], hnTmp );
}
}
}
}
// cleanup
delete heap; delete[] A;
}
void calcPathDistsInf( double* const E2,
const int n, const int l, const int k,
const double* const D2, const int* const NN,
const int* const subset )
{
// === variables
double* e2;
int* openHeap = new int[ n ];
int* openTrack = new int[ n ];
int nofOpen;
int ii;
register int j;
// === compute
e2 = E2;
for( ii = 0; ii < l; ++ii ) {
const int i = subset[ ii ];
// --- prepare (squared distances, set of open nodes)
for( j = 0; j < n; ++j ) {
e2[j] = mxGetInf();
openTrack[j] = -1;
}
e2[i] = 0;
nofOpen = 0;
insertIntoPq( nofOpen, openHeap, openTrack, e2, i );
// --- iteratively update squared distances
while( nofOpen > 0 ) {
const int jStar = fetchFirstFromPq( nofOpen, openHeap, openTrack, e2 );
register const double d2Star = e2[ jStar ];
if( k == 0 ) {
register const double* const d2 = D2 + jStar*n;
for( j = 0; j < n; ++j ) {
if( max( d2Star, d2[j] ) < e2[j] ) {
e2[j] = max( d2Star, d2[j] );
insertIntoPq( nofOpen, openHeap, openTrack, e2, j ); // or update, if already in queue
}
}
} else {
register const double* const d2 = D2 + jStar*k;
register const int* const nn = NN + jStar*k;
register int p;
for( p = 0; p < k; ++p ) {
register const int j = nn[ p ] - 1;
if( max( d2Star, d2[p] ) < e2[j] ) {
e2[j] = max( d2Star, d2[p] );
insertIntoPq( nofOpen, openHeap, openTrack, e2, j ); // or update, if already in queue
}
}
}
}
// --- next element of subset
e2 += n;
}
// === clean up
delete[] openHeap;
delete[] openTrack;
}
double icdfn(double x){
/* Coefficients in rational approximations. */
const double a[4] = { 0.886226899, -1.645349621, 0.914624893, -0.140543331};
const double b[4] = {-2.118377725, 1.442710462, -0.329097515, 0.012229801};
const double c[4] = {-1.970840454, -1.624906493, 3.429567803, 1.641345311};
const double d[2] = { 3.543889200, 1.637067800};
int i;
double z, x0;
/* Central range. */
x=2*x-1;
if (fabs(x)>=1){
if (fabs(x)>1) x=mxGetNaN();
else{ if (x>0) x=mxGetInf();
else x=-mxGetInf();}
}
else{
x0 = .7;
/* Near end points of range. */
if (x0 < x){
z = sqrt(-log((1-x)/2));
z = (((c[3]*z+c[2])*z+c[1])*z+c[0]) / ((d[1]*z+d[0])*z+1);
}
else if (x<-x0)
{
z = sqrt(-log((1+x)/2));
z = -(((c[3]*z+c[2])*z+c[1])*z+c[0]) / ((d[1]*z+d[0])*z+1);
}
else
{
z = x*x;
z = x*(((a[3]*z+a[2])*z+a[1])*z+a[0]) / ((((b[3]*z+b[2])*z+b[1])*z+b[0])*z+1);
}
/* Newton-Raphson correction to full accuracy.
Without these steps, erfinv() would be about 3 times
faster to compute, but accurate to only about 6 digits. */
for (i=0;i<2;i++)
z = z - (erf(z) - x) / (1.128379167095513 * exp(-z*z));
x = 1.414213562373095*z;
}
return(x);
}
//
// helper function to do 1d minimum convolution
//
void gdt1d(double *cost, double *out, int *loc, int *v, double *z, int sz_x) {
int k, q;
double s;
// set up
k = 0;
v[0] = 0;
z[0] = -mxGetInf();
z[1] = mxGetInf();
// compute
for (q=1; q<sz_x; q++) {
mxAssert(q<sz_x,"Q out of range");
mxAssert(k<sz_x,"v out of range");
mxAssert(v[k]+1<sz_x,"cost out of range");
s = ((cost[q]+q*q)-(cost[v[k]]+v[k]*v[k]))/(2*(q-v[k])); // intercept
while (s<=z[k]) {
k = k-1;
mxAssert(q<sz_x,"Q out of range");
mxAssert(k<sz_x,"v out of range");
mxAssert(v[k]+1<sz_x,"cost out of range");
s = ((cost[q]+q*q)-(cost[v[k]]+v[k]*v[k]))/(2*(q-v[k]));
}
k = k+1;
mxAssert(k<sz_x,"k out of range");
v[k] = q;
z[k] = s;
z[k+1] = mxGetInf();
}
k = 0;
for (q=0; q<sz_x; q++) {
while (z[k+1]<q) {
k++;
}
if (loc) {
loc[q] = v[k]; // no +1 here -- not ready to go to Matlab coords yet
}
out[q] = (q-v[k])*(q-v[k])+cost[v[k]];
}
}
void mexFunction (int nlhs, mxArray * plhs[], int nrhs, const mxArray * prhs[]) {
int rc;
double delay;
/* this function will be called upon unloading of the mex file */
mexAtExit(exitFun);
if (nrhs<1) {
/* show the status of the delayed exit */
pthread_mutex_lock(&mutextimer);
pthread_mutex_lock(&mutexstatus);
if (timerStatus) {
delay = difftime(timer-time(NULL));
if (nlhs<1)
mexPrintf("delayed exit scheduled at %d seconds\n", (int)delay);
else
plhs[0] = mxCreateDoubleScalar(delay);
}
else {
if (nlhs<1)
mexPrintf("no delayed exit scheduled\n");
else
plhs[0] = mxCreateDoubleScalar(mxGetInf());
}
pthread_mutex_unlock(&mutexstatus);
pthread_mutex_unlock(&mutextimer);
}
else {
/* the first argument is the delay in seconds */
if (!mxIsScalar(prhs[0]))
mexErrMsgTxt ("invalid input argument #1");
delay = mxGetScalar(prhs[0]);
/* set or update the timer */
pthread_mutex_lock(&mutextimer);
timer = time(NULL) + (unsigned int)delay;
pthread_mutex_unlock(&mutextimer);
/* start the thread */
rc = pthread_create(&timerThread, NULL, checktimer, (void *)NULL);
if (rc)
mexErrMsgTxt("problem with return code from pthread_create()");
}
} /* main */
void mexFunction(
int nlhs, mxArray *plhs[],
int nrhs, const mxArray *prhs[])
{ /*Note, m was removed since it is not used.*/
mwSize n, p, d; /*mxGetM, mxGetM, mxGetN*/
mwSize i, j, k, minind; /*0, 0, 0, 1*/
double *s, *S, *ind, minds, dsj, *sk, *Sk, inf; /*mxGetPr, mxGetPr, mxGetPr, inf, minds, s, S, mxGetInf*/
/* Error checking on inputs */
if (nrhs<2) mexErrMsgTxt("Not enough input arguments.");
if (nrhs>2) mexErrMsgTxt("Too many input arguments.");
if (nlhs>1) mexErrMsgTxt("Too many output arguments.");
for (i=0; i<nrhs; i++)
{
if (!mxIsDouble(prhs[i]) || mxIsSparse(prhs[i]))
mexErrMsgTxt("Function not defined for variables of input class");
if (mxIsComplex(prhs[i]))
mexErrMsgTxt("Arguments must be real.");
}
d=mxGetN(prhs[0]);
if (mxGetN(prhs[1])!=d)
mexErrMsgTxt("Inputs must have equal numbers of columns.");
p=mxGetM(prhs[0]);
n=mxGetM(prhs[1]);
plhs[0]=mxCreateDoubleMatrix(p,1,mxREAL);
s=mxGetPr(prhs[0]);
S=mxGetPr(prhs[1]);
ind=mxGetPr(plhs[0]);
inf=mxGetInf();
for (i=0; i<p; i++, s++){
minind=1;
minds=inf;
for (j=0; j<n;){
for (k=0, dsj=0, sk=s, Sk=S+j; k<d; k++, sk+=p, Sk+=n)
dsj+=fabs(*sk-*Sk);
j++;
if (dsj==0){minind=j; break;}
if (dsj<minds){minds=dsj; minind=j;}
}
ind[i]=minind;
}
}
double
CppNeighborPtsDensity (vector < unsigned int >edgeImage,
vector < unsigned int >dim_img,
unsigned int windowsize, double rowc, double colc)
{
// calculate the point density in the window defined by windowsize*windowsize centered at given point (rowc, colc).
// rowc, colc: based from 0.
if (windowsize % 2 == 0) {
#ifdef MATLABPRINT
mexPrintf ("CppNeighborPtsDensity ==> windowsize must be odd!\n");
#endif
return -1;
}
unsigned int relativecenter = windowsize / 2;
double meanpnum = 0;
// circle center at (rowc, cocl) derived by fitting might locate outside edgeImage
if ((rowc > static_cast < double >(dim_img[0])) ||(rowc < 0)
|| (colc > static_cast < double >(dim_img[1])) ||(colc < 0)) {
return mxGetInf ();
}
for (int c = -1 * relativecenter;
c < static_cast < int >(relativecenter + 1); c++) {
// if window pixel locates outside the image, its point number is assumed to be 0.
if (((colc + c) > static_cast < double >(dim_img[1])) ||((colc + c) < 0)) {
continue;
}
for (int r = -1 * relativecenter;
r < static_cast < int >(relativecenter + 1); r++) {
// if window pixel locates outside the image, its point number is assumed to be 0.
if (((rowc + r) > static_cast < double >(dim_img[0])) ||((rowc + r) <
0)) {
continue;
}
meanpnum +=
edgeImage[MatrixLZ::
sub2ind (static_cast < unsigned int >(rowc + r),
static_cast < unsigned int >(colc + c), dim_img[0],
dim_img[1], MatrixLZ::ColMajor)];
}
}
meanpnum /= windowsize * windowsize;
return meanpnum;
}
void
mexFunction( int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
{
mwSize i, n;
double *pr, *pi;
double inf, nan;
/* Check for proper number of input and output arguments */
if (nrhs != 1) {
mexErrMsgTxt("One input argument required.");
}
if(nlhs > 1){
mexErrMsgTxt("Too many output arguments.");
}
/* Check data type of input argument */
if (!mxIsDouble(prhs[0]) || mxIsComplex(prhs[0])) {
mexErrMsgTxt("Input argument must be of type real double.");
}
/* Duplicate input array */
plhs[0]=mxDuplicateArray(prhs[0]);
pr = mxGetPr(plhs[0]);
pi = mxGetPi(plhs[0]);
n = mxGetNumberOfElements(plhs[0]);
inf = mxGetInf();
nan=mxGetNaN();
/* Check for 0, in real part of data, if the data is zero, replace
with NaN. Also check for INT_MAX and INT_MIN and replace with
INF/-INF respectively. */
for(i=0; i < n; i++) {
if (pr[i] == 0){
pr[i]=nan;
}
else if ( pr[i]>= INT_MAX){
pr[i]=inf;
}
else if (pr[i]<= INT_MIN){
pr[i]=-(inf);
}
}
}
/**
* @brief Read direct method options.
* This function reads options for direct method routines from a
* Matlab struct (*in), and returns a pointer to a C struct
* options_direct (see direct_c.h).
*/
struct options_direct *ReadOptionsDirect(const mxArray *in)
{
struct options_direct *opts;
opts = (struct options_direct *)mxMalloc(sizeof(struct options_direct));
opts->t_start_flag = ReadOptionsDoubleMember(in,"start_time",&(opts->t_start));
opts->t_end_flag = ReadOptionsDoubleMember(in,"end_time",&(opts->t_end));
opts->Delta_flag = ReadOptionsDoubleMember(in,"counting_bin_size",&(opts->Delta));
opts->words_per_train_flag = ReadOptionsIntMember(in,"words_per_train",&(opts->words_per_train));
opts->sum_spike_trains_flag = ReadOptionsIntMember(in,"sum_spike_trains",&(opts->sum_spike_trains));
opts->permute_spike_trains_flag = ReadOptionsIntMember(in,"permute_spike_trains",&(opts->permute_spike_trains));
opts->legacy_binning_flag = ReadOptionsIntMember(in,"legacy_binning",&(opts->legacy_binning));
opts->letter_cap_flag = ReadOptionsIntMember(in,"letter_cap",&(opts->letter_cap));
if((opts->letter_cap_flag) && (opts->letter_cap==(int)mxGetInf()))
opts->letter_cap = 0;
return opts;
}
int BreadthFirstTraversal(struct queue *q,Node *root,Node *subformula[],int *i)
{
double infval = mxGetInf();
Node *p = NULL;
if (root == NULL) return 0;
enqueue(q,root); /* enqueue the root node*/
while (!queue_empty_p(q)) {
if(!q->first){
p = NULL;
}
else{ /* set subformula index*/
p = dupnode(q->first);
p = q->first;
p->index = *i;
subformula[*i] = dupnode(p);
subformula[*i] = p;
subformula[*i]->BoundCheck = 0;
subformula[*i]->UBound = -infval;
subformula[*i]->LBound = infval;
subformula[*i]->LBound_nxt = infval;
subformula[*i]->UBindicator = 0;
subformula[*i]->LBindicator = 0;
subformula[*i]->LBindicator_nxt = 0;
subformula[*i]->loop_end = 0;
(*i)++;
}
dequeue(q);
if (p->lft != NULL)
BreadthFirstTraversal( q,p->lft,subformula,i);
if (p->rgt != NULL)
BreadthFirstTraversal( q,p->rgt,subformula,i);
}
return (*i-1);
}
mxArray *WriteOptionsDirect(const mxArray *in,struct options_direct *opts)
{
mxArray *out;
out = mxDuplicateArray(in);
WriteOptionsDoubleMember(out,"start_time",opts->t_start,opts->t_start_flag);
WriteOptionsDoubleMember(out,"end_time",opts->t_end,opts->t_end_flag);
WriteOptionsDoubleMember(out,"counting_bin_size",opts->Delta,opts->Delta_flag);
WriteOptionsIntMember(out,"sum_spike_trains",opts->sum_spike_trains,opts->sum_spike_trains_flag);
WriteOptionsIntMember(out,"permute_spike_trains",opts->permute_spike_trains,opts->permute_spike_trains_flag);
WriteOptionsIntMember(out,"words_per_train",opts->words_per_train,opts->words_per_train_flag);
WriteOptionsIntMember(out,"legacy_binning",opts->legacy_binning,opts->legacy_binning_flag);
if((opts->letter_cap_flag) && (opts->letter_cap==0))
WriteOptionsDoubleMember(out,"letter_cap",mxGetInf(),opts->letter_cap_flag);
else
WriteOptionsIntMember(out,"letter_cap",opts->letter_cap,opts->letter_cap_flag);
mxFree(opts);
return out;
}
/* --------------------------------------------------------------
Usage: exitflag = qpbsvm_scas( &get_col, diag_H, f, UB, dim, tmax,
tolabs, tolrel, tolKKT, x, Nabla, &t, &History, verb )
-------------------------------------------------------------- */
int qpbsvm_scas(const void* (*get_col)(long,long),
double *diag_H,
double *f,
double UB,
long dim,
long tmax,
double tolabs,
double tolrel,
double tolKKT,
double *x,
double *Nabla,
long *ptr_t,
double **ptr_History,
long verb)
{
double *History;
double *col_H;
double *tmp_ptr;
double x_old;
double x_new;
double delta_x;
double max_x;
double xHx;
double Q_P;
double Q_D;
double xf;
double xi_sum;
double max_update;
double curr_update;
long History_size;
long t;
long i, j;
long max_i;
int exitflag;
int KKTsatisf;
/* ------------------------------------------------------------ */
/* Initialization */
/* ------------------------------------------------------------ */
t = 0;
History_size = (tmax < HISTORY_BUF ) ? tmax+1 : HISTORY_BUF;
History = mxCalloc(History_size*2,sizeof(double));
if( History == NULL ) mexErrMsgTxt("Not enough memory.");
/* compute Q_P and Q_D */
xHx = 0;
xf = 0;
xi_sum = 0;
for(i = 0; i < dim; i++ ) {
xHx += x[i]*(Nabla[i] - f[i]);
xf += x[i]*f[i];
xi_sum += MAX(0,-Nabla[i]);
}
Q_P = 0.5*xHx + xf;
Q_D = -0.5*xHx - UB*xi_sum;
History[INDEX(0,t,2)] = Q_P;
History[INDEX(1,t,2)] = Q_D;
if( verb > 0 ) {
mexPrintf("%d: Q_P=%f, Q_D=%f, Q_P-Q_D=%f, (Q_P-Q_D)/|Q_P|=%f \n",
t, Q_P, Q_D, Q_P-Q_D,(Q_P-Q_D)/ABS(Q_P));
}
exitflag = -1;
while( exitflag == -1 )
{
t++;
max_update = -mxGetInf();
for(i = 0; i < dim; i++ ) {
if( diag_H[i] > 0 ) {
/* variable update */
x_old = x[i];
x_new = MIN(UB,MAX(0, x[i] - Nabla[i]/diag_H[i]));
curr_update = -0.5*diag_H[i]*(x_new*x_new-x_old*x_old) -
(Nabla[i] - diag_H[i]*x_old)*(x_new - x_old);
if( curr_update > max_update ) {
max_i = i;
max_update = curr_update;
max_x = x_new;
}
}
}
x_old = x[max_i];
x[max_i] = max_x;
/* update Nabla */
delta_x = max_x - x_old;
if( delta_x != 0 ) {
col_H = (double*)get_col(max_i,-1);
for(j = 0; j < dim; j++ ) {
Nabla[j] += col_H[j]*delta_x;
}
}
/* compute Q_P and Q_D */
xHx = 0;
xf = 0;
xi_sum = 0;
KKTsatisf = 1;
for(i = 0; i < dim; i++ ) {
xHx += x[i]*(Nabla[i] - f[i]);
xf += x[i]*f[i];
xi_sum += MAX(0,-Nabla[i]);
if((x[i] > 0 && x[i] < UB && ABS(Nabla[i]) > tolKKT) ||
(x[i] == 0 && Nabla[i] < -tolKKT) ||
(x[i] == UB && Nabla[i] > tolKKT)) KKTsatisf = 0;
}
Q_P = 0.5*xHx + xf;
Q_D = -0.5*xHx - UB*xi_sum;
/* stopping conditions */
if(t >= tmax) exitflag = 0;
else if(Q_P-Q_D <= tolabs) exitflag = 1;
else if(Q_P-Q_D <= ABS(Q_P)*tolrel) exitflag = 2;
else if(KKTsatisf == 1) exitflag = 3;
if( verb > 0 && (t % verb == 0 || t==1)) {
mexPrintf("%d: Q_P=%f, Q_D=%f, Q_P-Q_D=%f, (Q_P-Q_D)/|Q_P|=%f \n",
t, Q_P, Q_D, Q_P-Q_D,(Q_P-Q_D)/ABS(Q_P));
}
/* Store UB LB to History buffer */
if( t < History_size ) {
History[INDEX(0,t,2)] = Q_P;
History[INDEX(1,t,2)] = Q_D;
}
else {
tmp_ptr = mxCalloc((History_size+HISTORY_BUF)*2,sizeof(double));
if( tmp_ptr == NULL ) mexErrMsgTxt("Not enough memory.");
for( i = 0; i < History_size; i++ ) {
tmp_ptr[INDEX(0,i,2)] = History[INDEX(0,i,2)];
tmp_ptr[INDEX(1,i,2)] = History[INDEX(1,i,2)];
}
tmp_ptr[INDEX(0,t,2)] = Q_P;
tmp_ptr[INDEX(1,t,2)] = Q_D;
History_size += HISTORY_BUF;
mxFree( History );
History = tmp_ptr;
}
}
(*ptr_t) = t;
(*ptr_History) = History;
return( exitflag );
}
void mexFunction( int nlhs, mxArray* plhs[], int nrhs, const mxArray* prhs[] )
{
// === check number of params
// - output params
if( !( 1 <= nlhs && nlhs <= 1 ) ) {
mexErrMsgTxt( syntax );
}
// - input params
if( !( 3 <= nrhs && nrhs <= 4 ) ) {
if( nrhs == 0 ) {
printf( "%s\n", copyright );
plhs[0] = mxCreateDoubleMatrix( 1, 1, mxREAL );
double* const results = mxGetPr( plhs[0] );
results[ 0 ] = mxGetNaN();
return;
}
mexErrMsgTxt( syntax );
}
// === process input
#define D2_ ( prhs[ 0 ] )
#define NN_ ( prhs[ 1 ] )
#define subset_ ( prhs[ 2 ] )
#define rho_ ( prhs[ 3 ] )
// - initialize variables
const double* const D2 = mxGetPr( D2_ );
const int* const NN = (int*) mxGetData( NN_ );
const double* const doubleSubset = mxGetPr( subset_ );
const double rho = ( nrhs < 3 ) ? 2 : *mxGetPr( rho_ );
const int k = ( NN == NULL ) ? 0 : mxGetM( D2_ );
const int n = mxGetN( D2_ );
const int l = mxGetN( subset_ );
// - check input
if( k == 0 ) {
if( !( mxGetM( D2_ ) == n ) ) {
printf( "Since no 'NN' is given, 'D2' must be square.\n" );
mexErrMsgTxt( syntax );
}
} else {
if( !( mxGetClassID( NN_ ) == mxINT32_CLASS ) ) {
printf( "'NN' must be an integer (INT32) matrix.\n" );
mexErrMsgTxt( syntax );
}
if( !( mxGetM( NN_ ) == k && mxGetN( NN_ ) == n ) ) {
printf( "'NN' and 'D2' must be of the same size.\n" );
mexErrMsgTxt( syntax );
}
}
if( D2 == NULL || doubleSubset == NULL ) {
printf( "'D2' and 'subset' must be real matrices.\n" );
mexErrMsgTxt( syntax );
}
if( !( mxGetM( subset_ ) == 1 ) ) {
printf( "'subset' must be a row vector.\n" );
mexErrMsgTxt( syntax );
}
if( !( rho >= 0 ) ) {
printf( "'rho' must be non-negative.\n" );
mexErrMsgTxt( syntax );
}
// === compute and return distances
// - prepare subset
int* subset = new int[ l ];
int i;
for( i = 0; i < l; ++i ) {
register const int subset_i = (int) doubleSubset[ i ] - 1;
if( !( 0 <= subset_i && subset_i < n ) ) {
mexErrMsgTxt( "Element of 'subset' out of range.\n" );
}
subset[i] = subset_i;
}
// - compute distances
plhs[0] = mxCreateDoubleMatrix( n, l, mxREAL );
double* const E2 = mxGetPr( plhs[0] );
if( rho == 0.0 ) {
calcPathDists0( E2, n, l, k, D2, NN, subset );
} else if( rho == mxGetInf() ) {
calcPathDistsInf( E2, n, l, k, D2, NN, subset );
} else {
calcPathDistsRho( E2, n, l, k, D2, NN, subset, rho );
}
// - clean up
delete[] subset;
}
/* --------------------------------------------------------------
Usage:
exitflag = gsmo_algo( &get_col, diag_H, f, a, b, LB, UB, x, Nabla,
dim, tmax, tolKKT, verb, &t );
-------------------------------------------------------------- */
int gsmo_algo(const void* (*get_col)(long,long),
double *diag_H,
double *f,
double *a,
double b,
double *LB,
double *UB,
double *x,
double *Nabla,
long dim,
long tmax,
double tolKKT,
int verb,
long *ptr_t)
{
double *col_u;
double *col_v;
double minF_up;
double maxF_low;
double tau;
double F_i;
double tau_ub, tau_lb;
double Q_P;
long t;
long i;
long u, v;
int exitflag;
/* ------------------------------------------------------------ */
/* Initialization */
/* ------------------------------------------------------------ */
t = 0;
exitflag = 0;
while( exitflag == 0 && t < tmax)
{
t++;
/* find the most violating pair of variables */
minF_up = mxGetInf();
maxF_low = -mxGetInf();
for(i = 0; i < dim; i++ )
{
F_i = Nabla[i]/a[i];
if(LB[i] < x[i] && x[i] < UB[i])
{ /* i is from I_0 */
if( minF_up > F_i) { minF_up = F_i; u = i; }
if( maxF_low < F_i) { maxF_low = F_i; v = i; }
}
else if((a[i] > 0 && x[i] == LB[i]) || (a[i] < 0 && x[i] == UB[i]))
{ /* i is from I_1 or I_2 */
if( minF_up > F_i) { minF_up = F_i; u = i; }
}
else if((a[i] > 0 && x[i] == UB[i]) || (a[i] < 0 && x[i] == LB[i]))
{ /* i is from I_3 or I_4 */
if( maxF_low < F_i) { maxF_low = F_i; v = i; }
}
}
/* check KKT conditions */
if( maxF_low - minF_up <= tolKKT )
exitflag = 1;
else
{
/* SMO update of the most violating pair */
col_u = (double*)get_col(u,-1);
col_v = (double*)get_col(v,-1);
if( a[u] > 0 )
{ tau_lb = (LB[u]-x[u])*a[u]; tau_ub = (UB[u]-x[u])*a[u]; }
else
{ tau_ub = (LB[u]-x[u])*a[u]; tau_lb = (UB[u]-x[u])*a[u]; }
if( a[v] > 0 )
{ tau_lb = MAX(tau_lb,(x[v]-UB[v])*a[v]); tau_ub = MIN(tau_ub,(x[v]-LB[v])*a[v]); }
else
{ tau_lb = MAX(tau_lb,(x[v]-LB[v])*a[v]); tau_ub = MIN(tau_ub,(x[v]-UB[v])*a[v]); }
tau = (Nabla[v]/a[v]-Nabla[u]/a[u])/
(diag_H[u]/(a[u]*a[u]) + diag_H[v]/(a[v]*a[v]) - 2*col_u[v]/(a[u]*a[v]));
tau = MIN(MAX(tau,tau_lb),tau_ub);
x[u] += tau/a[u];
x[v] -= tau/a[v];
/* update Nabla */
for(i = 0; i < dim; i++ )
{
Nabla[i] += col_u[i]*tau/a[u] - col_v[i]*tau/a[v];
}
}
if( verb > 0 && (t % verb == 0 || t==1 || t >= tmax || exitflag > 0))
{
for(Q_P=0, i = 0; i < dim; i++ ) Q_P += 0.5*(x[i]*Nabla[i]+x[i]*f[i]); /* objective function*/
mexPrintf("t=%d, KKTviol=%f, tau=%f, tau_lb=%f, tau_ub=%f, Q_P=%f\n",
t, maxF_low - minF_up, tau, tau_lb, tau_ub, Q_P);
}
}
(*ptr_t) = t;
return( exitflag );
}
void posterior_t_garch_hl_noS_hyper_mex(double *y, mwSignedIndex N, mwSignedIndex hp, mwSignedIndex T, double *S,
double *theta, double *hyper, double *VaR, double *GamMat, mwSignedIndex G, double *d)
{
mwSignedIndex *r1;
double *r2;
double *PL_hp;
double *rho;
double h, *pdf;
double rhoh;
mwSignedIndex i, j;
/* Variable size arrays */
r1 = mxMalloc((N)*sizeof(mwSignedIndex));
r2 = mxMalloc((N)*sizeof(double));
rho = mxMalloc((N)*sizeof(double));
pdf = mxMalloc((1)*sizeof(double));
PL_hp = mxMalloc((N)*sizeof(double));
predict_t_garch(theta, y, S,N, hp, T, PL_hp);
prior_t_garch_hl_hyper(theta, hyper, PL_hp, VaR, N, r1, r2);
/* Initialise */
for (i=0; i<N; i++)
{
// mexPrintf("PL_hp[%i] = %6.4f \n",i,PL_hp[i]);
rho[i] = (theta[i+4*N]-2)/theta[i+4*N];
// mexPrintf("omega[%i] = %6.4f\n",i,omega[i] );
// mexPrintf("rho[%i] = %6.4f\n",i,rho[i]);
// mexPrintf("alpha[%i] = %6.4f\n", i, theta[i]);
// mexPrintf("beta[%i] = %6.4f\n", i, theta[i+N]);
// mexPrintf("mu[%i] = %6.4f\n", i, theta[2*N+i]);
// mexPrintf("nu[%i] = %6.4f\n", i, theta[i+3*N]);
}
/* PDF */
for (i=0; i<N; i++)
{
// mexPrintf("r1[%i] = %i\n",i,r1[i]);
// mexPrintf("r2[%i] = %6.4f\n",i,r2[i]);
// mexPrintf("alpha[%i] = %6.4f\n", i, theta[i]);
// mexPrintf("beta[%i] = %6.4f\n", i, theta[i+N]);
// mexPrintf("mu[%i] = %6.4f\n", i, theta[2*N+i]);
// mexPrintf("nu[%i] = %6.4f\n", i, theta[i+3*N]);
// mexPrintf("eps[%i] = %6.4f\n", i, theta[i+4*N]);
if (r1[i]==1)
{
d[i] = r2[i]; /* prior */
h = S[0];
// mexPrintf("d[%i] = %6.4f\n",i,d[i]);
// mexPrintf("h = %6.4f\n", h);
for (j=1; j<T; j++)
{
// mexPrintf("i = %i\n",i);
// mexPrintf("j = %i\n",j);
// mexPrintf("y[%i] = %6.4f\n", j-1, y[j-1]);
//
// mexPrintf("alpha[%i] = %6.4f\n", i, theta[i]);
// mexPrintf("beta[%i] = %6.4f\n", i, theta[i+N]);
// mexPrintf("mu[%i] = %6.4f\n", i, theta[2*N+i]);
// mexPrintf("nu[%i] = %6.4f\n", i, theta[i+3*N]);
// // /* h[i] = beta[i]*h[i] + omega[i] + alpha[i]*(y[j-1]-mu[i])*(y[j-1]-mu[i]); */
h = theta[2*N+i]*h + theta[i] + theta[N+i]*(y[j-1]-theta[3*N+i])*(y[j-1]-theta[3*N+i]);
// mexPrintf("h = %6.4f\n", h);
rhoh = rho[i]*h;
// mexPrintf("rhoh = %6.4f\n", rhoh);
duvt_garch(y[j], theta[i+3*N], rhoh, theta[i+4*N], GamMat, G, pdf);
pdf[0] = log(pdf[0]);
// mexPrintf("pdf[%i] = %16.14f\n", j, pdf[0]);
d[i] = d[i] + pdf[0];
// mexPrintf("d[%i] = %6.4f\n",i,d[i]);
}
// mexPrintf("d[%i] = %16.14f\n",i,d[i]);
for (j=1; j<hp+1; j++) /* pdf of future disturbances */
{
duvt_garch(theta[i+N*(j+4)], 0, 1, theta[i+4*N], GamMat, G, pdf);
pdf[0] = log(pdf[0]);
// mexPrintf("eps[%i] = %16.14f\n", j, theta[i+N*(j+3)]);
// mexPrintf("nu[%i] = %16.14f\n", j, theta[i+3*N]);
// mexPrintf("pdf[%i] = %16.14f\n", j, pdf[0]);
d[i] = d[i] + pdf[0];
// mexPrintf("d[%i] = %6.4f\n",i,d[i]);
}
}
else
{
// d[i] = M;
d[i] = -mxGetInf();
}
// mexPrintf("d[%i] = %6.4f\n",i,d[i]);
}
/* Free allocated memory */
mxFree(r1);
mxFree(r2);
mxFree(rho);
mxFree(pdf);
mxFree(PL_hp);
}
void calcPathDistsRho( double* const E2,
const int n, const int l, const int k,
const double* const D2, const int* const NN,
const int* const subset,
const double rho )
{
// === variables
double* Dr = ( k == 0 ) ? new double[ n * n ] : new double[ k * n ];
double* er;
int* openHeap = new int[ n ];
int* openTrack = new int[ n ];
int nofOpen;
int ii;
register int j;
// === prepare function of distances
if( k == 0 ) {
for( j = 0; j < n*n; ++j ) {
Dr[j] = exp( rho * sqrt(D2[j]) ) - 1.0;
}
} else {
register int p;
for( p = 0; p < k*n; ++p ) {
Dr[p] = exp( rho * sqrt(D2[p]) ) - 1.0;
}
}
// === compute
er = E2;
for( ii = 0; ii < l; ++ii ) {
const int i = subset[ ii ];
// --- prepare (squared distances, set of open nodes)
for( j = 0; j < n; ++j ) {
er[j] = mxGetInf();
openTrack[j] = -1;
}
er[i] = 0;
nofOpen = 0;
insertIntoPq( nofOpen, openHeap, openTrack, er, i );
// --- iteratively update squared distances
while( nofOpen > 0 ) {
const int jStar = fetchFirstFromPq( nofOpen, openHeap, openTrack, er );
register const double drStar = er[ jStar ];
if( k == 0 ) {
const double* const dr = Dr + jStar*n;
for( j = 0; j < n; ++j ) {
if( drStar + dr[j] < er[j] ) {
er[j] = drStar + dr[j];
insertIntoPq( nofOpen, openHeap, openTrack, er, j ); // or update, if already in queue
}
}
} else {
register const int* const nn = NN + jStar*k;
const double* const dr = Dr + jStar*k;
register int p;
for( p = 0; p < k; ++p ) {
register const int j = nn[ p ] - 1;
if( drStar + dr[p] < er[j] ) {
er[j] = drStar + dr[p];
insertIntoPq( nofOpen, openHeap, openTrack, er, j ); // or update, if already in queue
}
}
}
}
// --- compute inverse function of distances
for( j = 0; j < n; ++j ) {
register const double t = log( 1 + er[j] ) / rho;
er[j] = t * t;
}
// --- next element of subset
er += n;
}
// === clean up
delete[] Dr;
delete[] openHeap;
delete[] openTrack;
}
void calcPathDists0( double* const E2,
const int n, const int l, const int k,
const double* const D2, const int* const NN,
const int* const subset )
{
// === variables
double* D0 = ( k == 0 ) ? new double[ n * n ] : new double[ k * n ];
double* e0;
int* openHeap = new int[ n ];
int* openTrack = new int[ n ];
int nofOpen;
int ii;
register int j;
// === un-square distances
if( k == 0 ) {
for( j = 0; j < n*n; ++j ) {
D0[j] = sqrt( D2[j] );
}
} else {
register int p;
for( p = 0; p < k*n; ++p ) {
D0[p] = sqrt( D2[p] );
}
}
// === compute
e0 = E2;
for( ii = 0; ii < l; ++ii ) {
const int i = subset[ ii ];
// --- prepare (squared distances, set of open nodes)
for( j = 0; j < n; ++j ) {
e0[j] = mxGetInf();
openTrack[j] = -1;
}
e0[i] = 0;
nofOpen = 0;
insertIntoPq( nofOpen, openHeap, openTrack, e0, i );
// --- iteratively update squared distances
while( nofOpen > 0 ) {
const int jStar = fetchFirstFromPq( nofOpen, openHeap, openTrack, e0 );
register const double d0Star = e0[ jStar ];
if( k == 0 ) {
const double* const d0 = D0 + jStar*n;
for( j = 0; j < n; ++j ) {
if( d0Star + d0[j] < e0[j] ) {
e0[j] = d0Star + d0[j];
insertIntoPq( nofOpen, openHeap, openTrack, e0, j ); // or update, if already in queue
}
}
} else {
register const int* const nn = NN + jStar*k;
const double* const d0 = D0 + jStar*k;
register int p;
for( p = 0; p < k; ++p ) {
register const int j = nn[ p ] - 1;
if( d0Star + d0[p] < e0[j] ) {
e0[j] = d0Star + d0[p];
insertIntoPq( nofOpen, openHeap, openTrack, e0, j ); // or update, if already in queue
}
}
}
}
// --- re-square path distances
for( j = 0; j < n; ++j ) {
register double e0_j = e0[j];
e0[j] = e0_j * e0_j;
}
// --- next element of subset
e0 += n;
}
// === clean up
delete[] D0;
delete[] openHeap;
delete[] openTrack;
}
void assignmentsuboptimal1(double *assignment, double *cost, double *distMatrixIn, int nOfRows, int nOfColumns)
{
bool infiniteValueFound, finiteValueFound, repeatSteps, allSinglyValidated, singleValidationFound;
int n, row, col, tmpRow, tmpCol, nOfElements;
int *nOfValidObservations, *nOfValidTracks;
double value, minValue, *distMatrix, inf;
inf = mxGetInf();
/* make working copy of distance Matrix */
nOfElements = nOfRows * nOfColumns;
distMatrix = (double *)mxMalloc(nOfElements * sizeof(double));
for(n=0; n<nOfElements; n++)
distMatrix[n] = distMatrixIn[n];
/* initialization */
*cost = 0;
#ifdef ONE_INDEXING
for(row=0; row<nOfRows; row++)
assignment[row] = 0.0;
#else
for(row=0; row<nOfRows; row++)
assignment[row] = -1.0;
#endif
/* allocate memory */
nOfValidObservations = (int *)mxCalloc(nOfRows, sizeof(int));
nOfValidTracks = (int *)mxCalloc(nOfColumns, sizeof(int));
/* compute number of validations */
infiniteValueFound = false;
finiteValueFound = false;
for(row=0; row<nOfRows; row++)
for(col=0; col<nOfColumns; col++)
if(mxIsFinite(distMatrix[row + nOfRows*col]))
{
nOfValidTracks[col] += 1;
nOfValidObservations[row] += 1;
finiteValueFound = true;
}
else
infiniteValueFound = true;
if(infiniteValueFound)
{
if(!finiteValueFound)
return;
repeatSteps = true;
while(repeatSteps)
{
repeatSteps = false;
/* step 1: reject assignments of multiply validated tracks to singly validated observations */
for(col=0; col<nOfColumns; col++)
{
singleValidationFound = false;
for(row=0; row<nOfRows; row++)
if(mxIsFinite(distMatrix[row + nOfRows*col]) && (nOfValidObservations[row] == 1))
{
singleValidationFound = true;
break;
}
if(singleValidationFound)
{
for(row=0; row<nOfRows; row++)
if((nOfValidObservations[row] > 1) && mxIsFinite(distMatrix[row + nOfRows*col]))
{
distMatrix[row + nOfRows*col] = inf;
nOfValidObservations[row] -= 1;
nOfValidTracks[col] -= 1;
repeatSteps = true;
}
}
}
/* step 2: reject assignments of multiply validated observations to singly validated tracks */
if(nOfColumns > 1)
{
for(row=0; row<nOfRows; row++)
{
singleValidationFound = false;
for(col=0; col<nOfColumns; col++)
if(mxIsFinite(distMatrix[row + nOfRows*col]) && (nOfValidTracks[col] == 1))
{
singleValidationFound = true;
break;
}
if(singleValidationFound)
{
for(col=0; col<nOfColumns; col++)
if((nOfValidTracks[col] > 1) && mxIsFinite(distMatrix[row + nOfRows*col]))
{
distMatrix[row + nOfRows*col] = inf;
nOfValidObservations[row] -= 1;
nOfValidTracks[col] -= 1;
repeatSteps = true;
}
}
}
}
} /* while(repeatSteps) */
/* for each multiply validated track that validates only with singly validated */
/* observations, choose the observation with minimum distance */
for(row=0; row<nOfRows; row++)
{
if(nOfValidObservations[row] > 1)
{
allSinglyValidated = true;
minValue = inf;
for(col=0; col<nOfColumns; col++)
{
value = distMatrix[row + nOfRows*col];
if(mxIsFinite(value))
{
if(nOfValidTracks[col] > 1)
{
allSinglyValidated = false;
break;
}
else if((nOfValidTracks[col] == 1) && (value < minValue))
{
tmpCol = col;
minValue = value;
}
}
}
if(allSinglyValidated)
{
#ifdef ONE_INDEXING
assignment[row] = tmpCol + 1;
#else
assignment[row] = tmpCol;
#endif
*cost += minValue;
for(n=0; n<nOfRows; n++)
distMatrix[n + nOfRows*tmpCol] = inf;
for(n=0; n<nOfColumns; n++)
distMatrix[row + nOfRows*n] = inf;
}
}
}
/* for each multiply validated observation that validates only with singly validated */
/* track, choose the track with minimum distance */
for(col=0; col<nOfColumns; col++)
{
if(nOfValidTracks[col] > 1)
{
allSinglyValidated = true;
minValue = inf;
for(row=0; row<nOfRows; row++)
{
value = distMatrix[row + nOfRows*col];
if(mxIsFinite(value))
{
if(nOfValidObservations[row] > 1)
{
allSinglyValidated = false;
break;
}
else if((nOfValidObservations[row] == 1) && (value < minValue))
{
tmpRow = row;
minValue = value;
}
}
}
if(allSinglyValidated)
{
#ifdef ONE_INDEXING
assignment[tmpRow] = col + 1;
#else
assignment[tmpRow] = col;
#endif
*cost += minValue;
for(n=0; n<nOfRows; n++)
distMatrix[n + nOfRows*col] = inf;
for(n=0; n<nOfColumns; n++)
distMatrix[tmpRow + nOfRows*n] = inf;
}
}
}
} /* if(infiniteValueFound) */
/* now, recursively search for the minimum element and do the assignment */
while(true)
{
/* find minimum distance observation-to-track pair */
minValue = inf;
for(row=0; row<nOfRows; row++)
for(col=0; col<nOfColumns; col++)
{
value = distMatrix[row + nOfRows*col];
if(mxIsFinite(value) && (value < minValue))
{
minValue = value;
tmpRow = row;
tmpCol = col;
}
}
if(mxIsFinite(minValue))
{
#ifdef ONE_INDEXING
assignment[tmpRow] = tmpCol+ 1;
#else
assignment[tmpRow] = tmpCol;
#endif
*cost += minValue;
for(n=0; n<nOfRows; n++)
distMatrix[n + nOfRows*tmpCol] = inf;
for(n=0; n<nOfColumns; n++)
distMatrix[tmpRow + nOfRows*n] = inf;
}
else
break;
} /* while(true) */
/* free allocated memory */
mxFree(nOfValidObservations);
mxFree(nOfValidTracks);
}
void posterior_t_garch_noS_hyper_mex(double *y, mwSignedIndex N, mwSignedIndex T, double *S,
double *theta, double *hyper, double *GamMat, mwSignedIndex G, double *d)
{
mwSignedIndex *r1;
double *r2;
// double *omega, *rho;
double *rho;
double h, *pdf;
double rhoh;
mwSignedIndex i, j;
/* Variable size arrays */
r1 = mxMalloc((N)*sizeof(mwSignedIndex));
r2 = mxMalloc((N)*sizeof(double));
// omega = mxMalloc((N)*sizeof(double));
rho = mxMalloc((N)*sizeof(double));
pdf = mxMalloc((1)*sizeof(double));
prior_t_garch_hyper(theta, hyper, N, r1, r2);
/* Initialise */
for (i=0; i<N; i++)
{
// omega[i] = S[0]*(1-theta[i]-theta[N+i]);
rho[i] = (theta[i+4*N]-2)/theta[i+4*N];
// mexPrintf("omega[%i] = %6.4f\n",i,omega[i] );
// mexPrintf("rho[%i] = %6.4f\n",i,rho[i]);
// mexPrintf("alpha[%i] = %6.4f\n", i, theta[i]);
// mexPrintf("beta[%i] = %6.4f\n", i, theta[i+N]);
// mexPrintf("mu[%i] = %6.4f\n", i, theta[2*N+i]);
// mexPrintf("nu[%i] = %6.4f\n", i, theta[i+3*N]);
}
/* PDF */
for (i=0; i<N; i++)
{
if (r1[i]==1)
{
d[i] = r2[i];
h = S[0];
// mexPrintf("d[%i] = %6.4f\n",i,d[i]);
// mexPrintf("h = %6.4f\n", h);
for (j=1; j<T; j++)
{
// mexPrintf("i = %i\n",i);
// mexPrintf("j = %i\n",j);
// mexPrintf("y[%i] = %6.4f\n", j-1, y[j-1]);
//
// mexPrintf("alpha[%i] = %6.4f\n", i, theta[i]);
// mexPrintf("beta[%i] = %6.4f\n", i, theta[i+N]);
// mexPrintf("mu[%i] = %6.4f\n", i, theta[2*N+i]);
// mexPrintf("nu[%i] = %6.4f\n", i, theta[i+3*N]);
// // /* h[i] = beta[i]*h[i] + omega[i] + alpha[i]*(y[j-1]-mu[i])*(y[j-1]-mu[i]); */
// h = theta[N+i]*h + omega[i] + theta[i]*(y[j-1]-theta[2*N+i])*(y[j-1]-theta[2*N+i]);
h = theta[2*N+i]*h + theta[i] + theta[N+i]*(y[j-1]-theta[3*N+i])*(y[j-1]-theta[3*N+i]);
// mexPrintf("h = %6.4f\n", h);
rhoh = rho[i]*h;
// mexPrintf("rhoh = %6.4f\n", rhoh);
duvt_garch(y[j], theta[i+3*N], rhoh, theta[i+4*N], GamMat, G, pdf);
// mexPrintf("pdf[%i] = %16.14f\n", j, pdf[0]);
d[i] = d[i] + log(pdf[0]);
}
}
else
{
// d[i] = M;
d[i] = -mxGetInf();
}
// mexPrintf("d[%i] = %6.4f\n",i,d[i]);
}
/* Free allocated memory */
mxFree(r1);
mxFree(r2);
mxFree(rho);
// mxFree(omega);
}
void mexFunction( int nlhs, mxArray *plhs[],
int nrhs, const mxArray *prhs[] )
{
mxLogical *image;
mxLogical *path;
bool *track;
double *dist, *idx;
mwSize w, h, size, sizeIdx, i, x, y;
long conn;
double inf = mxGetInf();
double nan = mxGetNaN();
if (nrhs < 2){
mexErrMsgIdAndTxt( "Image:distgeodesic:wrongInputCount",
"At least two input (image, startPoints) expected.");
}
if (nrhs > 3){
mexErrMsgIdAndTxt( "Image:distgeodesic:wrongInputCount",
"Too may input parameter.");
}
if (!mxIsDouble(prhs[1])){
mexErrMsgIdAndTxt( "Image:distgeodesic:wrongInputType",
"Index parameter has to be doubles.");
}
if (nrhs < 3 || mxGetM(prhs[2]) * mxGetN(prhs[2]) == 0){
conn = 6;
}
else {
if (mxIsDouble(prhs[2])){
if (mxGetM(prhs[2]) * mxGetN(prhs[2]) == 1){
conn = (long) mxGetPr(prhs[2])[0];
}
else {
mexErrMsgIdAndTxt( "Image:distgeodesic:wrongInputType",
"Connection parameter has to be a scalar.");
}
}
else {
mexErrMsgIdAndTxt( "Image:distgeodesic:wrongInputType",
"Connection parameter has to be a double.");
}
}
h = mxGetM(prhs[0]);
w = mxGetN(prhs[0]);
size = h*w;
/* The input must be a logical.*/
if (!mxIsLogical(prhs[0])){
image = mxGetLogicals(mxCreateLogicalMatrix(h, w));
double *data = mxGetPr(prhs[0]);
for (int i = 0; i < size; i += 1){
image[i] = (mxLogical) data[i];
}
}
else {
image = mxGetLogicals(prhs[0]);
}
/* Create matrix for the return argument. */
plhs[0] = mxCreateDoubleMatrix(h, w, mxREAL);
dist = mxGetPr(plhs[0]);
track = new bool[size];
for (i = 0; i < size; i += 1){
track[i] = false;
dist[i] = image[i]? inf: nan;
}
idx = mxGetPr(prhs[1]);
sizeIdx = mxGetM(prhs[1]) * mxGetN(prhs[1]);
bool any = false;
for (i = 0; i < sizeIdx; i += 1){
mwSize startIdx = (long) idx[i];
if (idx[i] != startIdx){
mexErrMsgIdAndTxt( "Image:distgeodesic:idxNotIntegral",
"Startindex has to be an integral number.");
}
if (startIdx <= 0 || startIdx > size){
mexErrMsgIdAndTxt( "Image:distgeodesic:idxOutOfRange",
"Startindex is out of range.");
}
track[startIdx - 1] = true;
dist[startIdx - 1] = 0;
any = true;
}
bool *trackCopy = new bool[size];
while (any){
any = false;
for (i = 0; i < size; i += 1){
trackCopy[i] = track[i];
}
for (i = 0; i < size; i += 1){ // cityblock
if (trackCopy[i]){
track[i] = false;
y = i % h;
x = (i - y) / h;
setDist(x - 1, y, i, 1);
setDist(x + 1, y, i, 1);
setDist(x, y - 1, i, 1);
setDist(x, y + 1, i, 1);
}
}
if (conn == 6){ // chessboard
for (i = 0; i < size; i += 1){
if (trackCopy[i]){
y = i % h;
x = (i - y) / h;
setDist(x - 1, y - 1, i, 1);
setDist(x - 1, y + 1, i, 1);
setDist(x + 1, y - 1, i, 1);
setDist(x + 1, y + 1, i, 1);
}
}
}
if (conn == 8){ // quasi-euclidean
for (i = 0; i < size; i += 1){
if (trackCopy[i]){
y = i % h;
x = (i - y) / h;
setDist(x - 1, y - 1, i, M_SQRT2);
setDist(x - 1, y + 1, i, M_SQRT2);
setDist(x + 1, y - 1, i, M_SQRT2);
setDist(x + 1, y + 1, i, M_SQRT2);
}
}
}
}
delete[] trackCopy;
trackCopy = nullptr;
/* free memory */
delete[] track;
track = nullptr;
}
void dodijk_sparse(
long int M,
long int N,
long int S,
double *D,
double *sr,
int *irs,
int *jcs,
HeapNode *A,
FibHeap *theHeap )
{
int finished;
long int i,startind,endind,whichneighbor,ndone,index,switchwith,closest,closesti;
long int *INDICES;
double closestD,arclength;
double INF,SMALL,olddist;
HeapNode *Min;
HeapNode Temp;
INF = mxGetInf();
SMALL = mxGetEps();
/* initialize */
for (i=0; i<M; i++)
{
if (i!=S) A[ i ] = (double) INF; else A[ i ] = (double) SMALL;
if (i!=S) D[ i ] = (double) INF; else D[ i ] = (double) SMALL;
theHeap->Insert( &A[i] );
A[ i ].SetIndexValue( (long int) i );
}
// Insert 0 then extract it. This will cause the
// Fibonacci heap to get balanced.
theHeap->Insert(&Temp);
theHeap->ExtractMin();
/*theHeap->Print();
for (i=0; i<M; i++)
{
closest = A[ i ].GetIndexValue();
closestD = A[ i ].GetKeyValue();
mexPrintf( "Index at i=%d =%d value=%f\n" , i , closest , closestD );
}*/
/* loop over nonreached nodes */
finished = 0;
ndone = 0;
while ((finished==0) && (ndone < M))
{
//if ((ndone % 100) == 0) mexPrintf( "Done with node %d\n" , ndone );
Min = (HeapNode *) theHeap->ExtractMin();
closest = Min->GetIndexValue();
closestD = Min->GetKeyValue();
if ((closest<0) || (closest>=M)) mexErrMsgTxt( "Minimum Index out of bound..." );
//theHeap->Print();
//mexPrintf( "EXTRACTED MINIMUM NDone=%d S=%d closest=%d closestD=%f\n" , ndone , S , closest , closestD );
//mexErrMsgTxt( "Exiting..." );
D[ closest ] = closestD;
if (closestD == INF) finished=1; else
{
/* add the closest to the determined list */
ndone++;
/* relax all nodes adjacent to closest */
startind = jcs[ closest ];
endind = jcs[ closest+1 ] - 1;
if (startind!=endind+1)
for (i=startind; i<=endind; i++)
{
whichneighbor = irs[ i ];
arclength = sr[ i ];
olddist = D[ whichneighbor ];
//mexPrintf( "INSPECT NEIGHBOR #%d olddist=%f newdist=%f\n" , whichneighbor , olddist , closestD+arclength );
if ( olddist > ( closestD + arclength ))
{
D[ whichneighbor ] = closestD + arclength;
Temp = A[ whichneighbor ];
Temp.SetKeyValue( closestD + arclength );
theHeap->DecreaseKey( &A[ whichneighbor ], Temp );
//mexPrintf( "UPDATING NODE #%d olddist=%f newdist=%f\n" , whichneighbor , olddist , closestD+arclength );
}
}
}
}
}
void
mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
{
int nA, nB, N;
int i, j;
/* Check for proper number of arguments */
if (nrhs != 2) {
mexErrMsgTxt("CLOSEST requires two input arguments.");
}
if (mxGetM(A_IN) != mxGetM(B_IN)) {
mexErrMsgTxt("Input arguments must have the same number of rows");
}
if (mxGetClassID(A_IN) != mxGetClassID(B_IN)) {
mexErrMsgTxt("Input arguments must have the same type");
}
N = mxGetM(A_IN);
nA = mxGetN(A_IN);
nB = mxGetN(B_IN);
if (mxGetClassID(A_IN) == mxDOUBLE_CLASS) {
/*****************************************************
* double precision
*****************************************************/
double *A, *B, *B0, *K, *D, *D2;
double *bins, *binnum;
A = mxGetPr(A_IN);
B0 = B = mxGetPr(B_IN);
K_OUT = mxCreateDoubleMatrix(1, nA, mxREAL);
K = mxGetPr(K_OUT);
if (nlhs >= 2) {
D_OUT = mxCreateDoubleMatrix(1, nA, mxREAL);
D = mxGetPr(D_OUT);
}
if (nlhs == 3) {
D2_OUT = mxCreateDoubleMatrix(1, nA, mxREAL);
D2 = mxGetPr(D2_OUT);
}
for (i=0; i<nA; i++) {
double min, min2;
int which;
min = mxGetInf();
min2 = mxGetInf();
for (j=0, B=B0; j<nB; j++) {
register double t=0, d, *p1, *p2;
register int k;
for (k=0, p1=A, p2=B; k<N; k++) {
d = (*p1++ - *p2++);
t += d*d;
/*
if (t > min)
goto shortcut;
*/
}
if (t<min) {
min = t;
which = j;
} else if (t<min2) {
min2 = t;
}
shortcut:
B += N; // step through columns of B
}
K[i] = which+1;
// optionally save the distance between the points
if (nlhs >= 2)
D[i] = sqrt(min);
if (nlhs == 3)
D2[i] = sqrt(min2);
A += N; // step through columns of A
}
} else if (mxGetClassID(A_IN) == mxSINGLE_CLASS) {
/*****************************************************
* single precision
*****************************************************/
float *A, *B, *B0, *K, *D, *D2;
float *bins, *binnum;
A = (float *)mxGetPr(A_IN);
B0 = B = (float *)mxGetPr(B_IN);
K_OUT = mxCreateNumericMatrix(1, nA, mxSINGLE_CLASS, mxREAL);
K = (float *)mxGetPr(K_OUT);
if (nlhs >= 2) {
D_OUT = mxCreateNumericMatrix(1, nA, mxSINGLE_CLASS, mxREAL);
D = (float *)mxGetPr(D_OUT);
}
if (nlhs == 3) {
D2_OUT = mxCreateNumericMatrix(1, nA, mxSINGLE_CLASS, mxREAL);
D2 = (float *)mxGetPr(D2_OUT);
}
for (i=0; i<nA; i++) {
float min, min2;
int which;
min = mxGetInf();
min2 = mxGetInf();
for (j=0, B=B0; j<nB; j++) {
register float t=0, d, *p1, *p2;
register int k;
for (k=0, p1=A, p2=B; k<N; k++) {
d = (*p1++ - *p2++);
t += d*d;
/*
if (t > min)
goto shortcut2;
*/
}
if (t<min) {
min = t;
which = j;
} else if (t<min2) {
min2 = t;
}
shortcut2:
B += N; // step through columns of B
}
K[i] = which+1;
// optionally save the distance between the points
if (nlhs >= 2)
D[i] = sqrt(min);
if (nlhs == 3)
D2[i] = sqrt(min2);
A += N; // step through columns of A
}
}
}
/*
* disp = stereo(L, R, w, disprange, metric)
*
* L and R are images (uint8 or double) of size NxM
* w is a 1-vector wx=wy=w or a 2-vector [wx wy] window size
* disprange is a 2-vector, D = abs(disprange(2)-disprange(1))
*
* disp is a NxMxD matrix of int16
*/
void
mexFunction(
int nlhs, mxArray *plhs[],
int nrhs, const mxArray *prhs[])
{
unsigned int width_l, height_l, height_r, width_r, width, height;
int wx, wy, i, j, dispmin, dispmax;
float *leftI, *rightI, *leftI_ptr, *rightI_ptr;
double *p, *scoresD;
mwSize dims[3];
double Z_NAN = mxGetNaN();
double Z_INFINITY = mxGetInf();
#ifdef TIMING
struct timeval t0, t1, t2, t3, t4;
#endif
char metric[8];
#ifdef TIMING
gettimeofday (&t0, NULL);
#endif
/* Check for proper number of arguments */
switch (nrhs) {
case 4:
strcpy(metric, "zncc");
break;
case 5:
if (!mxIsChar(prhs[4]))
mexErrMsgTxt("metric must be specified by a string");
mxGetString(prhs[4], metric, 8);
break;
default:
mexErrMsgTxt("expecting 4 or 5 arguments");
return;
}
/* Check that input images are same size */
height_l = mxGetM(prhs[0]);
width_l = mxGetN(prhs[0]);
height_r = mxGetM(prhs[1]);
width_r = mxGetN(prhs[1]);
if ((height_l != height_r) || (width_l != width_r))
mexErrMsgTxt("Left and right images must be the same size");
height = height_l;
width = width_l;
/* get window size */
switch (mxGetNumberOfElements(prhs[2])) {
case 1:
wx = wy = mxGetScalar(prhs[2]);
break;
case 2: {
double *t = mxGetPr(prhs[2]);
wx = t[0];
wy = t[1];
break;
}
default:
mexErrMsgTxt("Window size must be 1- or 2-vector");
return;
}
/* get disparity range */
switch (mxGetNumberOfElements(prhs[3])) {
case 1:
dispmin = 0;
dispmax = (int) mxGetScalar(prhs[3]);
break;
case 2: {
double *d = mxGetPr(prhs[3]);
dispmin = (int)d[0];
dispmax = (int)d[1];
break;
}
default:
mexErrMsgTxt("Disparities must be 1- or 2-vector");
return;
}
#ifdef TIMING
gettimeofday (&t1, NULL);
#endif
/* allocate float images to hold copies of the images */
leftI = (float*) mxCalloc (width * height, sizeof(float));
rightI = (float*) mxCalloc (width * height, sizeof(float));
leftI_ptr = leftI;
rightI_ptr = rightI;
if (mxGetClassID(prhs[0]) != mxGetClassID(prhs[1]))
mexErrMsgTxt("Images must be of the same class");
switch (mxGetClassID(prhs[0])) {
case mxDOUBLE_CLASS: {
double *l, *r;
l = (double *) mxGetData(prhs[0]);
r = (double *) mxGetData(prhs[1]);
for (i = 0; i < height; i++) {
for (j = 0; j < width; j++) {
*(leftI_ptr++) = (float) REF_ML(l,j,i);
*(rightI_ptr++) = (float) REF_ML(r,j,i);
}
}
}
break;
case mxUINT8_CLASS: {
PIX *l, *r;
l = (PIX *) mxGetData(prhs[0]);
r = (PIX *) mxGetData(prhs[1]);
for (i = 0; i < height; i++) {
for (j = 0; j < width; j++) {
*(leftI_ptr++) = (float) REF_ML(l,j,i);
*(rightI_ptr++) = (float) REF_ML(r,j,i);
}
}
}
break;
default:
mexErrMsgTxt("Images must be double or uint8 class");
}
#ifdef TIMING
gettimeofday (&t2, NULL);
#endif
/* Create an NxMxD matrix for the return arguments */
dims[0] = height_l;
dims[1] = width_l;
dims[2] = (int) (dispmax - dispmin);
if (dims[2] < 0)
dims[2] = -dims[2];
dims[2] += 1;
/* create 3D array to hold similarity scores */
plhs[0] = mxCreateNumericArray(3, dims, mxDOUBLE_CLASS, mxREAL);
scoresD = (double *) mxGetData(plhs[0]);
/* set all values to NaN now */
for (i=0; i<dims[0]*dims[1]*dims[2]; i++)
scoresD[i] = Z_NAN;
#ifdef notdef
/* set scores to NaN around the edges */
wx2 = (wx - 1) / 2;
wy2 = (wy - 1) / 2;
for (y=0; y<wy2; y++)
for (x=0; x<width; x++)
for (d=0; d<dims[2]; d++)
REF3(scoresD, x,y,d) = Z_NAN;
for (y=(height-wy2); y<height; y++)
for (x=0; x<width; x++)
for (d=0; d<dims[2]; d++)
REF3(scoresD, x,y,d) = Z_NAN;
for (x=0; x<wx2; x++)
for (y=0; y<height; y++)
for (d=0; d<dims[2]; d++)
REF3(scoresD, x,y,d) = Z_NAN;
for (x=(width-wx2); x<width; x++)
for (y=0; y<height; y++)
for (d=0; d<dims[2]; d++)
REF3(scoresD, x,y,d) = Z_NAN;
#endif
//mexPrintf("Image size: %d x %d\n", height, width);
//mexPrintf("Window size: %d x %d, disparity: %d - %d\n", wx, wy, dispmin, dispmax);
#ifdef TIMING
gettimeofday (&t3, NULL);
#endif
if (strcmp(metric, "zncc") == 0)
stereo_matching_zncc(leftI, rightI, scoresD, width, height, wx, wy, dispmin, dispmax);
else if (strcmp(metric, "ncc") == 0)
stereo_matching_ncc(leftI, rightI, scoresD, width, height, wx, wy, dispmin, dispmax);
else if (strcmp(metric, "ssd") == 0)
stereo_matching_ssd(leftI, rightI, scoresD, width, height, wx, wy, dispmin, dispmax);
else if (strcmp(metric, "sad") == 0)
stereo_matching_sad(leftI, rightI, scoresD, width, height, wx, wy, dispmin, dispmax);
else
mexErrMsgTxt("Unknown metric");
#ifdef TIMING
gettimeofday (&t4, NULL);
mexPrintf("arg checking %.1fms\n", time_diff(&t1, &t0)*1000);
mexPrintf("image conversion %.1fms\n", time_diff(&t2, &t1)*1000);
mexPrintf("score array %.1fms\n", time_diff(&t3, &t2)*1000);
mexPrintf("stereo %.1fms\n", time_diff(&t4, &t3)*1000);
#endif
/* free the temporary float images */
mxFree(leftI);
mxFree(rightI);
}
