/*
PsychAllocOutCellArray()
-If argument is optional we allocate the structure even if the argument is not present. If this behavior bothers you,
then check within your code for the presense of a return argument before creating the struct array. We
allocate space regardeless of whether the argument is present because this is consistant with other "PsychAllocOut*"
functions which behave this way because in some situations subfunctions might derive returned results from values
stored in an optional argument.
-If position is -1 then don't attempt to return the created structure to the calling environment. Instead just
allocate the structure and return it in pStruct. This is how to create a structure which is embeded within another
structure using PsychSetStructArrayStructArray(). Note that we use -1 as the flag and not NULL because NULL is 0 and
0 is reserved for future use as a reference to the subfunction name, of if none then the function name.
*/
boolean PsychAllocOutCellVector( int position,
PsychArgRequirementType isRequired,
int numElements,
PsychGenericScriptType **pCell)
{
mxArray **mxArrayOut;
int cellArrayNumDims=2;
int cellArrayDims[2];
PsychError matchError;
Boolean putOut;
cellArrayDims[0]=1;
cellArrayDims[1]=numElements;
if(position != kPsychNoArgReturn){ //Return the result to both the C caller and the scripting environment.
PsychSetReceivedArgDescriptor(position, PsychArgOut);
PsychSetSpecifiedArgDescriptor(position, PsychArgOut, PsychArgType_cellArray, isRequired, 1,1,numElements,numElements,0,0);
*pCell = mxCreateCellArray(cellArrayNumDims, cellArrayDims);
mxArrayOut = PsychGetOutArgMxPtr(position);
matchError=PsychMatchDescriptors();
putOut=PsychAcceptOutputArgumentDecider(isRequired, matchError);
if(putOut)
*mxArrayOut=*pCell;
return(putOut);
}else{ //Return the result only to the C caller, not to the calling environment. Ignore "required".
*pCell = mxCreateCellArray(cellArrayNumDims, cellArrayDims);
return(TRUE);
}
}
void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]){
int i, j, c, loop, nNodes, nCliques, ndomain, dims[2];
double *pClqs, *pr, *pt, *pSize;
mxArray *pTemp, *pTemp1, *pStruct, *pCliques, *pBigpot, *pSmallpot;
const char *field_names[] = {"domain", "T", "sizes"};
nNodes = mxGetNumberOfElements(prhs[1]);
pCliques = mxGetField(prhs[0], 0, "cliques");
nCliques = mxGetNumberOfElements(pCliques);
pTemp = mxGetField(prhs[0], 0, "eff_node_sizes");
pSize = mxGetPr(pTemp);
plhs[0] = mxCreateCellArray(1, &nCliques);
for(i=0; i<nCliques; i++){
pStruct = mxCreateStructMatrix(1, 1, 3, field_names);
mxSetCell(plhs[0], i, pStruct);
pTemp = mxGetCell(pCliques, i);
ndomain = mxGetNumberOfElements(pTemp);
pt = mxGetPr(pTemp);
pTemp1 = mxDuplicateArray(pTemp);
mxSetField(pStruct, 0, "domain", pTemp1);
pTemp = mxCreateDoubleMatrix(1, ndomain, mxREAL);
mxSetField(pStruct, 0, "sizes", pTemp);
pr = mxGetPr(pTemp);
for(j=0; j<ndomain; j++){
pr[j] = pSize[(int)pt[j]-1];
}
}
pClqs = mxGetPr(prhs[1]);
for(loop=0; loop<nNodes; loop++){
c = (int)pClqs[loop] - 1;
pSmallpot = mxGetCell(prhs[2], loop);
pTemp = mxGetField(pSmallpot, 0, "T");
pBigpot = mxGetCell(plhs[0], c);
pTemp1 = mxGetField(pBigpot, 0, "T");
if(pTemp1){
if(mxIsSparse(pTemp))
multiply_spPot_by_spPot(pBigpot, pSmallpot);
else multiply_spPot_by_fuPot(pBigpot, pSmallpot);
}
else{
if(mxIsSparse(pTemp))
multiply_null_by_spPot(pBigpot, pSmallpot);
else multiply_null_by_fuPot(pBigpot, pSmallpot);
}
}
dims[0] = nCliques;
dims[1] = nCliques;
plhs[1] = mxCreateCellArray(2, dims);
}
void mexFunction(int nlhs, mxArray *plhs[],
int nrhs, const mxArray *prhs[])
{
char *dir_name;
mxArray **files;
int nfiles, i;
if (nlhs > 1)
mexErrMsgTxt("Too many output arguments.");
if (nrhs == 0)
dir_name = ".";
else if (nrhs == 1) {
if (mxIsChar(prhs[0]) != 1)
mexErrMsgTxt("Input must be a string.");
if (mxGetM(prhs[0]) != 1)
mexErrMsgTxt("Input must be a row vector.");
dir_name = mxArrayToString(prhs[0]);
} else
mexErrMsgTxt("Too many input arguments.");
files = dir(dir_name, &nfiles);
plhs[0] = mxCreateCellArray(1,&nfiles);
for (i = 0; i < nfiles; i++) {
mxSetCell(plhs[0],i,files[i]);
}
mxFree(files);
if (nrhs > 0)
mxFree(dir_name);
return;
}
void mexFunction(int nlhs, mxArray* plhs[], int nrhs, const mxArray *prhs[])
{
if(nrhs!=3 || nlhs != 8)
{
mexErrMsgIdAndTxt("Drake:testMultipleTimeLinearPostureConstrainttmex:BadInputs","Usage [num_cnst,cnst_val,iAfun,jAvar,A,cnst_name,lb,ub] = testMultipleTimeLinearPostureConstraintmex(kinCnst,q,t)");
}
MultipleTimeLinearPostureConstraint* cnst = (MultipleTimeLinearPostureConstraint*) getDrakeMexPointer(prhs[0]);
int n_breaks = mxGetNumberOfElements(prhs[2]);
double* t_ptr = new double[n_breaks];
memcpy(t_ptr,mxGetPrSafe(prhs[2]),sizeof(double)*n_breaks);
int nq = cnst->getRobotPointer()->num_positions;
MatrixXd q(nq,n_breaks);
if(mxGetM(prhs[1]) != nq || mxGetN(prhs[1]) != n_breaks)
{
mexErrMsgIdAndTxt("Drake:testMultipleTimeLinearPostureConstraintmex:BadInputs","Argument 2 must be of size nq*n_breaks");
}
memcpy(q.data(),mxGetPrSafe(prhs[1]),sizeof(double)*nq*n_breaks);
int num_cnst = cnst->getNumConstraint(t_ptr,n_breaks);
VectorXd c(num_cnst);
cnst->feval(t_ptr,n_breaks,q,c);
VectorXi iAfun;
VectorXi jAvar;
VectorXd A;
cnst->geval(t_ptr,n_breaks,iAfun,jAvar,A);
std::vector<std::string> cnst_names;
cnst->name(t_ptr,n_breaks,cnst_names);
VectorXd lb(num_cnst);
VectorXd ub(num_cnst);
cnst->bounds(t_ptr,n_breaks,lb,ub);
VectorXd iAfun_tmp(iAfun.size());
VectorXd jAvar_tmp(jAvar.size());
for(int i = 0;i<iAfun.size();i++)
{
iAfun_tmp(i) = (double) iAfun(i)+1;
jAvar_tmp(i) = (double) jAvar(i)+1;
}
plhs[0] = mxCreateDoubleScalar((double) num_cnst);
plhs[1] = mxCreateDoubleMatrix(num_cnst,1,mxREAL);
memcpy(mxGetPrSafe(plhs[1]),c.data(),sizeof(double)*num_cnst);
plhs[2] = mxCreateDoubleMatrix(iAfun_tmp.size(),1,mxREAL);
memcpy(mxGetPrSafe(plhs[2]),iAfun_tmp.data(),sizeof(double)*iAfun_tmp.size());
plhs[3] = mxCreateDoubleMatrix(jAvar_tmp.size(),1,mxREAL);
memcpy(mxGetPrSafe(plhs[3]),jAvar_tmp.data(),sizeof(double)*jAvar_tmp.size());
plhs[4] = mxCreateDoubleMatrix(A.size(),1,mxREAL);
memcpy(mxGetPrSafe(plhs[4]),A.data(),sizeof(double)*A.size());
int name_ndim = 1;
mwSize name_dims[] = {(mwSize) num_cnst};
plhs[5] = mxCreateCellArray(name_ndim,name_dims);
mxArray *name_ptr;
for(int i = 0;i<num_cnst;i++)
{
name_ptr = mxCreateString(cnst_names[i].c_str());
mxSetCell(plhs[5],i,name_ptr);
}
plhs[6] = mxCreateDoubleMatrix(num_cnst,1,mxREAL);
plhs[7] = mxCreateDoubleMatrix(num_cnst,1,mxREAL);
memcpy(mxGetPrSafe(plhs[6]),lb.data(),sizeof(double)*num_cnst);
memcpy(mxGetPrSafe(plhs[7]),ub.data(),sizeof(double)*num_cnst);
delete[] t_ptr;
}
void mexFunction(int nlhs,mxArray* plhs[], int nrhs, const mxArray * prhs[])
{
if(nrhs!=3 || nlhs != 7)
{
mexErrMsgIdAndTxt("Drake:testMultipleTimeKinCnstmex:BadInputs","Usage [type,num_cnst,cnst_val,dcnst_val,cnst_name,lb,ub] = testMultipleTimeKinCnstmex(kinCnst,q,t)");
}
MultipleTimeKinematicConstraint* cnst = (MultipleTimeKinematicConstraint*) getDrakeMexPointer(prhs[0]);
int n_breaks = mxGetNumberOfElements(prhs[2]);
double* t_ptr = new double[n_breaks];
memcpy(t_ptr,mxGetPrSafe(prhs[2]),sizeof(double)*n_breaks);
int nq = cnst->getRobotPointer()->num_positions;
MatrixXd q(nq,n_breaks);
if(mxGetM(prhs[1]) != nq || mxGetN(prhs[1]) != n_breaks)
{
mexErrMsgIdAndTxt("Drake:testMultipleTimeKinCnstmex:BadInputs","Argument 2 must be of size nq*n_breaks");
}
memcpy(q.data(),mxGetPrSafe(prhs[1]),sizeof(double)*nq*n_breaks);
int type = cnst->getType();
int num_cnst = cnst->getNumConstraint(t_ptr,n_breaks);
VectorXd c(num_cnst);
MatrixXd dc(num_cnst,nq*n_breaks);
cnst->eval(t_ptr,n_breaks,q,c,dc);
VectorXd lb(num_cnst);
VectorXd ub(num_cnst);
cnst->bounds(t_ptr,n_breaks,lb,ub);
std::vector<std::string> cnst_names;
cnst->name(t_ptr,n_breaks,cnst_names);
int retvec_size;
if(num_cnst == 0)
{
retvec_size = 0;
}
else
{
retvec_size = 1;
}
plhs[0] = mxCreateDoubleScalar((double) type);
plhs[1] = mxCreateDoubleScalar((double) num_cnst);
plhs[2] = mxCreateDoubleMatrix(num_cnst,retvec_size,mxREAL);
memcpy(mxGetPrSafe(plhs[2]),c.data(),sizeof(double)*num_cnst);
plhs[3] = mxCreateDoubleMatrix(num_cnst,nq*n_breaks,mxREAL);
memcpy(mxGetPrSafe(plhs[3]),dc.data(),sizeof(double)*num_cnst*nq*n_breaks);
int name_ndim = 1;
mwSize name_dims[] = {(mwSize) num_cnst};
plhs[4] = mxCreateCellArray(name_ndim,name_dims);
mxArray *name_ptr;
for(int i = 0;i<num_cnst;i++)
{
name_ptr = mxCreateString(cnst_names[i].c_str());
mxSetCell(plhs[4],i,name_ptr);
}
plhs[5] = mxCreateDoubleMatrix(num_cnst,retvec_size,mxREAL);
plhs[6] = mxCreateDoubleMatrix(num_cnst,retvec_size,mxREAL);
memcpy(mxGetPrSafe(plhs[5]),lb.data(),sizeof(double)*num_cnst);
memcpy(mxGetPrSafe(plhs[6]),ub.data(),sizeof(double)*num_cnst);
delete[] t_ptr;
}
inline
mxArray* to_mxarray(const array_nd<array_nd<T> >& a)
{
mxArray* mx = mxCreateCellArray(a.ndims(),
reinterpret_cast<const mwSize*>(a.sizes().get_base_ptr()));
size_t len = a.length();
for(size_t i = 0; i < len; i++) {
mxSetCell(mx, mwIndex(i), to_mxarray(a[i]));
}
return mx;
}
mxArray*
Snapshot::StoreSnapshots0(vector<Snapshot>& snapshots)
{
mxArray* plhs;
const int dims[] = { snapshots.size(), 1 };
plhs = mxCreateCellArray((mwSize)2, (const mwSize*)dims);
for (int i = 0; i<snapshots.size(); ++i)
{
mxArray* a = StoreSnapshot0(snapshots[i]);
mxSetCell(plhs, i, a);
}
return plhs;
}
static int
p_create_cell_vector(void)
{
mwSize dims[1];
mxArray *mat;
dims[0] = YAP_IntOfTerm(YAP_ARG1);
if (!(mat = mxCreateCellArray(1, dims)))
return FALSE;
if (YAP_IsAtomTerm(YAP_ARG2)) {
return !engPutVariable(Meng, YAP_AtomName(YAP_AtomOfTerm(YAP_ARG2)), mat);
}
return YAP_Unify(YAP_ARG2,address2term(mat));
}
void eng_make_Cell(int *elems, int len, int *mmDims, int depth) {
std::vector<mwSize> mbDimsVec(depth);
std::reverse_copy(mmDims, mmDims+depth, mbDimsVec.begin());
mwSize *mbDims = &mbDimsVec[0];
mxArray *var = mxCreateCellArray(depth, mbDims);
for (int i=0; i < len; ++i) {
mxArray *el = handles.value(elems[i]);
handles.remove(elems[i]); // remove to avoid double mxFree()
mxSetCell(var, i, el);
}
returnHandle(var);
}
/**
* Convert unknown python object to empty cell array.
* Also send a warning with __repr__ of `obj` shown.
*
* :param obj: Object to convert [Borrow reference]
*/
mxArray *mx_from_py_unknown(PyObject* obj)
{
int dims[1] = { 0 };
char *buf;
PyObject *type = PyObject_Type(obj);
PyObject *repr = PyObject_Repr(type);
buf = PyString_AsString(repr);
mexWarnMsgTxt("Unknown Python object seen / conversion failed:");
mexWarnMsgTxt(buf);
Py_DECREF(repr);
Py_DECREF(type);
return mxCreateCellArray(1, dims);
}
/** Convert a numeric Python object array to a Matlab cell array
* :param obj: Object to convert [Borrow reference]
*/
mxArray *mx_from_py_arrayobject_object(PyArrayObject* obj)
{
stride_t s;
unsigned long index;
PyObject **r;
mxArray *arr;
arr = mxCreateCellArray(obj->nd, (int*)obj->dimensions);
if (!stride_init(obj->data, obj->nd, obj->dimensions, obj->strides, &s))
return arr;
index = 0;
do {
r = (PyObject**)stride_pos(&s);
mxSetCell(arr, index, mx_from_py(*r));
++index;
} while (stride_step(&s));
return arr;
}
/** Convert Python sequence to 1D Matlab cell array
* :param obj: Object to convert [Borrow reference]
*/
mxArray *mx_from_py_sequence(PyObject* obj)
{
mxArray *r;
int dims[1];
int k;
dims[0] = PySequence_Size(obj);
r = mxCreateCellArray(1, dims);
for (k = 0; k < dims[0]; ++k) {
PyObject *o;
o = PySequence_GetItem(obj, k);
if (o == NULL) {
mexWarnMsgTxt("Couldn't get item in sequence");
PyErr_Clear();
} else {
mxSetCell(r, k, mx_from_py(o));
Py_DECREF(o);
}
}
return r;
}
mxArray* sf_c1_VrSubsystem_get_post_codegen_info(void)
{
const char* fieldNames[] = { "exportedFunctionsUsedByThisChart",
"exportedFunctionsChecksum" };
mwSize dims[2] = { 1, 1 };
mxArray* mxPostCodegenInfo = mxCreateStructArray(2, dims, sizeof(fieldNames)/
sizeof(fieldNames[0]), fieldNames);
{
mxArray* mxExportedFunctionsChecksum = mxCreateString("");
mwSize exp_dims[2] = { 0, 1 };
mxArray* mxExportedFunctionsUsedByThisChart = mxCreateCellArray(2, exp_dims);
mxSetField(mxPostCodegenInfo, 0, "exportedFunctionsUsedByThisChart",
mxExportedFunctionsUsedByThisChart);
mxSetField(mxPostCodegenInfo, 0, "exportedFunctionsChecksum",
mxExportedFunctionsChecksum);
}
return mxPostCodegenInfo;
}
void mexFunction(int nlhs,mxArray *plhs[],int nrhs,const mxArray *prhs[])
{
double *m_w_d, *p_w_z, *p_z_d, *p_w_d;
mwIndex *ir, *jc;
size_t d, k;
size_t n_w, n_d, n_z;
mwSize ndim = 1, dims[1];
mwSize nsubs = 1;
mxArray *temp_array;
double *temp;
size_t beg_row_id, end_row_id, cur_row_id;
mwIndex index, subs[1];
mwIndex *beg_of_ir, *beg_of_jc;
mwSize nzmax;
size_t total = 0;
if(!mxIsSparse(prhs[0]))
{
printf("word-doc matrix should be sparse matrix\n");
return;
}
else if(nrhs != 4 || nlhs != 1)
{
printf("usage: p_z_wd = mex_Estep_sparse(m_w_d, p_w_z_n, p_z_d_n, p_w_d)\n");
return;
}
m_w_d = mxGetPr(prhs[0]);
jc = mxGetJc(prhs[0]);
ir = mxGetIr(prhs[0]);
nzmax = mxGetNzmax(prhs[0]);
n_w = mxGetM(prhs[0]);
n_d = mxGetN(prhs[0]);
p_w_z = mxGetPr(prhs[1]);
n_z = mxGetN(prhs[1]);
p_z_d = mxGetPr(prhs[2]);
p_w_d = mxGetPr(prhs[3]);
dims[0] = n_z;
plhs[0] = mxCreateCellArray(ndim, dims);
//total_num_of_cells = mxGetNumberOfElements(plhs[0]);
for(k = 0; k < n_z; k++)
{
total = 0;
subs[0] = k;
index = mxCalcSingleSubscript(plhs[0], nsubs, subs);
temp_array = mxCreateSparse(n_w, n_d, nzmax, mxREAL);
temp = mxGetPr(temp_array);
mxSetNzmax(temp_array, nzmax);
//Place ir data into the newly created sparse array.
beg_of_ir = mxGetIr(temp_array);
memcpy(beg_of_ir, ir, nzmax * sizeof(mwIndex));
//Place jc data into the newly created sparse array.
beg_of_jc = mxGetJc(temp_array);
memcpy(beg_of_jc, jc, (n_d + 1) * sizeof(mwIndex));
for (d = 0; d < n_d; d++)
{
beg_row_id = jc[d];
end_row_id = jc[d + 1];
if (beg_row_id == end_row_id)
continue;
else
{
for (cur_row_id = beg_row_id; cur_row_id < end_row_id; cur_row_id++)
{
temp[total] = p_w_z[k * n_w + ir[cur_row_id]] * p_z_d[d * n_z + k] / p_w_d[total];
total++;
}
}
}
mxSetCell(plhs[0], index, temp_array);
}
return;
}
mxArray *dynListToCellArray(DYN_LIST *dl)
{
int dims;
int i;
DYN_LIST **sublists;
mxArray *retval = NULL, *cell;
double *d;
switch(DYN_LIST_DATATYPE(dl)) {
case DF_LIST:
dims = DYN_LIST_N(dl);
retval = mxCreateCellArray(1, (const mwSize*)&dims);
sublists = (DYN_LIST **) DYN_LIST_VALS(dl);
for (i = 0; i < DYN_LIST_N(dl); i++) {
cell = dynListToCellArray(sublists[i]);
mxSetCell(retval, i, cell);
}
break;
case DF_INT:
{
int *vals = (int *) DYN_LIST_VALS(dl);
retval = mxCreateDoubleMatrix(DYN_LIST_N(dl), 1, mxREAL);
d = mxGetPr(retval);
for ( i = 0; i < DYN_LIST_N(dl); i++ )
d[i] = (double) vals[i];
}
break;
case DF_SHORT:
{
short *vals = (short *) DYN_LIST_VALS(dl);
retval = mxCreateDoubleMatrix(DYN_LIST_N(dl), 1, mxREAL);
d = mxGetPr(retval);
for ( i = 0; i < DYN_LIST_N(dl); i++ )
d[i] = (double) vals[i];
}
break;
case DF_FLOAT:
{
float *vals = (float *) DYN_LIST_VALS(dl);
retval = mxCreateDoubleMatrix(DYN_LIST_N(dl), 1, mxREAL);
d = mxGetPr(retval);
for ( i = 0; i < DYN_LIST_N(dl); i++ )
d[i] = (double) vals[i];
}
break;
case DF_CHAR:
{
char *vals = (char *) DYN_LIST_VALS(dl);
retval = mxCreateDoubleMatrix(DYN_LIST_N(dl), 1, mxREAL);
d = mxGetPr(retval);
for ( i = 0; i < DYN_LIST_N(dl); i++ )
d[i] = (double) vals[i];
}
break;
case DF_STRING:
{
char **vals = (char **) DYN_LIST_VALS(dl);
retval = mxCreateCharMatrixFromStrings(DYN_LIST_N(dl), (const char **)vals);
}
}
return retval;
}
// Main function ===============================================================
void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
{
const mxArray *In, *String;
mxArray *Out;
mwSize iC;
OutStyle_T OutStyle = Auto_Style; // Default: Auto-expanding
char StyleIn;
// Check number of inputs and outputs:
if (nrhs == 0 || nrhs > 2 || nlhs > 1) {
mexErrMsgIdAndTxt(ERR_ID "BadNInput",
ERR_HEAD "2 inputs allowed, 1 output allowed.");
}
// Parse 2nd input, if it is not empty:
if (nrhs == 2 && !mxIsEmpty(prhs[1])) {
if (!mxIsChar(prhs[1])) {
mexErrMsgIdAndTxt(ERR_ID "BadType",
ERR_HEAD "2nd input [Style] must be a string.");
}
// "Auto", "Lean", "Fat":
StyleIn = *((char *) mxGetData(prhs[1]));
switch (StyleIn) {
case 'a': // Fallthrough
case 'A': OutStyle = Auto_Style; break;
case 'l': // Fallthrough
case 'L': OutStyle = Lean_Style; break;
case 'f': // Fallthrough
case 'F': OutStyle = Fat_Style; break;
default:
mexErrMsgIdAndTxt(ERR_ID "BadStyle",
ERR_HEAD "Unknown Style.");
}
}
// Get input string or cell string and call the core: ------------------------
In = prhs[0];
if (mxIsChar(In)) { // Input is a string:
plhs[0] = Core((mxChar *) mxGetData(In), mxGetNumberOfElements(prhs[0]),
OutStyle);
} else if (mxIsCell(In)) { // Input is a cell string:
plhs[0] = mxCreateCellArray(mxGetNumberOfDimensions(In),
mxGetDimensions(In));
Out = plhs[0];
iC = mxGetNumberOfElements(In);
while (iC-- > 0) { // Backwards, faster than FOR
String = mxGetCell(In, iC);
if (String == NULL) { // Uninitialized cell:
mxSetCell(Out, iC,
Core(NULL, 0, OutStyle)); // Reply current directory
} else if (mxIsChar(String)) { // Cell element is a string
mxSetCell(Out, iC,
Core((mxChar *) mxGetData(String),
mxGetNumberOfElements(String), OutStyle));
} else { // Bad cell element
mexErrMsgIdAndTxt(ERR_ID "BadInputType",
ERR_HEAD "[FileName] must be a string or cell string.");
}
}
} else { // Bad input type:
mexErrMsgIdAndTxt(ERR_ID "BadInputType",
ERR_HEAD "[FileName] must be a string or cell string.");
}
return;
}
/* entry point */
void mexFunction(int nlhs, mxArray *plhs[],
int nrhs, const mxArray *prhs[])
{
int ind, i, x, y, o, dataDim, j, bytes_to_copy, nGaborFilter;
const mxArray *f;
const mxArray *pAS1Map;
mxArray *outPA;
mwSize ndim;
const mwSize* dims;
mwSize dimsOutput[2];
void* start_of_pr;
mxClassID datatype;
/*
* input variable 0: nGaborOri
*/
nGaborOri = (int)mxGetScalar(prhs[0]);
/*
* input variable 1: S1 maps
*/
pAS1Map = prhs[1];
dims = mxGetDimensions(pAS1Map);
nGaborFilter = dims[0] * dims[1];
nGaborScale = nGaborFilter / nGaborOri;
S1Map = (const float**)mxCalloc( nGaborFilter, sizeof(*S1Map) ); /* SUM1 maps */
for (i=0; i<nGaborFilter; ++i)
{
f = mxGetCell(pAS1Map, i);
datatype = mxGetClassID(f);
if (datatype != mxSINGLE_CLASS)
mexErrMsgTxt("warning !! single precision required.");
S1Map[i] = (const float*)mxGetPr(f); /* get the pointer to cell content */
height = mxGetM(f); /* overwriting is ok, since it is constant */
width = mxGetN(f);
}
/*
* input variable 2: location shift radius
*/
locationPerturb = (int)mxGetScalar(prhs[2]);
/*
* input variable 3: orientation shift radius
*/
orientationPerturb = (int)mxGetScalar(prhs[3]);
/*
* input variable 4: sub sample step length
*/
subsample = (int)mxGetScalar(prhs[4]);
StoreShift();
Compute();
/* =============================================
* Handle output variables.
* =============================================
*/
/*
* output variable 0: M1 map
*/
dimsOutput[0] = 1; dimsOutput[1] = nGaborScale * nGaborOri;
plhs[0] = mxCreateCellArray( 2, dimsOutput );
dimsOutput[0] = sizexSubsample; dimsOutput[1] = sizeySubsample;
for( i = 0; i < nGaborOri * nGaborScale; ++i )
{
outPA = mxCreateNumericArray( 2, dimsOutput, mxSINGLE_CLASS, mxREAL );
/* populate the real part of the created array */
start_of_pr = (float*)mxGetData(outPA);
bytes_to_copy = dimsOutput[0] * dimsOutput[1] * mxGetElementSize(outPA);
memcpy( start_of_pr, M1Map[i], bytes_to_copy );
mxSetCell( plhs[0], i, outPA );
}
/*
* output variable 1: M1 trace
*/
dimsOutput[0] = 1; dimsOutput[1] = nGaborScale * nGaborOri;
plhs[1] = mxCreateCellArray( 2, dimsOutput );
dimsOutput[0] = sizexSubsample; dimsOutput[1] = sizeySubsample;
for( i = 0; i < nGaborOri * nGaborScale; ++i )
{
outPA = mxCreateNumericArray( 2, dimsOutput, mxINT32_CLASS, mxREAL );
/* populate the real part of the created array */
start_of_pr = (int*)mxGetData(outPA);
bytes_to_copy = dimsOutput[0] * dimsOutput[1] * mxGetElementSize(outPA);
memcpy( start_of_pr, M1Trace[i], bytes_to_copy );
mxSetCell( plhs[1], i, outPA );
}
/*
* output variable 2: stored Gabor shifts : row shifts
*/
dimsOutput[0] = nGaborScale * nGaborOri; dimsOutput[1] = 1;
plhs[2] = mxCreateCellArray( 2, dimsOutput );
for( i = 0; i < nGaborScale*nGaborOri; ++i )
{
dimsOutput[0] = numShift; dimsOutput[1] = 1;
outPA = mxCreateNumericArray( 2, dimsOutput, mxINT32_CLASS, mxREAL );
start_of_pr = (int*)mxGetData(outPA);
bytes_to_copy = dimsOutput[0] * dimsOutput[1] * mxGetElementSize(outPA);
memcpy( start_of_pr, rowShift[i], bytes_to_copy );
mxSetCell( plhs[2], i, outPA );
}
/*
* output variable 3: stored Gabor shifts : col shifts
*/
dimsOutput[0] = nGaborScale * nGaborOri; dimsOutput[1] = 1;
plhs[3] = mxCreateCellArray( 2, dimsOutput );
for( i = 0; i < nGaborScale*nGaborOri; ++i )
{
dimsOutput[0] = numShift; dimsOutput[1] = 1;
outPA = mxCreateNumericArray( 2, dimsOutput, mxINT32_CLASS, mxREAL );
start_of_pr = (int*)mxGetData(outPA);
bytes_to_copy = dimsOutput[0] * dimsOutput[1] * mxGetElementSize(outPA);
memcpy( start_of_pr, colShift[i], bytes_to_copy );
mxSetCell( plhs[3], i, outPA );
}
/*
* output variable 4: stored Gabor shifts : orientation shifts
*/
dimsOutput[0] = nGaborScale * nGaborOri; dimsOutput[1] = 1;
plhs[4] = mxCreateCellArray( 2, dimsOutput );
for( i = 0; i < nGaborScale*nGaborOri; ++i )
{
dimsOutput[0] = numShift; dimsOutput[1] = 1;
outPA = mxCreateNumericArray( 2, dimsOutput, mxINT32_CLASS, mxREAL );
start_of_pr = (int*)mxGetData(outPA);
bytes_to_copy = dimsOutput[0] * dimsOutput[1] * mxGetElementSize(outPA);
memcpy( start_of_pr, orientShift[i], bytes_to_copy );
mxSetCell( plhs[4], i, outPA );
}
}
void
mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
{
MatlabCPPInitialize(false);
// Check for proper number of arguments
if ( (nrhs < NR_IN) || (nrhs > NR_IN + NR_IN_OPT) || \
(nlhs < NR_OUT) || (nlhs > NR_OUT + NR_OUT_OPT) ) {
mexErrMsgTxt("Wrong number of arguments.");
}
// fetch the input (we support matrices and cell arrays)
std::vector< Eigen::VectorXd > theta_unary;
std::vector< Eigen::VectorXd > theta_pair;
if (!mxIsCell(THETA_UNARY_IN)) {
GetMatlabPotentialFromMatrix(THETA_UNARY_IN, theta_unary);
GetMatlabPotentialFromMatrix(THETA_PAIR_IN, theta_pair);
}
else {
GetMatlabPotential(THETA_UNARY_IN, theta_unary);
GetMatlabPotential(THETA_PAIR_IN, theta_pair);
}
Eigen::MatrixXi edges;
GetMatlabMatrix(EDGES_IN, edges);
// decomposition: cell array of edge index vectors
std::vector< std::vector<size_t> > decomposition;
size_t num = mxGetNumberOfElements(DECOMPOSITION_IN);
decomposition.resize(num);
for (size_t d_idx=0; d_idx<num; d_idx++) {
const mxArray* v = mxGetCell(DECOMPOSITION_IN, d_idx);
size_t num_edges = mxGetM(v);
decomposition[d_idx].resize(num_edges);
const double* ptr = mxGetPr(v);
for (size_t e_idx=0; e_idx<num_edges; e_idx++) {
decomposition[d_idx][e_idx] = static_cast<size_t>(ptr[e_idx]);
}
}
// parse options
MexSmoothDDOptions options;
if (nrhs >= 5) {
bool opts_parsed = options.parse(OPTIONS_IN);
if (!opts_parsed) {
MatlabCPPExit();
return;
}
}
// set algorithm & parameters according to options
SmoothDualDecomposition* sdd;
/*if (options.solver == SOLVER_FISTADESCENT) {
sdd = new SmoothDualDecompositionFistaDescent(theta_unary, theta_pair, edges, decomposition);
}
if (options.solver == SOLVER_FISTA) {
sdd = new SmoothDualDecompositionFista(theta_unary, theta_pair, edges, decomposition);
}
else if (options.solver == SOLVER_GRADIENTDESCENT) {
sdd = new SmoothDualDecompositionGradientDescent(theta_unary, theta_pair, edges, decomposition);
}
else if (options.solver == SOLVER_NESTEROV) {
sdd = new SmoothDualDecompositionNesterov(theta_unary, theta_pair, edges, decomposition);
}
else {
mxAssert(0, "Solver not found. Should not happen!");
}*/
// TODO
sdd = new SmoothDualDecompositionFistaDescent(theta_unary, theta_pair, edges, decomposition);
//sdd = new SmoothDualDecompositionLBFGS(theta_unary, theta_pair, edges, decomposition);
sdd->setMaximumNumberOfIterations(options.num_max_iter);
sdd->setEpsilonGradientNorm(options.eps_gnorm);
// run the computations
sdd->run(options.rho);
// return marginals (if input was provided as a matrix, also return
// marginals in a matrix)
std::vector< Eigen::VectorXd >& mu_unary = sdd->getUnaryMarginals();
if (!mxIsCell(THETA_UNARY_IN)) {
size_t num_states = mxGetM(THETA_UNARY_IN);
size_t num_vars = mxGetN(THETA_UNARY_IN);
MARGINALS_UNARY_OUT = mxCreateNumericMatrix(
num_states,
num_vars,
mxDOUBLE_CLASS, mxREAL);
double* mu_res_p = mxGetPr(MARGINALS_UNARY_OUT);
for (size_t v_idx=0; v_idx<mu_unary.size(); v_idx++) {
for (size_t idx=0; idx<mu_unary[v_idx].size(); idx++) {
mu_res_p[v_idx*num_states+idx] = mu_unary[v_idx](idx);
}
}
}
else {
mwSize dim_0 = static_cast<mwSize>(mu_unary.size());
MARGINALS_UNARY_OUT = mxCreateCellArray(1, &dim_0);
for (size_t v_idx=0; v_idx<mu_unary.size(); v_idx++) {
mxArray* m = mxCreateNumericMatrix(
static_cast<int>(mu_unary[v_idx].size()),
1,
mxDOUBLE_CLASS, mxREAL);
double* mu_res_p = mxGetPr(m);
for (size_t idx=0; idx<mu_unary[v_idx].size(); idx++) {
mu_res_p[idx] = mu_unary[v_idx](idx);
}
mxSetCell(MARGINALS_UNARY_OUT, v_idx, m);
}
}
// return history
/*if (nlhs > 1) {
std::vector<double> history_obj = lpqp.getHistoryObjective();
std::vector<double> history_obj_qp = lpqp.getHistoryObjectiveQP();
std::vector<double> history_obj_lp = lpqp.getHistoryObjectiveLP();
std::vector<double> history_obj_decoded = lpqp.getHistoryObjectiveDecoded();
std::vector<size_t> history_iteration = lpqp.getHistoryIteration();
std::vector<double> history_beta = lpqp.getHistoryBeta();
std::vector< Eigen::VectorXi > history_decoded = lpqp.getHistoryDecoded();
const char *field_names[] = {"obj", "obj_qp", "obj_lp", "obj_decoded", "iteration", "beta", "decoded"};
mwSize dims[2];
dims[0] = 1;
dims[1] = history_obj.size();
HISTORY_OUT = mxCreateStructArray(2, dims, sizeof(field_names)/sizeof(*field_names), field_names);
int field_obj, field_obj_qp, field_obj_lp, field_obj_decoded, field_iteration, field_beta, field_decoded;
field_obj = mxGetFieldNumber(HISTORY_OUT, "obj");
field_obj_qp = mxGetFieldNumber(HISTORY_OUT, "obj_qp");
field_obj_lp = mxGetFieldNumber(HISTORY_OUT, "obj_lp");
field_obj_decoded = mxGetFieldNumber(HISTORY_OUT, "obj_decoded");
field_iteration = mxGetFieldNumber(HISTORY_OUT, "iteration");
field_beta = mxGetFieldNumber(HISTORY_OUT, "beta");
field_decoded = mxGetFieldNumber(HISTORY_OUT, "decoded");
for (size_t i=0; i<history_obj.size(); i++) {
mxArray *field_value;
field_value = mxCreateDoubleMatrix(1,1,mxREAL);
*mxGetPr(field_value) = history_obj[i];
mxSetFieldByNumber(HISTORY_OUT, i, field_obj, field_value);
field_value = mxCreateDoubleMatrix(1,1,mxREAL);
*mxGetPr(field_value) = history_obj_qp[i];
mxSetFieldByNumber(HISTORY_OUT, i, field_obj_qp, field_value);
field_value = mxCreateDoubleMatrix(1,1,mxREAL);
*mxGetPr(field_value) = history_obj_lp[i];
mxSetFieldByNumber(HISTORY_OUT, i, field_obj_lp, field_value);
field_value = mxCreateDoubleMatrix(1,1,mxREAL);
*mxGetPr(field_value) = history_obj_decoded[i];
mxSetFieldByNumber(HISTORY_OUT, i, field_obj_decoded, field_value);
field_value = mxCreateDoubleMatrix(1,1,mxREAL);
*mxGetPr(field_value) = static_cast<double>(history_iteration[i]);
mxSetFieldByNumber(HISTORY_OUT, i, field_iteration, field_value);
field_value = mxCreateDoubleMatrix(1,1,mxREAL);
*mxGetPr(field_value) = history_beta[i];
mxSetFieldByNumber(HISTORY_OUT, i, field_beta, field_value);
if (options.with_decoded_history) {
field_value = mxCreateDoubleMatrix(history_decoded[i].size(),1,mxREAL);
double* decoded_res_p = mxGetPr(field_value);
for (size_t idx=0; idx<history_decoded[i].size(); idx++) {
decoded_res_p[idx] = static_cast<double>(history_decoded[i][idx]);
}
mxSetFieldByNumber(HISTORY_OUT, i, field_decoded, field_value);
}
}
}*/
// return pairwise marginals
if (nlhs > 1) {
std::vector< Eigen::VectorXd >& mu_pair = sdd->getPairwiseMarginals();
if (!mxIsCell(THETA_UNARY_IN)) {
size_t num_states = mxGetM(THETA_PAIR_IN);
size_t num_edges = mxGetN(THETA_PAIR_IN);
MARGINALS_PAIR_OUT = mxCreateNumericMatrix(
num_states,
num_edges,
mxDOUBLE_CLASS, mxREAL);
double* mu_res_p = mxGetPr(MARGINALS_PAIR_OUT);
for (size_t e_idx=0; e_idx<mu_pair.size(); e_idx++) {
for (size_t idx=0; idx<mu_pair[e_idx].size(); idx++) {
mu_res_p[e_idx*num_states+idx] = mu_pair[e_idx](idx);
}
}
}
else {
mwSize dim_0 = static_cast<mwSize>(mu_pair.size());
MARGINALS_PAIR_OUT = mxCreateCellArray(1, &dim_0);
for (size_t e_idx=0; e_idx<mu_pair.size(); e_idx++) {
mxArray* m = mxCreateNumericMatrix(
static_cast<int>(mu_pair[e_idx].size()),
1,
mxDOUBLE_CLASS, mxREAL);
double* mu_res_p = mxGetPr(m);
for (size_t idx=0; idx<mu_pair[e_idx].size(); idx++) {
mu_res_p[idx] = mu_pair[e_idx](idx);
}
mxSetCell(MARGINALS_PAIR_OUT, e_idx, m);
}
}
}
delete sdd;
MatlabCPPExit();
}
void mexFunction(int nlhs, mxArray *plhs[],
int nrhs, const mxArray *prhs[])
{
char buf[MAXLENGTH];
char numStr[20];
double x,y,z,tx,ty,tz;
double *pwrench,*ploc,*perr,*pind;
int i,j,numBodies,numContacts;
int sockd;
mxArray *wrench,*loc,*err;
/* Check for proper number of arguments */
if (nrhs > 1) {
mexErrMsgTxt("get average contacts takes at most one input argument.");
} else if (nlhs > 3) {
mexErrMsgTxt("get average contacts takes at most three output arguments.");
}
sockd = ConnectTo("localhost",4765);
if (sockd < 0)
mexErrMsgTxt("Could not connect");
// send how many types of date we're collecting
if (nlhs == 0 ) nlhs=1;
sprintf(buf,"getContacts %d ",nlhs);
// if no bodies were specified by user, read total number of bodies
if (nrhs==0) {
strcat(buf,"ALL\n");
Writeline(sockd,buf,strlen(buf));
Readline(sockd,buf,MAXLENGTH);
sscanf(buf,"%d\n",&numBodies);
}
// otherwise send the body list
else {
numBodies = mxGetNumberOfElements(prhs[0]);
if (numBodies>0) {
sprintf(numStr,"%d ",numBodies);
strcat(buf,numStr);
pind = mxGetPr(prhs[0]);
for (i=0;i<numBodies-1;i++) {
sprintf(numStr,"%d ",(int)pind[i]-1);
strcat(buf,numStr);
}
sprintf(numStr,"%d\n",(int)pind[i]-1);
strcat(buf,numStr);
Writeline(sockd,buf,strlen(buf));
}
}
if (numBodies == 0) {
plhs[0] = NULL;
plhs[0] = NULL;
}
else {
if (numBodies > 1){
for (i=0;i<nlhs;i++)
plhs[i] = mxCreateCellArray(1,&numBodies);
}
for (i=0;i<numBodies;i++) {
Readline(sockd,buf,MAXLENGTH);
sscanf(buf,"%d\n",&numContacts);
if (numContacts == 0) {
wrench = mxCreateScalarDouble(0);
if (nlhs > 1) {loc = mxCreateScalarDouble(0);}
if (nlhs > 2) {err = mxCreateScalarDouble(0);}
}
else {
wrench = mxCreateDoubleMatrix(numContacts,6,mxREAL);
pwrench = mxGetPr(wrench);
if (nlhs > 1) {loc = mxCreateDoubleMatrix(numContacts,3,mxREAL); ploc=mxGetPr(loc);}
if (nlhs > 2) {err = mxCreateDoubleMatrix(numContacts,1,mxREAL); perr=mxGetPr(err);}
}
for (j=0;j<numContacts;j++) {
Readline(sockd,buf,MAXLENGTH);
sscanf(buf,"%lf %lf %lf %lf %lf %lf\n",&x,&y,&z,&tx,&ty,&tz);
pwrench[j] = x;
pwrench[numContacts+j] = y;
pwrench[2*numContacts+j] = z;
pwrench[3*numContacts+j] = tx;
pwrench[4*numContacts+j] = ty;
pwrench[5*numContacts+j] = tz;
if (nlhs > 1) {
Readline(sockd,buf,MAXLENGTH);
sscanf(buf,"%lf %lf %lf\n",&x,&y,&z);
ploc[j] = x;
ploc[numContacts+j] = y;
ploc[2*numContacts+j] = z;
}
if (nlhs > 2) {
Readline(sockd,buf,MAXLENGTH);
sscanf(buf,"%lf\n",perr+j);
}
}
if (numBodies == 1) {
plhs[0] = wrench;
if (nlhs > 1) plhs[1] = loc;
if (nlhs > 2) plhs[2] = err;
} else {
mxSetCell(plhs[0],i,wrench);
if (nlhs > 1) mxSetCell(plhs[1],i,loc);
if (nlhs > 2) mxSetCell(plhs[2],i,err);
}
}
}
CloseConnection(sockd);
return;
}
/*
* [type, num_constraint, constraint_val, dconstraint_val, constraint_name, lower_bound, upper_bound]
* = testSingleTimeKinCnstmex(kinCnst_ptr, q, t)
* @param kinCnst_ptr A pointer to a SingleTimeKinematicConstraint
* object
* @param q A nqx1 double vector
* @param t A double scalar, the time to evaluate constraint
* value, bounds and name. This is optional.
* @retval type The type of the constraint
* @retval num_constraint The number of constraint active at time t
* @retval constraint_val The value of the constraint at time t
* @retval dconstraint_val The gradient of the constraint w.r.t q at time t
* @retval constraint_name The name of the constraint at time t
* @retval lower_bound The lower bound of the constraint at time t
* @retval upper_bound The upper bound of the constraint at time t
* */
void mexFunction(int nlhs, mxArray* plhs[], int nrhs, const mxArray* prhs[]) {
if ((nrhs != 3 && nrhs != 2) || nlhs != 7) {
mexErrMsgIdAndTxt("Drake:testSingleTimeKinCnstmex:BadInputs",
"Usage [type, "
"num_cnst, cnst_val, dcnst_val, cnst_name, lb, ub] = "
"testSingleTimeKinKinCnstmex(kinCnst, q, t)");
}
SingleTimeKinematicConstraint* cnst =
(SingleTimeKinematicConstraint*)getDrakeMexPointer(prhs[0]);
double* t_ptr;
if (nrhs == 2) {
t_ptr = nullptr;
} else {
size_t num_t = mxGetNumberOfElements(prhs[2]);
if (num_t == 0) {
t_ptr = nullptr;
}
if (num_t == 1) {
t_ptr = mxGetPrSafe(prhs[2]);
}
if (num_t > 1) {
mexErrMsgIdAndTxt("Drake:testSingleTimeKinCnstmex:BadInputs",
"t must be either empty or a single number");
}
}
int type = cnst->getType();
int num_cnst = cnst->getNumConstraint(t_ptr);
// mexPrintf("num_cnst = %d\n", num_cnst);
int nq = cnst->getRobotPointer()->number_of_positions();
Eigen::Map<Eigen::VectorXd> q(mxGetPrSafe(prhs[1]), nq);
KinematicsCache<double> cache = cnst->getRobotPointer()->doKinematics(q);
Eigen::VectorXd c(num_cnst);
Eigen::MatrixXd dc(num_cnst, nq);
cnst->eval(t_ptr, cache, c, dc);
// mexPrintf("get c, dc\n");
Eigen::VectorXd lb(num_cnst);
Eigen::VectorXd ub(num_cnst);
cnst->bounds(t_ptr, lb, ub);
// mexPrintf("get lb, ub\n");
std::vector<std::string> cnst_names;
cnst->name(t_ptr, cnst_names);
// mexPrintf("get name\n");
int retvec_size;
if (num_cnst == 0) {
retvec_size = 0;
} else {
retvec_size = 1;
}
plhs[0] = mxCreateDoubleScalar((double)type);
plhs[1] = mxCreateDoubleScalar((double)num_cnst);
plhs[2] = mxCreateDoubleMatrix(num_cnst, retvec_size, mxREAL);
memcpy(mxGetPrSafe(plhs[2]), c.data(), sizeof(double) * num_cnst);
plhs[3] = mxCreateDoubleMatrix(num_cnst, nq, mxREAL);
memcpy(mxGetPrSafe(plhs[3]), dc.data(), sizeof(double) * num_cnst * nq);
int name_ndim = 1;
mwSize name_dims[] = {(mwSize)num_cnst};
plhs[4] = mxCreateCellArray(name_ndim, name_dims);
mxArray* name_ptr;
for (int i = 0; i < num_cnst; i++) {
name_ptr = mxCreateString(cnst_names[i].c_str());
mxSetCell(plhs[4], i, name_ptr);
}
plhs[5] = mxCreateDoubleMatrix(num_cnst, retvec_size, mxREAL);
plhs[6] = mxCreateDoubleMatrix(num_cnst, retvec_size, mxREAL);
memcpy(mxGetPrSafe(plhs[5]), lb.data(), sizeof(double) * num_cnst);
memcpy(mxGetPrSafe(plhs[6]), ub.data(), sizeof(double) * num_cnst);
}
void mexFunction(int nlhs, mxArray *plhs[],
int nrhs, const mxArray *prhs[])
{
char buf[MAXLENGTH];
char numStr[20];
double value;
double *pvals,*pind;
int i,j,numRobots,numDOF;
int sockd;
mxArray *vals;
/* Check for proper number of arguments */
if (nrhs > 1) {
mexErrMsgTxt("getDOFVals takes at most one input argument.");
} else if (nlhs > 1) {
mexErrMsgTxt("getDOFVals takes one output argument.");
}
if (nlhs == 0 ) nlhs=1;
sockd = ConnectTo("localhost",4765);
if (sockd < 0)
mexErrMsgTxt("Could not connect");
strcpy(buf,"getDOFVals ");
// if no robots were specified by user, read total number of robots
if (nrhs==0) {
strcat(buf,"ALL\n");
Writeline(sockd,buf,strlen(buf));
Readline(sockd,buf,MAXLENGTH);
sscanf(buf,"%d\n",&numRobots);
}
// otherwise send the body list
else {
numRobots = mxGetNumberOfElements(prhs[0]);
if (numRobots>0) {
sprintf(numStr,"%d ",numRobots);
strcat(buf,numStr);
pind = mxGetPr(prhs[0]);
for (i=0;i<numRobots-1;i++) {
sprintf(numStr,"%d ",(int)pind[i]-1);
strcat(buf,numStr);
}
sprintf(numStr,"%d\n",(int)pind[i]-1);
strcat(buf,numStr);
Writeline(sockd,buf,strlen(buf));
}
}
if (numRobots == 0) {
plhs[0] = NULL;
plhs[0] = NULL;
}
else {
if (numRobots > 1){
for (i=0;i<nlhs;i++)
plhs[i] = mxCreateCellArray(1,&numRobots);
}
for (i=0;i<numRobots;i++) {
Readline(sockd,buf,MAXLENGTH);
if (!strncmp(buf,"Error",5)) {
mexErrMsgTxt(buf);
break;
}
sscanf(buf,"%d\n",&numDOF);
sprintf(buf,"NumDOF read: %d",numDOF);
if (numDOF == 0) {
vals = mxCreateScalarDouble(0);
}
else {
vals = mxCreateDoubleMatrix(numDOF,1,mxREAL);
pvals = mxGetPr(vals);
}
for (j=0;j<numDOF;j++) {
Readline(sockd,buf,MAXLENGTH);
sscanf(buf,"%lf\n",&value);
pvals[j] = value;
}
if (numRobots == 1) {
plhs[0] = vals;
} else {
mxSetCell(plhs[0],i,vals);
}
}
}
CloseConnection(sockd);
return;
}
void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]) {
int i, j;
mxArray *xm, *cell, *xm_cell;
double *src;
double *dest;
double *dest_cell;
int valid_vars;
int steps;
int this_var_errors;
int warned_diags;
int prepare_for_c = 0;
int extra_solves;
const char *status_names[] = {"optval", "gap", "steps", "converged"};
mwSize dims1x1of1[1] = {1};
mwSize dims[1];
const char *var_names[] = {"u_0", "u_1", "u_2", "u_3", "u_4", "u_5", "u_6", "u_7", "u_8", "u_9", "u_10", "u_11", "u_12", "u_13", "u_14", "u_15", "u_16", "u_17", "u_18", "u_19", "u_20", "x_1", "x_2", "x_3", "x_4", "x_5", "x_6", "x_7", "x_8", "x_9", "x_10", "x_11", "x_12", "x_13", "x_14", "x_15", "x_16", "x_17", "x_18", "x_19", "x_20", "x_21", "u", "x"};
const int num_var_names = 44;
/* Avoid compiler warnings of unused variables by using a dummy assignment. */
warned_diags = j = 0;
extra_solves = 0;
set_defaults();
/* Check we got the right number of arguments. */
if (nrhs == 0)
mexErrMsgTxt("Not enough arguments: You need to specify at least the parameters.\n");
if (nrhs > 1) {
/* Assume that the second argument is the settings. */
if (mxGetField(prhs[1], 0, "eps") != NULL)
settings.eps = *mxGetPr(mxGetField(prhs[1], 0, "eps"));
if (mxGetField(prhs[1], 0, "max_iters") != NULL)
settings.max_iters = *mxGetPr(mxGetField(prhs[1], 0, "max_iters"));
if (mxGetField(prhs[1], 0, "refine_steps") != NULL)
settings.refine_steps = *mxGetPr(mxGetField(prhs[1], 0, "refine_steps"));
if (mxGetField(prhs[1], 0, "verbose") != NULL)
settings.verbose = *mxGetPr(mxGetField(prhs[1], 0, "verbose"));
if (mxGetField(prhs[1], 0, "better_start") != NULL)
settings.better_start = *mxGetPr(mxGetField(prhs[1], 0, "better_start"));
if (mxGetField(prhs[1], 0, "verbose_refinement") != NULL)
settings.verbose_refinement = *mxGetPr(mxGetField(prhs[1], 0,
"verbose_refinement"));
if (mxGetField(prhs[1], 0, "debug") != NULL)
settings.debug = *mxGetPr(mxGetField(prhs[1], 0, "debug"));
if (mxGetField(prhs[1], 0, "kkt_reg") != NULL)
settings.kkt_reg = *mxGetPr(mxGetField(prhs[1], 0, "kkt_reg"));
if (mxGetField(prhs[1], 0, "s_init") != NULL)
settings.s_init = *mxGetPr(mxGetField(prhs[1], 0, "s_init"));
if (mxGetField(prhs[1], 0, "z_init") != NULL)
settings.z_init = *mxGetPr(mxGetField(prhs[1], 0, "z_init"));
if (mxGetField(prhs[1], 0, "resid_tol") != NULL)
settings.resid_tol = *mxGetPr(mxGetField(prhs[1], 0, "resid_tol"));
if (mxGetField(prhs[1], 0, "extra_solves") != NULL)
extra_solves = *mxGetPr(mxGetField(prhs[1], 0, "extra_solves"));
else
extra_solves = 0;
if (mxGetField(prhs[1], 0, "prepare_for_c") != NULL)
prepare_for_c = *mxGetPr(mxGetField(prhs[1], 0, "prepare_for_c"));
}
valid_vars = 0;
this_var_errors = 0;
xm = mxGetField(prhs[0], 0, "A");
if (xm == NULL) {
printf("could not find params.A.\n");
} else {
if (!((mxGetM(xm) == 8) && (mxGetN(xm) == 8))) {
printf("A must be size (8,8), not (%d,%d).\n", mxGetM(xm), mxGetN(xm));
this_var_errors++;
}
if (mxIsComplex(xm)) {
printf("parameter A must be real.\n");
this_var_errors++;
}
if (!mxIsClass(xm, "double")) {
printf("parameter A must be a full matrix of doubles.\n");
this_var_errors++;
}
if (mxIsSparse(xm)) {
printf("parameter A must be a full matrix.\n");
this_var_errors++;
}
if (this_var_errors == 0) {
dest = params.A;
src = mxGetPr(xm);
for (i = 0; i < 64; i++)
*dest++ = *src++;
valid_vars++;
}
}
this_var_errors = 0;
xm = mxGetField(prhs[0], 0, "B");
if (xm == NULL) {
printf("could not find params.B.\n");
} else {
if (!((mxGetM(xm) == 8) && (mxGetN(xm) == 2))) {
printf("B must be size (8,2), not (%d,%d).\n", mxGetM(xm), mxGetN(xm));
this_var_errors++;
}
if (mxIsComplex(xm)) {
printf("parameter B must be real.\n");
this_var_errors++;
}
if (!mxIsClass(xm, "double")) {
printf("parameter B must be a full matrix of doubles.\n");
this_var_errors++;
}
if (mxIsSparse(xm)) {
printf("parameter B must be a full matrix.\n");
this_var_errors++;
}
if (this_var_errors == 0) {
dest = params.B;
src = mxGetPr(xm);
for (i = 0; i < 16; i++)
*dest++ = *src++;
valid_vars++;
}
}
this_var_errors = 0;
xm = mxGetField(prhs[0], 0, "Ff");
if (xm == NULL) {
printf("could not find params.Ff.\n");
} else {
if (!((mxGetM(xm) == 1) && (mxGetN(xm) == 8))) {
printf("Ff must be size (1,8), not (%d,%d).\n", mxGetM(xm), mxGetN(xm));
this_var_errors++;
}
if (mxIsComplex(xm)) {
printf("parameter Ff must be real.\n");
this_var_errors++;
}
if (!mxIsClass(xm, "double")) {
printf("parameter Ff must be a full matrix of doubles.\n");
this_var_errors++;
}
if (mxIsSparse(xm)) {
printf("parameter Ff must be a full matrix.\n");
this_var_errors++;
}
if (this_var_errors == 0) {
dest = params.Ff;
src = mxGetPr(xm);
for (i = 0; i < 8; i++)
*dest++ = *src++;
valid_vars++;
}
}
this_var_errors = 0;
xm = mxGetField(prhs[0], 0, "Fu");
if (xm == NULL) {
printf("could not find params.Fu.\n");
} else {
if (!((mxGetM(xm) == 4) && (mxGetN(xm) == 2))) {
printf("Fu must be size (4,2), not (%d,%d).\n", mxGetM(xm), mxGetN(xm));
this_var_errors++;
}
if (mxIsComplex(xm)) {
printf("parameter Fu must be real.\n");
this_var_errors++;
}
if (!mxIsClass(xm, "double")) {
printf("parameter Fu must be a full matrix of doubles.\n");
this_var_errors++;
}
if (mxIsSparse(xm)) {
printf("parameter Fu must be a full matrix.\n");
this_var_errors++;
}
if (this_var_errors == 0) {
dest = params.Fu;
src = mxGetPr(xm);
dest[0] = src[0]; /* (1,1) entry. */
dest[1] = src[5]; /* (2,2) entry. */
dest[2] = src[2]; /* (3,1) entry. */
dest[3] = src[7]; /* (4,2) entry. */
valid_vars++;
}
}
this_var_errors = 0;
xm = mxGetField(prhs[0], 0, "Fxs");
if (xm == NULL) {
printf("could not find params.Fxs.\n");
} else {
if (!((mxGetM(xm) == 4) && (mxGetN(xm) == 8))) {
printf("Fxs must be size (4,8), not (%d,%d).\n", mxGetM(xm), mxGetN(xm));
this_var_errors++;
}
if (mxIsComplex(xm)) {
printf("parameter Fxs must be real.\n");
this_var_errors++;
}
if (!mxIsClass(xm, "double")) {
printf("parameter Fxs must be a full matrix of doubles.\n");
this_var_errors++;
}
if (mxIsSparse(xm)) {
printf("parameter Fxs must be a full matrix.\n");
this_var_errors++;
}
if (this_var_errors == 0) {
dest = params.Fxs;
src = mxGetPr(xm);
dest[0] = src[0]; /* (1,1) entry. */
dest[1] = src[5]; /* (2,2) entry. */
dest[2] = src[10]; /* (3,3) entry. */
dest[3] = src[15]; /* (4,4) entry. */
valid_vars++;
}
}
this_var_errors = 0;
xm = mxGetField(prhs[0], 0, "Q");
if (xm == NULL) {
printf("could not find params.Q.\n");
} else {
if (!((mxGetM(xm) == 8) && (mxGetN(xm) == 8))) {
printf("Q must be size (8,8), not (%d,%d).\n", mxGetM(xm), mxGetN(xm));
this_var_errors++;
}
if (mxIsComplex(xm)) {
printf("parameter Q must be real.\n");
this_var_errors++;
}
if (!mxIsClass(xm, "double")) {
printf("parameter Q must be a full matrix of doubles.\n");
this_var_errors++;
}
if (mxIsSparse(xm)) {
printf("parameter Q must be a full matrix.\n");
this_var_errors++;
}
if (this_var_errors == 0) {
dest = params.Q;
src = mxGetPr(xm);
for (i = 0; i < 64; i++)
*dest++ = *src++;
valid_vars++;
}
}
this_var_errors = 0;
xm = mxGetField(prhs[0], 0, "Q_final");
if (xm == NULL) {
printf("could not find params.Q_final.\n");
} else {
if (!((mxGetM(xm) == 8) && (mxGetN(xm) == 8))) {
printf("Q_final must be size (8,8), not (%d,%d).\n", mxGetM(xm), mxGetN(xm));
this_var_errors++;
}
if (mxIsComplex(xm)) {
printf("parameter Q_final must be real.\n");
this_var_errors++;
}
if (!mxIsClass(xm, "double")) {
printf("parameter Q_final must be a full matrix of doubles.\n");
this_var_errors++;
}
if (mxIsSparse(xm)) {
printf("parameter Q_final must be a full matrix.\n");
this_var_errors++;
}
if (this_var_errors == 0) {
dest = params.Q_final;
src = mxGetPr(xm);
for (i = 0; i < 64; i++)
*dest++ = *src++;
valid_vars++;
}
}
this_var_errors = 0;
xm = mxGetField(prhs[0], 0, "R");
if (xm == NULL) {
printf("could not find params.R.\n");
} else {
if (!((mxGetM(xm) == 2) && (mxGetN(xm) == 2))) {
printf("R must be size (2,2), not (%d,%d).\n", mxGetM(xm), mxGetN(xm));
this_var_errors++;
}
if (mxIsComplex(xm)) {
printf("parameter R must be real.\n");
this_var_errors++;
}
if (!mxIsClass(xm, "double")) {
printf("parameter R must be a full matrix of doubles.\n");
this_var_errors++;
}
if (mxIsSparse(xm)) {
printf("parameter R must be a full matrix.\n");
this_var_errors++;
}
if (this_var_errors == 0) {
dest = params.R;
src = mxGetPr(xm);
for (i = 0; i < 4; i++)
*dest++ = *src++;
valid_vars++;
}
}
this_var_errors = 0;
xm = mxGetField(prhs[0], 0, "d");
if (xm == NULL) {
printf("could not find params.d.\n");
} else {
if (!((mxGetM(xm) == 8) && (mxGetN(xm) == 1))) {
printf("d must be size (8,1), not (%d,%d).\n", mxGetM(xm), mxGetN(xm));
this_var_errors++;
}
if (mxIsComplex(xm)) {
printf("parameter d must be real.\n");
this_var_errors++;
}
if (!mxIsClass(xm, "double")) {
printf("parameter d must be a full matrix of doubles.\n");
this_var_errors++;
}
if (mxIsSparse(xm)) {
printf("parameter d must be a full matrix.\n");
this_var_errors++;
}
if (this_var_errors == 0) {
dest = params.d;
src = mxGetPr(xm);
for (i = 0; i < 8; i++)
*dest++ = *src++;
valid_vars++;
}
}
this_var_errors = 0;
xm = mxGetField(prhs[0], 0, "f");
if (xm == NULL) {
printf("could not find params.f.\n");
} else {
if (!((mxGetM(xm) == 4) && (mxGetN(xm) == 1))) {
printf("f must be size (4,1), not (%d,%d).\n", mxGetM(xm), mxGetN(xm));
this_var_errors++;
}
if (mxIsComplex(xm)) {
printf("parameter f must be real.\n");
this_var_errors++;
}
if (!mxIsClass(xm, "double")) {
printf("parameter f must be a full matrix of doubles.\n");
this_var_errors++;
}
if (mxIsSparse(xm)) {
printf("parameter f must be a full matrix.\n");
this_var_errors++;
}
if (this_var_errors == 0) {
dest = params.f;
src = mxGetPr(xm);
for (i = 0; i < 4; i++)
*dest++ = *src++;
valid_vars++;
}
}
this_var_errors = 0;
xm = mxGetField(prhs[0], 0, "ff");
if (xm == NULL) {
printf("could not find params.ff.\n");
} else {
if (!((mxGetM(xm) == 1) && (mxGetN(xm) == 1))) {
printf("ff must be size (1,1), not (%d,%d).\n", mxGetM(xm), mxGetN(xm));
this_var_errors++;
}
if (mxIsComplex(xm)) {
printf("parameter ff must be real.\n");
this_var_errors++;
}
if (!mxIsClass(xm, "double")) {
printf("parameter ff must be a full matrix of doubles.\n");
this_var_errors++;
}
if (mxIsSparse(xm)) {
printf("parameter ff must be a full matrix.\n");
this_var_errors++;
}
if (this_var_errors == 0) {
dest = params.ff;
src = mxGetPr(xm);
for (i = 0; i < 1; i++)
*dest++ = *src++;
valid_vars++;
}
}
this_var_errors = 0;
xm = mxGetField(prhs[0], 0, "fs");
if (xm == NULL) {
printf("could not find params.fs.\n");
} else {
if (!((mxGetM(xm) == 4) && (mxGetN(xm) == 1))) {
printf("fs must be size (4,1), not (%d,%d).\n", mxGetM(xm), mxGetN(xm));
this_var_errors++;
}
if (mxIsComplex(xm)) {
printf("parameter fs must be real.\n");
this_var_errors++;
}
if (!mxIsClass(xm, "double")) {
printf("parameter fs must be a full matrix of doubles.\n");
this_var_errors++;
}
if (mxIsSparse(xm)) {
printf("parameter fs must be a full matrix.\n");
this_var_errors++;
}
if (this_var_errors == 0) {
dest = params.fs;
src = mxGetPr(xm);
for (i = 0; i < 4; i++)
*dest++ = *src++;
valid_vars++;
}
}
this_var_errors = 0;
xm = mxGetField(prhs[0], 0, "x_0");
if (xm == NULL) {
printf("could not find params.x_0.\n");
} else {
if (!((mxGetM(xm) == 8) && (mxGetN(xm) == 1))) {
printf("x_0 must be size (8,1), not (%d,%d).\n", mxGetM(xm), mxGetN(xm));
this_var_errors++;
}
if (mxIsComplex(xm)) {
printf("parameter x_0 must be real.\n");
this_var_errors++;
}
if (!mxIsClass(xm, "double")) {
printf("parameter x_0 must be a full matrix of doubles.\n");
this_var_errors++;
}
if (mxIsSparse(xm)) {
printf("parameter x_0 must be a full matrix.\n");
this_var_errors++;
}
if (this_var_errors == 0) {
dest = params.x_0;
src = mxGetPr(xm);
for (i = 0; i < 8; i++)
*dest++ = *src++;
valid_vars++;
}
}
this_var_errors = 0;
xm = mxGetField(prhs[0], 0, "xt");
if (xm == NULL) {
printf("could not find params.xt.\n");
} else {
if (!((mxGetM(xm) == 8) && (mxGetN(xm) == 1))) {
printf("xt must be size (8,1), not (%d,%d).\n", mxGetM(xm), mxGetN(xm));
this_var_errors++;
}
if (mxIsComplex(xm)) {
printf("parameter xt must be real.\n");
this_var_errors++;
}
if (!mxIsClass(xm, "double")) {
printf("parameter xt must be a full matrix of doubles.\n");
this_var_errors++;
}
if (mxIsSparse(xm)) {
printf("parameter xt must be a full matrix.\n");
this_var_errors++;
}
if (this_var_errors == 0) {
dest = params.xt;
src = mxGetPr(xm);
for (i = 0; i < 8; i++)
*dest++ = *src++;
valid_vars++;
}
}
if (valid_vars != 14) {
printf("Error: %d parameters are invalid.\n", 14 - valid_vars);
mexErrMsgTxt("invalid parameters found.");
}
if (prepare_for_c) {
printf("settings.prepare_for_c == 1. thus, outputting for C.\n");
for (i = 0; i < 8; i++)
printf(" params.x_0[%d] = %.6g;\n", i, params.x_0[i]);
for (i = 0; i < 8; i++)
printf(" params.xt[%d] = %.6g;\n", i, params.xt[i]);
for (i = 0; i < 64; i++)
printf(" params.Q[%d] = %.6g;\n", i, params.Q[i]);
for (i = 0; i < 4; i++)
printf(" params.R[%d] = %.6g;\n", i, params.R[i]);
for (i = 0; i < 4; i++)
printf(" params.Fxs[%d] = %.6g;\n", i, params.Fxs[i]);
for (i = 0; i < 4; i++)
printf(" params.fs[%d] = %.6g;\n", i, params.fs[i]);
for (i = 0; i < 64; i++)
printf(" params.Q_final[%d] = %.6g;\n", i, params.Q_final[i]);
for (i = 0; i < 64; i++)
printf(" params.A[%d] = %.6g;\n", i, params.A[i]);
for (i = 0; i < 16; i++)
printf(" params.B[%d] = %.6g;\n", i, params.B[i]);
for (i = 0; i < 8; i++)
printf(" params.d[%d] = %.6g;\n", i, params.d[i]);
for (i = 0; i < 4; i++)
printf(" params.Fu[%d] = %.6g;\n", i, params.Fu[i]);
for (i = 0; i < 4; i++)
printf(" params.f[%d] = %.6g;\n", i, params.f[i]);
for (i = 0; i < 8; i++)
printf(" params.Ff[%d] = %.6g;\n", i, params.Ff[i]);
for (i = 0; i < 1; i++)
printf(" params.ff[%d] = %.6g;\n", i, params.ff[i]);
}
/* Perform the actual solve in here. */
steps = solve();
/* For profiling purposes, allow extra silent solves if desired. */
settings.verbose = 0;
for (i = 0; i < extra_solves; i++)
solve();
/* Update the status variables. */
plhs[1] = mxCreateStructArray(1, dims1x1of1, 4, status_names);
xm = mxCreateDoubleMatrix(1, 1, mxREAL);
mxSetField(plhs[1], 0, "optval", xm);
*mxGetPr(xm) = work.optval;
xm = mxCreateDoubleMatrix(1, 1, mxREAL);
mxSetField(plhs[1], 0, "gap", xm);
*mxGetPr(xm) = work.gap;
xm = mxCreateDoubleMatrix(1, 1, mxREAL);
mxSetField(plhs[1], 0, "steps", xm);
*mxGetPr(xm) = steps;
xm = mxCreateDoubleMatrix(1, 1, mxREAL);
mxSetField(plhs[1], 0, "converged", xm);
*mxGetPr(xm) = work.converged;
/* Extract variable values. */
plhs[0] = mxCreateStructArray(1, dims1x1of1, num_var_names, var_names);
/* Create cell arrays for indexed variables. */
dims[0] = 20;
cell = mxCreateCellArray(1, dims);
mxSetField(plhs[0], 0, "u", cell);
dims[0] = 21;
cell = mxCreateCellArray(1, dims);
mxSetField(plhs[0], 0, "x", cell);
xm = mxCreateDoubleMatrix(2, 1, mxREAL);
mxSetField(plhs[0], 0, "u_0", xm);
dest = mxGetPr(xm);
src = vars.u_0;
for (i = 0; i < 2; i++) {
*dest++ = *src++;
}
xm = mxCreateDoubleMatrix(2, 1, mxREAL);
mxSetField(plhs[0], 0, "u_1", xm);
xm_cell = mxCreateDoubleMatrix(2, 1, mxREAL);
cell = mxGetField(plhs[0], 0, "u");
mxSetCell(cell, 0, xm_cell);
dest = mxGetPr(xm);
dest_cell = mxGetPr(xm_cell);
src = vars.u_1;
for (i = 0; i < 2; i++) {
*dest++ = *src;
*dest_cell++ = *src++;
}
xm = mxCreateDoubleMatrix(2, 1, mxREAL);
mxSetField(plhs[0], 0, "u_2", xm);
xm_cell = mxCreateDoubleMatrix(2, 1, mxREAL);
cell = mxGetField(plhs[0], 0, "u");
mxSetCell(cell, 1, xm_cell);
dest = mxGetPr(xm);
dest_cell = mxGetPr(xm_cell);
src = vars.u_2;
for (i = 0; i < 2; i++) {
*dest++ = *src;
*dest_cell++ = *src++;
}
xm = mxCreateDoubleMatrix(2, 1, mxREAL);
mxSetField(plhs[0], 0, "u_3", xm);
xm_cell = mxCreateDoubleMatrix(2, 1, mxREAL);
cell = mxGetField(plhs[0], 0, "u");
mxSetCell(cell, 2, xm_cell);
dest = mxGetPr(xm);
dest_cell = mxGetPr(xm_cell);
src = vars.u_3;
for (i = 0; i < 2; i++) {
*dest++ = *src;
*dest_cell++ = *src++;
}
xm = mxCreateDoubleMatrix(2, 1, mxREAL);
mxSetField(plhs[0], 0, "u_4", xm);
xm_cell = mxCreateDoubleMatrix(2, 1, mxREAL);
cell = mxGetField(plhs[0], 0, "u");
mxSetCell(cell, 3, xm_cell);
dest = mxGetPr(xm);
dest_cell = mxGetPr(xm_cell);
src = vars.u_4;
for (i = 0; i < 2; i++) {
*dest++ = *src;
*dest_cell++ = *src++;
}
xm = mxCreateDoubleMatrix(2, 1, mxREAL);
mxSetField(plhs[0], 0, "u_5", xm);
xm_cell = mxCreateDoubleMatrix(2, 1, mxREAL);
cell = mxGetField(plhs[0], 0, "u");
mxSetCell(cell, 4, xm_cell);
dest = mxGetPr(xm);
dest_cell = mxGetPr(xm_cell);
src = vars.u_5;
for (i = 0; i < 2; i++) {
*dest++ = *src;
*dest_cell++ = *src++;
}
xm = mxCreateDoubleMatrix(2, 1, mxREAL);
mxSetField(plhs[0], 0, "u_6", xm);
xm_cell = mxCreateDoubleMatrix(2, 1, mxREAL);
cell = mxGetField(plhs[0], 0, "u");
mxSetCell(cell, 5, xm_cell);
dest = mxGetPr(xm);
dest_cell = mxGetPr(xm_cell);
src = vars.u_6;
for (i = 0; i < 2; i++) {
*dest++ = *src;
*dest_cell++ = *src++;
}
xm = mxCreateDoubleMatrix(2, 1, mxREAL);
mxSetField(plhs[0], 0, "u_7", xm);
xm_cell = mxCreateDoubleMatrix(2, 1, mxREAL);
cell = mxGetField(plhs[0], 0, "u");
mxSetCell(cell, 6, xm_cell);
dest = mxGetPr(xm);
dest_cell = mxGetPr(xm_cell);
src = vars.u_7;
for (i = 0; i < 2; i++) {
*dest++ = *src;
*dest_cell++ = *src++;
}
xm = mxCreateDoubleMatrix(2, 1, mxREAL);
mxSetField(plhs[0], 0, "u_8", xm);
xm_cell = mxCreateDoubleMatrix(2, 1, mxREAL);
cell = mxGetField(plhs[0], 0, "u");
mxSetCell(cell, 7, xm_cell);
dest = mxGetPr(xm);
dest_cell = mxGetPr(xm_cell);
src = vars.u_8;
for (i = 0; i < 2; i++) {
*dest++ = *src;
*dest_cell++ = *src++;
}
xm = mxCreateDoubleMatrix(2, 1, mxREAL);
mxSetField(plhs[0], 0, "u_9", xm);
xm_cell = mxCreateDoubleMatrix(2, 1, mxREAL);
cell = mxGetField(plhs[0], 0, "u");
mxSetCell(cell, 8, xm_cell);
dest = mxGetPr(xm);
dest_cell = mxGetPr(xm_cell);
src = vars.u_9;
for (i = 0; i < 2; i++) {
*dest++ = *src;
*dest_cell++ = *src++;
}
xm = mxCreateDoubleMatrix(2, 1, mxREAL);
mxSetField(plhs[0], 0, "u_10", xm);
xm_cell = mxCreateDoubleMatrix(2, 1, mxREAL);
cell = mxGetField(plhs[0], 0, "u");
mxSetCell(cell, 9, xm_cell);
dest = mxGetPr(xm);
dest_cell = mxGetPr(xm_cell);
src = vars.u_10;
for (i = 0; i < 2; i++) {
*dest++ = *src;
*dest_cell++ = *src++;
}
xm = mxCreateDoubleMatrix(2, 1, mxREAL);
mxSetField(plhs[0], 0, "u_11", xm);
xm_cell = mxCreateDoubleMatrix(2, 1, mxREAL);
cell = mxGetField(plhs[0], 0, "u");
mxSetCell(cell, 10, xm_cell);
dest = mxGetPr(xm);
dest_cell = mxGetPr(xm_cell);
src = vars.u_11;
for (i = 0; i < 2; i++) {
*dest++ = *src;
*dest_cell++ = *src++;
}
xm = mxCreateDoubleMatrix(2, 1, mxREAL);
mxSetField(plhs[0], 0, "u_12", xm);
xm_cell = mxCreateDoubleMatrix(2, 1, mxREAL);
cell = mxGetField(plhs[0], 0, "u");
mxSetCell(cell, 11, xm_cell);
dest = mxGetPr(xm);
dest_cell = mxGetPr(xm_cell);
src = vars.u_12;
for (i = 0; i < 2; i++) {
*dest++ = *src;
*dest_cell++ = *src++;
}
xm = mxCreateDoubleMatrix(2, 1, mxREAL);
mxSetField(plhs[0], 0, "u_13", xm);
xm_cell = mxCreateDoubleMatrix(2, 1, mxREAL);
cell = mxGetField(plhs[0], 0, "u");
mxSetCell(cell, 12, xm_cell);
dest = mxGetPr(xm);
dest_cell = mxGetPr(xm_cell);
src = vars.u_13;
for (i = 0; i < 2; i++) {
*dest++ = *src;
*dest_cell++ = *src++;
}
xm = mxCreateDoubleMatrix(2, 1, mxREAL);
mxSetField(plhs[0], 0, "u_14", xm);
xm_cell = mxCreateDoubleMatrix(2, 1, mxREAL);
cell = mxGetField(plhs[0], 0, "u");
mxSetCell(cell, 13, xm_cell);
dest = mxGetPr(xm);
dest_cell = mxGetPr(xm_cell);
src = vars.u_14;
for (i = 0; i < 2; i++) {
*dest++ = *src;
*dest_cell++ = *src++;
}
xm = mxCreateDoubleMatrix(2, 1, mxREAL);
mxSetField(plhs[0], 0, "u_15", xm);
xm_cell = mxCreateDoubleMatrix(2, 1, mxREAL);
cell = mxGetField(plhs[0], 0, "u");
mxSetCell(cell, 14, xm_cell);
dest = mxGetPr(xm);
dest_cell = mxGetPr(xm_cell);
src = vars.u_15;
for (i = 0; i < 2; i++) {
*dest++ = *src;
*dest_cell++ = *src++;
}
xm = mxCreateDoubleMatrix(2, 1, mxREAL);
mxSetField(plhs[0], 0, "u_16", xm);
xm_cell = mxCreateDoubleMatrix(2, 1, mxREAL);
cell = mxGetField(plhs[0], 0, "u");
mxSetCell(cell, 15, xm_cell);
dest = mxGetPr(xm);
dest_cell = mxGetPr(xm_cell);
src = vars.u_16;
for (i = 0; i < 2; i++) {
*dest++ = *src;
*dest_cell++ = *src++;
}
xm = mxCreateDoubleMatrix(2, 1, mxREAL);
mxSetField(plhs[0], 0, "u_17", xm);
xm_cell = mxCreateDoubleMatrix(2, 1, mxREAL);
cell = mxGetField(plhs[0], 0, "u");
mxSetCell(cell, 16, xm_cell);
dest = mxGetPr(xm);
dest_cell = mxGetPr(xm_cell);
src = vars.u_17;
for (i = 0; i < 2; i++) {
*dest++ = *src;
*dest_cell++ = *src++;
}
xm = mxCreateDoubleMatrix(2, 1, mxREAL);
mxSetField(plhs[0], 0, "u_18", xm);
xm_cell = mxCreateDoubleMatrix(2, 1, mxREAL);
cell = mxGetField(plhs[0], 0, "u");
mxSetCell(cell, 17, xm_cell);
dest = mxGetPr(xm);
dest_cell = mxGetPr(xm_cell);
src = vars.u_18;
for (i = 0; i < 2; i++) {
*dest++ = *src;
*dest_cell++ = *src++;
}
xm = mxCreateDoubleMatrix(2, 1, mxREAL);
mxSetField(plhs[0], 0, "u_19", xm);
xm_cell = mxCreateDoubleMatrix(2, 1, mxREAL);
cell = mxGetField(plhs[0], 0, "u");
mxSetCell(cell, 18, xm_cell);
dest = mxGetPr(xm);
dest_cell = mxGetPr(xm_cell);
src = vars.u_19;
for (i = 0; i < 2; i++) {
*dest++ = *src;
*dest_cell++ = *src++;
}
xm = mxCreateDoubleMatrix(2, 1, mxREAL);
mxSetField(plhs[0], 0, "u_20", xm);
xm_cell = mxCreateDoubleMatrix(2, 1, mxREAL);
cell = mxGetField(plhs[0], 0, "u");
mxSetCell(cell, 19, xm_cell);
dest = mxGetPr(xm);
dest_cell = mxGetPr(xm_cell);
src = vars.u_20;
for (i = 0; i < 2; i++) {
*dest++ = *src;
*dest_cell++ = *src++;
}
xm = mxCreateDoubleMatrix(8, 1, mxREAL);
mxSetField(plhs[0], 0, "x_1", xm);
xm_cell = mxCreateDoubleMatrix(8, 1, mxREAL);
cell = mxGetField(plhs[0], 0, "x");
mxSetCell(cell, 0, xm_cell);
dest = mxGetPr(xm);
dest_cell = mxGetPr(xm_cell);
src = vars.x_1;
for (i = 0; i < 8; i++) {
*dest++ = *src;
*dest_cell++ = *src++;
}
xm = mxCreateDoubleMatrix(8, 1, mxREAL);
mxSetField(plhs[0], 0, "x_2", xm);
xm_cell = mxCreateDoubleMatrix(8, 1, mxREAL);
cell = mxGetField(plhs[0], 0, "x");
mxSetCell(cell, 1, xm_cell);
dest = mxGetPr(xm);
dest_cell = mxGetPr(xm_cell);
src = vars.x_2;
for (i = 0; i < 8; i++) {
*dest++ = *src;
*dest_cell++ = *src++;
}
xm = mxCreateDoubleMatrix(8, 1, mxREAL);
mxSetField(plhs[0], 0, "x_3", xm);
xm_cell = mxCreateDoubleMatrix(8, 1, mxREAL);
cell = mxGetField(plhs[0], 0, "x");
mxSetCell(cell, 2, xm_cell);
dest = mxGetPr(xm);
dest_cell = mxGetPr(xm_cell);
src = vars.x_3;
for (i = 0; i < 8; i++) {
*dest++ = *src;
*dest_cell++ = *src++;
}
xm = mxCreateDoubleMatrix(8, 1, mxREAL);
mxSetField(plhs[0], 0, "x_4", xm);
xm_cell = mxCreateDoubleMatrix(8, 1, mxREAL);
cell = mxGetField(plhs[0], 0, "x");
mxSetCell(cell, 3, xm_cell);
dest = mxGetPr(xm);
dest_cell = mxGetPr(xm_cell);
src = vars.x_4;
for (i = 0; i < 8; i++) {
*dest++ = *src;
*dest_cell++ = *src++;
}
xm = mxCreateDoubleMatrix(8, 1, mxREAL);
mxSetField(plhs[0], 0, "x_5", xm);
xm_cell = mxCreateDoubleMatrix(8, 1, mxREAL);
cell = mxGetField(plhs[0], 0, "x");
mxSetCell(cell, 4, xm_cell);
dest = mxGetPr(xm);
dest_cell = mxGetPr(xm_cell);
src = vars.x_5;
for (i = 0; i < 8; i++) {
*dest++ = *src;
*dest_cell++ = *src++;
}
xm = mxCreateDoubleMatrix(8, 1, mxREAL);
mxSetField(plhs[0], 0, "x_6", xm);
xm_cell = mxCreateDoubleMatrix(8, 1, mxREAL);
cell = mxGetField(plhs[0], 0, "x");
mxSetCell(cell, 5, xm_cell);
dest = mxGetPr(xm);
dest_cell = mxGetPr(xm_cell);
src = vars.x_6;
for (i = 0; i < 8; i++) {
*dest++ = *src;
*dest_cell++ = *src++;
}
xm = mxCreateDoubleMatrix(8, 1, mxREAL);
mxSetField(plhs[0], 0, "x_7", xm);
xm_cell = mxCreateDoubleMatrix(8, 1, mxREAL);
cell = mxGetField(plhs[0], 0, "x");
mxSetCell(cell, 6, xm_cell);
dest = mxGetPr(xm);
dest_cell = mxGetPr(xm_cell);
src = vars.x_7;
for (i = 0; i < 8; i++) {
*dest++ = *src;
*dest_cell++ = *src++;
}
xm = mxCreateDoubleMatrix(8, 1, mxREAL);
mxSetField(plhs[0], 0, "x_8", xm);
xm_cell = mxCreateDoubleMatrix(8, 1, mxREAL);
cell = mxGetField(plhs[0], 0, "x");
mxSetCell(cell, 7, xm_cell);
dest = mxGetPr(xm);
dest_cell = mxGetPr(xm_cell);
src = vars.x_8;
for (i = 0; i < 8; i++) {
*dest++ = *src;
*dest_cell++ = *src++;
}
xm = mxCreateDoubleMatrix(8, 1, mxREAL);
mxSetField(plhs[0], 0, "x_9", xm);
xm_cell = mxCreateDoubleMatrix(8, 1, mxREAL);
cell = mxGetField(plhs[0], 0, "x");
mxSetCell(cell, 8, xm_cell);
dest = mxGetPr(xm);
dest_cell = mxGetPr(xm_cell);
src = vars.x_9;
for (i = 0; i < 8; i++) {
*dest++ = *src;
*dest_cell++ = *src++;
}
xm = mxCreateDoubleMatrix(8, 1, mxREAL);
mxSetField(plhs[0], 0, "x_10", xm);
xm_cell = mxCreateDoubleMatrix(8, 1, mxREAL);
cell = mxGetField(plhs[0], 0, "x");
mxSetCell(cell, 9, xm_cell);
dest = mxGetPr(xm);
dest_cell = mxGetPr(xm_cell);
src = vars.x_10;
for (i = 0; i < 8; i++) {
*dest++ = *src;
*dest_cell++ = *src++;
}
xm = mxCreateDoubleMatrix(8, 1, mxREAL);
mxSetField(plhs[0], 0, "x_11", xm);
xm_cell = mxCreateDoubleMatrix(8, 1, mxREAL);
cell = mxGetField(plhs[0], 0, "x");
mxSetCell(cell, 10, xm_cell);
dest = mxGetPr(xm);
dest_cell = mxGetPr(xm_cell);
src = vars.x_11;
for (i = 0; i < 8; i++) {
*dest++ = *src;
*dest_cell++ = *src++;
}
xm = mxCreateDoubleMatrix(8, 1, mxREAL);
mxSetField(plhs[0], 0, "x_12", xm);
xm_cell = mxCreateDoubleMatrix(8, 1, mxREAL);
cell = mxGetField(plhs[0], 0, "x");
mxSetCell(cell, 11, xm_cell);
dest = mxGetPr(xm);
dest_cell = mxGetPr(xm_cell);
src = vars.x_12;
for (i = 0; i < 8; i++) {
*dest++ = *src;
*dest_cell++ = *src++;
}
xm = mxCreateDoubleMatrix(8, 1, mxREAL);
mxSetField(plhs[0], 0, "x_13", xm);
xm_cell = mxCreateDoubleMatrix(8, 1, mxREAL);
cell = mxGetField(plhs[0], 0, "x");
mxSetCell(cell, 12, xm_cell);
dest = mxGetPr(xm);
dest_cell = mxGetPr(xm_cell);
src = vars.x_13;
for (i = 0; i < 8; i++) {
*dest++ = *src;
*dest_cell++ = *src++;
}
xm = mxCreateDoubleMatrix(8, 1, mxREAL);
mxSetField(plhs[0], 0, "x_14", xm);
xm_cell = mxCreateDoubleMatrix(8, 1, mxREAL);
cell = mxGetField(plhs[0], 0, "x");
mxSetCell(cell, 13, xm_cell);
dest = mxGetPr(xm);
dest_cell = mxGetPr(xm_cell);
src = vars.x_14;
for (i = 0; i < 8; i++) {
*dest++ = *src;
*dest_cell++ = *src++;
}
xm = mxCreateDoubleMatrix(8, 1, mxREAL);
mxSetField(plhs[0], 0, "x_15", xm);
xm_cell = mxCreateDoubleMatrix(8, 1, mxREAL);
cell = mxGetField(plhs[0], 0, "x");
mxSetCell(cell, 14, xm_cell);
dest = mxGetPr(xm);
dest_cell = mxGetPr(xm_cell);
src = vars.x_15;
for (i = 0; i < 8; i++) {
*dest++ = *src;
*dest_cell++ = *src++;
}
xm = mxCreateDoubleMatrix(8, 1, mxREAL);
mxSetField(plhs[0], 0, "x_16", xm);
xm_cell = mxCreateDoubleMatrix(8, 1, mxREAL);
cell = mxGetField(plhs[0], 0, "x");
mxSetCell(cell, 15, xm_cell);
dest = mxGetPr(xm);
dest_cell = mxGetPr(xm_cell);
src = vars.x_16;
for (i = 0; i < 8; i++) {
*dest++ = *src;
*dest_cell++ = *src++;
}
xm = mxCreateDoubleMatrix(8, 1, mxREAL);
mxSetField(plhs[0], 0, "x_17", xm);
xm_cell = mxCreateDoubleMatrix(8, 1, mxREAL);
cell = mxGetField(plhs[0], 0, "x");
mxSetCell(cell, 16, xm_cell);
dest = mxGetPr(xm);
dest_cell = mxGetPr(xm_cell);
src = vars.x_17;
for (i = 0; i < 8; i++) {
*dest++ = *src;
*dest_cell++ = *src++;
}
xm = mxCreateDoubleMatrix(8, 1, mxREAL);
mxSetField(plhs[0], 0, "x_18", xm);
xm_cell = mxCreateDoubleMatrix(8, 1, mxREAL);
cell = mxGetField(plhs[0], 0, "x");
mxSetCell(cell, 17, xm_cell);
dest = mxGetPr(xm);
dest_cell = mxGetPr(xm_cell);
src = vars.x_18;
for (i = 0; i < 8; i++) {
*dest++ = *src;
*dest_cell++ = *src++;
}
xm = mxCreateDoubleMatrix(8, 1, mxREAL);
mxSetField(plhs[0], 0, "x_19", xm);
xm_cell = mxCreateDoubleMatrix(8, 1, mxREAL);
cell = mxGetField(plhs[0], 0, "x");
mxSetCell(cell, 18, xm_cell);
dest = mxGetPr(xm);
dest_cell = mxGetPr(xm_cell);
src = vars.x_19;
for (i = 0; i < 8; i++) {
*dest++ = *src;
*dest_cell++ = *src++;
}
xm = mxCreateDoubleMatrix(8, 1, mxREAL);
mxSetField(plhs[0], 0, "x_20", xm);
xm_cell = mxCreateDoubleMatrix(8, 1, mxREAL);
cell = mxGetField(plhs[0], 0, "x");
mxSetCell(cell, 19, xm_cell);
dest = mxGetPr(xm);
dest_cell = mxGetPr(xm_cell);
src = vars.x_20;
for (i = 0; i < 8; i++) {
*dest++ = *src;
*dest_cell++ = *src++;
}
xm = mxCreateDoubleMatrix(8, 1, mxREAL);
mxSetField(plhs[0], 0, "x_21", xm);
xm_cell = mxCreateDoubleMatrix(8, 1, mxREAL);
cell = mxGetField(plhs[0], 0, "x");
mxSetCell(cell, 20, xm_cell);
dest = mxGetPr(xm);
dest_cell = mxGetPr(xm_cell);
src = vars.x_21;
for (i = 0; i < 8; i++) {
*dest++ = *src;
*dest_cell++ = *src++;
}
}
/* MEX interface function */
void mexFunction( int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[] )
{
FreemanPyramidCntrlStruct *cntrl = NULL;
FreemanPyramidStruct *fp = NULL;
FreemanCacheCntrlStruct *fpcc = NULL;
float *image, *tmp;
float epsAmp = 0.0;
float epsC = 0.0;
int nxImage, nyImage;
int iChannel, iScale, iOrient;
int iCell, iOut, iBasis;
int k, n;
mwSize dims[3];
mxArray *mxTmp;
/* check for the required input arguments */
if( nrhs < 2 || nrhs > 3 )
mexErrMsgTxt( "Input expected: CNTRL, IMAGE, CACHECNTRL" );
/* decode the matlab version of the pyramid control structure
and allocate the pyramid structure */
mexFreemanPyramidCntrlStruct( prhs[0], &cntrl, &epsAmp, &epsC );
fp = newFreemanPyramidStruct( cntrl );
if( nrhs == 3 )
mexFreemanCacheCntrlStruct( prhs[2], &fpcc );
/* get the image to be filtered */
mex2float( prhs[1], &image, &nxImage, &nyImage );
/* build the pyramid */
if( fpcc == NULL ) {
computeFreemanPyramid_Float(fp, image, nxImage, nyImage,
epsAmp, epsC);
} else {
recoverFreemanPyramid_Float(fp, image, nxImage, nyImage,
epsAmp, epsC, fpcc );
}
/* return the results to matlab in the order
they appear in cntrl->storeMap */
iOut = 0;
/* return the gradient maps if necessary */
if( cntrl->storeMap[isGrad] ) {
if( iOut >= nlhs )
mexErrMsgTxt( "Too few output arguments" );
dims[0] = cntrl->nScale;
dims[1] = cntrl->nOrient;
dims[2] = 2;
plhs[iOut] = mxCreateCellArray( 3, dims );
for( iScale = 0; iScale < cntrl->nScale; iScale++ ) {
for( iOrient = 0; iOrient < cntrl->nOrient; iOrient++ ) {
iChannel = 2*(cntrl->nOrient*iScale + iOrient);
dims[0] = iScale;
dims[1] = iOrient;
float2mex( fp->gradMap[iChannel], &mxTmp,
fp->nxFilt[iScale],
fp->nyFilt[iScale] );
dims[2] = 0;
iCell = mxCalcSingleSubscript( plhs[iOut], 3, dims );
mxSetCell( plhs[iOut], iCell, mxTmp );
float2mex( fp->gradMap[iChannel+1], &mxTmp,
fp->nxFilt[iScale],
fp->nyFilt[iScale] );
dims[2] = 1;
iCell = mxCalcSingleSubscript( plhs[0], 3, dims );
mxSetCell( plhs[iOut], iCell, mxTmp );
}
}
iOut++;
}
/* return the phase maps if necessary */
if( cntrl->storeMap[isPhase] ) {
if( iOut >= nlhs )
mexErrMsgTxt( "Too few output arguments" );
dims[0] = cntrl->nScale;
dims[1] = cntrl->nOrient;
plhs[iOut] = mxCreateCellArray( 2, dims );
for( iScale = 0; iScale < cntrl->nScale; iScale++ ) {
for( iOrient = 0; iOrient < cntrl->nOrient; iOrient++ ) {
iChannel = cntrl->nOrient*iScale + iOrient;
dims[0] = iScale;
dims[1] = iOrient;
float2mex( fp->phaseMap[iChannel], &mxTmp,
fp->nxFilt[iScale],
fp->nyFilt[iScale] );
iCell = mxCalcSingleSubscript( plhs[iOut], 2, dims );
mxSetCell( plhs[iOut], iCell, mxTmp );
}
}
iOut++;
}
/* return the amplitude maps if necessary */
if( cntrl->storeMap[isAmp] ) {
if( iOut >= nlhs )
mexErrMsgTxt( "Too few output arguments" );
dims[0] = cntrl->nScale;
dims[1] = cntrl->nOrient;
plhs[iOut] = mxCreateCellArray( 2, dims );
for( iScale = 0; iScale < cntrl->nScale; iScale++ ) {
for( iOrient = 0; iOrient < cntrl->nOrient; iOrient++ ) {
iChannel = cntrl->nOrient*iScale + iOrient;
dims[0] = iScale;
dims[1] = iOrient;
float2mex( fp->ampMap[iChannel], &mxTmp,
fp->nxFilt[iScale],
fp->nyFilt[iScale] );
iCell = mxCalcSingleSubscript( plhs[iOut], 2, dims );
mxSetCell( plhs[iOut], iCell, mxTmp );
}
}
iOut++;
}
/* return the filter maps if necessary */
if( cntrl->storeMap[isFilter] ) {
if( iOut >= nlhs )
mexErrMsgTxt( "Too few output arguments" );
dims[0] = cntrl->nScale;
dims[1] = cntrl->nOrient;
dims[2] = 2;
plhs[iOut] = mxCreateCellArray( 3, dims );
for( iScale = 0; iScale < cntrl->nScale; iScale++ ) {
for( iOrient = 0; iOrient < cntrl->nOrient; iOrient++ ) {
iChannel = 2*(cntrl->nOrient*iScale + iOrient);
dims[0] = iScale;
dims[1] = iOrient;
float2mex( fp->filterMap[iChannel], &mxTmp,
fp->nxFilt[iScale],
fp->nyFilt[iScale] );
dims[2] = 0;
iCell = mxCalcSingleSubscript( plhs[iOut], 3, dims );
mxSetCell( plhs[iOut], iCell, mxTmp );
float2mex( fp->filterMap[iChannel+1], &mxTmp,
fp->nxFilt[iScale],
fp->nyFilt[iScale] );
dims[2] = 1;
iCell = mxCalcSingleSubscript( plhs[0], 3, dims );
mxSetCell( plhs[iOut], iCell, mxTmp );
}
}
iOut++;
}
/* return the basis maps if necessary */
if( cntrl->storeMap[isBasis] ) {
if( iOut >= nlhs )
mexErrMsgTxt( "Too few output arguments" );
dims[0] = cntrl->nScale;
dims[1] = NUMBASIS;
plhs[iOut] = mxCreateCellArray( 2, dims );
for( iScale = 0; iScale < cntrl->nScale; iScale++ ) {
for( iBasis = 0; iBasis < NUMBASIS; iBasis++ ) {
iChannel = NUMBASIS*iScale + iBasis;
dims[0] = iScale;
dims[1] = iBasis;
float2mex( fp->basisMap[iChannel], &mxTmp,
fp->nxFilt[iScale],
fp->nyFilt[iScale] );
iCell = mxCalcSingleSubscript( plhs[iOut], 2, dims );
mxSetCell( plhs[iOut], iCell, mxTmp );
}
}
iOut++;
}
/* return the pyramid maps if necessary */
if( cntrl->storeMap[isPyr] ) {
if( iOut >= nlhs )
mexErrMsgTxt( "Too few output arguments" );
dims[0] = cntrl->nPyr;
plhs[iOut] = mxCreateCellArray( 1, dims );
for( iScale = 0; iScale < cntrl->nPyr; iScale++ ) {
dims[0] = iScale;
float2mex( fp->pyrMap[iScale], &mxTmp,
fp->nxPyr[iScale],
fp->nyPyr[iScale] );
iCell = mxCalcSingleSubscript( plhs[iOut], 1, dims );
mxSetCell( plhs[iOut], iCell, mxTmp );
}
iOut++;
}
/* free the pyramid, etc. */
freeFreemanPyramid( fp );
utilFree( (void **)&fp );
utilFree( (void **)&cntrl );
utilFree( (void **)&fpcc );
return;
}
// To run SAXParser in MATLAB:
// [v, ds, ws, es, ews, des, ss, ess, ns, ens] = SAXParser('')
void MATProcedure(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]) {
// pass the inverted index back to matlab
// here we have to individually pass all the data from the maps into mxArrays
// create the vocab array
//int vocab_size = InvertedIndex::instance()->getVocabSize();
map<string, int>* vocab = &(InvertedIndex::instance()->vocab);
int vocab_size = InvertedIndex::instance()->vocab.size();
//int i = 0;
mwSize vsize = static_cast<mwSize>(vocab_size);
plhs[0] = mxCreateCellArray(1, &vsize);
for(map<string, int>::iterator it = vocab->begin(); it != vocab->end(); it++) {
mxSetCell(plhs[0], (*it).second - 1, mxCreateString((*it).first.c_str()));
}
// create WS and DS arrays
// WS, DS is size 1 x n where n is the number of word tokens.
// The word stream WS contains word indices in order of occurence with WS=0
// representing the end-of-sentence-end marker.
// The document indices DS contains all document indices and max(DS) = D = number of documents
// they should be double-precision vectors
int numwords = InvertedIndex::instance()->docs.size();
plhs[1] = mxCreateDoubleMatrix(numwords, 1, mxREAL);
plhs[2] = mxCreateDoubleMatrix(numwords, 1, mxREAL);
double *Z = mxGetPr(plhs[1]);
double *D = mxGetPr(plhs[2]);
for(multimap<int, int>::const_iterator it = InvertedIndex::instance()->docs.begin();
it != InvertedIndex::instance()->docs.end();
it++) {
//cout << (*it).first << " " << (*it).second << "\n";
*(Z++) = (*it).first;
*(D++) = (*it).second;
}
// Create sparse arrays for entities
int rows, cols, nzmax;
rows = InvertedIndex::instance()->entities.highest(); // highest index in entities;
cols = vocab_size;
nzmax = InvertedIndex::instance()->entities.size();
multimap<int, int> rev = InvertedIndex::instance()->entities.reverseMap();
plhs[3] = createMATSparseMatrix(rows, cols, nzmax, rev.begin(), rev.end());
// create an array for entity_weights
mwSize dims[2];
dims[1] = 1;
dims[0] = InvertedIndex::instance()->entities.highest();
plhs[4] = mxCreateNumericArray(2, dims, mxINT32_CLASS, mxREAL);
int *W = (int*)mxGetData(plhs[4]);
for(multimap<int,int>::const_iterator it = InvertedIndex::instance()->entity_weights.begin();
it != InvertedIndex::instance()->entity_weights.end();
it++) {
W[(*it).first-1] = (*it).second;
//cout << (*it).second << " " << W[(*it).first] << "\n";
}
// create a sparse matrix for doc_entities
rows = InvertedIndex::instance()->docs.highest();
cols = InvertedIndex::instance()->entities.highest();
nzmax = InvertedIndex::instance()->doc_entities.size();
rev = InvertedIndex::instance()->doc_entities.reverseMap();
plhs[5] = createMATSparseMatrix(rows, cols, nzmax, rev.begin(), rev.end());
// crate a sparse matrix for syns
rows = InvertedIndex::instance()->syns.highest();
cols = vocab_size;
nzmax = InvertedIndex::instance()->syns.size();
rev = InvertedIndex::instance()->syns.reverseMap();
plhs[6] = createMATSparseMatrix(rows, cols, nzmax, rev.begin(), rev.end());
// create a sparse matrix for entity_syn
rows = InvertedIndex::instance()->entities.highest();
cols = InvertedIndex::instance()->syns.highest();
nzmax = InvertedIndex::instance()->entity_syns.size();
rev = InvertedIndex::instance()->entity_syns.reverseMap();
plhs[7] = createMATSparseMatrix(rows, cols, nzmax, rev.begin(), rev.end());
// crate a sparse matrix for names
rows = InvertedIndex::instance()->names.highest();
cols = vocab_size;
nzmax = InvertedIndex::instance()->names.size();
rev = InvertedIndex::instance()->names.reverseMap();
plhs[8] = createMATSparseMatrix(rows, cols, nzmax, rev.begin(), rev.end());
// create a sparse matrix for entity_names
rows = InvertedIndex::instance()->entities.highest();
cols = InvertedIndex::instance()->names.highest();
nzmax = InvertedIndex::instance()->entity_names.size();
rev = InvertedIndex::instance()->entity_names.reverseMap();
plhs[9] = createMATSparseMatrix(rows, cols, nzmax, rev.begin(), rev.end());
}
void mexFunction( int nlhs, mxArray *plhs[3], int nrhs, const mxArray *prhs[4] ) {
//int i, j;
int Ver_r, Ver_c;
int Tri_r, Tri_c;
int NoV, NoF;
double *temp1, *temp2;
double *Ver, *Tri;
//double *output;
int MNL; // MAX NEIGHBOR LAVEL
int ShowRunTime; // flag controls showing or not
const char **fnames; /* pointers to field names */
mxArray *Mk1, *Mk2, *Mt1, *Mt2, *MVc; // matrix
mxArray *Ridges, *Valleys; // cell
double *Pk1, *Pk2, *Pt1, *Pt2, *PVc;
mwSize ndim = 2;
mwSize *dims;
long int Tic;
double Toc;
Tic = Current_Clock();
dims = new mwSize[ndim];
dims[0] = 3;
dims[1] = 1;
Polyhedron *mymesh = new Polyhedron();
/*
if (nrhs != nlhs)
mexErrMsgTxt("The number of input and output arguments must be the same.");
*/
if ( nrhs != 4 ) {
delete mymesh;
mexErrMsgTxt( "The number of inputs must be 4: Ver & Tri arrays, MNL & ShowRunTime integers." );
}
/* Find the dimensions of the data */
Ver_r = (int)mxGetM( prhs[0] ); /* # of rows in Ver */
Ver_c = (int)mxGetN( prhs[0] ); /* # of columns in Ver */
if ( Ver_c != 3 )
mexErrMsgTxt("Ver should be Nx3!");
NoV = Ver_r;
Tri_r = (int)mxGetM( prhs[1] ); /* # of rows in Tri */
Tri_c = (int)mxGetN( prhs[1] ); /* # of columns in Tri */
if ( Tri_c != 3 )
mexErrMsgTxt("Tri should be Nx3!");
NoF = Tri_r;
Ver = mxGetPr( prhs[0] );
Tri = mxGetPr( prhs[1] );
temp1 = mxGetPr( prhs[2] );
temp2 = mxGetPr( prhs[3] );
MNL = (int)*temp1; // MAX NEIGHBOR LAVEL
ShowRunTime = (int)*temp2;
if ( ShowRunTime == 1 ) {
mexPrintf("|----------------------------------------------|\n" );
mexPrintf("|Function RVLines begins! Please wait...\n" );
}
//mexPrintf( "MNL: %d\n", MNL );
//mexPrintf( "ShowRunTime: %d\n", ShowRunTime );
//mexPrintf( "Ver(1,1:3): %lf %lf %lf\n", Ver[0], Ver[NoV], Ver[2*NoV] );
//mexPrintf( "Tri(1,1:3): %d %d %d\n", (int)Tri[0], (int)Tri[NoF], (int)Tri[2*NoF] );
/* Create 5 mxArrays for the output data */
Mk1 = mxCreateDoubleMatrix( NoV, 1, mxREAL );
Mk2 = mxCreateDoubleMatrix( NoV, 1, mxREAL );
Mt1 = mxCreateDoubleMatrix( NoV, 3, mxREAL );
Mt2 = mxCreateDoubleMatrix( NoV, 3, mxREAL );
MVc = mxCreateDoubleMatrix( NoV, 3, mxREAL );
/* Create 5 pointers to the output data */
Pk1 = mxGetPr( Mk1 );
Pk2 = mxGetPr( Mk2 );
Pt1 = mxGetPr( Mt1 );
Pt2 = mxGetPr( Mt2 );
PVc = mxGetPr( MVc );
Ridges = mxCreateCellArray( ndim, dims );
Valleys = mxCreateCellArray( ndim, dims );
// ======================================================================================
mymesh -> loadMesh( Ver, Tri, NoV, NoF, MNL, ShowRunTime );
mymesh -> setProperty( Ridges, Valleys, ShowRunTime );
mymesh -> putRmesh( Pk1, Pk2, Pt1, Pt2, PVc );
// ======================================================================================
//mexPrintf("%lf %lf %lf %lf %lf\n",output[0],output[NoV],output[2*NoV],output[3*NoV],output[4*NoV]);
/* set field names */
fnames = new const char*[7];
fnames[0] = "PrinCur_k1"; // principal curvature k1 at each vector
fnames[1] = "PrinCur_k2"; // principal curvature k2 at each vector
fnames[2] = "PrinDir_t1"; // principal direction t1 at each vector
fnames[3] = "PrinDir_t2"; // principal direction t2 at each vector
fnames[4] = "VectorNorm"; // vector normal at each vector
fnames[5] = "Valleys";
fnames[6] = "Ridges";
/* create a 1x1 struct matrix for output */
plhs[0] = mxCreateStructMatrix( 1, 1, 7, fnames );
/* set each field in output structure */
mxSetFieldByNumber( plhs[0], 0, 0, Mk1 );
mxSetFieldByNumber( plhs[0], 0, 1, Mk2 );
mxSetFieldByNumber( plhs[0], 0, 2, Mt1 );
mxSetFieldByNumber( plhs[0], 0, 3, Mt2 );
mxSetFieldByNumber( plhs[0], 0, 4, MVc );
mxSetFieldByNumber( plhs[0], 0, 5, Valleys );
mxSetFieldByNumber( plhs[0], 0, 6, Ridges );
Toc = (double)(Current_Clock() - Tic) / MY_TIMER_CONST;
if ( ShowRunTime == 1 ) {
mexPrintf("| Total runtime: %.4f second.\n", Toc );
mexPrintf("|Function RVLines done! Congratulations!\n" );
mexPrintf("|----------------------------------------------|\n" );
}
delete [] dims;
delete [] fnames;
delete mymesh;
return;
}
void mexFunction (
int nlhs, mxArray *plhs[],
int nrhs, const mxArray *prhs[]
)
{
int col, row, r_find , movie_length, traces_col, min_length, trace_length, r_integrate, fnumber_size, trace_number;
double *movie, h_min, *p2Matrix, *cur_trace, *fnumber, *pos;
std::vector< std::vector< std::vector<int> > > itraces;
std::vector< std::vector< int > > positions;
std::vector<int> framenumber;
/* Check for proper number of arguments */
if (nrhs != 5 ) {
mexErrMsgTxt ("append_traces_to_positions requires: movie, itraces, framenumbers, positions, r_integrate");
} else if (nlhs != 1 ) {
mexErrMsgTxt ("append_traces_to_positions returns list of itraces ");
}
/* Get arguments from MATLAB */
//
movie = mxGetPr(prhs[0]); //columns * rows * frame
movie_length = (int) mxGetDimensions(prhs[0])[2]; //length of video = number of frames
col = (int) mxGetDimensions(prhs[0])[0]; //number of columns
row = (int) mxGetDimensions(prhs[0])[1]; //number of rows
r_integrate = (int)(mxGetScalar(prhs[4]));
//fill framenumbers
fnumber = mxGetPr(prhs[2]); //columns * rows * frame
for(int i=0; i< PeakProcessing::max(mxGetM(prhs[2]), mxGetN(prhs[2]) ); i++){
framenumber.push_back( (int)fnumber[i] );
}
trace_number = mxGetM(prhs[1]);
//fill itraces
for(int i=0; i<trace_number; i++){
cur_trace = mxGetPr(mxGetCell(prhs[1],i));
trace_length = mxGetM(mxGetCell(prhs[1],i));
std::vector< std::vector<int> > tmptrace;
//loop through ith trace
for(int j=0; j<trace_length; j++){
std::vector<int> addthis;
addthis.push_back(cur_trace[trace_length*0+j]); //framenumber
addthis.push_back(cur_trace[trace_length*1+j]); //x
addthis.push_back(cur_trace[trace_length*2+j]); //y
addthis.push_back(cur_trace[trace_length*3+j]); //intensity
tmptrace.push_back(addthis);
}
itraces.push_back(tmptrace);
}
//load positions
pos= mxGetPr(prhs[3]); //columns * rows * frame
int pos_col = mxGetM(prhs[3]);
for(int i=0; i< pos_col; i++){
std::vector< int > tmp;
for (int j=0; j < mxGetN(prhs[3]) ; j++) {
tmp.push_back( (int)pos[j*pos_col + i]);
}
positions.push_back(tmp);
}
PeakProcessing::append_traces_to_position(movie, movie_length, col, row, &itraces, &positions, r_integrate, &framenumber);
//delete short traces
/*int trace_number=0;
for(int i=0; i< traces.size(); i++){
if(traces[i].size() < min_length){
traces.erase(traces.begin()+i);
i--;
}
} */
mexPrintf("movie_length = %i \tImage = %i , %i\tr_integrate = %i\t%i itraces\n", movie_length, col, row, r_integrate, itraces.size());
//Allocate storage place for the result
int dims[1]; dims[0]=(int) itraces.size(); //number of traces
plhs[0] = mxCreateCellArray(1, dims );
for(int i=0; i< itraces.size(); i++){
traces_col = itraces[i].size();
mxArray * tmp = mxCreateDoubleMatrix((int)itraces[i].size(),4, mxREAL); //make mxArray to store the ith trace
p2Matrix = mxGetPr(tmp);
//fill this array
for(int j=0; j<itraces[i].size(); j++){
p2Matrix[traces_col*0+j] = itraces[i][j][0]; //framenumber
p2Matrix[traces_col*1+j] = itraces[i][j][1]; //x
p2Matrix[traces_col*2+j] = itraces[i][j][2]; //y
p2Matrix[traces_col*3+j] = itraces[i][j][3]; //intensity
//mexPrintf("%i\t %i \t %i\n", traces[i][j][0], traces[i][j][1], traces[i][j][2]);
}
mxSetCell(plhs[0], i,tmp);
}
}
void
mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
{
MatlabCPPInitialize(false);
// Check for proper number of arguments
if ( (nrhs < NR_IN) || (nrhs > NR_IN + NR_IN_OPT) || \
(nlhs < NR_OUT) || (nlhs > NR_OUT + NR_OUT_OPT) ) {
mexErrMsgTxt("Wrong number of arguments.");
}
// fetch the input (we support matrices and cell arrays)
std::vector< Eigen::VectorXd > theta_unary;
std::vector< Eigen::VectorXd > theta_pair;
if (!mxIsCell(THETA_UNARY_IN)) {
GetMatlabPotentialFromMatrix(THETA_UNARY_IN, theta_unary);
GetMatlabPotentialFromMatrix(THETA_PAIR_IN, theta_pair);
}
else {
GetMatlabPotential(THETA_UNARY_IN, theta_unary);
GetMatlabPotential(THETA_PAIR_IN, theta_pair);
}
Eigen::MatrixXi edges;
GetMatlabMatrix(EDGES_IN, edges);
double beta = mxGetScalar(BETA_IN);
TreeInference inf = TreeInference(theta_unary, theta_pair, edges);
inf.run(beta);
LOGZ_OUT = mxCreateNumericMatrix(1, 1, mxDOUBLE_CLASS, mxREAL);
double* logz_p = mxGetPr(LOGZ_OUT);
logz_p[0] = inf.getLogPartitionSum();
// return marginals (if input was provided as a matrix, also return
// marginals in a matrix)
if (nlhs > 1) {
std::vector< Eigen::VectorXd >& mu_unary = inf.getUnaryMarginals();
if (!mxIsCell(THETA_UNARY_IN)) {
size_t num_states = mxGetM(THETA_UNARY_IN);
size_t num_vars = mxGetN(THETA_UNARY_IN);
MARGINALS_UNARY_OUT = mxCreateNumericMatrix(
num_states,
num_vars,
mxDOUBLE_CLASS, mxREAL);
double* mu_res_p = mxGetPr(MARGINALS_UNARY_OUT);
for (size_t v_idx=0; v_idx<mu_unary.size(); v_idx++) {
for (size_t idx=0; idx<mu_unary[v_idx].size(); idx++) {
mu_res_p[v_idx*num_states+idx] = mu_unary[v_idx](idx);
}
}
}
else {
mwSize dim_0 = static_cast<mwSize>(mu_unary.size());
MARGINALS_UNARY_OUT = mxCreateCellArray(1, &dim_0);
for (size_t v_idx=0; v_idx<mu_unary.size(); v_idx++) {
mxArray* m = mxCreateNumericMatrix(
static_cast<int>(mu_unary[v_idx].size()),
1,
mxDOUBLE_CLASS, mxREAL);
double* mu_res_p = mxGetPr(m);
for (size_t idx=0; idx<mu_unary[v_idx].size(); idx++) {
mu_res_p[idx] = mu_unary[v_idx](idx);
}
mxSetCell(MARGINALS_UNARY_OUT, v_idx, m);
}
}
}
// return pairwise marginals
if (nlhs > 2) {
std::vector< Eigen::VectorXd >& mu_pair = inf.getPairwiseMarginals();
if (!mxIsCell(THETA_UNARY_IN)) {
size_t num_states = mxGetM(THETA_PAIR_IN);
size_t num_edges = mxGetN(THETA_PAIR_IN);
MARGINALS_PAIR_OUT = mxCreateNumericMatrix(
num_states,
num_edges,
mxDOUBLE_CLASS, mxREAL);
double* mu_res_p = mxGetPr(MARGINALS_PAIR_OUT);
for (size_t e_idx=0; e_idx<mu_pair.size(); e_idx++) {
for (size_t idx=0; idx<mu_pair[e_idx].size(); idx++) {
mu_res_p[e_idx*num_states+idx] = mu_pair[e_idx](idx);
}
}
}
else {
mxAssert(0, "not implemented yet!");
// TODO: not implemented yet, see above!
// see unary case above!
}
}
MatlabCPPExit();
}
void mexFunction( int nlhs, mxArray *plhs[],
int nrhs, const mxArray *prhs[] )
{
static char buff[80+1];
buff[0]=0;
mxGetString(prhs[0],buff,80);
if (strcmp(buff,"Init") == 0) {
//DWORD threadId;
if (threadId > 0) {
mexPrintf("Init called twice before a Release was called. Trying to release first......\n");
PostThreadMessage(threadId, WM_QUIT, 0, 0);
Sleep(1000);
}
CloseHandle(CreateThread(NULL, 0, Thread, NULL, 0, &threadId));
//const unsigned int dim_array[2] = {1,1};
//plhs[0] = mxCreateNumericArray(2,(const mwSize*)dim_array, mxDOUBLE_CLASS, mxREAL);
for (int k=0;k< iMAXMICE;k++)
HookMiceArray[k] = false;
//double *buffer = (double*)mxGetPr(plhs[0]);
//*buffer = threadId;
return;
}
if (strcmp(buff,"Hook") == 0) {
int iNumHooks = mxGetNumberOfElements(prhs[1]);
double *aiHookArray = mxGetPr(prhs[1]);
for (int k=0; k < iNumHooks ;k++)
HookMiceArray[int(aiHookArray[k])] = true;
}
if (strcmp(buff,"Unhook") == 0) {
int iNumHooks = mxGetNumberOfElements(prhs[1]);
double *aiHookArray = mxGetPr(prhs[1]);
for (int k=0; k < iNumHooks ;k++)
HookMiceArray[int(aiHookArray[k])] = false;
}
if (strcmp(buff,"Release") == 0) {
//DWORD threadId = (DWORD) (*(double*)mxGetPr(prhs[1]));
if (threadId > 0) {
PostThreadMessage(threadId, WM_QUIT, 0, 0);
threadId = 0;
} else {
mexPrintf("Nothing To Release. Init was not called...\n");
}
return;
}
if (strcmp(buff,"GetMiceName") == 0) {
int NumMice = raw_mouse_count();
mwSize Dim[2] = {1,NumMice };
mxArray *mxNames = mxCreateCellArray(2,Dim);
for (int k=0;k<NumMice;k++) {
char name[5000];
int nSize = get_raw_mouse_name(k, name);
mxSetCell(mxNames,k, mxCreateString(name));
}
plhs[0] = mxNames;
}
if (strcmp(buff,"GetNumMice") == 0) {
const unsigned int dim_array[2] = {1,1};
plhs[0] = mxCreateNumericArray(2,(const mwSize*)dim_array, mxDOUBLE_CLASS, mxREAL);
double *buffer = (double*)mxGetPr(plhs[0]);
if (threadId == 0) {
mexPrintf("Call The Init Function First!\n");
*buffer = -1;
} else {
*buffer = raw_mouse_count();
}
return;
}
if (strcmp(buff,"GetMouseData") == 0) {
int MouseIndex= int(*(double*)mxGetPr(prhs[1]));
const unsigned int dim_array[2] = {1,3};
plhs[0] = mxCreateNumericArray(2,(const mwSize*)dim_array, mxDOUBLE_CLASS, mxREAL);
double *buffer = (double*)mxGetPr(plhs[0]);
if (threadId == 0) {
mexPrintf("Call The Init Function First!\n");
buffer[0] = 0;
buffer[1] = 0;
buffer[2] = 0;
} else {
buffer[0] = get_raw_mouse_x_delta(MouseIndex);
buffer[1] = get_raw_mouse_y_delta(MouseIndex);
buffer[2] = get_raw_mouse_z_delta(MouseIndex);
}
return;
}
if (strcmp(buff,"GetButtons") == 0) {
double *MouseIndex= (double*)mxGetPr(prhs[1]);
const unsigned int dim_array[2] = {1,3};
plhs[0] = mxCreateNumericArray(2,(const mwSize*)dim_array, mxDOUBLE_CLASS, mxREAL);
double *buffer = (double*)mxGetPr(plhs[0]);
if (threadId == 0) {
mexPrintf("Call The Init Function First!\n");
for (int k=0;k<3;k++) {
buffer[k] = 0;
}
} else {
for (int k=0;k<3;k++) {
buffer[k] = is_raw_mouse_button_pressed(int(MouseIndex[k]),k);
}
}
return;
}
if (strcmp(buff,"GetWheels") == 0) {
double *MouseIndex= (double*)mxGetPr(prhs[1]);
if (MouseIndex == NULL) {
mwSize dim[2] = {0,0};
plhs[0] = mxCreateNumericArray(2,(const mwSize*)dim, mxDOUBLE_CLASS, mxREAL);
return;
}
int NumEl = mxGetNumberOfElements(prhs[1]);
mwSize dim[2] = {1,NumEl};
plhs[0] = mxCreateNumericArray(2,(const mwSize*)dim, mxDOUBLE_CLASS, mxREAL);
double *buffer = (double*)mxGetPr(plhs[0]);
if (threadId == 0) {
mexPrintf("Call The Init Function First!\n");
for (int k=0;k<MAX(dim[0],dim[1]);k++) {
buffer[k] = 0;
}
} else {
for (int k=0;k<NumEl;k++) {
ULONG Z = get_raw_mouse_z_delta(int(MouseIndex[k]));
buffer[k] = double((Z > 4294967295/2) ? double(Z)-4294967295 : Z);
}
}
return;
}
}
int bipartition(int *set, int setSize, int nMax, int option, mxArray *B1, mxArray *B2) {
if(option == 1)
nMax = floor(nMax/2);
nB = 0;
for(int i = 0; i <= nMax; i++)
nB = nB + nchoosek(setSize,i);
int cellDims[2] = (nB,1);
B1 = mxCreateCellArray (2,cellDims);
B2 = mxCreateCellArray (2,cellDims);
int iB = 0;
//open up MATLAB engine to use the nchoosek function
// TODO: !!! CAN WE USE mexCallMATLAB INSTEAD!?
Engine *ep;
if (!(ep = engOpen("\0"))) {
fprintf(stderr, "\nCan't start MATLAB engine\n");
//return EXIT_FAILURE;
}
for (int i = 0; i <= nMax; i++) {
B1cellPtr = mxGetCell(B1, iB);
B2cellPtr = mxGetCell(B2, iB);
if(i == 0) {
B1cellPtr = NULL;
B2cellPtr = set; //do we need memcpy here?
iB++;
} else {
mxArray *iMATLAB = mxCreateDoubleMatrix(1, 1, mxREAL);
memcpy((void *)mxGetPr(iMATLAB), (void *)i, sizeof(i));
mxArray *setSizeMATLAB = mxCreateDoubleMatrix(1, 1, mxREAL);
memcpy((void *)mxGetPr(setSizeMATLAB), (void *)setSize, sizeof(setSize));
engPutVariable(ep, "i", iMATLAB);
engPutVariable(ep, "setSize", setSizeMATLAB);
engEvalString(ep, "C_b = nchoosek(1:setSize,i);");
int *chooseSets;
mxArray *C_bmxArray = engGetVariable(ep,"C_b");
double *temp = mxGetPr(C_bmxArray);
int chooseSetsSizeCols = mxGetN(C_bmxArray);
int chooseSetsSizeRows = mxGetM(C_bmxArray);
int chooseSetSize = chooseSetsSizeCols*chooseSetsSizeRows;
int chooseSets[chooseSetSize];
for(int j = 0; j < chooseSetSize; j++)
chooseSets[j] = temp[j];
for(int j = 0; j < C_bSizeRows; j++) {
int chooseSetIndex = 0;
int B1index = 0;
int B2index = 0;
for(int k =0; k < setSize; k++) {
if((j + chooseSetIndex*C_bSizeRows) < chooseSetSize)
&& chooseSet[j + chooseSetIndex*C_bSizeRows] == set[k]) {
B1cellPtr[B1index] = set[k];
chooseSetIndex++;
B1index++;
} else {
B2cellPtr[B2index] = set[k];
B2index++;
}
iB++;
}
}
mxDestroyArray(iMATLAB);
mxDestroyArray(setSize);
}
}
void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
{
// variables for GBP
int*** assignInd = 0;
double* bethe = 0;
GBPPreProcessor* processor = 0;
MRF* reg_mrf = 0;
int* extractSingle = 0;
int** extractPairs = 0;
RegionLevel* regions = 0;
vector<RegionLevel>* in_allRegions = 0;
// variables for user
Region** all_regions = 0;
// **************************************************************************
// reading input arguments
// **************************************************************************
if (nrhs != 8) {
mexErrMsgTxt("Incorrect number of input arguments.");
}
// check number of output arguments
if ((nlhs > 0) && (nlhs != 7)) {
mexErrMsgTxt("Incorrect number of output arguments.");
}
// get number of nodes and adjMat
vector<Nodes>* adjMat = new vector<Nodes>();
fillAdjMat(prhs[0],*adjMat);
int num_nodes = adjMat->size();
// define the MRF
MRF* mrf = new MRF(*adjMat);
// get local potentials
fillLocalMat(prhs[2],mrf);
// get pairwise potentials
fillPsiMat(prhs[1],mrf);
// get regions
bool allLevels = (int)(mxGetScalar(prhs[4])) > 0;
if (allLevels) {
in_allRegions = new vector<RegionLevel>();
in_allRegions->clear();
fillRegionLevels(prhs[3],*in_allRegions);
}
else {
regions = new RegionLevel();
fillRegions(prhs[3],*regions);
}
bool trw = (int)(mxGetScalar(prhs[5])) > 0;
double* countingNode = 0;
if (trw) {
countingNode = new double[num_nodes];
fillDouble(prhs[6], countingNode, num_nodes);
}
bool full = (int)(mxGetScalar(prhs[7])) > 0;
// **************************************************************************
// making pre-process
// **************************************************************************
if (allLevels) {
processor = new GBPPreProcessor(in_allRegions, mrf, trw, full, countingNode);
}
else {
processor = new GBPPreProcessor(*regions, mrf, trw, full, countingNode);
regions->clear();
delete regions;
regions = 0;
}
all_regions = processor->getAllRegions();
reg_mrf = processor->getRegionMRF();
assignInd = processor->getAssignTable();
bethe = processor->getBethe();
extractSingle = processor->getExtractSingle();
extractPairs = processor->getExtractPairs();
// **************************************************************************
// writing output arguments
// **************************************************************************
// output: 1. regions - a cell array with vectors of the nodes in each region
// 2. regions' adj-matrix
// 3. regions' local potentials
// 4. assignment-indices (for each parent's assignment, the index of
// the relevant son's assignment)
// 5. bethe
// 6. extractSingle (for each node, from which region its belief
// should be extracted)
// 7. extractPairs (for each pair of neighbours, from which region
// its pairwise beliefs should be extracted)
if (nlhs > 0) {
int num_regs = reg_mrf->N;
int regs_dims[2] = {1,num_regs};
// assign: 1. regions 2. regions' adj-matrix & 3. regions' local potentials
plhs[0] = mxCreateCellArray(2,regs_dims);
plhs[1] = mxCreateCellArray(2,regs_dims);
plhs[2] = mxCreateCellArray(2,regs_dims);
for (int i=0; i<num_regs; i++) {
// regions
Region* region = all_regions[i];
int reg_i_size = (int)(region->size());
int reg_i_dims[2] = {1,reg_i_size};
mxArray* reg_i = mxCreateNumericArray(2,reg_i_dims,mxDOUBLE_CLASS, mxREAL);
double* reg_i_ptr = mxGetPr(reg_i);
for (int n=0; n<reg_i_size; n++) {
reg_i_ptr[n] = (double)((*region)[n]);
}
mxSetCell(plhs[0],i,reg_i);
// adj-matrix
int adj_dims[2] = {1,reg_mrf->neighbNum(i)};
mxArray* adj_i = mxCreateNumericArray(2,adj_dims,mxDOUBLE_CLASS, mxREAL);
double* adj_i_ptr = mxGetPr(adj_i);
for (int n=0; n<reg_mrf->neighbNum(i); n++) {
adj_i_ptr[n] = (double)(reg_mrf->adjMat[i][n] + 1);
}
mxSetCell(plhs[1],i,adj_i);
// local potentials
int local_dims[2] = {reg_mrf->V[i],1};
mxArray* local_i = mxCreateNumericArray(2, local_dims, mxDOUBLE_CLASS, mxREAL);
double* local_i_ptr = mxGetPr(local_i);
for (int xi=0; xi<reg_mrf->V[i]; xi++) {
local_i_ptr[xi] = reg_mrf->localMat[i][xi];
}
mxSetCell(plhs[2],i,local_i);
}
// assign: 4. assignment indices
int ass_dims[2] = {1,num_regs};
plhs[3] = mxCreateCellArray(2,ass_dims);
for (int i=0; i<num_regs; i++) {
int ass_i_dims[2] = {1,reg_mrf->neighbNum(i)};
mxArray* ass_i = mxCreateCellArray(2,ass_i_dims);
for (int n=0; n<reg_mrf->neighbNum(i); n++) {
int j = reg_mrf->adjMat[i][n];
if (i<j) {
int ass_val_dims[2] = {1,reg_mrf->V[i]};
mxArray* ass_ij = mxCreateNumericArray(2, ass_val_dims, mxDOUBLE_CLASS, mxREAL);
double* ass_ij_ptr = mxGetPr(ass_ij);
for (int xi=0; xi<reg_mrf->V[i]; xi++) {
ass_ij_ptr[xi] = (double)(assignInd[i][n][xi]);
}
mxSetCell(ass_i, n, ass_ij);
}
}
mxSetCell(plhs[3], i, ass_i);
}
// assign: 5. bethe
plhs[4] = mxCreateNumericArray(2, regs_dims, mxDOUBLE_CLASS, mxREAL);
double* bethe_ptr = mxGetPr(plhs[4]);
for (int i=0; i<num_regs; i++) {
bethe_ptr[i] = bethe[i];
}
// assign: 6. extractSingle
int extract_dims[2] = {1,num_nodes};
plhs[5] = mxCreateNumericArray(2, extract_dims, mxDOUBLE_CLASS, mxREAL);
double* exSing_ptr = mxGetPr(plhs[5]);
for (int i=0; i<num_nodes; i++) {
exSing_ptr[i] = extractSingle[i];
}
// assign: 7. extractPairs
plhs[6] = mxCreateCellArray(2, extract_dims);
for (int i=0; i<num_nodes; i++) {
int exPair_i_dim[2] = {1,mrf->neighbNum(i)};
mxArray* exPair_i = mxCreateNumericArray(2, exPair_i_dim, mxDOUBLE_CLASS, mxREAL);
double* exPair_i_ptr = mxGetPr(exPair_i);
for (int n=0; n<mrf->neighbNum(i); n++) {
exPair_i_ptr[n] = extractPairs[i][n];
}
mxSetCell(plhs[6], i, exPair_i);
}
}
// **************************************************************************
// free memory
// **************************************************************************
delete mrf;
mrf = 0;
delete adjMat;
adjMat = 0;
if (countingNode != 0) {
delete[] countingNode;
countingNode = 0;
}
}
