int readMxCharDataFromBytes(mxArray *mx, mxGramInfo *info, int nBytes) {
int ii;
int numel = info->dataM * info->dataN;
mxChar* mxData = mxGetChars(mx);
const char *gramData = info->dataBytes;
if (nBytes >= mxGetElementSize(mx)*numel) {
if (info->dataSize == 1) {
// read 1-byte characters from data bytes
for(ii=0; ii<numel; ii++) {
mxData[ii] = *gramData;
gramData += info->dataSize;
}
} else {
// read 2-byte characters from data bytes
for(ii=0; ii<numel; ii++) {
mxData[ii] = *((mxChar*)gramData);
gramData += info->dataSize;
}
}
return(info->dataSize*numel);
} else
return(-1);
}
void MxArray::fromVector(const std::vector<char>& v)
{
const mwSize size[] = {1, v.size()};
p_ = mxCreateCharArray(2, size);
if (!p_)
mexErrMsgIdAndTxt("mexopencv:error", "Allocation error");
std::copy(v.begin(), v.end(), mxGetChars(p_));
}
/**
* @brief Converts a Matlab string to a C string
* Converts a Matlab string pointed to by the mxArray pointer in to a null
* terminated C string. This function is the reverse of the function
* CStringTomxString.
*/
void mxStringToCString(mxArray *in,char *out)
{
mxChar *temp_string;
int i,len;
len = mxGetNumberOfElements(in);
temp_string = mxGetChars(in);
for(i=0;i<len;i++)
out[i]=(char) temp_string[i];
for(i=len;i<MAXCHARS;i++)
out[i]='\0';
}
void
mexFunction (int nlhs, mxArray *plhs[], int nrhs,
const mxArray *prhs[])
{
mwIndex i, j;
mwSize m, n;
mxChar *pi, *po;
if (nrhs != 1 || ! mxIsChar (prhs[0]) ||
mxGetNumberOfDimensions (prhs[0]) > 2)
mexErrMsgTxt ("expecting char matrix");
m = mxGetM (prhs[0]);
n = mxGetN (prhs[0]);
pi = mxGetChars (prhs[0]);
plhs[0] = mxCreateNumericMatrix (m, n, mxCHAR_CLASS,
mxREAL);
po = mxGetChars (plhs[0]);
for (j = 0; j < n; j++)
for (i = 0; i < m; i++)
po [j*m + m - 1 - i] = pi [j*m + i];
}
int writeMxCharDataToBytes(const mxArray *mx, mxGramInfo *info, int nBytes) {
int ii;
int numel = info->dataM * info->dataN;
mxChar* mxData = mxGetChars(mx);
char *gramData = info->dataBytes;
if (nBytes >= info->dataSize*numel) {
// write 1- or 2-byte chars to data bytes
for(ii=0; ii<numel; ii++) {
*((mxChar*)gramData) = mxData[ii];
gramData += info->dataSize;
}
return(info->dataSize*numel);
} else
return(-1);
}
static void* calcSize(mxArray* data, uint64_t* size) {
uint64_t numel = mxGetNumberOfElements(data);
switch (mxGetClassID(data)) {
case mxLOGICAL_CLASS: ;
case mxINT8_CLASS:
case mxUINT8_CLASS:
*size = numel;
return mxGetData(data);
case mxCHAR_CLASS:
*size = numel * sizeof(mxChar);
return mxGetChars(data);
case mxDOUBLE_CLASS:
*size = numel * sizeof(double);
return mxGetPr(data);
case mxSINGLE_CLASS:
*size = numel * sizeof(float);
return mxGetData(data);
case mxINT16_CLASS:
case mxUINT16_CLASS:
*size = numel * sizeof(short);
return mxGetData(data);
case mxINT32_CLASS:
case mxUINT32_CLASS:
*size = numel * sizeof(int);
return mxGetData(data);
case mxINT64_CLASS:
case mxUINT64_CLASS:
*size = numel * sizeof(int64_t);
return mxGetData(data);
default: {
char s[256];
sprintf(s, "GridFS - Unsupported type (%s)\n", mxGetClassName(data));
mexErrMsgTxt(s);
return 0;
}
}
}
char* CMatlabInterface::get_string(int32_t& len)
{
bool zero_terminate=true;
const mxArray* s=get_arg_increment();
if ( !(mxIsChar(s)) || (mxGetM(s)!=1) )
SG_ERROR("Expected String as argument %d\n", m_rhs_counter);
len=mxGetN(s);
char* string=NULL;
if (zero_terminate)
string=new char[len+1];
else
string=new char[len];
mxChar* c=mxGetChars(s);
ASSERT(c);
for (int32_t i=0; i<len; i++)
string[i]= (char) (c[i]);
if (zero_terminate)
string[len]='\0';
return string;
}
void agent_act(unsigned char * img_bytes, int img_width, int img_height, bool_t img_is_belly, int pass_button,
navdata_unpacked_t * navdata, commands_t * commands) {
// Need engine
if (!ep) {
return;
}
// Create flat matrix big enough to hold WIDTH X HEIGHT X 3 (RGB) image data
mxArray * img = create_numeric_array(img_height*img_width*3, mxUINT8_CLASS);
// Copy image bytes to matrix in order appropriate for reshaping
unsigned char * p = (unsigned char *)mxGetChars(img);
int rgb, row, col;;
for (rgb=0; rgb<3; ++rgb) {
for (col=0; col<img_width; ++col) {
for (row=0; row<img_height; ++row) {
*p = img_bytes[2-rgb + 3*(row * img_width + col)];
p++;
}
}
}
// Put the image variable into the Matlab environment
put_variable("img", img);
// Create a variable for passing navigation data to Matlab function
mxArray * mx_navdata = create_numeric_array(10, mxDOUBLE_CLASS);
// Build command using reshaped IMG variable and constants from navdata structure
char cmd[200];
double * np = (double *)mxGetData(mx_navdata);
navdata_demo_t demo = navdata->navdata_demo;
*np++ = (double)(img_is_belly?1:0);
*np++ = (double)pass_button;
*np++ = (double)demo.ctrl_state;
*np++ = (double)demo.vbat_flying_percentage;
*np++ = demo.theta;
*np++ = demo.phi;
*np++ = demo.psi;
*np++ = (double)navdata->navdata_altitude.altitude_raw;
navdata_vision_raw_t vision_raw = navdata->navdata_vision_raw;
*np++ = (double)vision_raw.vision_tx_raw;
*np++ = (double)vision_raw.vision_ty_raw;
// Put the navdata variable into the Matlab environment
put_variable("navdata", mx_navdata);
// Build and evaluate command
sprintf(cmd,"commands = %s(reshape(img, %d, %d, 3), navdata);", FUNCTION_NAME, img_height, img_width);
if (engEvalString(ep, cmd)) {
fprintf(stderr, "Error evaluating command: %s\n", cmd);
}
// Get output variables
double *cp = (double *)mxGetData(engGetVariable(ep, "commands"));
commands->zap = (int)cp[0];
commands->phi = cp[1];
commands->theta = cp[2];
commands->gaz = cp[3];
commands->yaw = cp[4];
// Deallocate memory
mxDestroyArray(img);
mxDestroyArray(mx_navdata);
}
mxArray *matrix_from_ft_type_data(UINT32_T type, UINT32_T rows, UINT32_T cols, const void *data) {
mxArray *A;
switch (type) {
case DATATYPE_CHAR:
{
/* TODO: is there a proper opposite of mxArrayToString that we could use here???
DATATYPE_CHAR in the FieldTrip buffer is defined to be 8 bits, but
mxChar elements are 16-bit unicode (I think).
*/
mxChar *dest;
mwSize dim[2];
const char *src = (const char *) data;
UINT32_T n;
dim[0] = rows;
dim[1] = cols;
A = mxCreateCharArray(2, dim);
dest = mxGetChars(A);
for (n=0;n<rows*cols;n++) {
*dest++ = *src++;
}
}
break;
case DATATYPE_UINT8:
A = mxCreateNumericMatrix(rows, cols, mxUINT8_CLASS, mxREAL);
memcpy(mxGetData(A), data, rows*cols*WORDSIZE_UINT8);
break;
case DATATYPE_UINT16:
A = mxCreateNumericMatrix(rows, cols, mxUINT16_CLASS, mxREAL);
memcpy(mxGetData(A), data, rows*cols*WORDSIZE_UINT16);
break;
case DATATYPE_UINT32:
A = mxCreateNumericMatrix(rows, cols, mxUINT32_CLASS, mxREAL);
memcpy(mxGetData(A), data, rows*cols*WORDSIZE_UINT32);
break;
case DATATYPE_UINT64:
A = mxCreateNumericMatrix(rows, cols, mxUINT64_CLASS, mxREAL);
memcpy(mxGetData(A), data, rows*cols*WORDSIZE_UINT64);
break;
case DATATYPE_INT8:
A = mxCreateNumericMatrix(rows, cols, mxINT8_CLASS, mxREAL);
memcpy(mxGetData(A), data, rows*cols*WORDSIZE_INT8);
break;
case DATATYPE_INT16:
A = mxCreateNumericMatrix(rows, cols, mxINT16_CLASS, mxREAL);
memcpy(mxGetData(A), data, rows*cols*WORDSIZE_INT16);
break;
case DATATYPE_INT32:
A = mxCreateNumericMatrix(rows, cols, mxINT32_CLASS, mxREAL);
memcpy(mxGetData(A), data, rows*cols*WORDSIZE_INT32);
break;
case DATATYPE_INT64:
A = mxCreateNumericMatrix(rows, cols, mxINT64_CLASS, mxREAL);
memcpy(mxGetData(A), data, rows*cols*WORDSIZE_INT64);
break;
case DATATYPE_FLOAT32:
A = mxCreateNumericMatrix(rows, cols, mxSINGLE_CLASS, mxREAL);
memcpy(mxGetData(A), data, rows*cols*WORDSIZE_FLOAT32);
break;
case DATATYPE_FLOAT64:
A = mxCreateNumericMatrix(rows, cols, mxDOUBLE_CLASS, mxREAL);
memcpy(mxGetData(A), data, rows*cols*WORDSIZE_FLOAT64);
break;
default:
A = mxCreateDoubleMatrix(0,0, mxREAL);
/* TODO: should we rather return NULL here, or maybe
A = mxCreateString("--invalid data type--");
*/
}
return A;
}
void mexFunction( int nlhs, mxArray *plhs[],
int nrhs, const mxArray *prhs[] ) {
int cols;
int rowsE;
int rowsN;
int rowsZ;
int rowsH;
double* E;
double* N;
mxChar* utmzone;
double* h;
double* lla;
double bet[7] = {0, bet1, bet2, bet3, bet4, bet5, bet6};
int i;
double x,y;
double zone;
double lon0;
double xi;
double eta;
int xisign;
int etasign;
int backside;
double c0;
double ch0;
double s0;
double sh0;
double ar;
double ai;
int n;
double xip0;
double etap0;
double xip1;
double etap1;
double yr0;
double yi0;
double yr1;
double yi1;
double xip;
double etap;
double s;
double c;
double r;
double lam;
double phi;
double taup;
double tau;
double stol;
int j;
double tau1;
double sig;
double taupa;
double dtau;
double lat;
double lon;
/* Check for proper number and size of arguments */
if (nrhs != 4) {
mexErrMsgTxt("Four input arguments required.");
}
if (nlhs != 1) {
mexErrMsgTxt("One output argument required.");
}
cols = (int)mxGetN(prhs[0]);
rowsE = (int)mxGetM(prhs[0]);
rowsN = (int)mxGetM(prhs[1]);
rowsZ = (int)mxGetM(prhs[2]);
rowsH = (int)mxGetM(prhs[3]);
if ((rowsE != 1)||(rowsN != 1)||(rowsZ != 3)||(rowsH != 1)) {
mexErrMsgTxt("Input has wrong dimensions.");
}
/* get pointers */
E = mxGetPr(prhs[0]);
N = mxGetPr(prhs[1]);
utmzone = mxGetChars(prhs[2]);
h = mxGetPr(prhs[3]);
/* Create a matrix for the return argument */
plhs[0] = mxCreateDoubleMatrix(3, cols, mxREAL);
lla = mxGetPr(plhs[0]);
for(i=0; i<cols; i++) {
if (utmzone[2+3*i]>'X' || utmzone[2+3*i]<'C') {
mexPrintf("utmToLla: Warning utmzone should be a vector of strings like 30T, not 30t\n");
}
x=E[i];
y=N[i];
x = x -FE;
y = (utmzone[2+3*i]>'M') ? y : y-FN;
zone = (utmzone[3*i]-'0')*10+(utmzone[1+3*i]-'0');
lon0 = ( ( zone * 6 ) - 183 );
xi = y / (a1*k0);
eta = x / (a1*k0);
/* Explicitly enforce the parity*/
xisign = (xi < 0) ? -1 : 1;
etasign = (eta < 0) ? -1 : 1;
xi = xi*xisign;
eta = eta*etasign;
backside = (xi > M_PI/2);
if (backside) {
xi = M_PI - xi;
}
c0 = cos(2 * xi);
ch0 = cosh(2 * eta);
s0 = sin(2 * xi);
sh0 = sinh(2 * eta);
ar = 2 * c0 * ch0;
ai = -2 * s0 * sh0;
n = MAXPOW;
/* Accumulators for zeta'*/
xip0 = (n & 1 ? -bet[n] : 0);
etap0 = 0;
xip1 = 0;
etap1 = 0;
/* Accumulators for dzeta'/dzeta*/
yr0 = (n & 1 ? - 2 * MAXPOW * bet[n--] : 0);
yi0 = 0;
yr1 = 0;
yi1 = 0;
while (n>0) {
xip1 = ar * xip0 - ai * etap0 - xip1 - bet[n];
etap1 = ai * xip0 + ar * etap0 - etap1;
yr1 = ar * yr0 - ai * yi0 - yr1 - 2 * n * bet[n];
yi1 = ai * yr0 + ar * yi0 - yi1;
n=n-1;
xip0 = ar * xip1 - ai * etap1 - xip0 - bet[n];
etap0 = ai * xip1 + ar * etap1 - etap0;
yr0 = ar * yr1 - ai * yi1 - yr0 - 2 * n * bet[n];
yi0 = ai * yr1 + ar * yi1 - yi0;
n=n-1;
}
ar = s0 * ch0;
ai = c0 * sh0;
xip = xi + ar * xip0 - ai * etap0;
etap = eta + ai * xip0 + ar * etap0;
/* Convergence and scale for Gauss-Schreiber TM to Gauss-Krueger TM.*/
s = sinh(etap);
c = MAX(0, cos(xip)); /* cos(M_PI/2) might be negative*/
r = hypot(s, c);
if (r != 0) {
lam = atan2(s, c); /* Krueger p 17 (25)*/
/* Use Newton's method to solve for tau*/
taup = sin(xip)/r;
/* To lowest order in e^2, taup = (1 - e^2) * tau = _e2m * tau; so use
* % tau = taup/_e2m as a starting guess. Only 1 iteration is needed for
* % |lat| < 3.35 deg, otherwise 2 iterations are needed. If, instead,
* % tau = taup is used the mean number of iterations increases to 1.99
* % (2 iterations are needed except near tau = 0).*/
tau = taup/e2m;
stol = tol_ * MAX(1, fabs(taup));
for (j = 1; j<=5; j++) {
tau1 = hypot(1, tau);
sig = sinh( e*atanh(e* tau / tau1 ) );
taupa = hypot(1, sig) * tau - sig * tau1;
dtau = (taup-taupa)*(1+e2m*tau*tau)/(e2m*tau1*hypot(1, taupa));
tau = tau + dtau;
if (!(fabs(dtau) >= stol)) {
break;
}
}
phi = atan(tau);
} else {
phi = M_PI/2;
lam = 0;
}
lat = phi / (M_PI/180) * xisign;
lon = lam / (M_PI/180);
if (backside) {
lon = 180 - lon;
}
lon = lon*etasign;
/* Avoid losing a bit of accuracy in lon (assuming lon0 is an integer) */
if (lon + lon0 >= 180) {
lon = lon + lon0 - 360;
} else {
if (lon + lon0 < -180) {
lon = lon + lon0 + 360;
} else {
lon = lon + lon0;
}
}
lla[3*i] = lat;
lla[1+3*i] = lon;
lla[2+3*i] = h[i];
}
}
// Takes a MATLAB variable and writes in in Mathematica form to link
void toMma(const mxArray *var, MLINK link) {
// the following may occur when retrieving empty struct fields
// it showsup as [] in MATLAB so we return {}
// note that non-existent variables are caught and handled in eng_get()
if (var == NULL) {
MLPutFunction(link, "List", 0);
return;
}
// get size information
mwSize depth = mxGetNumberOfDimensions(var);
const mwSize *mbDims = mxGetDimensions(var);
// handle zero-size arrays
if (mxIsEmpty(var)) {
if (mxIsChar(var))
MLPutString(link, "");
else
MLPutFunction(link, "List", 0);
return;
}
// translate dimension information to Mathematica order
std::vector<int> mmDimsVec(depth);
std::reverse_copy(mbDims, mbDims + depth, mmDimsVec.begin());
int *mmDims = &mmDimsVec[0];
int len = mxGetNumberOfElements(var);
// numerical (sparse or dense)
if (mxIsNumeric(var)) {
mxClassID classid = mxGetClassID(var);
// verify that var is of a supported class
switch (classid) {
case mxDOUBLE_CLASS:
case mxSINGLE_CLASS:
case mxINT32_CLASS:
case mxINT16_CLASS:
case mxUINT16_CLASS:
case mxINT8_CLASS:
case mxUINT8_CLASS:
break;
default:
putUnknown(var, link);
return;
}
if (mxIsSparse(var)) {
// Note: I realised that sparse arrays can only hold double precision numerical types
// in MATLAB R2013a. I will leave the below implementation for single precision & integer
// types in case future versions of MATLAB will add support for them.
int ncols = mxGetN(var); // number of columns
mwIndex *jc = mxGetJc(var);
mwIndex *ir = mxGetIr(var);
int nnz = jc[ncols]; // number of nonzeros
MLPutFunction(link, CONTEXT "matSparseArray", 4);
mlpPutIntegerList(link, jc, ncols + 1);
mlpPutIntegerList(link, ir, nnz);
// if complex, put as im*I + re
if (mxIsComplex(var)) {
MLPutFunction(link, "Plus", 2);
MLPutFunction(link, "Times", 2);
MLPutSymbol(link, "I");
switch (classid) {
case mxDOUBLE_CLASS:
MLPutReal64List(link, mxGetPi(var), nnz); break;
case mxSINGLE_CLASS:
MLPutReal32List(link, (float *) mxGetImagData(var), nnz); break;
case mxINT16_CLASS:
MLPutInteger16List(link, (short *) mxGetImagData(var), nnz); break;
case mxINT32_CLASS:
MLPutInteger32List(link, (int *) mxGetImagData(var), nnz); break;
default:
assert(false); // should never reach here
}
}
switch (classid) {
case mxDOUBLE_CLASS:
MLPutReal64List(link, mxGetPr(var), nnz); break;
case mxSINGLE_CLASS:
MLPutReal32List(link, (float *) mxGetData(var), nnz); break;
case mxINT16_CLASS:
MLPutInteger16List(link, (short *) mxGetData(var), nnz); break;
case mxINT32_CLASS:
MLPutInteger32List(link, (int *) mxGetData(var), nnz); break;
default:
assert(false); // should never reach here
}
MLPutInteger32List(link, mmDims, depth);
}
else // not sparse
{
MLPutFunction(link, CONTEXT "matArray", 2);
// if complex, put as im*I + re
if (mxIsComplex(var)) {
MLPutFunction(link, "Plus", 2);
MLPutFunction(link, "Times", 2);
MLPutSymbol(link, "I");
switch (classid) {
case mxDOUBLE_CLASS:
MLPutReal64Array(link, mxGetPi(var), mmDims, NULL, depth); break;
case mxSINGLE_CLASS:
MLPutReal32Array(link, (float *) mxGetImagData(var), mmDims, NULL, depth); break;
case mxINT32_CLASS:
MLPutInteger32Array(link, (int *) mxGetImagData(var), mmDims, NULL, depth); break;
case mxINT16_CLASS:
MLPutInteger16Array(link, (short *) mxGetImagData(var), mmDims, NULL, depth); break;
case mxUINT16_CLASS:
{
int *arr = new int[len];
unsigned short *mbData = (unsigned short *) mxGetImagData(var);
std::copy(mbData, mbData + len, arr);
MLPutInteger32Array(link, arr, mmDims, NULL, depth);
delete [] arr;
break;
}
case mxINT8_CLASS:
{
short *arr = new short[len];
char *mbData = (char *) mxGetImagData(var);
std::copy(mbData, mbData + len, arr);
MLPutInteger16Array(link, arr, mmDims, NULL, depth);
delete [] arr;
break;
}
case mxUINT8_CLASS:
{
short *arr = new short[len];
unsigned char *mbData = (unsigned char *) mxGetImagData(var);
std::copy(mbData, mbData + len, arr);
MLPutInteger16Array(link, arr, mmDims, NULL, depth);
delete [] arr;
break;
}
default:
assert(false); // should never reach here
}
}
switch (classid) {
case mxDOUBLE_CLASS:
MLPutReal64Array(link, mxGetPr(var), mmDims, NULL, depth); break;
case mxSINGLE_CLASS:
MLPutReal32Array(link, (float *) mxGetData(var), mmDims, NULL, depth); break;
case mxINT32_CLASS:
MLPutInteger32Array(link, (int *) mxGetData(var), mmDims, NULL, depth); break;
case mxINT16_CLASS:
MLPutInteger16Array(link, (short *) mxGetData(var), mmDims, NULL, depth); break;
case mxUINT16_CLASS:
{
int *arr = new int[len];
unsigned short *mbData = (unsigned short *) mxGetData(var);
std::copy(mbData, mbData + len, arr);
MLPutInteger32Array(link, arr, mmDims, NULL, depth);
delete [] arr;
break;
}
case mxINT8_CLASS:
{
short *arr = new short[len];
char *mbData = (char *) mxGetData(var);
std::copy(mbData, mbData + len, arr);
MLPutInteger16Array(link, arr, mmDims, NULL, depth);
delete [] arr;
break;
}
case mxUINT8_CLASS:
{
short *arr = new short[len];
unsigned char *mbData = (unsigned char *) mxGetData(var);
std::copy(mbData, mbData + len, arr);
MLPutInteger16Array(link, arr, mmDims, NULL, depth);
delete [] arr;
break;
}
default:
assert(false); // should never reach here
}
MLPutInteger32List(link, mmDims, depth);
}
}
// logical (sparse or dense)
else if (mxIsLogical(var))
if (mxIsSparse(var)) {
int ncols = mxGetN(var); // number of columns
mwIndex *jc = mxGetJc(var);
mwIndex *ir = mxGetIr(var);
mxLogical *logicals = mxGetLogicals(var);
int nnz = jc[ncols]; // number of nonzeros
MLPutFunction(link, CONTEXT "matSparseLogical", 4);
mlpPutIntegerList(link, jc, ncols + 1);
mlpPutIntegerList(link, ir, nnz);
short *integers = new short[nnz];
std::copy(logicals, logicals+nnz, integers);
MLPutInteger16List(link, integers, nnz);
MLPutInteger32List(link, mmDims, depth);
delete [] integers;
}
else // not sparse
{
mxLogical *logicals = mxGetLogicals(var);
short *integers = new short[len];
std::copy(logicals, logicals+len, integers);
MLPutFunction(link, CONTEXT "matLogical", 2);
MLPutInteger16Array(link, integers, mmDims, NULL, depth);
MLPutInteger32List(link, mmDims, depth);
delete [] integers;
}
// char array
else if (mxIsChar(var)) {
assert(sizeof(mxChar) == sizeof(unsigned short));
// 1 by N char arrays (row vectors) are sent as a string
if (depth == 2 && mbDims[0] == 1) {
const mxChar *str = mxGetChars(var);
MLPutFunction(link, CONTEXT "matString", 1);
MLPutUTF16String(link, reinterpret_cast<const unsigned short *>(str), len); // cast may be required on other platforms: (mxChar *) str
}
// general char arrays are sent as an array of characters
else {
MLPutFunction(link, CONTEXT "matCharArray", 2);
const mxChar *str = mxGetChars(var);
MLPutFunction(link, "List", len);
for (int i=0; i < len; ++i)
MLPutUTF16String(link, reinterpret_cast<const unsigned short *>(str + i), 1);
MLPutInteger32List(link, mmDims, depth);
}
}
// struct
else if (mxIsStruct(var)) {
int nfields = mxGetNumberOfFields(var);
MLPutFunction(link, CONTEXT "matStruct", 2);
MLPutFunction(link, "List", len);
for (int j=0; j < len; ++j) {
MLPutFunction(link, "List", nfields);
for (int i=0; i < nfields; ++i) {
const char *fieldname;
fieldname = mxGetFieldNameByNumber(var, i);
MLPutFunction(link, "Rule", 2);
MLPutString(link, fieldname);
toMma(mxGetFieldByNumber(var, j, i), link);
}
}
MLPutInteger32List(link, mmDims, depth);
}
// cell
else if (mxIsCell(var)) {
MLPutFunction(link, CONTEXT "matCell", 2);
MLPutFunction(link, "List", len);
for (int i=0; i < len; ++i)
toMma(mxGetCell(var, i), link);
MLPutInteger32List(link, mmDims, depth);
}
// unknown or failure; TODO distinguish between unknown and failure
else
{
putUnknown(var, link);
}
}
void json_encode_item(const mxArray *obj) {
mxArray *item;
mxChar *chars;
unsigned int field, nfields;
const char *fieldname;
mxLogical *logical_ptr;
double *double_ptr;
float *single_ptr;
uint8_t *uint8_ptr;
int8_t *int8_ptr;
uint16_t *uint16_ptr;
int16_t *int16_ptr;
uint32_t *uint32_ptr;
int32_t *int32_ptr;
uint64_t *uint64_ptr;
int64_t *int64_ptr;
unsigned int i, n;
n = mxGetNumberOfElements(obj);
if (mxIsChar(obj)) {
json_append_char('"');
chars = mxGetChars(obj);
for (i=0; i<n; i++) {
switch (chars[i]) {
case '"':
case '\\':
case '/':
json_append_char('\\');
json_append_char(*(char*)&chars[i]);
break;
case '\b':
json_append_string("\\b");
break;
case '\f':
json_append_string("\\f");
break;
case '\n':
json_append_string("\\n");
break;
case '\r':
json_append_string("\\r");
break;
case '\t':
json_append_string("\\t");
break;
default:
if ((chars[i] < 32) || (chars[i] > 126)) json_append_string("\\u%04hx",chars[i]);
else json_append_char(*(char*)&chars[i]);
}
}
json_append_char('"');
} else if (n == 0) {
json_append_string("[]");
} else {
if (n > 1) json_append_char('[');
switch (mxGetClassID(obj)) {
case mxSTRUCT_CLASS:
nfields = mxGetNumberOfFields(obj);
for (i=0; i<n; i++) {
json_append_char('{');
for (field=0; field<nfields; field++) {
fieldname = mxGetFieldNameByNumber(obj,field);
item = mxGetFieldByNumber(obj,i,field);
if (item != NULL) {
json_append_string("\"%s\":",fieldname);
json_encode_item(item);
json_append_char(',');
}
}
if (nfields > 0) json_strpos--;
json_append_string("},");
}
break;
case mxCELL_CLASS:
for (i=0; i<n; i++) {
json_encode_item(mxGetCell(obj,i));
json_append_char(',');
}
break;
case mxLOGICAL_CLASS:
logical_ptr = mxGetData(obj);
for (i=0; i<n; i++) {
if (logical_ptr[i]) {
json_append_string("true,");
} else {
json_append_string("false,");
}
}
break;
case mxDOUBLE_CLASS:
double_ptr = mxGetData(obj);
for (i=0; i<n; i++) json_append_float(double_ptr[i]);
break;
case mxSINGLE_CLASS:
single_ptr = mxGetData(obj);
for (i=0; i<n; i++) json_append_float(single_ptr[i]);
break;
case mxINT8_CLASS:
int8_ptr = mxGetData(obj);
for (i=0; i<n; i++) json_append_string("%i,",int8_ptr[i]);
break;
case mxUINT8_CLASS:
uint8_ptr = mxGetData(obj);
for (i=0; i<n; i++) json_append_string("%u,",uint8_ptr[i]);
break;
case mxINT16_CLASS:
int16_ptr = mxGetData(obj);
for (i=0; i<n; i++) json_append_string("%i,",int16_ptr[i]);
break;
case mxUINT16_CLASS:
uint16_ptr = mxGetData(obj);
for (i=0; i<n; i++) json_append_string("%u,",uint16_ptr[i]);
break;
case mxINT32_CLASS:
int32_ptr = mxGetData(obj);
for (i=0; i<n; i++) json_append_string("%i,",int32_ptr[i]);
break;
case mxUINT32_CLASS:
uint32_ptr = mxGetData(obj);
for (i=0; i<n; i++) json_append_string("%u,",uint32_ptr[i]);
break;
case mxINT64_CLASS:
int64_ptr = mxGetData(obj);
for (i=0; i<n; i++) json_append_string("%lli,",int64_ptr[i]);
break;
case mxUINT64_CLASS:
uint64_ptr = mxGetData(obj);
for (i=0; i<n; i++) json_append_string("%llu,",uint64_ptr[i]);
break;
default:
error_unsupported_class(obj);
}
if (json_strpos) json_strpos--;
if (n > 1) json_append_char(']');
}
}
_mps_matlab_options
mps_parse_matlab_options (const mxArray * optionStruct)
{
_mps_matlab_options options = {
MPS_ALGORITHM_SECULAR_GA,
MPS_OUTPUT_GOAL_APPROXIMATE,
16,
false
};
if (!optionStruct)
return options;
if (mxIsChar (optionStruct))
{
mxChar * value = mxGetChars (optionStruct);
if (strcmp ((char*) value, "s") == 0)
options.algorithm = MPS_ALGORITHM_SECULAR_GA;
else if (strcmp ((char*) value, "u") == 0)
options.algorithm = MPS_ALGORITHM_STANDARD_MPSOLVE;
else
mexErrMsgTxt ("Invalid value specified for the algorithm. Only 'u' or 's' are allowed.\n");
}
else if (! mxIsStruct (optionStruct))
mexErrMsgTxt ("Only chars and struct values are allowed as MPSolve options\n");
if (optionStruct)
{
int nFields = mxGetNumberOfFields (optionStruct);
int i;
for (i = 0; i < nFields; i++)
{
const char * optionName = mxGetFieldNameByNumber (optionStruct, i);
mxArray * field = mxGetFieldByNumber (optionStruct, 0, i);
if (strcmp (optionName, "algorithm") == 0)
{
if (! mxIsChar (field))
mexErrMsgTxt ("Please specify only 'u' or 's' for the algorithm property\n");
else
{
mxChar * value = mxGetChars (field);
if (strcmp ((char*) value, "s") == 0)
options.algorithm = MPS_ALGORITHM_SECULAR_GA;
else if (strcmp ((char*) value, "u") == 0)
options.algorithm = MPS_ALGORITHM_STANDARD_MPSOLVE;
else
mexErrMsgTxt ("Invalid value specified for the property: 'algorithm'. Only 'u' or 's' are allowed.\n");
}
}
else if (strcmp (optionName, "digits") == 0)
{
if (! mxIsNumeric (field))
mexErrMsgTxt ("Please specify a positive integer for the digits property\n");
else
{
double digits = mxGetScalar (field);
if (digits <= 0)
mexErrMsgTxt ("Please specify a positive integer for the digits property\n");
else
options.digits = (int) digits;
}
}
else if (strcmp (optionName, "goal") == 0)
{
if (! mxIsChar (field))
mexErrMsgTxt ("Please specify only 'a' or 'i' as value for the goal property\n");
else
{
mxChar * value = mxGetChars (field);
if (strcmp ((char*) value, "i") == 0)
options.goal = MPS_OUTPUT_GOAL_ISOLATE;
else if (strcmp ((char*) value, "a") == 0)
options.goal = MPS_OUTPUT_GOAL_APPROXIMATE;
else
mexErrMsgTxt ("Please specify only 'a' or 'i' as value for the goal property\n");
}
}
else if (strcmp (optionName, "radius") == 0)
{
if (! mxIsLogicalScalar (field))
mexErrMsgTxt ("Please specify 'true' or 'false' as a value for the radius property\n");
else
{
options.radius = *mxGetLogicals (field);
}
}
else
{
char * buffer = (char*) mxMalloc (sizeof (char) * (strlen ("The property '' is invalid\n") + strlen ((char*) optionName) + 1));
sprintf (buffer, "The property '%s' is invalid\n", optionName);
mexErrMsgTxt (buffer);
mxFree (buffer);
}
}
}
return options;
}
void mexFunction(
int nlhs, mxArray *plhs[],
int nrhs, const mxArray *prhs[]
) {
int k, p;
if( (nrhs<3) || (nrhs>5) ) {
mexErrMsgIdAndTxt("USB:writestring","must have 3 or 4 arguments");
}
if(nlhs>1) {
mexErrMsgIdAndTxt("USB:writestring","must have at most 1 result");
}
/**********************************************
** Check the device. */
int mrows, ncols;
k=0;
struct usb_dev_handle *usbhandle;
UINT64_T hval;
hval = scalarcheck(prhs[k],k,1);
/* This assignment generates warnings about
* pointer/integer size mismatch. */
usbhandle = (struct usb_dev_handle *)(hval);
/**********************************************
** Check the endpoint. */
k=1;
int ep;
ep = scalarcheck(prhs[k],k,0);
/**********************************************
** Check the delay value. */
int usbtimeout = 10;
if(nrhs > 3) {
k=3;
usbtimeout = scalarcheck(prhs[k],k,0);
}
// mexPrintf("Setting timeout to %d ms\n",usbtimeout);
/**********************************************
** Check the bulk/int flag. */
int do_int = 0;
if(nrhs > 4) {
k=4;
do_int = scalarcheck(prhs[k],k,0);
}
if( (do_int!=0) && (do_int!=1) ) {
mexErrMsgIdAndTxt("USB:writestring","Write type is %d. Must be 0(bulk) or 1(int)",do_int);
}
/**********************************************
** Check and extract the string. Tricky:
** Matlab uses 16-bit chars (UTF-16, with only
** the 16-bit chars allowed? Essentially UCS-2? )
** FIXME: mxChar is 8 bits in Octave, 16 bits in Matlab.
*/
k=2;
mrows = mxGetM(prhs[k]);
ncols = mxGetN(prhs[k]);
// mexPrintf("rows %d cols %d\n",mrows,ncols);
mxChar *mxch;
mxch = mxGetChars(prhs[k]);
// mexPrintf("size of mxChar is %d vs char %d\n",sizeof(mxChar),sizeof(char));
// if(mxch==0) mexPrintf("mxch is NULL\n");
// else {
// for(p=0;p<mrows*ncols;p++) mexPrintf("%d %4.4x %c %lc\n",
// p,mxch[p],mxch[p],mxch[p]);
// }
if(mrows!=1) {
mexErrMsgIdAndTxt("USB:writestring","usb_writestring: can only handle 1-row character arrays");
}
char *str;
str = mxCalloc(ncols+1,sizeof(char));
if(str==NULL) {
mexErrMsgIdAndTxt("USB:writestring","usb_writestring: Trouble allocating internal memory");
}
/* Copy, possibly with a lossy conversion from 16 to 8 bits. */
for(p=0;p<ncols;p++) str[p] = mxch[p];
str[p] = '\0'; /* Not needed. */
// mexPrintf("handle %x ep %d %x Output string %s\n",
// usbhandle,ep,ep, str);
int rtn;
if(do_int==0) {
rtn = usb_bulk_write(usbhandle,ep,str,ncols,usbtimeout);
} else {
rtn = usb_interrupt_write(usbhandle,ep,str,ncols,usbtimeout);
}
mxFree(str);
/* If there's a return value, return rtn. **
* else, if rtn != size, throw an error. */
if(nlhs==1) {
int ndims = 2;
int dims[2] = {1,1};
double *val;
plhs[0] = mxCreateNumericArray(ndims,dims,mxDOUBLE_CLASS,mxREAL);
val = mxGetData(plhs[0]);
*val = rtn;
} else if(rtn < 0) {
mexErrMsgIdAndTxt("USB:writestring","write error code %d",rtn);
} else if(rtn != ncols) {
mexErrMsgIdAndTxt("USB:writestring","write size %d actual write was %d",ncols,rtn);
}
}
void crbm_reconstruct2D(double v_sample[], double v_input[], double h_state[],
const mxArray *model,const mxArray *layer)
{
int ni,N,n_map_v,n_map_h,nh,nv,j,i,jj,ii,id,
Hstride,Wstride,Hfilter,Wfilter,Hres,Wres,H,W,ndim_v1;
double *s_filter, *stride, *v, *weights, *v_bias, *h_state_off;
mwSize *dim_vi, *dim_hi, *dim_v1, *dim_offset;
mxChar *type;
type = mxGetChars(mxGetField(layer,0,"type_input"));
n_map_h = mxGetScalar(mxGetField(layer,0,"n_map_h"));
n_map_v = mxGetScalar(mxGetField(layer,0,"n_map_v"));
s_filter = mxGetPr(mxGetField(layer,0,"s_filter"));
stride = mxGetPr(mxGetField(layer,0,"stride"));
dim_hi = mxGetDimensions(mxGetField(model,0,"h_input"));
weights = mxGetPr(mxGetField(model,0,"W"));
dim_v1 = mxGetDimensions(mxGetField(model,0,"v_input"));
ndim_v1 = mxGetNumberOfDimensions(mxGetField(model,0,"v_input"));
dim_vi = (mwSize*)mxMalloc(sizeof(mwSize)*4);
dim_vi[0] = dim_v1[0];
dim_vi[1] = dim_v1[1];
if (ndim_v1 == 2){
dim_vi[2] = 1;
dim_vi[3] = 1;
}
else if(ndim_v1 == 3){
dim_vi[2] = dim_v1[2];
dim_vi[3] = 1;
}
else{
dim_vi[2] = dim_v1[2];
dim_vi[3] = dim_v1[3];
}
N = dim_vi[3];
v = mxGetPr(mxCreateNumericArray(4,dim_vi,mxDOUBLE_CLASS,mxREAL));
v_bias = mxGetPr(mxGetField(model,0,"v_bias"));
Hstride = stride[0];
Wstride = stride[1];
Hfilter = s_filter[0];
Wfilter = s_filter[1];
H = dim_vi[0];
W = dim_vi[1];
Hres = dim_hi[0];
Wres = dim_hi[1];
int offset_h = (H-1)*Hstride+Hfilter-Hres;
int offset_w = (W-1)*Wstride+Wfilter-Wres;
int H_off = Hres + offset_h;
int W_off = Wres + offset_w;
dim_offset = (mwSize*)mxMalloc(sizeof(mwSize)*4);
dim_offset[0] = H_off; dim_offset[1] = W_off; dim_offset[2] = n_map_h; dim_offset[3] = dim_vi[3];
h_state_off = mxGetPr(mxCreateNumericArray(4,dim_offset,mxDOUBLE_CLASS,mxREAL));
for(ni = 0; ni < N; ni++){
for(nh = 0; nh < n_map_h; nh++){
for(j = 0; j < Wres; j++){
for(i = 0; i < Hres; i++){
h_state_off[i+offset_h/2+H_off*(j+offset_w/2)+H_off*W_off*nh+H_off*W_off*n_map_h*ni]
= h_state[i+Hres*j+Hres*Wres*nh+Hres*Wres*n_map_h*ni];
}
}
}
}
for(ni = 0; ni < N; ni++){
for(nv = 0; nv < n_map_v; nv++){
for (j = 0; j< W; j++){
for (i = 0; i< H; i++){
id = i + H*j + H*W*nv + H*W*n_map_v*ni;
v[id] = 0;
for (nh = 0; nh < n_map_h; nh++){
for (jj = 0; jj < Wfilter; jj ++){
for (ii = 0; ii < Hfilter; ii++){
v[id] += h_state_off[(i*Hstride+ii)+H_off*(j*Wstride+jj)+H_off*W_off*nh+H_off*W_off*n_map_h*ni]
* weights[Hfilter*Wfilter-1-(ii+Hfilter*jj)+Hfilter*Wfilter*nv+Hfilter*Wfilter*n_map_v*nh];
}
}
}
v_input[id] = v[id] + v_bias[nv];
/* If the input type is not binary: v_sample[id] = v_input[id]; */
/*If the input is binary:v_sample[id] = 1.0/(1.0+exp(-v_input[id])); */
if (type[0] == 'B')
v_sample[id] = 1.0/(1.0+exp(-v_input[id]));
if (type[0] == 'G')
v_sample[id] = v_input[id];
}
}
}
}
mxFree(dim_vi);
mxFree(dim_offset);
return;
}
void crbm_inference2D(double h_sample[], double h_input[],double h_state[],
const mxArray *model,const mxArray *layer,
const mxArray *batch_data)
{
int ni,N,n_map_h, n_map_v, nh,nv,j,i,jj,ii,id,ndim_v1,
Hstride,Wstride,Hfilter,Wfilter,Hres,Wres,H,W,Hpool,Wpool;
int *_id;
double *s_filter, *stride, *h, *data, *weights, *h_bias, *block, *pool, *gaussian;
mwSize *dim_vi, *dim_hi, *dim_id, *dim_h;
int ndim_hi;
mxChar *type;
type = mxGetChars(mxGetField(layer,0,"type_input"));
n_map_h = mxGetScalar(mxGetField(layer,0,"n_map_h"));
n_map_v = mxGetScalar(mxGetField(layer,0,"n_map_v"));
s_filter = mxGetPr(mxGetField(layer,0,"s_filter"));
stride = mxGetPr(mxGetField(layer,0,"stride"));
data = mxGetPr(batch_data);
dim_vi = mxGetDimensions(batch_data);
dim_hi = mxGetDimensions(mxGetField(model,0,"h_input"));
ndim_hi = mxGetNumberOfDimensions(mxGetField(model,0,"h_input"));
gaussian = mxGetPr(mxGetField(model,0,"start_gau"));
dim_h = (mwSize*)mxMalloc(sizeof(mwSize)*4);
dim_h[0] = dim_hi[0];
dim_h[1] = dim_hi[1];
if (ndim_hi == 2){
dim_h[2] = 1;
dim_h[3] = 1;
}
else if (ndim_hi == 3){
dim_h[2] = dim_hi[2];
dim_h[3] = 1;
}
else{
dim_h[2] = dim_hi[2];
dim_h[3] = dim_hi[3];
}
weights = mxGetPr(mxGetField(model,0,"W"));
h = mxGetPr(mxCreateNumericArray(4,dim_h,mxDOUBLE_CLASS,mxREAL));
h_bias = mxGetPr(mxGetField(model,0,"h_bias"));
block = mxGetPr(mxCreateNumericArray(4,dim_h,mxDOUBLE_CLASS,mxREAL));
pool = mxGetPr(mxGetField(layer,0,"s_pool"));
ndim_v1 = mxGetNumberOfDimensions(batch_data);
mxFree(dim_h);
if (ndim_v1 == 2 || ndim_v1 == 3)
N = 1;
else
N = dim_vi[3];
Hstride = stride[0];
Wstride = stride[1];
Hfilter = s_filter[0];
Wfilter = s_filter[1];
H = dim_vi[0];
W = dim_vi[1];
Hres = dim_hi[0];
Wres = dim_hi[1];
Hpool = pool[0];
Wpool = pool[1];
/*mexPrintf("inference: Hstride = %d, Hfilter = %d H = %d Hres = %d\n",Hstride,Hfilter, H, Hres);*/
dim_id = (mwSize*)mxMalloc(sizeof(mwSize)*2);
dim_id[0]= pool[0]; dim_id[1] = pool[1];
_id = mxGetPr(mxCreateNumericArray(2,dim_id,mxUINT32_CLASS,mxREAL));
mxFree(dim_id);
for (ni = 0; ni < N; ni++) {
for (nh = 0; nh < n_map_h; nh++) {
for (j = 0; j < Wres; j++) {
for (i = 0; i < Hres; i++) {
id = i + Hres * j + Hres * Wres * nh + Hres*Wres*n_map_h*ni;
h_input[id] = 0;
for (nv = 0; nv < n_map_v; nv++)
for (jj = 0; jj < Wfilter; jj++)
for (ii = 0; ii < Hfilter; ii++){
h_input[id] += data[(i*Hstride+ii)+H*(j*Wstride+jj)+H*W*nv+H*W*n_map_v*ni]
* weights[(ii+Hfilter*jj)+Hfilter*Wfilter*nv+Hfilter*Wfilter*n_map_v*nh];
}
h_input[id] = h_input[id] + h_bias[nh];
/* for crbm blocksum*/
if (type[0] == 'B')
block[id] = exp(h_input[id]);
if (type[0] == 'G')
block[id] = exp(1.0/(gaussian[0]*gaussian[0])*h_input[id]);
}
}
/* crbm blocksum: hidden activation summation */
for (j = 0; j < floor(Wres/Wpool); j++) {
for (i = 0; i < floor(Hres/Hpool); i++) {
double sum = 0.0;
for (jj = 0; jj < Wpool; jj++){
_id[jj*Hpool] = i*Hpool+(j*Wpool+jj)*Hres + Hres*Wres*nh+Hres*Wres*n_map_h*ni;
sum += block[_id[jj*Hpool]];
for (ii = 1; ii < Hpool; ii++){
_id[jj*Hpool+ii] = _id[jj*Hpool+ii-1] + 1;
sum += block[_id[jj*Hpool+ii]];
}
}
bool done = false;
double rnd = rand() % 10000 / 10000.0;
double pro_sum = 0.0;
for (jj = 0; jj < Hpool*Wpool; jj++){
h_sample[_id[jj]] = block[_id[jj]]/(1.0+sum);
pro_sum += h_sample[_id[jj]];
/* Randomly generate the hidden state: at most one unit is activated */
if(done == false){
if (pro_sum >= rnd){
h_state[_id[jj]] = 1;
done = true;
}
}
}
}
}
}
}
return;
}
void eng_make_String(const unsigned short *str, int len, int characters) {
mwSize mbDims[2] = {1, len}; // use len, not characters, because no support for 4-byte characters in either Mma 9 or MATLAB
mxArray *var = mxCreateCharArray(2, mbDims);
std::copy(str, str+len, (unsigned short *) mxGetChars(var));
returnHandle(var);
}
void mexFunction( int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[] )
{
char action = ((char*)mxGetChars(ACTION))[0];
if (action==ACTION_ALLOCATE)
{
#ifdef DEBUG_OUTPUT
mexPrintf("======== Starting QPBO Prealloc =========\n");
#endif
int numNodes = (int)mxGetScalar(NUM_NODES);
qpbo = new QPBO<double>(numNodes,numNodes*4);
labels = new int[numNodes];
labels_toOne = new int[numNodes];
#ifdef DEBUG_OUTPUT
mexPrintf("======== Starting QPBO Prealloc Finished =========\n");
#endif
}
else if (action==ACTION_PROCESS)
{
#ifdef DEBUG_OUTPUT
mexPrintf("======== Starting QPBO Processing =========\n");
#endif
int numNodes = (int)mxGetScalar(NUM_NODES);
qpbo->Reset();
qpbo->AddNode(numNodes);
#ifdef DEBUG_OUTPUT
mexPrintf("======== QPBO Re-Init finished =========\n");
#endif
//int numNodes = static_cast<int>(*mxGetPr(NUM_NODES));
double *unary = mxGetPr(UNARY);
double *pairwise = mxGetPr(PAIRWISE);
int m_unary = mxGetM(UNARY);
int n_unary = mxGetN(UNARY);
int m_pairwise = mxGetM(PAIRWISE);
int n_pairwise = mxGetN(PAIRWISE);
//mexPrintf("nodes=%d\n",numNodes);
//mexPrintf("unar_m=%d,unary_n=%d\n",m_unary,n_unary);
//mexPrintf("pairwise_m=%d,pairwise_n=%d\n",m_pairwise,n_pairwise);
//add terms
int column;
if(n_pairwise > 0){
for(column = 0; column < n_pairwise ; column++){
// mexPrintf("Adding pairwise term (%d,%d) (%f,%f,%f,%f)\n",static_cast<int>(pairwise[column*6]-1),static_cast<int>(pairwise[column*6+1]-1),(pairwise[column*6+2]),(pairwise[column*6+3]),(pairwise[column*6+4]),(pairwise[column*6+5]));//(pairwise[column*6+3]),(pairwise[column*6+4]),(pairwise[column*6+5]));
qpbo->AddPairwiseTerm(static_cast<int>(pairwise[column*6]-1),static_cast<int>(pairwise[column*6+1]-1),(pairwise[column*6+2]),(pairwise[column*6+3]),(pairwise[column*6+4]),(pairwise[column*6+5]));
}
}
for(column=0;column<n_unary;column++){
// mexPrintf("Adding unary term (%d) (%d,%d)\n",static_cast<int>(unary[column*3]-1),static_cast<int>(unary[column*3+1]),static_cast<int>(unary[column*3+2]));
qpbo->AddUnaryTerm(static_cast<int>(unary[column*3]-1),(unary[column*3+1]),(unary[column*3+2]));
}
qpbo->MergeParallelEdges();
qpbo->Solve();
qpbo->ComputeWeakPersistencies();
for(int i = 0;i<numNodes;i++){
labels[i] = (qpbo->GetLabel(i));
//mexPrintf("Node %d is in %d\n",i,qpbo.GetLabel(i));
}
double energy_toZero = qpbo->ComputeTwiceEnergy(0);
for(int i = 0;i<numNodes;i++){
if(labels[i] < 0){
labels_toOne[i]=1;
}else{
labels_toOne[i] = labels[i];
}
}
double energy_toOne = qpbo->ComputeTwiceEnergy(labels_toOne);
//return labeling
plhs[0] = mxCreateDoubleMatrix(numNodes,1,mxREAL);
plhs[1] = mxCreateDoubleMatrix(1,1,mxREAL);
double *output = mxGetPr(plhs[0]);
double *energy_out = mxGetPr(plhs[1]);
if(energy_toZero < energy_toOne){
for(int i = 0;i<numNodes;i++){
if(labels[i] < 0){
output[i]=0.;
}else{
output[i] = static_cast<double>(labels[i]);
}
}
energy_out[0] = energy_toZero;
}else{
for(int i = 0;i<numNodes;i++){
output[i] = static_cast<double>(labels_toOne[i]);
}
energy_out[0] = energy_toOne;
}
#ifdef DEBUG_OUTPUT
mexPrintf("======== Finished QPBO processing =========\n");
#endif
}
else
{
#ifdef DEBUG_OUTPUT
mexPrintf("======== Starting QPBO dealloc =========\n");
#endif
if(labels)
{
delete [] labels;
labels = 0;
}
if (labels_toOne)
{
delete [] labels_toOne;
labels_toOne = 0;
}
if (qpbo)
{
delete qpbo;
qpbo = 0;
}
#ifdef DEBUG_OUTPUT
mexPrintf("======== Finished QPBO dealloc =========\n");
#endif
}
}
