void getMxArray(const std::string& variableName)
{
if(m_hierarchicalNameVector.size() == 1)
m_pMxArray = matGetVariable(m_pMatlabFile,m_matlabVariableName.c_str());
else {
m_vecOfpMxArray.clear();
//push back pMxArray pointer returned by getVariable call on to vector
m_vecOfpMxArray.push_back(matGetVariable(m_pMatlabFile, m_matlabVariableName.c_str()));
//iterator points to beginning of vector<mxArray*>
std::vector<mxArray*>::iterator mxArrayIter = m_vecOfpMxArray.begin();
//main vector creation loop
for(std::vector<std::string>::iterator iter = m_hierarchicalNameVector.begin() + 1;
iter != m_hierarchicalNameVector.end();
++iter, ++mxArrayIter)
{
// temp pointer to mxArray
mxArray* pMxArrayTemp;
const char* tempChar = (*iter).c_str();
pMxArrayTemp=mxGetField(*mxArrayIter,0,tempChar);
if( pMxArrayTemp )
{
m_vecOfpMxArray.push_back(pMxArrayTemp);
}
}
m_pMxArray = *(m_vecOfpMxArray.end()-1);
}
if(!m_pMxArray)
throw std::runtime_error("Can't find: " + variableName + ", in: " + m_fileName);
}
int test()
{
string str_file_name = "E:\\research\\working\\test_data\\test_gk_means.mat";
SMatrix<double> matZ;
Vector<SMatrix<double> > vecmatDictionary;
SMatrix<CodeType> mat_target;
int num_dic;
int is_initialize;
int num_grouped;
{
MATFile* fp = matOpen(str_file_name.c_str(), "r");
SMART_ASSERT(fp).Exit();
mexConvert(matGetVariable(fp, "Z"), matZ);
mexConvert(matGetVariable(fp, "all_D"), vecmatDictionary);
mexConvert(matGetVariable(fp, "num_sub_dic_each_partition"), num_dic);
mexConvert(matGetVariable(fp, "mat_compact_B"), mat_target);
mxArray * para_encode = matGetVariable(fp, "para_encode");
mexConvert(mxGetField(para_encode, 0, "is_initialize"), is_initialize);
mexConvert(mxGetField(para_encode, 0, "num_grouped"), num_grouped);
matClose(fp);
}
IndexEncoding ie;
ie.SetIsInitialize(is_initialize);
ie.SetNumberGroup(num_grouped);
ie.SetEncodingType(Type_gk_means);
ie.SetEncodingType(Type_additive_quantization);
SMatrix<CodeType> matRepresentation;
matRepresentation.AllocateSpace(mat_target.Rows(), mat_target.Cols());
int num_selected_rows = 10;
matRepresentation.AllocateSpace(num_selected_rows, mat_target.Cols());
ie.Solve(SMatrix<double>(matZ.Ptr(), num_selected_rows, matZ.Cols()),
vecmatDictionary,
num_dic,
matRepresentation);
PRINT << "good\n";
for (int i = 0; i < matRepresentation.Rows(); i++)
{
for (int j = 0; j < matRepresentation.Cols(); j++)
{
cout << (int)(matRepresentation[i][j]) << "\t";
}
cout << "\n";
}
//for (int i = 0; i < mat_target.Rows(); i++)
//{
// for (int j = 0; j < mat_target.Cols(); j++)
// {
// SMART_ASSERT(matRepresentation[i][j] == mat_target[i][j]).Exit();
// }
//}
return 0;
}
//--------------------------------------------
void read_solution(urdme_model *model, char*file){
mxArray *tspan,*U;
MATFile *input_file;
/* Open mat-file (read-only) */
input_file = matOpen(file,"r");
if (input_file == NULL){
printf("Failed to open mat-file '%s'.\n",file);
return;
}
printf("read in '%s'\n'",file);
/* Get U-matrix. */
init_sol(model,1);
U = matGetVariable(input_file, "U");
if (U == NULL){
printf("No U solution variable in mat-file '%s'\n.",file);
return;
}
int Usz = mxGetNumberOfElements(U);
model->U[0] =(int *)malloc(Usz*sizeof(int));
double*Utmp = mxGetPr(U);
model->nsol = 1;
int i;
for(i=0;i<Usz;i++){
model->U[0][i] = (int) Utmp[i];
}
/* time span (optional) */
tspan = matGetVariable(input_file, "tspan");
if (tspan != NULL){
model->tspan = mxGetPr(tspan);
model->tlen = mxGetNumberOfElements(tspan);
}
}
void Transmission::LoadTransmission() {
MATFile *matFileHandle;
matFileHandle = matOpen(transmissionFileName, "r");
int numberOfDirectories;
const char** directoryField= (const char** )matGetDir(matFileHandle, &numberOfDirectories);
for(int i=0; i < numberOfDirectories; i++) {
//std::cout<<directoryField[i]<<std::endl;
mxArray* matArrayHandle = matGetVariable(matFileHandle, directoryField[i]);
int numberOfRows = mxGetM(matArrayHandle);
int numberOfColumns = mxGetN(matArrayHandle);
double* matArrayAddress = mxGetPr(matArrayHandle);
std::vector<double> transmissionDataEntry;
for(int j=0;j<numberOfColumns;j++) {
double matrixEntry = *(matArrayAddress+j);
transmissionDataEntry.push_back(matrixEntry);
}
transmissionData.push_back(transmissionDataEntry);
}
gearRatios = transmissionData[0];
gearEfficiency = transmissionData[1];
gearInertias = transmissionData[2];
//std::cout<<"Transmission data successfully loaded"<<std::endl;
if (matClose(matFileHandle) != 0) {
std::cout<<"Error closing file"<<'\n';
}
}
pair<double, double> ProbabilisticWvmClassifier::loadSigmoidParamsFromMatlab(const string& thresholdsFilename)
{
// Load sigmoid stuff:
double logisticA, logisticB;
MATFile *pmatfile;
mxArray *pmxarray; // =mat
pmatfile = matOpen(thresholdsFilename.c_str(), "r");
if (pmatfile == 0) {
throw invalid_argument("ProbabilisticWvmClassifier: Unable to open the thresholds-file: " + thresholdsFilename);
}
//TODO is there a case (when svm+wvm from same trainingdata) when there exists only a posterior_svm, and I should use this here?
pmxarray = matGetVariable(pmatfile, "posterior_wrvm");
if (pmxarray == 0) {
throw runtime_error("ProbabilisticWvmClassifier: Unable to find the vector posterior_wrvm. If you don't want probabilistic output, don't use a probabilistic classifier.");
//logisticA = logisticB = 0; // TODO: Does it make sense to continue?
} else {
double* matdata = mxGetPr(pmxarray);
const mwSize *dim = mxGetDimensions(pmxarray);
if (dim[1] != 2) {
throw runtime_error("ProbabilisticWvmClassifier: Size of vector posterior_wrvm !=2. If you don't want probabilistic output, don't use a probabilistic classifier.");
//logisticA = logisticB = 0; // TODO: Does it make sense to continue?
} else {
logisticB = matdata[0];
logisticA = matdata[1];
}
// Todo: delete *matdata and *dim??
mxDestroyArray(pmxarray);
}
matClose(pmatfile);
return make_pair(logisticA, logisticB);
}
bool CPPMaster::CreateBVP(int tau, std::string file_name,
std::vector<double> goal_pt){
double* x_data;
double* y_data;
double* z_data;
int row_count;
int col_count;
// I: Read the file out into a form that we can use.
MATFile *pmat;
mxArray *pa;
pmat = matOpen(file_name.c_str(), "r");
// Since we already know the fields, we can match them up ourselves.
pa = matGetVariable(pmat, "xx");
x_data = mxGetPr(pa);
pa = matGetVariable(pmat, "yy");
y_data = mxGetPr(pa);
pa = matGetVariable(pmat, "zz");
z_data = mxGetPr(pa);
pa = matGetVariable(pmat, "row_count");
row_count = int(*mxGetPr(pa));
pa = matGetVariable(pmat, "col_count");
col_count = int(*mxGetPr(pa));
double* start_point = new double[4];
start_point[0] = 1;
start_point[1] = -.5;
start_point[2] = PI/2;
start_point[3] = 0;
double* goal_point = goal_pt.data();
// I don't know how else to do this, 'cause I'm ig'nant.
params_.Clear();
for (int ii=0; ii<4;ii++) {
params_.add_start_param(start_point[ii]);
params_.add_goal_param(goal_point[ii]);
}
for (int ii=0;ii<(row_count*col_count);ii++) {
params_.add_x_data(x_data[ii]);
params_.add_y_data(y_data[ii]);
params_.add_z_data(z_data[ii]);
}
params_.set_col_count(col_count);
params_.set_row_count(row_count);
params_.set_tau(tau);
}
void Machine::LoadMachine() {
MATFile *matFileHandle;
matFileHandle = matOpen(machineFileName, "r");
int numberOfDirectories;
const char** directoryField= (const char** )matGetDir(matFileHandle, &numberOfDirectories);
for(int i=0; i < numberOfDirectories; i++) {
//std::cout<<directoryField[i]<<std::endl;
mxArray* matArrayHandle = matGetVariable(matFileHandle, directoryField[i]);
int numberOfRows = mxGetM(matArrayHandle);
int numberOfColumns = mxGetN(matArrayHandle);
double* matArrayAddress = mxGetPr(matArrayHandle);
std::vector<std::vector<double>> machineData;
for(int j=0;j<numberOfRows;j++) {
std::vector<double> singleRowInArray;
for(int k=0;k<numberOfColumns;k++) {
double matrixEntry = *((matArrayAddress+j)+(k*numberOfRows));
singleRowInArray.push_back(matrixEntry);
}
machineData.push_back(singleRowInArray);
}
switch(i) {
case 0:
machineRPMRange = machineData[0];
//std::cout<<"machineRPMRange"<<std::endl;
//PrintMachineData(machineRPMRange);
break;
case 1:
machineTorqueRange = machineData[0];
//std::cout<<"machineTorqueRange"<<std::endl;
//PrintMachineData(machineTorqueRange);
break;
case 2:
machineEfficiencyData = machineData;
//std::cout<<"machineEfficiencyData"<<std::endl;
//PrintMachineData(machineEfficiencyData);
break;
case 3:
rpmRangeForMaximumBrakeTorque = machineData[0];
//std::cout<<"rpmRangeForMaximumBrakeTorque"<<std::endl;
//PrintMachineData(rpmRangeForMaximumBrakeTorque);
break;
case 4:
machineMaximumBrakeTorque = machineData[0];
//std::cout<<"machineMaximumBrakeTorque"<<std::endl;
//PrintMachineData(machineMaximumBrakeTorque);
break;
case 5:
machineMaximumRegenerationTorque = machineData[0];
//std::cout<<"machineMaximumRegenerationTorque"<<std::endl;
//PrintMachineData(machineMaximumRegenerationTorque);
break;
}
}
//std::cout<<"Data for machine "<<machineIndex<<" successfully loaded"<<std::endl;
}
int read_mat_matrix(Matrix* M, const char* file_name, const char* varname)
{
MATFile* matf = NULL;
mxArray* Mx = NULL;
int res;
unsigned int i,j;
double* data;
assert(file_name);
assert(M);
assert(varname);
/* let's open the file*/
matf = matOpen(file_name, "r" );
if(!matf) {
return ERROR_IO;
}
/* let's read the variable varname */
Mx = matGetVariable(matf, varname);
if (!Mx) {
matClose(matf);
return ERROR_IO_MAT;
}
/* let's close the file */
res = matClose(matf);
assert(!res);
/* check: the variable must be a 2D matrix!! */
if(mxGetNumberOfDimensions(Mx) > 2) {
matClose(matf);
return ERROR_IO_MAT;
}
/* let's create the Matrix */
M->n_rows = (unsigned int) mxGetM(Mx);
M->n_cols = (unsigned int) mxGetN(Mx);
res = create_matrix(M, M->n_rows, M->n_cols);
if (res) {
mxDestroyArray(Mx);
return res;
}
/* copy the data from the mxArray. Remember that matlab save in column-order!! */
data = mxGetPr(Mx);
for (j=0; j < M->n_cols; ++j) {
for (i=0; i < M->n_rows; ++i) {
matrix_set(M, i, j, (float) *data);
++data;
}
}
/* destroy mxArray*/
mxDestroyArray(Mx);
return 0;
}
void CMATLAB::matGet(string filename,ArrayXd &X,string Xname)
{
MATFile *pmat=matOpen(filename,string("r"));
if (!pmat) throw "CMATLAB::importFrom error: matlab.matOpen failed";
mxArray *pa=matGetVariable(pmat,Xname);
if (!pa) throw "CMATLAB::importFrom error: matlab.matGetVariable failed";
int N=mxGetNumberOfElements(pa);
if (N<=0) throw "CMATLAB::importFrom error: matlab.mxGetNumberOfElements failed";
X.resize(N);
memcpy((void *)(mxGetPr(pa)), (void *)X.data(), sizeof((double *)X.data()));
mxDestroyArray(pa);
if (!matClose(pmat)) throw "CMATLAB::importFrom error: matlab.matClose failed";
}
pair<double, double> ProbabilisticSvmClassifier::loadSigmoidParamsFromMatlab(const string& logisticFilename)
{
Logger logger = Loggers->getLogger("classification");
#ifdef WITH_MATLAB_CLASSIFIER
// Load sigmoid stuff:
double logisticA = 0;
double logisticB = 0;
MATFile *pmatfile;
mxArray *pmxarray; // =mat
pmatfile = matOpen(logisticFilename.c_str(), "r");
if (pmatfile == 0) {
throw runtime_error("ProbabilisticSvmClassifier: Unable to open the logistic file to read the logistic parameters (wrong format?):" + logisticFilename);
}
else {
//read posterior_wrvm parameter for probabilistic WRVM output
//TODO 2012: is there a case (when svm+wvm from same trainingdata) when there exists only a posterior_svm, and I should use this here?
//TODO new2013: the posterior_svm is the same for all thresholds file, so the thresholds file is not needed for SVMs. The posterior_svm should go into the classifier.mat!
pmxarray = matGetVariable(pmatfile, "posterior_svm");
if (pmxarray == 0) {
std::cout << "[DetSVM] WARNING: Unable to find the vector posterior_svm, disable prob. SVM output;" << std::endl;
// TODO prob. output cannot be disabled in the new version of this lib -> throw exception or log info message or something
logisticA = logisticB = 0;
}
else {
double* matdata = mxGetPr(pmxarray);
const mwSize *dim = mxGetDimensions(pmxarray);
if (dim[1] != 2) {
std::cout << "[DetSVM] WARNING: Size of vector posterior_svm !=2, disable prob. SVM output;" << std::endl;
// TODO same as previous TODO
logisticA = logisticB = 0;
}
else {
logisticB = matdata[0];
logisticA = matdata[1];
}
// TODO delete *matdata and *dim? -> No I don't think so, no 'new'
mxDestroyArray(pmxarray);
}
matClose(pmatfile);
}
return make_pair(logisticA, logisticB);
#else
string errorMessage("ProbabilisticSvmClassifier: Cannot load a Matlab classifier, library compiled without support for Matlab. Please re-run CMake with WITH_MATLAB_CLASSIFIER enabled.");
logger.error(errorMessage);
throw std::runtime_error(errorMessage);
#endif
}
idx<T> matlab::load_matrix(const char *name) {
if (!fp) eblthrow("null file pointer");
#ifdef __MATLAB__
// get variable corresponding to name
mxArray *var = matGetVariable(fp, name);
if (!var)
eblthrow("variable \"" << name << "\" not found in matlab object");
int ndims = mxGetNumberOfDimensions(var);
if (ndims > MAXDIMS)
eblthrow("cannot load matrix with " << ndims
<< " dimensions, libidx was compiled to support " << MAXDIMS
<< " at most. Modify MAXDIMS and recompile.");
intg nelements = mxGetNumberOfElements(var);
const mwSize *dims = mxGetDimensions(var);
// allocate matrix
idxdim d;
for (uint i = 0; i < ndims; ++i)
d.insert_dim(i, dims[i]);
idx<T> m(d);
// load data
mxClassID type = mxGetClassID(var);
switch (type) {
case mxCELL_CLASS: eblthrow("cannot load matrix from type cell"); break ;
case mxSTRUCT_CLASS: eblthrow("cannot load matrix from type struct"); break ;
case mxLOGICAL_CLASS: eblthrow("cannot load matrix from type logical"); break ;
case mxFUNCTION_CLASS: eblthrow("cannot load matrix from type function"); break ;
case mxINT8_CLASS:
case mxUINT8_CLASS:
case mxCHAR_CLASS: read_cast_matrix<ubyte>(var, m); break ;
case mxINT16_CLASS: read_cast_matrix<int16>(var, m); break ;
case mxUINT16_CLASS: read_cast_matrix<uint16>(var, m); break ;
case mxINT32_CLASS: read_cast_matrix<uint32>(var, m); break ;
case mxUINT32_CLASS: read_cast_matrix<uint32>(var, m); break ;
case mxINT64_CLASS: read_cast_matrix<int64>(var, m); break ;
case mxUINT64_CLASS: read_cast_matrix<uint64>(var, m); break ;
case mxSINGLE_CLASS: read_cast_matrix<float>(var, m); break ;
case mxDOUBLE_CLASS: read_cast_matrix<double>(var, m); break ;
case mxVOID_CLASS: eblthrow("unsupported type: void");
case mxUNKNOWN_CLASS: eblthrow("unknown type"); break ;
}
// delete array
if (var) mxDestroyArray(var);
#else
idx<T> m;
#endif
return m;
}
/*@C
PetscViewerMatlabGetArray - Gets a variable from a MATLAB viewer into an array
Not Collective; only processor zero reads in the array
Input Parameters:
+ mfile - the MATLAB file viewer
. m,n - the dimensions of the array
. array - the array (represented in one dimension)
- name - the name of the array
Level: advanced
Notes: Only reads in array values on processor 0.
@*/
PetscErrorCode PetscViewerMatlabGetArray(PetscViewer mfile,int m,int n,PetscScalar *array,const char *name)
{
PetscErrorCode ierr;
PetscViewer_Matlab *ml = (PetscViewer_Matlab*)mfile->data;
mxArray *mat;
PetscFunctionBegin;
if (!ml->rank) {
ierr = PetscInfo1(mfile,"Getting MATLAB array %s\n",name);
CHKERRQ(ierr);
mat = matGetVariable(ml->ep,name);
if (!mat) SETERRQ1(PETSC_COMM_SELF,PETSC_ERR_LIB,"Unable to get array %s from matlab",name);
ierr = PetscMemcpy(array,mxGetPr(mat),m*n*sizeof(PetscScalar));
CHKERRQ(ierr);
ierr = PetscInfo1(mfile,"Got MATLAB array %s\n",name);
CHKERRQ(ierr);
}
PetscFunctionReturn(0);
}
void Transmission::LoadTransmission() {
MATFile *matFileHandle;
matFileHandle = matOpen(transmissionFileName, "r");
int numberOfDirectories;
const char** directoryField= (const char** )matGetDir(matFileHandle, &numberOfDirectories);
for(int i=0; i < numberOfDirectories; i++) {
std::cout<<directoryField[i]<<std::endl;
mxArray* matArrayHandle = matGetVariable(matFileHandle, directoryField[i]);
int numberOfRows = mxGetM(matArrayHandle);
int numberOfColumns = mxGetN(matArrayHandle);
double* matArrayAddress = mxGetPr(matArrayHandle);
std::vector<double> transmissionDataEntry;
for(int j=0;j<numberOfColumns;j++) {
double matrixEntry = *(matArrayAddress+j);
transmissionDataEntry.push_back(matrixEntry);
}
transmissionData.push_back(transmissionDataEntry);
}
std::cout<<"Transmission data successfully loaded"<<std::endl<<std::endl;
}
void Buffer::LoadBuffer() {
MATFile *matFileHandle;
matFileHandle = matOpen(bufferFileName, "r");
int numberOfDirectories;
if(matFileHandle==NULL) {
std::cout<<"ERROR IN FILE OPENING"<<std::endl;
}
const char** directoryField= (const char** )matGetDir(matFileHandle, &numberOfDirectories);
for(int i=0; i < numberOfDirectories; i++) {
//std::cout<<directoryField[i]<<std::endl;
mxArray* matArrayHandle = matGetVariable(matFileHandle, directoryField[i]);
int numberOfRows = mxGetM(matArrayHandle);
int numberOfColumns = mxGetN(matArrayHandle);
double* matArrayAddress = mxGetPr(matArrayHandle);
for(int j=0;j<numberOfColumns;j++) {
double matrixEntry = *(matArrayAddress+j);
bufferData.push_back(matrixEntry);
}
}
maximumBufferLevel = bufferData[0];
//std::cout<<"Max. buffer level "<<maximumBufferLevel<<std::endl;
minimumBufferLevel = bufferData[1];
//std::cout<<"Min. buffer level "<<minimumBufferLevel<<std::endl;
maximumStateOfBuffer = bufferData[2];
//std::cout<<"Max. state of buffer "<<maximumStateOfBuffer<<std::endl;
referenceStateOfBuffer = bufferData[3];
// A dummy reference is given to all buffers, Fuel Tanks and Batteries. For batteries,
// this value is updated eat each step. It remains constant for fuel tanks.
//std::cout<<"Min. allowed state of buffer "<<referenceStateOfBuffer<<std::endl;
openCircuitVoltage = bufferData[4];
//std::cout<<"openCircuitVoltage "<<openCircuitVoltage<<std::endl;
instantaneousBufferLevel=maximumStateOfBuffer*maximumBufferLevel;
//bufferLevelOverMission.push_back(instantaneousBufferLevel);
//std::cout<<"Inst. buffer level "<<instantaneousBufferLevel<<std::endl;
//std::cout<<"Buffer data successfully loaded"<<std::endl;
if (matClose(matFileHandle) != 0) {
std::cout<<"Error closing file"<<'\n';
}
}
void LoadMission(std::vector<std::vector<double>>& missionData) {
MATFile *matFileHandle;
matFileHandle = matOpen("data/MissionData.mat", "r");
int numberOfDirectories;
const char** directoryField= (const char** )matGetDir(matFileHandle, &numberOfDirectories);
//std::cout<<"Number of directories: "<<numberOfDirectories<<std::endl;
for(int i=0; i < numberOfDirectories; i++) {
//std::cout<<directoryField[i]<<std::endl;
mxArray* matArrayHandle = matGetVariable(matFileHandle, directoryField[i]);
int numberOfRows = mxGetM(matArrayHandle);
int numberOfColumns = mxGetN(matArrayHandle);
double* matArrayAddress = mxGetPr(matArrayHandle);
std::vector<double> missionDataEntry;
for(int j=0;j<numberOfColumns;j++) {
double matrixEntry = *(matArrayAddress+j);
missionDataEntry.push_back(matrixEntry);
}
missionData.push_back(missionDataEntry);
}
std::cout<<"Mission data successfully loaded"<<std::endl<<std::endl;
}
int main (int argc, char**argv) {
if (argc < 3) {
printf ("Usage:\n\nmih <infile> <outfile> [options]\n\n");
printf ("Options:\n");
printf (" -nMs <n1 n2 n3 ...> Set an array of multiples of 'one million items' to experiment with\n");
printf (" -nM <number> Set the index from the nMs array to be used for this run (1 is the first)\n");
printf (" -Q <number> Set the number of query points to use from <infile>, default all\n");
printf (" -B <number> Set the number of bits per code, default autodetect\n");
printf (" -m <number> Set the number of chunks to use, default 1\n");
printf ("\n");
return 0;
}
char *infile = argv[1];
char *outfile = argv[2];
UINT32 N = 0;
int B = 0;
int m = 1;
UINT32 K = 100;
int nM = 0;
int NQ = 0;
double *nMs = NULL;
int nnMs = 0;
for (int argnum = 3; argnum < argc; argnum++) {
if (argv[argnum][0] == '-') {
switch (argv[argnum][1]) {
case 'B':
B = atoi(argv[++argnum]);
break;
case 'K':
K = atoi(argv[++argnum]);
break;
case 'n':
if (!strcmp(argv[argnum], "-nMs")) {
nMs = new double[100];
while (++argnum < argc)
if (argv[argnum][0] != '-') {
nMs[nnMs++] = atof(argv[argnum]);
} else {
argnum--;
break;
}
} else if (!strcmp(argv[argnum], "-nM")) {
nM = atoi(argv[++argnum]);
}
break;
case 'Q':
NQ = atoi(argv[++argnum]);
break;
case 'm':
m = atoi(argv[++argnum]);
break;
default:
printf ("Unrecognized Option or Missing Parameter when parsing: %s\n", argv[argnum]);
return EXIT_FAILURE;
}
} else {
printf ("Invalid Argument: %s\n", argv[argnum]);
return EXIT_FAILURE;
}
}
MATFile *ifp = NULL;
mxArray *mxnMs = NULL;
mxArray *mxret = NULL;
/* Opening output file to read "ret" and "nMs" if available */
ifp = matOpen(outfile, "r");
if (ifp) {
mxnMs = matGetVariable(ifp, "nMs");
mxret = matGetVariable(ifp, "ret");
double *nMs2 = (double*)mxGetPr(mxnMs);
int nnMs2 = mxGetN(mxnMs);
if (nMs != NULL) {
if (nnMs != nnMs2) {
printf("#nMs is different from the #nMs read from the output file %d vs. %d.\n", nnMs, nnMs2);
return EXIT_FAILURE;
}
for (int i=0; i<nnMs; i++)
if (int(nMs[i]*1.0e6) != int(nMs2[i] * 1.0e6)) {
printf("nMs are different from the nMs read from the output file.\n");
return EXIT_FAILURE;
}
delete[] nMs;
}
nnMs = nnMs2;
nMs = nMs2;
matClose (ifp);
} else {
mxnMs = mxCreateNumericMatrix(1, nnMs, mxDOUBLE_CLASS, mxREAL);
double *nMs2 = (double*)mxGetPr(mxnMs);
for (int i=0; i<nnMs; i++)
nMs2[i] = nMs[i];
delete[] nMs;
nMs = nMs2;
}
if (mxret == NULL) {
const char* ab[] = {"res", "nres", "stat", "wt", "cput", "vm", "rss", "m"};
mxret = mxCreateStructMatrix(1000, nnMs, 8, ab);
}
/* Done with initializing mxnMs and mxret and sanity checks */
/* Loading the codes and queries from the input file */
ifp = matOpen (infile, "r");
if (!ifp) {
printf ("Failed to open input file. Aborting.\n");
return EXIT_FAILURE;
}
printf ("Loading codes... ");
fflush (stdout);
mxArray *mxcodes = matGetVariable (ifp, "B");
printf ("done.\n");
printf ("Loading queries... ");
fflush (stdout);
mxArray *mxqueries = matGetVariable (ifp, "Q");
printf ("done.\n");
matClose (ifp);
/* Done with the inputs */
int dim1codes = mxGetM(mxcodes);
int dim1queries = mxGetM(mxqueries);
if (!B)
B = mxGetM(mxcodes)*8;
if (B % 8 != 0) {
printf ("Non-multiple of 8 code lengths are not currently supported.\n");
return EXIT_FAILURE;
}
N = 1.0e6 * nMs[nM-1];
if (N)
N = std::min ( (UINT32)N, (UINT32)mxGetN(mxcodes) );
if (NQ > 0)
NQ = std::min( NQ, (int)mxGetN(mxqueries) );
else
NQ = mxGetN (mxqueries);
printf("nM = %d |", nM);
printf(" N = %.0e |", (double)N);
printf(" NQ = %d |", NQ);
printf(" B = %d |", B);
printf(" m = %d", m);
printf("\n");
/* Run multi-index hashing for 1,10,100,1000 NN and store the required stats */
int mold = -1;
mihasher* MIH = NULL;
clock_t start0, end0;
time_t start1, end1;
qstat *stats = (qstat*) new qstat[NQ];
for (K = 1; K<=1000; K *= 10) {
mxArray *mxresults = mxCreateNumericMatrix (K, NQ, mxUINT32_CLASS, mxREAL);
mxArray *mxnumres = mxCreateNumericMatrix (B+1, NQ, mxUINT32_CLASS, mxREAL);
mxArray *mxctime = mxCreateNumericMatrix (1, 1, mxDOUBLE_CLASS, mxREAL);
mxArray *mxwtime = mxCreateNumericMatrix (1, 1, mxDOUBLE_CLASS, mxREAL);
mxArray *mxvm = mxCreateNumericMatrix (1, 1, mxDOUBLE_CLASS, mxREAL);
mxArray *mxrss = mxCreateNumericMatrix (1, 1, mxDOUBLE_CLASS, mxREAL);
mxArray *mxstats = mxCreateNumericMatrix (6, NQ, mxDOUBLE_CLASS, mxREAL);
UINT32 *numres = (UINT32*) mxGetPr (mxnumres);
UINT32 *results = (UINT32*) mxGetPr (mxresults);
double *ctime = (double*) mxGetPr (mxctime);
double *wtime = (double*) mxGetPr (mxwtime);
double *vm = (double*) mxGetPr (mxvm);
double *rss = (double*) mxGetPr (mxrss);
double *stats_d = (double*) mxGetPr (mxstats);
/* if m changes from K to K */
if (mold != m) {
if (MIH != NULL) {
printf ("Clearing up... ");
fflush (stdout);
delete MIH;
printf ("done. \r");
fflush (stdout);
}
MIH = new mihasher(B, m);
printf ("Populating %d hashtables with %.0e entries...\n", m, (double)N);
fflush (stdout);
start1 = time(NULL);
start0 = clock();
MIH->populate ( (UINT8*) mxGetPr(mxcodes), N, dim1codes);
end0 = clock();
end1 = time(NULL);
double ct = (double)(end0-start0) / (CLOCKS_PER_SEC);
double wt = (double)(end1-start1);
printf ("done. | cpu %.0fm%.0fs | wall %.0fm%.0fs\n", ct/60, ct-60*int(ct/60), wt/60, wt-60*int(wt/60));
// printf ("done. \r");
// fflush (stdout);
}
printf(" m = %2d |", m);
printf(" K = %4d |", K);
printf(" ");
MIH->setK(K);
printf("query... ");
fflush (stdout);
start1 = time(NULL);
start0 = clock();
MIH->batchquery (results, numres, stats, (UINT8*) mxGetPr(mxqueries), NQ, dim1queries);
end0 = clock();
end1 = time(NULL);
*ctime = (double)(end0-start0) / (CLOCKS_PER_SEC) / NQ;
*wtime = (double)(end1-start1) / NQ;
process_mem_usage(vm, rss);
*vm /= double(1024*1024);
*rss /= double(1024*1024);
printf ("done | cpu %.3fs | wall %.3fs | VM %.1fgb | RSS %.1fgb \n", *ctime, *wtime, *vm, *rss);
int ind = 1000*(nM-1) + K-1;
double *pstats_d = stats_d;
for (int i=0; i<NQ; i++) {
pstats_d[0] = stats[i].numres;
pstats_d[1] = stats[i].numcand;
pstats_d[2] = stats[i].numdups;
pstats_d[3] = stats[i].numlookups;
pstats_d[4] = stats[i].maxrho;
pstats_d[5] = (double) stats[i].ticks / CLOCKS_PER_SEC;
pstats_d += 6;
}
mxSetFieldByNumber(mxret, ind, 0, mxresults);
mxSetFieldByNumber(mxret, ind, 1, mxnumres);
mxSetFieldByNumber(mxret, ind, 2, mxstats);
mxSetFieldByNumber(mxret, ind, 3, mxwtime);
mxSetFieldByNumber(mxret, ind, 4, mxctime);
mxSetFieldByNumber(mxret, ind, 5, mxvm);
mxSetFieldByNumber(mxret, ind, 6, mxrss);
mxSetFieldByNumber(mxret, ind, 7, mxCreateDoubleScalar(m));
mold = m;
}
printf ("Clearing up... ");
fflush (stdout);
delete MIH;
printf ("done. \r");
fflush (stdout);
/* Done with mulit-index hashing and storing the stats */
/* Opening the output file for writing the results */
MATFile *ofp = matOpen (outfile, "w");
if (!ofp) {
printf ("Failed to create/open output file. Aborting.\n");
return EXIT_FAILURE;
}
printf("Writing results to file %s... ", outfile);
fflush(stdout);
matPutVariable(ofp, "nMs", mxnMs);
matPutVariable(ofp, "ret", mxret);
printf("done.\n");
matClose(ofp);
/* Done with the output file */
delete[] stats;
mxDestroyArray (mxnMs);
mxDestroyArray (mxret);
mxDestroyArray (mxcodes);
mxDestroyArray (mxqueries);
/* skip deleting nMs if it is initialized by new double[] */
return 0;
}
int main(int argc, char *argv[])
{
char *infile,*outfile;
int i, nt=1, report_level=1;
/* TODO. Parsing of input with error checking.
Input syntax on form: -numtraj=4 etc. ?*/
if (argc < 3){
printf("To few arguments to nsm.");
exit(-1);
}
/* Input file. */
infile = argv[1];
/* Output file (or directory). */
outfile = argv[2];
/* Read model specification */
urdme_model *model;
model = read_model(infile);
model->infile = infile;
if (model == NULL){
printf("Fatal error. Couldn't load model file or currupt model file.");
return(-1);
}
/*reopen MAT file*/
MATFile *input_file;
mxArray *temp,*sopts;
input_file = matOpen(infile,"r");
if (input_file == NULL){
printf("Failed to open mat-file.\n");
return(-1);
}
/* Initialize RNG(s). */
/* Look for seed */
temp = matGetVariable(input_file, "seed");
/* Initialize rng from GSL. */
const gsl_rng_type * T;
T = gsl_rng_taus2;
gsl_rng_env_setup();
runif = gsl_rng_alloc (T);
//gsl_rng_set (runif, 463728648); //Why would we seed the name number each time??
long int seed;
/* If seed is provided as a parameter, it takes precedence. */
if (argc > 3) {
srand48((long int)atoi(argv[3]));
gsl_rng_set(runif,(long int)atoi(argv[3]));
}else if(temp != NULL){
seed = (long int) mxGetScalar(temp);
srand48(seed);
gsl_rng_set(runif,seed);
}else{
/* Not a foolproof solution */
//srand48((long int)time(NULL)+(long int)(1e9*clock()));
srand48( time(NULL) * getpid() );
gsl_rng_set(runif,time(NULL) * getpid());
}
/* Look for an optional parameter matrix. */
const double *matfile_parameters;
int mpar = 0;
int npar = 0;
int param_case=0;
temp = matGetVariable(input_file, "parameters");
if (temp != NULL) {
matfile_parameters = (double *)mxGetPr(temp);
mpar = mxGetM(temp);
npar = mxGetN(temp);
}
/* Look if a parameter case if supplied as a parameter. */
if (argc > 4) {
param_case = (int)atoi(argv[4]);
}
if (param_case > npar ) {
printf("nsmcore: Fatal error, parameter case is larger than n-dimension in parameter matrix.\n");
exit(-1);
}
/* Create global parameter variable for this parameter case. */
parameters = (double *)malloc(mpar*sizeof(double));
memcpy(parameters,&matfile_parameters[npar*param_case],mpar*sizeof(double));
/* Initialize extra args */
model->num_extra_args = 4;
model->extra_args=malloc(model->num_extra_args*sizeof(void *));
/* Set report level */
temp = matGetVariable(input_file, "report");
if (temp != NULL)
report_level = (int) mxGetScalar(temp);
else
if (nt > 1)
report_level=0;
else
report_level=1;
model->extra_args[0] = malloc(sizeof(int));
*(int *)(model->extra_args[0]) = report_level;
/* Set tau_d */
sopts = matGetVariable(input_file, "sopts");
if(sopts==NULL){
printf("Step length (tau_d) is missing in the model file\n");
return(-1);
}
model->extra_args[1] = malloc(sizeof(double));
model->extra_args[2] = malloc(sizeof(double));
model->extra_args[3] = malloc(sizeof(double));
double*sopts_tmp = mxGetPr(sopts);
//printf("HERE: tau_d=%e\n",sopts_tmp[0]);
//printf("HERE: max_jump=%e\n",sopts_tmp[1]);
//printf("HERE: err_tol=%e\n",sopts_tmp[2]);
*(double*)model->extra_args[1] = sopts_tmp[0];
*(double*)model->extra_args[2] = sopts_tmp[1];
*(double*)model->extra_args[3] = sopts_tmp[2];
//printf("HERE: tau_d=%e\n",*(double *)(model->extra_args[1]));
//printf("HERE: max_jump=%e\n",*(double *)(model->extra_args[2]));
//printf("HERE: err_tol=%e\n",*(double *)(model->extra_args[3]));
/* close MAT file*/
matClose(input_file);
/* Allocate memory to hold nt solutions. */
init_sol(model,nt);
/* Get a writer to store the output trajectory on a hdf5 file. */
urdme_output_writer *writer;
writer = get_urdme_output_writer(model,outfile);
printf("writer = get_urdme_output_writer(model,%s);\n",outfile);
printf("dfsp_core\n");
dfsp_core(model, writer);
/* Write the timespan vector to the output file */
printf("write_tspan\n");
write_tspan(writer,model);
//free(parameters);
printf("destroy_output_writer\n");
destroy_output_writer(writer);
printf("destroy_model\n");
destroy_model(model);
return(0);
}
shared_ptr<RvmClassifier> RvmClassifier::loadFromMatlab(const string& classifierFilename, const string& thresholdsFilename)
{
Logger logger = Loggers->getLogger("classification");
logger.info("Loading RVM classifier from Matlab file: " + classifierFilename);
MATFile *pmatfile;
mxArray *pmxarray; // =mat
double *matdata;
pmatfile = matOpen(classifierFilename.c_str(), "r");
if (pmatfile == NULL) {
throw invalid_argument("RvmClassifier: Could not open the provided classifier filename: " + classifierFilename);
}
pmxarray = matGetVariable(pmatfile, "num_hk");
if (pmxarray == 0) {
throw runtime_error("RvmClassifier: There is a no num_hk in the classifier file.");
// TODO (concerns the whole class): I think we leak memory here (all the MATFile and double pointers etc.)?
}
matdata = mxGetPr(pmxarray);
int numFilter = (int)matdata[0];
mxDestroyArray(pmxarray);
logger.debug("Found " + lexical_cast<string>(numFilter) + " reduced set vectors (RSVs).");
float nonlinThreshold;
int nonLinType;
float basisParam;
int polyPower;
float divisor;
pmxarray = matGetVariable(pmatfile, "param_nonlin1_rvm");
if (pmxarray != 0) {
matdata = mxGetPr(pmxarray);
nonlinThreshold = (float)matdata[0];
nonLinType = (int)matdata[1];
basisParam = (float)(matdata[2]/65025.0); // because the training images gray level values were divided by 255
polyPower = (int)matdata[3];
divisor = (float)matdata[4];
mxDestroyArray(pmxarray);
} else {
pmxarray = matGetVariable(pmatfile, "param_nonlin1");
if (pmxarray != 0) {
matdata = mxGetPr(pmxarray);
nonlinThreshold = (float)matdata[0];
nonLinType = (int)matdata[1];
basisParam = (float)(matdata[2]/65025.0); // because the training images gray level values were divided by 255
polyPower = (int)matdata[3];
divisor = (float)matdata[4];
mxDestroyArray(pmxarray);
} else {
throw runtime_error("RvmClassifier: Could not find kernel parameters and bias.");
// TODO tidying up?!
}
}
shared_ptr<Kernel> kernel;
if (nonLinType == 1) { // polynomial kernel
kernel.reset(new PolynomialKernel(1 / divisor, basisParam / divisor, polyPower));
} else if (nonLinType == 2) { // RBF kernel
kernel.reset(new RbfKernel(basisParam));
} else {
throw runtime_error("RvmClassifier: Unsupported kernel type. Currently, only polynomial and RBF kernels are supported.");
// TODO We should also throw/print the unsupported nonLinType value to the user
}
// TODO: logger.debug("Loaded kernel with params... ");
shared_ptr<RvmClassifier> rvm = make_shared<RvmClassifier>(kernel);
rvm->bias = nonlinThreshold;
logger.debug("Reading the " + lexical_cast<string>(numFilter) + " non-linear filters support_hk* and weight_hk* ...");
char str[100];
sprintf(str, "support_hk%d", 1);
pmxarray = matGetVariable(pmatfile, str);
if (pmxarray == 0) {
throw runtime_error("RvmClassifier: Unable to find the matrix 'support_hk1' in the classifier file.");
}
if (mxGetNumberOfDimensions(pmxarray) != 2) {
throw runtime_error("RvmClassifier: The matrix 'support_hk1' in the classifier file should have 2 dimensions.");
}
const mwSize *dim = mxGetDimensions(pmxarray);
int filterSizeY = (int)dim[0]; // height
int filterSizeX = (int)dim[1]; // width TODO check if this is right with eg 24x16
mxDestroyArray(pmxarray);
// Alloc space for SV's and alphas (weights)
rvm->supportVectors.reserve(numFilter);
rvm->coefficients.reserve(numFilter);
// Read the reduced vectors and coefficients (weights):
for (int i = 0; i < numFilter; ++i) {
sprintf(str, "support_hk%d", i+1);
pmxarray = matGetVariable(pmatfile, str);
if (pmxarray == 0) {
throw runtime_error("RvmClassifier: Unable to find the matrix 'support_hk" + lexical_cast<string>(i+1) + "' in the classifier file.");
}
if (mxGetNumberOfDimensions(pmxarray) != 2) {
throw runtime_error("RvmClassifier: The matrix 'support_hk" + lexical_cast<string>(i+1) + "' in the classifier file should have 2 dimensions.");
}
// Read the reduced-vector:
matdata = mxGetPr(pmxarray);
int k = 0;
Mat supportVector(filterSizeY, filterSizeX, CV_32F);
float* values = supportVector.ptr<float>(0);
for (int x = 0; x < filterSizeX; ++x) // column-major order (ML-convention)
for (int y = 0; y < filterSizeY; ++y)
values[y * filterSizeX + x] = static_cast<float>(matdata[k++]); // because the training images gray level values were divided by 255
rvm->supportVectors.push_back(supportVector);
mxDestroyArray(pmxarray);
sprintf(str, "weight_hk%d", i+1);
pmxarray = matGetVariable(pmatfile, str);
if (pmxarray != 0) {
const mwSize *dim = mxGetDimensions(pmxarray);
if ((dim[1] != i+1) && (dim[0] != i+1)) {
throw runtime_error("RvmClassifier: The matrix " + lexical_cast<string>(str) + " in the classifier file should have a dimensions 1x" + lexical_cast<string>(i+1) + " or " + lexical_cast<string>(i+1) + "x1");
}
vector<float> coefficientsForFilter;
matdata = mxGetPr(pmxarray);
for (int j = 0; j <= i; ++j) {
coefficientsForFilter.push_back(static_cast<float>(matdata[j]));
}
rvm->coefficients.push_back(coefficientsForFilter);
mxDestroyArray(pmxarray);
}
} // end for over numHKs
logger.debug("Vectors and weights successfully read.");
if (matClose(pmatfile) != 0) {
logger.warn("RvmClassifier: Could not close file " + classifierFilename);
// TODO What is this? An error? Info? Throw an exception?
}
logger.info("RVM successfully read.");
//MATFile *mxtFile = matOpen(args->threshold, "r");
logger.info("Loading RVM thresholds from Matlab file: " + thresholdsFilename);
pmatfile = matOpen(thresholdsFilename.c_str(), "r");
if (pmatfile == 0) {
throw runtime_error("RvmClassifier: Unable to open the thresholds file (wrong format?):" + thresholdsFilename);
} else {
pmxarray = matGetVariable(pmatfile, "hierar_thresh");
if (pmxarray == 0) {
throw runtime_error("RvmClassifier: Unable to find the matrix hierar_thresh in the thresholds file.");
} else {
double* matdata = mxGetPr(pmxarray);
const mwSize *dim = mxGetDimensions(pmxarray);
for (int o=0; o<(int)dim[1]; ++o) {
rvm->hierarchicalThresholds.push_back(static_cast<float>(matdata[o]));
}
mxDestroyArray(pmxarray);
}
matClose(pmatfile);
}
if(rvm->hierarchicalThresholds.size() != rvm->coefficients.size()) {
throw runtime_error("RvmClassifier: Something seems to be wrong, hierarchicalThresholds.size() != coefficients.size(): " + lexical_cast<string>(rvm->hierarchicalThresholds.size()) + "!=" + lexical_cast<string>(rvm->coefficients.size()));
}
logger.info("RVM thresholds successfully read.");
rvm->setNumFiltersToUse(numFilter);
return rvm;
}
/** Loads the data from a .mat file. */
int ArmModel::loadData(bool onlyReset) {
char filename[20];
sprintf(filename, "T%d.mat",inputFileNr);
// Open MAT-File
MATFile *mf = matOpen(filename, "r");
if (mf == NULL) {
TheCsimError.add("ArmModel::loadData Cannot find input file %s\n",filename);
return -1;
}
if (!onlyReset) {
/* *********************************************************
READ ALL DATA FROM FILE
********************************************************* */
// Read Xdest and store it
mxArray *mxXdest = matGetVariable(mf, "Xdest");
if (mxXdest == NULL) {
TheCsimError.add("ArmModel::loadData Cannot find Xdest in input file %s\n",filename);
return -1;
}
TIMESTEPS = mxGetN(mxXdest);
csimPrintf("Read data for %d timesteps!\n", TIMESTEPS);
Xdest = (double *) realloc(Xdest, TIMESTEPS*50*sizeof(double));
memcpy(Xdest, mxGetPr(mxXdest), TIMESTEPS*50*sizeof(double));
// Read Ydest and store it
mxArray *mxYdest = matGetVariable(mf, "Ydest");
if (mxYdest == NULL) {
TheCsimError.add("ArmModel::loadData Cannot find Ydest in input file %s\n",filename);
return -1;
}
Ydest = (double *) realloc(Ydest, TIMESTEPS*50*sizeof(double));
memcpy(Ydest, mxGetPr(mxYdest), TIMESTEPS*50*sizeof(double));
mxDestroyArray(mxYdest);
// Read Theta1 and store it
mxArray *mxTheta1 = matGetVariable(mf, "theta1");
if (mxTheta1 == NULL) {
TheCsimError.add("ArmModel::loadData Cannot find theta1 in input file %s\n",filename);
return -1;
}
theta1 = (double *) realloc(theta1, TIMESTEPS*50*sizeof(double));
memcpy(theta1, mxGetPr(mxTheta1), TIMESTEPS*50*sizeof(double));
mxDestroyArray(mxTheta1);
// Read Theta2 and store it
mxArray *mxTheta2 = matGetVariable(mf, "theta2");
if (mxTheta2 == NULL) {
TheCsimError.add("ArmModel::loadData Cannot find theta2 in input file %s\n",filename);
return -1;
}
theta2 = (double *) realloc(theta2, TIMESTEPS*50*sizeof(double));
memcpy(theta2, mxGetPr(mxTheta2), TIMESTEPS*50*sizeof(double));
mxDestroyArray(mxTheta2);
// Read nu1 and store it
mxArray *mxNu1 = matGetVariable(mf, "nu1");
if (mxNu1 == NULL) {
TheCsimError.add("ArmModel::loadData Cannot find nu1 in input file %s\n",filename);
return -1;
}
nu1 = (double *) realloc(nu1, TIMESTEPS*50*sizeof(double));
memcpy(nu1, mxGetPr(mxNu1), TIMESTEPS*50*sizeof(double));
mxDestroyArray(mxNu1);
// Read nu2 and store it
mxArray *mxNu2 = matGetVariable(mf, "nu2");
if (mxNu2 == NULL) {
TheCsimError.add("ArmModel::loadData Cannot find nu2 in input file %s\n",filename);
return -1;
}
nu2 = (double *) realloc(nu2, TIMESTEPS*50*sizeof(double));
memcpy(nu2, mxGetPr(mxNu2), TIMESTEPS*50*sizeof(double));
mxDestroyArray(mxNu2);
// Read c_theta1 and store it
mxArray *mxCTheta1 = matGetVariable(mf, "c_theta1");
if (mxCTheta1 == NULL) {
TheCsimError.add("ArmModel::loadData Cannot find c_theta1 in input file %s\n",filename);
return -1;
}
c_theta1 = (double *) realloc(c_theta1, TIMESTEPS*sizeof(double));
memcpy(c_theta1, mxGetPr(mxCTheta1), TIMESTEPS*sizeof(double));
mxDestroyArray(mxCTheta1);
// Read c_theta2 and store it
mxArray *mxCTheta2 = matGetVariable(mf, "c_theta2");
if (mxCTheta2 == NULL) {
TheCsimError.add("ArmModel::loadData Cannot find c_theta2 in input file %s\n",filename);
return -1;
}
c_theta2 = (double *) realloc(c_theta2, TIMESTEPS*sizeof(double));
memcpy(c_theta2, mxGetPr(mxCTheta2), TIMESTEPS*sizeof(double));
mxDestroyArray(mxCTheta2);
// Read c_u1 and store it
mxArray *mxCU1 = matGetVariable(mf, "c_u1");
if (mxCU1 == NULL) {
TheCsimError.add("ArmModel::loadData Cannot find c_u1 in input file %s\n",filename);
return -1;
}
c_u1 = (double *) realloc(c_u1, TIMESTEPS*sizeof(double));
memcpy(c_u1, mxGetPr(mxCU1), TIMESTEPS*sizeof(double));
mxDestroyArray(mxCU1);
// Read c_u2 and store it
mxArray *mxCU2 = matGetVariable(mf, "c_u2");
if (mxCU2 == NULL) {
TheCsimError.add("ArmModel::loadData Cannot find c_u2 in input file %s\n",filename);
return -1;
}
c_u2 = (double *) realloc(c_u2, TIMESTEPS*sizeof(double));
memcpy(c_u2, mxGetPr(mxCU2), TIMESTEPS*sizeof(double));
mxDestroyArray(mxCU2);
// Get the values which will be added as baselines.
int pos = 0;
for (int i=0; i<TIMESTEPS; i++) {
for (int j=0; j<50; j++) {
if (pos == 0) {
mintheta1 = theta1[0];
mintheta2 = theta2[0];
minU1 = nu1[0];
minU2 = nu2[0];
// HERE THE GET_ACT_VAL AND MAXU1, MAXU2 ARE MISSING, DO WE NEED IT?
}
else {
if (theta1[pos] < mintheta1)
mintheta1 = theta1[pos];
if (theta2[pos] < mintheta2)
mintheta2 = theta2[pos];
if (nu1[pos] < minU1)
minU1 = nu1[pos];
if (nu2[pos] < minU2)
minU2 = nu2[pos];
}
pos++;
}
}
mintheta1 = fabs(mintheta1);
mintheta2 = fabs(mintheta2);
minU1 = fabs(minU1);
minU2 = fabs(minU2);
}
// Do the rest for a simple reset
/* *********************************************************
INITIALIZE THE OUTPUTS OF THE MODEL
********************************************************* */
for (int i=0; i<50; i++) {
out_buffer[i][0] = Xdest[i];
out_buffer[i+50][0] = Ydest[i];
out_buffer[i+100][0] = theta1[i];
if (out_buffer[i+100][0] != 0)
out_buffer[i+100][0] += mintheta1;
out_buffer[i+150][0] = theta2[i];
if (out_buffer[i+150][0] != 0)
out_buffer[i+150][0] += mintheta2;
out_buffer[i+200][0] = nu1[i];
if (out_buffer[i+200][0] != 0)
out_buffer[i+200][0] += minU1;
out_buffer[i+250][0] = nu2[i];
if (out_buffer[i+250][0] != 0)
out_buffer[i+250][0] += minU2;
}
buffer_position = 0;
/* *********************************************************
Calculate initial values for u, t and w
********************************************************* */
// Read u1(1) and store it
mxArray *mxU1 = matGetVariable(mf, "u1");
if (mxU1 == NULL) {
TheCsimError.add("ArmModel::loadData Cannot find u1 in input file %s\n",filename);
return -1;
}
u1 = *(mxGetPr(mxU1));
mxDestroyArray(mxU1);
// Read u2(1) and store it
mxArray *mxU2 = matGetVariable(mf, "u2");
if (mxU2 == NULL) {
TheCsimError.add("ArmModel::loadData Cannot find u2 in input file %s\n",filename);
return -1;
}
u2 = *(mxGetPr(mxU2));
mxDestroyArray(mxU2);
// Read t1(1) and store it
mxArray *mxT1 = matGetVariable(mf, "t1");
if (mxT1 == NULL) {
TheCsimError.add("ArmModel::loadData Cannot find t1 in input file %s\n",filename);
return -1;
}
t1 = *(mxGetPr(mxT1));
mxDestroyArray(mxT1);
// Read t2(1) and store it
mxArray *mxT2 = matGetVariable(mf, "t2");
if (mxT2 == NULL) {
TheCsimError.add("ArmModel::loadData Cannot find t2 in input file %s\n",filename);
return -1;
}
t2 = *(mxGetPr(mxT2));
mxDestroyArray(mxT2);
// Read dt1(1) and store it
mxArray *mxDT1 = matGetVariable(mf, "dt1");
if (mxDT1 == NULL) {
TheCsimError.add("ArmModel::loadData Cannot find dt1 in input file %s\n",filename);
return -1;
}
w1 = *(mxGetPr(mxDT1));
mxDestroyArray(mxDT1);
// Read dt2(1) and store it
mxArray *mxDT2 = matGetVariable(mf, "dt2");
if (mxDT2 == NULL) {
TheCsimError.add("ArmModel::loadData Cannot find dt2 in input file %s\n",filename);
return -1;
}
w2 = *(mxGetPr(mxDT2));
mxDestroyArray(mxDT2);
// Close the input file
if (matClose(mf) == EOF) {
TheCsimError.add("ArmModel::loadData Error while closing the input file!\n");
return -1;
}
return 0;
}
void
mexFunction( int nlhs, mxArray *plhs[],
int nrhs, const mxArray *prhs[] )
{
MATFile *ph;
char *matfile;
char *varnamelist[100];
int counter=0;
(void) nlhs; /* unused parameters */
(void) plhs;
if(nrhs==0) {
/*
* no arguments; so load by default from 'matlab.mat'
*/
matfile=(char *)mxCalloc(11,sizeof(char));
matfile=strcat(matfile,"matlab.mat");
}
else {
/*
* parse the argument list for the
* filename and any variable names
*
* note: the first argument must be a filename.
* therefore you can't request specific
* variables from 'matlab.mat' without
* passing the filename in
*/
/*
* we're adding 5 to the size of the string.
* 1 for '\0' and 4 in case the '.mat' was
* forgotten
*/
mwSize buflen=mxGetN(prhs[0])+5;
int index;
/* filename */
matfile=(char *)mxCalloc(buflen,sizeof(char));
mxGetString(prhs[0],matfile,buflen);
/*
* make sure '.mat' suffix exists
*/
{
const char *lastdot = strrchr( matfile, '.' );
const char *lastslash = strrchr( matfile, '/' );
const char *lastbslash = strrchr( lastslash ? lastslash : matfile, '\\');
lastslash = lastbslash ? lastbslash : lastslash;
if(!lastdot || (!lastslash || lastdot < lastslash))
matfile=strcat(matfile,".mat");
}
/* variable name list */
for(index=1;index<nrhs;index++) {
mwSize varlength=mxGetN(prhs[index])+1;
varnamelist[counter]=(char *)mxCalloc(varlength,sizeof(char));
mxGetString(prhs[index],varnamelist[counter],varlength);
counter++;
}
}
/* open the MAT-File to read from */
ph=matOpen(matfile,"r");
if(ph<(MATFile *)3) {
char errmsg[]="Failed to open '";
strcat(errmsg,matfile);
strcat(errmsg,"'");
mxFree(matfile);
mexErrMsgTxt(errmsg);
}
if(counter>0) {
/* there where variables in the argument list */
mxArray *var;
int index;
for(index=0;index<counter;index++) {
var=matGetVariable(ph,varnamelist[index]);
mexPutVariable("caller", varnamelist[index],var);
mxFree(varnamelist[index]);
}
}
else {
/*
* the variable argument list
* was empty so get everything
*/
mxArray *var;
const char *var_name;
while((var=matGetNextVariable(ph, &var_name))!=NULL) {
mexPutVariable("caller", var_name, var);
}
}
matClose(ph);
mxFree(matfile);
}
int main()
{
#ifdef TRACK
char file[]="/media/fantaosha/Documents/JHU/Summer 2015/quad_rotor_traj/traj_full_12s_1_small.mat";
mxArray *mxR;
mxArray *mxw;
mxArray *mxxq;
mxArray *mxvq;
mxArray *mxU;
mxR=matGetVariable(matOpen(file,"r"),"state_R");
mxw=matGetVariable(matOpen(file,"r"),"state_w");
mxxq=matGetVariable(matOpen(file,"r"),"state_xq");
mxvq=matGetVariable(matOpen(file,"r"),"state_vq");
mxU=matGetVariable(matOpen(file,"r"),"U");
size_t N=mxGetN(mxU);
size_t sN=10;
void *pR=mxGetPr(mxR);
void *pw=mxGetPr(mxw);
void *pxq=mxGetPr(mxxq);
void *pvq=mxGetPr(mxvq);
void *pU=mxGetPr(mxU);
std::vector<typename quadrotor::State> xrefs;
xrefs.reserve(N/sN+1);
for(int i=0;i<N;i+=sN)
{
Eigen::Matrix<double,3,3> R;
Eigen::Matrix<double,3,1> w,xq,vq;
memcpy(R.data(),pR,sizeof(double)*R.rows()*R.cols());
// R=(Vec3()<<1,-1,-1).finished().asDiagonal();
// w<<0,0,0;
memcpy(w.data(),pw,sizeof(double)*w.rows()*w.cols());
memcpy(xq.data(),pxq,sizeof(double)*xq.rows()*xq.cols());
memcpy(vq.data(),pvq,sizeof(double)*vq.rows()*vq.cols());
xrefs.emplace_back(R,xq,w,R.transpose()*vq);
pR+=sizeof(double)*R.rows()*R.cols()*sN;
pw+=sizeof(double)*w.rows()*w.cols()*sN;
pxq+=sizeof(double)*xq.rows()*xq.cols()*sN;
pvq+=sizeof(double)*vq.rows()*vq.cols()*sN;
}
sN=10;
std::vector<typename quadrotor::U> us;
us.reserve(N/sN+1);
for(int i=0;i<N;i+=sN)
{
Eigen::Matrix<double,4,1> u;
memcpy(u.data(),pU,sizeof(double)*u.rows()*u.cols());
us.push_back(u.cwiseProduct(u));
pU+=sizeof(double)*u.rows()*u.cols()*sN;
}
#else
quadrotor::State xref;
xref.g.block(0,0,3,3)=SO3::exp((Vec3()<<0,0,0).finished());
xref.g.block(0,3,3,1)=(Vec3()<<0,0,0).finished();
std::vector<quadrotor::State> xrefs(4000,xref);
std::vector<quadrotor::U> us(4000, Vec4::Zero());
#endif
std::srand((unsigned int) time(0));
// Set up quadrotor parameters
double m=0.6;
double Ix=8*1e-3; // moment of inertia in X- Y- and Z-axis
double Iy=7.5*1e-3;
double Iz=13.5*1e-3;
double d=0.2; // displacement of rotors
double kt=0.6;
double km=0.15;
quadrotor::System sys(Ix,Iy,Iz,m,d,km,kt);
// Set up cost function
Mat12 Mf=5*Mat12::Identity()/10;
Mf.block(0,0,3,3)*=1;
Mf.block(3,3,3,3)*=6;
// Mf.block(6,6,6,6)=Mf.block(0,0,6,6);
Mat12 M=Mf/2;
Mat4 R=Mat4::Identity()/50;
DDP<quadrotor>::Params params(M,R,Mf);
// Set up initial state
quadrotor::State x0=xrefs[0];
x0.g.block(0,3,3,1)-=(Vec3::Random()).normalized()*3;
x0.g.block(0,0,3,3)*=SO3::exp(Vec3::Random().normalized()*1);
// x0.g.block(0,0,3,3)*=SO3::exp((Vec3()<<3.14,0,0).finished());
x0.v.head(3)-=Vec3::Random().normalized()*0;
x0.v.tail(3)-=Vec3::Random().normalized()*0;
// Set up simulator
double dt=0.01;
size_t num=200;
Sim<quadrotor> sim(sys,dt);
sim.init(x0,num);
Vec4 umin=-Vec4::Ones()*0;
Vec4 umax=Vec4::Ones()*6;
DDP<quadrotor> ddp(sys,dt);
double ts=0.02;
double T=36;
double Tp=1.5;
size_t SN=size_t(ts/dt+0.5);
size_t ND=size_t(Tp/dt+0.5)+1;
int sn=2;
int itr_max=20;
for(int i=0;i<2000;i+=sn)
{
timespec T_start, T_end;
clock_gettime(CLOCK_MONOTONIC,&T_start);
ddp.init(sim.get_state(), us, xrefs, params, umin, umax, 150);
ddp.iterate(itr_max,us);
clock_gettime(CLOCK_MONOTONIC,&T_end);
std::cout<<"time consumed is "<<(T_end.tv_sec-T_start.tv_sec)+(T_end.tv_nsec-T_start.tv_nsec)/1000000000.0<<"s"<<std::endl;
for(int j=0;j<sn;j++)
{
sim.update(us.front());
xrefs.erase(xrefs.begin());
us.erase(us.begin());
}
Vec12 error=quadrotor::State::diff(sim.get_state(),xrefs[0]);
std::cout<<dt*i<<": "<<error.head(3).norm()<<" "<<error.head(6).tail(3).norm()<<std::endl;
if(error.norm()<0.1)
break;
}
sim.save("/media/fantaosha/Documents/JHU/Summer 2015/results/quadrotor_ddp.mat");
}
int matFiles::readMatFile(const char *file, std::vector <std::vector<double> >& earSignals, double *fsHz) {
MATFile *pmat;
const char **dir;
const char *name;
int ndir;
int i;
mxArray *pa;
mxArray *var;
const size_t *dims;
size_t ndims;
double *data;
/*
* Open file to get directory
*/
pmat = matOpen(file, "r");
if (pmat == NULL) {
printf("Error opening file %s\n", file);
return(1);
}
/*
* get directory of MAT-file
*/
dir = (const char **)matGetDir(pmat, &ndir);
if (dir == NULL) {
printf("Error reading directory of file %s\n", file);
return(1);
}
mxFree(dir);
/* In order to use matGetNextXXX correctly, reopen file to read in headers. */
if (matClose(pmat) != 0) {
printf("Error closing file %s\n",file);
return(1);
}
pmat = matOpen(file, "r");
if (pmat == NULL) {
printf("Error reopening file %s\n", file);
return(1);
}
/* Get headers of all variables */
/* Examining the header for each variable */
for (i=0; i < ndir; i++) {
pa = matGetNextVariableInfo(pmat, &name);
var = matGetVariable(pmat, name);
data = mxGetPr(var);
if (pa == NULL) {
printf("Error reading in file %s\n", file);
return(1);
}
if ( strcmp(name,"earSignals") == 0 ) {
ndims = mxGetNumberOfDimensions(pa);
dims = mxGetDimensions(pa);
earSignals.resize(ndims);
for ( size_t ii = 0 ; ii < ndims ; ++ii )
earSignals[ii].resize(dims[0],0);
size_t ii, iii;
for ( ii = 0 ; ii < dims[0] ; ++ii )
earSignals[0][ii] = data[ii];
for ( ii = dims[0], iii = 0 ; ii < dims[0] * 2 ; ++ii, ++iii )
earSignals[1][iii] = data[ii];
} else if ( strcmp(name,"fsHz") == 0 ) {
ndims = mxGetNumberOfDimensions(pa);
dims = mxGetDimensions(pa);
assert( ndims == 2 );
assert( dims[0] == 1 );
*fsHz = data[0];
}
mxDestroyArray(pa);
}
if (matClose(pmat) != 0) {
printf("Error closing file %s\n",file);
return(1);
}
return(0);
}
urdme_model *read_model(char *file)
{
int i,Ndofs;
mxArray *D,*N,*G,*vol,*u0,*tspan,*data,*sd,*K;
urdme_model *model;
model = (urdme_model *)malloc(sizeof(urdme_model));
/* Open mat-file (read-only) */
MATFile *input_file;
input_file = matOpen(file,"r");
if (input_file == NULL){
printf("Failed to open mat-file.\n");
return NULL;
}
/* Get D-matrix. */
D = matGetVariable(input_file, "D");
if (D == NULL){
printf("The diffusion matrix D is missing in the model file.\n");
return NULL;
}
Ndofs = mxGetN(D);
size_t *mxirD;
size_t *mxjcD;
double *mxprD;
mxirD = mxGetIr(D);
mxjcD = mxGetJc(D);
mxprD = mxGetPr(D);
int nnzD = mxGetNzmax(D);
model->jcD = (size_t *)malloc((Ndofs+1)*sizeof(size_t));
memcpy(model->jcD,mxjcD,(Ndofs+1)*sizeof(size_t));
model->irD = (size_t*)malloc(nnzD*sizeof(size_t));
memcpy(model->irD,mxirD,nnzD*sizeof(size_t));
model->prD = (double *)malloc(nnzD*sizeof(double));
memcpy(model->prD,mxprD,nnzD*sizeof(double));
mxDestroyArray(D);
/* initial condition */
u0 = matGetVariable(input_file, "u0");
if (u0 == NULL){
printf("Initial condition (u0) is missing in the model file.\n");
return NULL;
}
/* Typecast */
model->Mspecies = (int) mxGetM(u0);
int *u0int;
u0int = (int*)malloc(Ndofs*sizeof(int));
double *u0temp;
u0temp = mxGetPr(u0);
for (i=0;i<Ndofs;i++)
u0int[i] = (int) u0temp[i];
model->u0 = u0int;
mxDestroyArray(u0);
/* Stoichiometry matrix */
N = matGetVariable(input_file, "N");
if (N == NULL){
printf("The stoichiometry matrix is missing in the model file.\n");
return NULL;
}
if (mxIsSparse(N)){
/* This is always true if the matrix is created by the Matlab interface. */
/* Typecast to int */
double *tempN;
tempN = mxGetPr(N);
int nnzN = (int) mxGetNzmax(N);
int *prN;
prN = (int*)malloc(nnzN*sizeof(int));
for (i=0;i<nnzN;i++)
prN[i] = (int) tempN[i];
model->prN = prN;
model->irN = (size_t *)malloc(nnzN*sizeof(size_t));
memcpy(model->irN,(size_t *)mxGetIr(N),nnzN*sizeof(size_t));
model->jcN = (size_t *)malloc((mxGetN(N)+1)*sizeof(size_t));
memcpy(model->jcN,(size_t *)mxGetJc(N),(mxGetN(N)+1)*sizeof(size_t));
}
else{
if (mxGetN(N)>0||mxGetM(N)>0){
printf("The stoichiometry matrix must be a sparse CCS matrix.");
return NULL;
}
else{
/* This little pice of code is needed to cover the case where
there are no reactions and the matrix is passed as an empty dense
array, as is necessary if it is generated by pyurdme using scipy. */
model->irN = (size_t *)malloc(0*sizeof(size_t));
model->jcN = (size_t *)malloc(0*sizeof(size_t));
model->prN = (int *)malloc(0*sizeof(int));
}
}
model->Mreactions = (int) mxGetN(N);
model->Ncells=Ndofs/model->Mspecies;
mxDestroyArray(N);
/* Connectivity matrix */
K = matGetVariable(input_file, "K");
if (K == NULL){
printf("The Connectivity matrix K is missing in the model file.\n");
return NULL;
}
int nK = mxGetN(K);
size_t *mxirK;
size_t *mxjcK;
double *mxprK;
mxirK = mxGetIr(K);
mxjcK = mxGetJc(K);
mxprK = mxGetPr(K);
int nnzK = mxGetNzmax(K);
model->jcK = (size_t *)malloc((nK+1)*sizeof(size_t));
memcpy(model->jcK,mxjcK,(nK+1)*sizeof(size_t));
model->irK = (size_t*)malloc(nnzK*sizeof(size_t));
memcpy(model->irK,mxirK,nnzK*sizeof(size_t));
model->prK = (double *)malloc(nnzK*sizeof(double));
memcpy(model->prK,mxprK,nnzK*sizeof(double));
mxDestroyArray(K);
/* Volume vector */
vol = matGetVariable(input_file,"vol");
if (vol == NULL){
printf("The volume vector is missing in the model file.\n");
return NULL;
}
model->vol = (double *)malloc(model->Ncells*sizeof(double));
memcpy(model->vol,(double *)mxGetPr(vol),model->Ncells*sizeof(double));
mxDestroyArray(vol);
/* Dependency graph */
G = matGetVariable(input_file, "G");
if (G == NULL){
printf("The dependency graph (G) is missing in the model file.\n");
return NULL;
}
if (mxIsSparse(G)){
int nnzG = mxGetNzmax(G);
model->irG = (size_t *)malloc(nnzG*sizeof(size_t));
memcpy(model->irG,(size_t *)mxGetIr(G),nnzG*sizeof(size_t));
model->jcG = (size_t *)malloc((mxGetN(G)+1)*sizeof(size_t));
memcpy(model->jcG,(size_t *)mxGetJc(G),(mxGetN(G)+1)*sizeof(size_t));
mxDestroyArray(G);
}
else{
model->irG = (size_t *)malloc(0*sizeof(size_t));
model->irG = (size_t *)malloc(0*sizeof(size_t));
model->jcG = (size_t *)malloc(0*sizeof(size_t));
}
/* time span */
tspan = matGetVariable(input_file, "tspan");
if (tspan == NULL){
printf("Time span (tspan) is missing in the model file.\n");
return NULL;
}
model->tlen = mxGetNumberOfElements(tspan);
model->tspan = (double *)malloc(model->tlen*sizeof(double));
memcpy(model->tspan,(double *)mxGetPr(tspan),model->tlen*sizeof(double));
mxDestroyArray(tspan);
/* Subdomain vector */
sd = matGetVariable(input_file, "sd");
if (sd == NULL){
printf("Subdomain vector (sd) is missing in the model file.\n");
return NULL;
}
/* typecast */
int *sdint = (int*)malloc(model->Ncells*sizeof(int));
double *sdtemp;
sdtemp = mxGetPr(sd);
for (i=0;i<model->Ncells;i++)
sdint[i]=(int) sdtemp[i];
model->sd = sdint;
mxDestroyArray(sd);
/* data matrix */
data = matGetVariable(input_file, "data");
if (data == NULL){
printf("Data matrix (data) is missing in the model file.\n");
return NULL;
}
model->dsize = mxGetM(data);
model->data = (double *)malloc(model->dsize*model->Ncells*sizeof(double));
memcpy(model->data,(double *)mxGetPr(data),model->dsize*model->Ncells*sizeof(double));
mxDestroyArray(data);
/* Maximum number of solutions defaults to one. */
model->nsolmax = 1;
matClose(input_file);
return model;
}
int main(int argc, char** argv){
double delta = (double)pow(10,-5);
int max_iter = 100;
size_t size;
//load Series from .mat file
MATFile *pmat;
const char* file =argv[1];
const char* varname="Series";
mxArray* Series_mat;
pmat = matOpen(file, "r");
if(pmat == NULL){
printf("Error reopening file%s\n", file);
return(NULL);
}
Series_mat = matGetVariable(pmat, varname);
if(Series_mat == NULL){
printf("Error reading in file%s\n", file);
return(NULL);
}
matClose(pmat);
mwSize row, col; // mwSize is int
mwSize nRow = mxGetM(Series_mat);
mwSize nCol = mxGetN(Series_mat);
int T = nCol;
int N = nRow;
double *Series_Pr = mxGetPr(Series_mat);
Matrix Series;
Series.width = nCol;
Series.height = nRow;
size = Series.width*Series.height*sizeof(double);
Series.elements = (double*)malloc(size);
for(row = 0; row < nRow; row++) {
for(col = 0; col < nCol; col++) {
Series.elements[row*Series.width+col] = Series_Pr[nRow * col + row];// this needs testing
}
}
int i,j;
Matrix A;
//create matrix A
A.width = N;
A.height = N;
size = A.width*A.height*sizeof(double);
A.elements = (double*)malloc(size);
//time counter
double tstart,tstop,ttime;
tstart =(double)clock()/CLOCKS_PER_SEC;
int counter = 0;
while(counter<10){
//initialize A
for(i =0; i < A.height; i++){
for(j = 0; j < A.width; j++){
A.elements[i*A.width+j] = 0;
}
}
// Gradient_descent
Gradient_descent(A,Series,max_iter,delta,N,T);
counter++;
}
tstop = (double)clock()/CLOCKS_PER_SEC;
ttime=tstop-tstart;
printf("time:%fs\n",ttime);
free(A.elements);
free(Series.elements);
return 0;
}
MVT_3D_Object::MVT_3D_Object(ENUM_OBJECT_CATEGORY object_category, const char* filepath_3dobject_model)
{
m_object_category = object_category;
MATFile* mat = matOpen(filepath_3dobject_model,"r");
if( mat != NULL )
{
mxArray* pmxCad = matGetVariable(mat, "cad");
pmxCad = mxGetCell(pmxCad,0);
/*
* Names of parts
*/
mxArray* pmxNames = mxGetField(pmxCad, 0, "pnames");
m_num_of_partsNroots = mxGetNumberOfElements(pmxNames);
for( unsigned int i=0; i<m_num_of_partsNroots; i++)
{
mxArray* pmxName = mxGetCell(pmxNames, i);
m_names_partsNroots.push_back(MxArray(pmxName).toString());
}
/*
* Parts2d_Front
*/
mxArray* pmxPars2d_front = mxGetField(pmxCad, 0, "parts2d_front");
for( unsigned int i=0; i<m_num_of_partsNroots; i++)
{
MVT_2D_Part_Front front;
front.width = MxArray(mxGetField(pmxPars2d_front, i, "width" )).toDouble();
front.height = MxArray(mxGetField(pmxPars2d_front, i, "height" )).toDouble();
front.distance = MxArray(mxGetField(pmxPars2d_front, i, "distance")).toDouble();
cv::Mat mat_tmp = MxArray(mxGetField(pmxPars2d_front, i, "vertices")).toMat();
unsigned int n_row = mat_tmp.rows;
for( unsigned int r=0; r<n_row ; r++)
{
front.vertices.push_back(cv::Point2d(mat_tmp.at<double>(r,0), mat_tmp.at<double>(r,1)));
}
front.center = MxArray(mxGetField(pmxPars2d_front, i, "center")).toPoint_<double>();
front.viewport = MxArray(mxGetField(pmxPars2d_front, i, "viewport")).toDouble();
front.name = MxArray(mxGetField(pmxPars2d_front, i, "pname")).toString();
m_2dparts_front.push_back(front);
}
/*
* Part
*/
mxArray* pmxParts = mxGetField(pmxCad, 0, "parts");
m_num_of_parts = mxGetNumberOfElements(pmxParts);
for( unsigned int i=0 ; i<m_num_of_parts; i++ )
{
MVT_3D_Part part;
/* Vertices */
mxArray* mxVertices = mxGetField(pmxParts, i, "vertices");
cv::Mat matVertices = MxArray(mxVertices).toMat();
unsigned int n_rows = matVertices.rows;
for( unsigned int r=0; r<n_rows; r++)
{
cv::Mat matTmp = matVertices.row(r);
part.vertices.push_back
(
cv::Point3d( matTmp.at<double>(0),
matTmp.at<double>(1),
matTmp.at<double>(2) )
);
}
/* plane */
MxArray MxPlane(mxGetField(pmxParts, i, "plane"));
for( int j=0 ; j<4; j++ )
{
part.plane[j] = MxPlane.at<double>(j);
}
/* center */
MxArray MxCenter(mxGetField(pmxParts, i, "center"));
part.center.x = MxCenter.at<double>(0);
part.center.y = MxCenter.at<double>(1);
part.center.z = MxCenter.at<double>(2);
/* xaxis */
MxArray MxXAxis(mxGetField(pmxParts, i, "xaxis"));
part.xaxis.x = MxXAxis.at<double>(0);
part.xaxis.y = MxXAxis.at<double>(1);
part.xaxis.z = MxXAxis.at<double>(2);
/* yaxis */
MxArray MxYAxis(mxGetField(pmxParts, i, "yaxis"));
part.yaxis.x = MxYAxis.at<double>(0);
part.yaxis.y = MxYAxis.at<double>(1);
part.yaxis.z = MxYAxis.at<double>(2);
m_3dparts.push_back(part);
/*
* Occlusion Information
*/
mxArray* pmxAzimuth = mxGetField(pmxCad, 0, "azimuth" );
m_disc_azimuth = MxArray(pmxAzimuth).toVector<MVT_AZIMUTH>();
mxArray* pmxElevation = mxGetField(pmxCad, 0, "elevation" );
m_disc_elevation = MxArray(pmxElevation).toVector<MVT_ELEVATION>();
mxArray* pmxDistance = mxGetField(pmxCad, 0, "distance" );
m_disc_distance = MxArray(pmxDistance).toVector<MVT_DISTANCE>();
mxArray* pmxParts2d = mxGetField(pmxCad, 0, "parts2d");
m_num_of_disc_azimuth = m_disc_azimuth.size();
m_num_of_disc_elevation = m_disc_elevation.size();
m_num_of_disc_distance = m_disc_distance.size();
m_is_occluded = new bool***[m_num_of_disc_azimuth];
for( unsigned int a=0; a<m_num_of_disc_azimuth; a++ )
{
m_is_occluded[a] = new bool**[m_num_of_disc_elevation];
for( unsigned int e=0; e<m_num_of_disc_elevation; e++ )
{
m_is_occluded[a][e] = new bool*[m_num_of_disc_distance];
for( unsigned int d=0; d<m_num_of_disc_distance; d++ )
{
m_is_occluded[a][e][d] = new bool[m_num_of_partsNroots];
unsigned int idx = m_num_of_disc_distance*m_num_of_disc_elevation*a +
m_num_of_disc_distance*e +
d;
for( unsigned int p=0; p<m_num_of_partsNroots; p++)
{
mxArray* pmxPart = mxGetField(pmxParts2d, idx, m_names_partsNroots[p].c_str());
m_is_occluded[a][e][d][p] = mxIsEmpty(pmxPart);
}
}
}
}
}
mxDestroyArray(pmxCad);
matClose(mat);
}
}
