Exemple #1
0
/* tz_fcmp(x1, x2) compare two double float numbers.
 * tz_fcmp(x1, x2, ep) compare the double float numbers at the accuracy ep, 
 * which is 1e-5 by default.
 */
void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
{
  check_argin(nrhs, prhs);
  
  double epsilon = 1e-5;

  if (nrhs == 3) {
    epsilon = *mxGetPr(prhs[2]);
  }

  mwSize length;
  mwSize length1 = tz_mxGetL(prhs[0]);
  mwSize length2 = tz_mxGetL(prhs[1]);
  const mxArray *array = NULL;

  if (mxIsScalar(prhs[0]) && mxIsScalar(prhs[1])) {
    plhs[0] = mxCreateNumericMatrix(1, 1, mxINT8_CLASS, mxREAL);
    length = 1;
  } else if (!mxIsScalar(prhs[0]) && !mxIsScalar(prhs[1])) {
    plhs[0] = mxCreateNumericArray(mxGetNumberOfDimensions(prhs[0]), 
				   mxGetDimensions(prhs[0]), mxINT8_CLASS,
				   mxREAL);
    length = length1;
  } else {
    if (length1 == 1) {
      array = prhs[1];
      length = length2;
    } else {
      array = prhs[0];
      length = length1;
    }
    
    plhs[0] = mxCreateNumericArray(mxGetNumberOfDimensions(array), 
				   mxGetDimensions(array), mxINT8_CLASS,
				   mxREAL);
  }

  INT8_T *y = (INT8_T *) mxGetPr(plhs[0]);
  double *x1 = mxGetPr(prhs[0]);
  double *x2 = mxGetPr(prhs[1]);

  mwSize i;
  if (length1 == length2) {
    for (i = 0; i < length; i++) {
      y[i] = (INT8_T) gsl_fcmp(x1[i], x2[i], epsilon);
    }
  } else if (length1 > length2) {
    for (i = 0; i < length; i++) {
      y[i] = (INT8_T) gsl_fcmp(x1[i], x2[0], epsilon);
    }
  } else {
    for (i = 0; i < length; i++) {
      y[i] = (INT8_T) gsl_fcmp(x1[0], x2[i], epsilon);
    }
  }
}
/*
 *	h a s O p t i o n s V a l u e
 */
BooleanType hasOptionsValue( const mxArray* optionsPtr, const char* const optionString, double** optionValue )
{
	mxArray* optionName = mxGetField( optionsPtr,0,optionString );

	if ( optionName == 0 )
	{
		char msg[MAX_STRING_LENGTH];
		snprintf(msg, MAX_STRING_LENGTH, "Option struct does not contain entry '%s', using default value instead!", optionString );
		mexWarnMsgTxt( msg );
		return BT_FALSE;
	}

	if ( ( mxIsEmpty(optionName) == false ) && ( mxIsScalar( optionName ) == true ) )
	{
		*optionValue = mxGetPr( optionName );
		return BT_TRUE;
	}
	else
	{
		char msg[MAX_STRING_LENGTH];
		snprintf(msg, MAX_STRING_LENGTH, "Option '%s' is not a scalar, using default value instead!", optionString );
		mexWarnMsgTxt( msg );
		return BT_FALSE;
	}
}
/* gft1d(sig, windowType)
 *
 * Performs a 1d GFT using an asymmetric partitioning
 * scheme for real signals
 */
void mexFunction(int nlhs, mxArray *plhs[], 
                 int nrhs, const mxArray *prhs[])
{
    if (nrhs != 1) 
        mexErrMsgTxt("gft1dRealPartitions: Wrong number of inputs");
    else if (nlhs>1)
        mexErrMsgTxt("gft1dRealPartitions: Too many output arguments");

    mwSize n = mxGetNumberOfDimensions(prhs[0]);
    if (!mxIsDouble(prhs[0]) || mxIsComplex(prhs[0]) || !mxIsScalar(prhs[0]))
        mexErrMsgTxt("gft1dRealPartitions: requires scalar argument");

    mwSize Nele;
    double *inpr = mxGetPr(prhs[0]);
	Nele = inpr[0];

    int *pars = gft_1dRealPartitions(Nele);
	int i = 0;
	int parlength;

	/* count number of elements in partition vector */
	while (pars[i]>0) i++;
	parlength = i;
			
    mxArray *out = mxCreateNumericMatrix(1, parlength, mxDOUBLE_CLASS, mxREAL);
    double *outpr = mxGetPr(out);

    for(i=0; i<parlength; i++)
    {
        outpr[i] = pars[i];
    }

    free(pars);
    plhs[0] = out;
}
Exemple #4
0
static int ID_check(int arg, const mxArray *prhs[]){
	int i;
	if(mxINT32_CLASS != mxGetClassID(prhs[arg]) || !mxIsScalar(prhs[arg])){
		mexErrMsgIdAndTxt("S4:invalidInput", "Expected handle argument after command.");
	}
	i = *(int*)(mxGetData(prhs[arg]));
	return i;
}
Exemple #5
0
static void complex_check(int arg, const mxArray *prhs[], const char *argname, double *ret){
	if(!mxIsNumeric(prhs[arg]) || !mxIsScalar(prhs[arg])){
		mexErrMsgIdAndTxt("S4:invalidInput", "Expected complex number.");
	}
	ret[0] = *(mxGetPr(prhs[arg]));
	if(mxIsComplex(prhs[arg])){
		ret[1] = *(mxGetPi(prhs[arg]));
	}
}
Exemple #6
0
static void S4mex_Simulation_SetMaterial(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]){
	S4_Simulation *S = Sptr_check(prhs);
	S4_real eps[18] = {0};
	S4_MaterialID M;
	
	if(3 != nrhs){
		mexErrMsgIdAndTxt("S4:invalidInput", "Expected two additional arguments to 'set_material'.");
	}
	
	if(mxIsScalar(prhs[2])){
		eps[0] = *(mxGetPr(prhs[2]));
		if(mxIsComplex(prhs[2])){
			eps[1] = *(mxGetPi(prhs[2]));
			M = S4_Simulation_SetMaterial(S, -1, NULL, S4_MATERIAL_TYPE_SCALAR_COMPLEX, eps);
		}else{
			M = S4_Simulation_SetMaterial(S, -1, NULL, S4_MATERIAL_TYPE_SCALAR_REAL, eps);
		}
	}else if(3 == mxGetM(prhs[2]) && 3 == mxGetN(prhs[2])){
		int i, j;
		const double *p = mxGetPr(prhs[3]);
		for(j = 0; j < 3; ++j){
			for(i = 0; i < 3; ++i){
				eps[2*(i+j*3)+0] = p[i+j*3];
			}
		}
		if(mxIsComplex(prhs[2])){
			p = mxGetPi(prhs[2]);
			for(j = 0; j < 3; ++j){
				for(i = 0; i < 3; ++i){
					eps[2*(i+j*3)+1] = p[i+j*3];
				}
			}
		}
		{
			S4_real abcde[10] = {
				eps[0], eps[1], eps[6], eps[7],
				eps[2], eps[3], eps[8], eps[9],
				eps[16], eps[17]
			};
			for(i = 0; i < 10; ++i){
				eps[i] = abcde[i];
			}
		}
		M = S4_Simulation_SetMaterial(S, -1, NULL, S4_MATERIAL_TYPE_XYTENSOR_COMPLEX, eps);
	}else{
		mexErrMsgIdAndTxt("S4:invalidInput", "Third additional argument (epsilon) must be a scalar or 3x3 matrix.");
	}
	
	MEX_TRACE("> S4mex_Simulation_SetMaterial(S = %p, epsilon = %f ...)\n", S, eps[0]);
	
	plhs[0] = mxCreateNumericMatrix(1, 1, mxINT32_CLASS, mxREAL);
	*(int*)(mxGetData(plhs[0])) = M;
	
	MEX_TRACE("< S4mex_Simulation_SetMaterial %d\n", M);
}
Exemple #7
0
static int S_check(const mxArray *prhs[]){
	int i;
	if(mxINT32_CLASS != mxGetClassID(prhs[1]) || !mxIsScalar(prhs[1])){
		mexErrMsgIdAndTxt("S4:invalidInput", "Expected simulation handle argument after command.");
	}
	i = *(int*)(mxGetData(prhs[1]));
	if(i < 0 || i >= nS_alloc){
		mexErrMsgIdAndTxt("S4:invalidInput", "Invalid simulation handle argument after command.");
	}
	return i;
}
Exemple #8
0
void mexFunction (int nlhs, mxArray * plhs[], int nrhs, const mxArray * prhs[]) {
		int rc;
		double delay;

		/* this function will be called upon unloading of the mex file */
		mexAtExit(exitFun);

		if (nrhs<1) {
				/* show the status of the delayed exit */

				pthread_mutex_lock(&mutextimer);
				pthread_mutex_lock(&mutexstatus);
				if (timerStatus) {
						delay = difftime(timer-time(NULL));
						if (nlhs<1)
								mexPrintf("delayed exit scheduled at %d seconds\n", (int)delay);
						else
								plhs[0] = mxCreateDoubleScalar(delay);
				}
				else {
						if (nlhs<1)
								mexPrintf("no delayed exit scheduled\n");
						else
								plhs[0] = mxCreateDoubleScalar(mxGetInf());
				}
				pthread_mutex_unlock(&mutexstatus);
				pthread_mutex_unlock(&mutextimer);

		}

		else {
				/* the first argument is the delay in seconds */
				if (!mxIsScalar(prhs[0]))
						mexErrMsgTxt ("invalid input argument #1");

				delay = mxGetScalar(prhs[0]);

				/* set or update the timer */
				pthread_mutex_lock(&mutextimer);
				timer = time(NULL) + (unsigned int)delay;
				pthread_mutex_unlock(&mutextimer);

				/* start the thread */
				rc = pthread_create(&timerThread, NULL, checktimer, (void *)NULL);
				if (rc)
						mexErrMsgTxt("problem with return code from pthread_create()");
		}

} /* main */
void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]){
	 char msg[2000];
	 const char *docstring ="";

	 /* check proper input and output */
	 if(nrhs!=3){
		  sprintf(msg, ">> Need 3 inputs.\n%s\n", docstring);
		  mexErrMsgTxt(msg);
	 } else if(!mxIsDouble(prhs[0]) || !is_mex_vector(prhs[0]) ){
		  sprintf(msg, ">> First Input must be double vector.\n%s\n", docstring);
		  mexErrMsgTxt(msg);
	  } else if( !(mxIsScalar(prhs[1]) && mxIsScalar(prhs[2])) ){
		  sprintf(msg, ">> Second and Third Inputs must be scalars.\n%s\n", docstring);
		  mexErrMsgTxt(msg);
	  }

	 /* getting the appropriate array representation */
	 Array *s = mex_mxarray_to_array( prhs[0] );
	 int m=(int)get_mex_double( prhs[1]);
	 int tau=(int)get_mex_double( prhs[2]);
	 double *x=s->data;
	 int n=s->size[0];

	 /* computation */
	 TimeDelayReconstruction* td = tdelay_init ( m, tau, x, n );
	 Array *tda = tdelay_to_array( td );

	 /* conversion to MATLAB */
	 plhs[0]=mex_array_to_mxarray( tda );

	 /* cleaning up */
	 array_free( s );
	 tdelay_free( td );

  return;
}
Exemple #10
0
static void S4mex_Simulation_SetFrequency(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]){
	S4_Simulation *S = Sptr_check(prhs);
	S4_real freq[2] = {0, 0};
	
	if(3 != nrhs){
		mexErrMsgIdAndTxt("S4:invalidInput", "Expected two additional arguments to 'set_frequency'.");
	}
	if(!mxIsScalar(prhs[2])){
		mexErrMsgIdAndTxt("S4:invalidInput", "First additional argument (frequency) must be a scalar number.");
	}
	freq[0] = *(mxGetPr(prhs[2]));
	
	MEX_TRACE("> S4mex_Simulation_SetFrequency(S = %p, freq = %f)\n", S, freq[0]);
	
	S4_Simulation_SetFrequency(S, freq);
	
	MEX_TRACE("< S4mex_Simulation_SetFrequency\n");
}
void mexFunction( int nlhs1, mxArray *plhs[],    
                  int nrhs, const mxArray *prhs[] )
{ 
    double w1, w2, w3, eps;
    mwSize n1, n2, n3, nDims; 
    const mwSize *pDims;
    const mwSize pDims2[] = {1,1};
	unsigned int flags; 
	double *x;

    double *fx;
    double *gx; 

    /* Control of the number of inputs and outputs */ 
    if (nrhs > 6)
        mexErrMsgTxt("6 input argument required."); 
    else if (nlhs1 !=2) 
        mexErrMsgTxt("2 output argument required.");
    
    /* Control of the inputs */
	if (!mxIsNumeric(prhs[0]) || !mxIsDouble(prhs[0])) 
        mexErrMsgTxt("The first input must be numerical double array.");
    if (!mxIsNumeric(prhs[1]) || !mxIsScalar(prhs[1])
        || !mxIsNumeric(prhs[2]) || !mxIsScalar(prhs[2]) 
		|| !mxIsNumeric(prhs[3]) || !mxIsScalar(prhs[3])
        || !mxIsNumeric(prhs[4]) || !mxIsScalar(prhs[4]) 
		|| !mxIsNumeric(prhs[5]) || !mxIsScalar(prhs[5]) ) 
        mexErrMsgTxt("The 5 last input must be scalar."); 
    
    if ( (double )(unsigned int )mxGetScalar(prhs[5]) != mxGetScalar(prhs[5]) )
        mexErrMsgTxt("The last input flags must be an unsigned integer.");
    
    /* Get the inputs */
    w1        = (double )mxGetScalar(prhs[1]);
    w2        = (double )mxGetScalar(prhs[2]);
	w3        = (double )mxGetScalar(prhs[3]);
    eps       = (double )mxGetScalar(prhs[4]);
    flags     = (unsigned int )mxGetScalar(prhs[5]);
    flags    |= RGL_INTEGRATE_GRADIENT;
		
	nDims = mxGetNumberOfDimensions(prhs[0]);
    pDims = mxGetDimensions(prhs[0]);
    x = mxGetPr(prhs[0]);	
    
    n1        = pDims[0];
    n2        = pDims[1];
	n3        = pDims[2];

    /* Display the inputs */
    /* mexPrintf("n1 = %d, n2 = %d, n3 = %d, w1 = %f, w2 = %f, w3 = %f, eps = %f, flags = %d\n", n1, n2, n3, w1, w2, w3, eps, flags); */
    
    /* Create the output arguments */
    /* Argument fx */
    plhs[0] = mxCreateNumericArray(2, pDims2, mxDOUBLE_CLASS, mxREAL);
    fx = mxGetPr(plhs[0]);
    
    /* Argument gx */
    plhs[1] = mxCreateNumericArray(nDims, pDims, mxDOUBLE_CLASS, mxREAL);
    gx = mxGetPr(plhs[1]);
   
    /* Call to the rgl_tv3d function */
    *fx = rgl_tv3d(x,n1,n2,n3,w1,w2,w3,eps,gx,flags); 
}
/*
 *	s e t u p A u x i l i a r y I n p u t s
 */
returnValue setupAuxiliaryInputs(	const mxArray* auxInput, uint_t nV, uint_t nC,
									HessianType* hessianType, double** x0, double** guessedBounds, double** guessedConstraints, double** R
									)
{
	mxArray* curField = 0;

	/* hessianType */
	curField = mxGetField( auxInput,0,"hessianType" );
	if ( curField == NULL )
		mexWarnMsgTxt( "auxInput struct does not contain entry 'hessianType'!\n         Type 'help qpOASES_auxInput' for further information." );
	else
	{
		if ( mxIsEmpty(curField) == true )
		{
			*hessianType = HST_UNKNOWN;
		}
		else
		{
			if ( mxIsScalar(curField) == false )
				return RET_INVALID_ARGUMENTS;

			double* hessianTypeTmp = mxGetPr(curField);
			int_t hessianTypeInt = (int_t)*hessianTypeTmp;
			if ( hessianTypeInt < 0 ) 
				hessianTypeInt = 6; /* == HST_UNKNOWN */
			if ( hessianTypeInt > 5 ) 
				hessianTypeInt = 6; /* == HST_UNKNOWN */
			*hessianType = (REFER_NAMESPACE_QPOASES HessianType)hessianTypeInt;
		}
	}

	/* x0 */
	curField = mxGetField( auxInput,0,"x0" );
	if ( curField == NULL )
		mexWarnMsgTxt( "auxInput struct does not contain entry 'x0'!\n         Type 'help qpOASES_auxInput' for further information." );
	else
	{
		*x0 = mxGetPr(curField);
		if ( smartDimensionCheck( x0,nV,1, BT_TRUE,((const mxArray**)&curField),0 ) != SUCCESSFUL_RETURN )
			return RET_INVALID_ARGUMENTS;
	}

	/* guessedWorkingSetB */
	curField = mxGetField( auxInput,0,"guessedWorkingSetB" );
	if ( curField == NULL )
		mexWarnMsgTxt( "auxInput struct does not contain entry 'guessedWorkingSetB'!\n         Type 'help qpOASES_auxInput' for further information." );
	else
	{
		*guessedBounds = mxGetPr(curField);
		if ( smartDimensionCheck( guessedBounds,nV,1, BT_TRUE,((const mxArray**)&curField),0 ) != SUCCESSFUL_RETURN )
			return RET_INVALID_ARGUMENTS;
	}

	/* guessedWorkingSetC */
	curField = mxGetField( auxInput,0,"guessedWorkingSetC" );
	if ( curField == NULL )
		mexWarnMsgTxt( "auxInput struct does not contain entry 'guessedWorkingSetC'!\n         Type 'help qpOASES_auxInput' for further information." );
	else
	{
		*guessedConstraints = mxGetPr(curField);
		if ( smartDimensionCheck( guessedConstraints,nC,1, BT_TRUE,((const mxArray**)&curField),0 ) != SUCCESSFUL_RETURN )
			return RET_INVALID_ARGUMENTS;
	}

	/* R */
	curField = mxGetField( auxInput,0,"R" );
	if ( curField == NULL )
		mexWarnMsgTxt( "auxInput struct does not contain entry 'R'!\n         Type 'help qpOASES_auxInput' for further information." );
	else
	{
		*R = mxGetPr(curField);
		if ( smartDimensionCheck( R,nV,nV, BT_TRUE,((const mxArray**)&curField),0 ) != SUCCESSFUL_RETURN )
			return RET_INVALID_ARGUMENTS;
	}

	return SUCCESSFUL_RETURN;
}
/*
 *	m e x F u n c t i o n
 */
void mexFunction( int nlhs, mxArray* plhs[], int nrhs, const mxArray* prhs[] )
{
	/* inputs */
	char typeString[2];

	real_t *g=0, *lb=0, *ub=0, *lbA=0, *ubA=0;
	HessianType hessianType = HST_UNKNOWN;
	double *x0=0, *R=0, *R_for=0;
	double *guessedBounds=0, *guessedConstraints=0;

	int_t H_idx=-1, g_idx=-1, A_idx=-1, lb_idx=-1, ub_idx=-1, lbA_idx=-1, ubA_idx=-1;
	int_t x0_idx=-1, auxInput_idx=-1;

	BooleanType isSimplyBoundedQp = BT_FALSE;
	#ifdef SOLVER_MA57
	BooleanType isSparse = BT_TRUE; /* This will be set to BT_FALSE later if a dense matrix is encountered. */
	#else
	BooleanType isSparse = BT_FALSE;
	#endif

	Options options;
	options.printLevel = PL_LOW;
	#ifdef __DEBUG__
	options.printLevel = PL_HIGH;
	#endif
	#ifdef __SUPPRESSANYOUTPUT__
	options.printLevel = PL_NONE;
	#endif

	/* dimensions */
	uint_t nV=0, nC=0, handle=0;
	int_t nWSRin;
	real_t maxCpuTimeIn = -1.0;
	QPInstance* globalQP = 0;

	/* I) CONSISTENCY CHECKS: */
	/* 1) Ensure that qpOASES is called with a feasible number of input arguments. */
	if ( ( nrhs < 5 ) || ( nrhs > 10 ) )
	{
		if ( nrhs != 2 )
		{
			myMexErrMsgTxt( "ERROR (qpOASES): Invalid number of input arguments!\nType 'help qpOASES_sequence' for further information." );
			return;
		}
	}
	
	/* 2) Ensure that first input is a string ... */
	if ( mxIsChar( prhs[0] ) != 1 )
	{
		myMexErrMsgTxt( "ERROR (qpOASES): First input argument must be a string!" );
		return;
	}

	mxGetString( prhs[0], typeString, 2 );

	/*    ... and if so, check if it is an allowed one. */
	if ( ( strcmp( typeString,"i" ) != 0 ) && ( strcmp( typeString,"I" ) != 0 ) &&
		 ( strcmp( typeString,"h" ) != 0 ) && ( strcmp( typeString,"H" ) != 0 ) &&
		 ( strcmp( typeString,"m" ) != 0 ) && ( strcmp( typeString,"M" ) != 0 ) &&
		 ( strcmp( typeString,"e" ) != 0 ) && ( strcmp( typeString,"E" ) != 0 ) &&
		 ( strcmp( typeString,"c" ) != 0 ) && ( strcmp( typeString,"C" ) != 0 ) )
	{
		myMexErrMsgTxt( "ERROR (qpOASES): Undefined first input argument!\nType 'help qpOASES_sequence' for further information." );
		return;
	}


	/* II) SELECT RESPECTIVE QPOASES FUNCTION CALL: */
	/* 1) Init (without or with initial guess for primal solution). */
	if ( ( strcmp( typeString,"i" ) == 0 ) || ( strcmp( typeString,"I" ) == 0 ) )
	{
		/* consistency checks */
		if ( ( nlhs < 1 ) || ( nlhs > 7 ) )
		{
			myMexErrMsgTxt( "ERROR (qpOASES): Invalid number of output arguments!\nType 'help qpOASES_sequence' for further information." );
			return;
		}

		if ( ( nrhs < 5 ) || ( nrhs > 10 ) )
		{
			myMexErrMsgTxt( "ERROR (qpOASES): Invalid number of input arguments!\nType 'help qpOASES_sequence' for further information." );
			return;
		}

		g_idx = 2;

		if ( mxIsEmpty(prhs[1]) == 1 )
		{
			H_idx = -1;
			nV = (uint_t)mxGetM( prhs[ g_idx ] ); /* row number of Hessian matrix */
		}
		else
		{
			H_idx = 1;
			nV = (uint_t)mxGetM( prhs[ H_idx ] ); /* row number of Hessian matrix */
		}


		/* ensure that data is given in double precision */
		if ( ( ( H_idx >= 0 ) && ( mxIsDouble( prhs[ H_idx ] ) == 0 ) ) ||
		     ( mxIsDouble( prhs[ g_idx ] ) == 0 ) )
		{
			myMexErrMsgTxt( "ERROR (qpOASES): All data has to be provided in double precision!" );
			return;
		}

		if ( ( H_idx >= 0 ) && ( ( mxGetN( prhs[ H_idx ] ) != nV ) || ( mxGetM( prhs[ H_idx ] ) != nV ) ) )
		{
			myMexErrMsgTxt( "ERROR (qpOASES): Hessian matrix dimension mismatch!" );
			return;
		}


		/* Check for 'Inf' and 'Nan' in Hessian */
		if (containsNaNorInf( prhs,H_idx, 0 ) == BT_TRUE)
			return;

		/* Check for 'Inf' and 'Nan' in gradient */
		if (containsNaNorInf(prhs,g_idx, 0 ) == BT_TRUE)
			return;

		/* determine whether is it a simply bounded QP */
		if ( nrhs <= 7 )
			isSimplyBoundedQp = BT_TRUE;
		else
			isSimplyBoundedQp = BT_FALSE;

		if ( isSimplyBoundedQp == BT_TRUE )
		{
			lb_idx = 3;
			ub_idx = 4;

			if (containsNaNorInf( prhs,lb_idx, 1 ) == BT_TRUE)
				return;

			if (containsNaNorInf( prhs,ub_idx, 1 ) == BT_TRUE)
				return;

			/* Check inputs dimensions and assign pointers to inputs. */
			nC = 0; /* row number of constraint matrix */


			if ( smartDimensionCheck( &g,nV,1, BT_FALSE,prhs,2 ) != SUCCESSFUL_RETURN )
				return;

			if ( smartDimensionCheck( &lb,nV,1, BT_TRUE,prhs,3 ) != SUCCESSFUL_RETURN )
				return;

			if ( smartDimensionCheck( &ub,nV,1, BT_TRUE,prhs,4 ) != SUCCESSFUL_RETURN )
				return;

			/* default value for nWSR */
			nWSRin = 5*nV;

			/* Check whether x0 and options are specified .*/
			if ( nrhs >= 6 )
			{
				if ((!mxIsEmpty(prhs[5])) && (mxIsStruct(prhs[5])))
					setupOptions( &options,prhs[5],nWSRin,maxCpuTimeIn );

				if ( ( nrhs >= 7 ) && ( !mxIsEmpty(prhs[6]) ) )
				{ 
					/* auxInput specified */
					if ( mxIsStruct(prhs[6]) )
					{
						auxInput_idx = 6;
						x0_idx = -1;
					}
					else
					{
						auxInput_idx = -1;
						x0_idx = 6;
					}
				}
				else
				{
					auxInput_idx = -1;
					x0_idx = -1;
				}
			}
		}
		else
		{
			A_idx = 3;

			/* ensure that data is given in double precision */
			if ( mxIsDouble( prhs[ A_idx ] ) == 0 )
			{
				myMexErrMsgTxt( "ERROR (qpOASES): All data has to be provided in double precision!" );
				return;
			}
		
			/* Check inputs dimensions and assign pointers to inputs. */
			nC = (uint_t)mxGetM( prhs[ A_idx ] ); /* row number of constraint matrix */

			lb_idx = 4;
			ub_idx = 5;
			lbA_idx = 6;
			ubA_idx = 7;

			if (containsNaNorInf( prhs,A_idx, 0 ) == BT_TRUE)
				return;

			if (containsNaNorInf( prhs,lb_idx, 1 ) == BT_TRUE)
				return;

			if (containsNaNorInf( prhs,ub_idx, 1 ) == BT_TRUE)
				return;

			if (containsNaNorInf( prhs,lbA_idx, 1 ) == BT_TRUE)
				return;

			if (containsNaNorInf( prhs,ubA_idx, 1 ) == BT_TRUE)
				return;

			if ( ( mxGetN( prhs[ A_idx ] ) != 0 ) && ( mxGetN( prhs[ A_idx ] ) != nV ) )
			{
				myMexErrMsgTxt( "ERROR (qpOASES): Constraint matrix dimension mismatch!" );
				return;
			}
		
			if ( smartDimensionCheck( &g,nV,1, BT_FALSE,prhs,g_idx ) != SUCCESSFUL_RETURN )
				return;

			if ( smartDimensionCheck( &lb,nV,1, BT_TRUE,prhs,lb_idx ) != SUCCESSFUL_RETURN )
				return;

			if ( smartDimensionCheck( &ub,nV,1, BT_TRUE,prhs,ub_idx ) != SUCCESSFUL_RETURN )
				return;

			if ( smartDimensionCheck( &lbA,nC,1, BT_TRUE,prhs,lbA_idx ) != SUCCESSFUL_RETURN )
				return;
			
			if ( smartDimensionCheck( &ubA,nC,1, BT_TRUE,prhs,ubA_idx ) != SUCCESSFUL_RETURN )
				return;

			/* default value for nWSR */
			nWSRin = 5*(nV+nC);

			/* Check whether x0 and options are specified .*/
			if ( nrhs >= 9 )
			{
				if ((!mxIsEmpty(prhs[8])) && (mxIsStruct(prhs[8])))
					setupOptions( &options,prhs[8],nWSRin,maxCpuTimeIn );

				if ( ( nrhs >= 10 ) && ( !mxIsEmpty(prhs[9]) ) )
				{ 
					/* auxInput specified */
					if ( mxIsStruct(prhs[9]) )
					{
						auxInput_idx = 9;
						x0_idx = -1;
					}
					else
					{
						auxInput_idx = -1;
						x0_idx = 9;
					}
				}
				else
				{
					auxInput_idx = -1;
					x0_idx = -1;
				}
			}
		}


		/* check dimensions and copy auxInputs */
		if ( smartDimensionCheck( &x0,nV,1, BT_TRUE,prhs,x0_idx ) != SUCCESSFUL_RETURN )
			return;

		if ( auxInput_idx >= 0 )
			setupAuxiliaryInputs( prhs[auxInput_idx],nV,nC, &hessianType,&x0,&guessedBounds,&guessedConstraints,&R_for );

		/* convert Cholesky factor to C storage format */
		if ( R_for != 0 )
		{
			R = new real_t[nV*nV];
			convertFortranToC( R_for, nV,nV, R );
		}

		/* check if QP is sparse */
		if ( H_idx >= 0 && !mxIsSparse( prhs[H_idx] ) )
			isSparse = BT_FALSE;
		if ( nC > 0 && A_idx >= 0 && !mxIsSparse( prhs[A_idx] ) )
			isSparse = BT_FALSE;
		
		/* allocate instance */
		handle = allocateQPInstance( nV,nC,hessianType, isSimplyBoundedQp, isSparse, &options );	
		globalQP = getQPInstance( handle );

		/* make a deep-copy of the user-specified Hessian matrix (possibly sparse) */
		if ( H_idx >= 0 )
			setupHessianMatrix(	prhs[H_idx],nV, &(globalQP->H),&(globalQP->Hir),&(globalQP->Hjc),&(globalQP->Hv) );
		
		/* make a deep-copy of the user-specified constraint matrix (possibly sparse) */
		if ( ( nC > 0 ) && ( A_idx >= 0 ) )
			setupConstraintMatrix( prhs[A_idx],nV,nC, &(globalQP->A),&(globalQP->Air),&(globalQP->Ajc),&(globalQP->Av) );

		/* Create output vectors and assign pointers to them. */
		allocateOutputs( nlhs,plhs, nV,nC,1,handle );

		/* Call qpOASES. */
		if ( isSimplyBoundedQp == BT_TRUE )
		{
			QProblemB_init(	handle,
							globalQP->H,g,
							lb,ub,
							nWSRin,maxCpuTimeIn,
							x0,&options,
							nlhs,plhs,
							guessedBounds,R
							);
		}
		else
		{
			SQProblem_init(	handle,
							globalQP->H,g,globalQP->A,
							lb,ub,lbA,ubA,
							nWSRin,maxCpuTimeIn,
							x0,&options,
							nlhs,plhs,
							guessedBounds,guessedConstraints,R
							);
		}

		if (R != 0) delete R;
		return;
	}

	/* 2) Hotstart. */
	if ( ( strcmp( typeString,"h" ) == 0 ) || ( strcmp( typeString,"H" ) == 0 ) )
	{
		/* consistency checks */
		if ( ( nlhs < 1 ) || ( nlhs > 6 ) )
		{
			myMexErrMsgTxt( "ERROR (qpOASES): Invalid number of output arguments!\nType 'help qpOASES_sequence' for further information." );
			return;
		}

		if ( ( nrhs < 5 ) || ( nrhs > 8 ) )
		{
			myMexErrMsgTxt( "ERROR (qpOASES): Invalid number of input arguments!\nType 'help qpOASES_sequence' for further information." );
			return;
		}

		/* determine whether is it a simply bounded QP */
		if ( nrhs < 7 )
			isSimplyBoundedQp = BT_TRUE;
		else
			isSimplyBoundedQp = BT_FALSE;


		if ( ( mxIsDouble( prhs[1] ) == false ) || ( mxIsScalar( prhs[1] ) == false ) )
		{
			myMexErrMsgTxt( "ERROR (qpOASES): Expecting a handle to QP object as second argument!\nType 'help qpOASES_sequence' for further information." );
			return;
		}

		/* get QP instance */
		handle = (uint_t)mxGetScalar( prhs[1] );
		globalQP = getQPInstance( handle );
		if ( globalQP == 0 )
		{
			myMexErrMsgTxt( "ERROR (qpOASES): Invalid handle to QP instance!" );
			return;
		}

		nV = globalQP->getNV();

		g_idx = 2;
		lb_idx = 3;
		ub_idx = 4;

		if (containsNaNorInf( prhs,g_idx, 0 ) == BT_TRUE)
			return;

		if (containsNaNorInf( prhs,lb_idx, 1 ) == BT_TRUE)
			return;

		if (containsNaNorInf( prhs,ub_idx, 1 ) == BT_TRUE)
			return;


		/* Check inputs dimensions and assign pointers to inputs. */
		if ( isSimplyBoundedQp == BT_TRUE )
		{
			nC = 0;

			if ( smartDimensionCheck( &g,nV,1, BT_FALSE,prhs,g_idx ) != SUCCESSFUL_RETURN )
				return;

			if ( smartDimensionCheck( &lb,nV,1, BT_TRUE,prhs,lb_idx ) != SUCCESSFUL_RETURN )
				return;

			if ( smartDimensionCheck( &ub,nV,1, BT_TRUE,prhs,ub_idx ) != SUCCESSFUL_RETURN )
				return;

			/* default value for nWSR */
			nWSRin = 5*nV;

			/* Check whether options are specified .*/
			if ( nrhs == 6 )
				if ( ( !mxIsEmpty( prhs[5] ) ) && ( mxIsStruct( prhs[5] ) ) )
					setupOptions( &options,prhs[5],nWSRin,maxCpuTimeIn );
		}
		else
		{
			nC = globalQP->getNC( );

			lbA_idx = 5;
			ubA_idx = 6;

			if (containsNaNorInf( prhs,lbA_idx, 1 ) == BT_TRUE)
				return;

			if (containsNaNorInf( prhs,ubA_idx, 1 ) == BT_TRUE)
				return;

			if ( smartDimensionCheck( &g,nV,1, BT_FALSE,prhs,g_idx ) != SUCCESSFUL_RETURN )
				return;

			if ( smartDimensionCheck( &lb,nV,1, BT_TRUE,prhs,lb_idx ) != SUCCESSFUL_RETURN )
				return;

			if ( smartDimensionCheck( &ub,nV,1, BT_TRUE,prhs,ub_idx ) != SUCCESSFUL_RETURN )
				return;

			if ( smartDimensionCheck( &lbA,nC,1, BT_TRUE,prhs,lbA_idx ) != SUCCESSFUL_RETURN )
				return;

			if ( smartDimensionCheck( &ubA,nC,1, BT_TRUE,prhs,ubA_idx ) != SUCCESSFUL_RETURN )
				return;

			/* default value for nWSR */
			nWSRin = 5*(nV+nC);

			/* Check whether options are specified .*/
			if ( nrhs == 8 )
				if ( ( !mxIsEmpty( prhs[7] ) ) && ( mxIsStruct( prhs[7] ) ) )
					setupOptions( &options,prhs[7],nWSRin,maxCpuTimeIn );
		}

		/* Create output vectors and assign pointers to them. */
		allocateOutputs( nlhs,plhs, nV,nC );

		/* call qpOASES */
		if ( isSimplyBoundedQp == BT_TRUE )
		{
			QProblemB_hotstart(	handle, g,
								lb,ub,
								nWSRin,maxCpuTimeIn,
								&options,
								nlhs,plhs
								);
		}
		else
		{
			QProblem_hotstart(	handle, g,
								lb,ub,lbA,ubA,
								nWSRin,maxCpuTimeIn,
								&options,
								nlhs,plhs
								);
		}

		return;
	}

	/* 3) Modify matrices. */
	if ( ( strcmp( typeString,"m" ) == 0 ) || ( strcmp( typeString,"M" ) == 0 ) )
	{
		/* consistency checks */
		if ( ( nlhs < 1 ) || ( nlhs > 6 ) )
		{
			myMexErrMsgTxt( "ERROR (qpOASES): Invalid number of output arguments!\nType 'help qpOASES_sequence' for further information." );
			return;
		}

		if ( ( nrhs < 9 ) || ( nrhs > 10 ) )
		{
			myMexErrMsgTxt( "ERROR (qpOASES): Invalid number of input arguments!\nType 'help qpOASES_sequence' for further information." );
			return;
		}

		if ( ( mxIsDouble( prhs[1] ) == false ) || ( mxIsScalar( prhs[1] ) == false ) )
		{
			myMexErrMsgTxt( "ERROR (qpOASES): Expecting a handle to QP object as second argument!\nType 'help qpOASES_sequence' for further information." );
			return;
		}


		/* get QP instance */
		handle = (uint_t)mxGetScalar( prhs[1] );
		globalQP = getQPInstance( handle );
		if ( globalQP == 0 )
		{
			myMexErrMsgTxt( "ERROR (qpOASES): Invalid handle to QP instance!" );
			return;
		}

		/* Check inputs dimensions and assign pointers to inputs. */
		g_idx = 3;
		
		if ( mxIsEmpty(prhs[2]) == 1 )
		{
			H_idx = -1;
			nV = (uint_t)mxGetM( prhs[ g_idx ] ); /* if Hessian is empty, row number of gradient vector */
		}
		else
		{
			H_idx = 2;
			nV = (uint_t)mxGetM( prhs[ H_idx ] ); /* row number of Hessian matrix */
		}
		
		A_idx = 4;
		nC = (uint_t)mxGetM( prhs[ A_idx ] ); /* row number of constraint matrix */
				
		lb_idx = 5;
		ub_idx = 6;
		lbA_idx = 7;
		ubA_idx = 8;


		/* ensure that data is given in double precision */
		if ( ( ( H_idx >= 0 ) && ( mxIsDouble( prhs[H_idx] ) == 0 ) ) ||
			 ( mxIsDouble( prhs[g_idx] ) == 0 ) ||
			 ( mxIsDouble( prhs[A_idx] ) == 0 ) )
		{
			myMexErrMsgTxt( "ERROR (qpOASES): All data has to be provided in real_t precision!" );
			return;
		}

		/* check if supplied data contains 'NaN' or 'Inf' */
		if (containsNaNorInf(prhs,H_idx, 0) == BT_TRUE)
			return;

		if (containsNaNorInf( prhs,g_idx, 0 ) == BT_TRUE)
			return;

		if (containsNaNorInf( prhs,A_idx, 0 ) == BT_TRUE)
			return;

		if (containsNaNorInf( prhs,lb_idx, 1 ) == BT_TRUE)
			return;

		if (containsNaNorInf( prhs,ub_idx, 1 ) == BT_TRUE)
			return;

		if (containsNaNorInf( prhs,lbA_idx, 1 ) == BT_TRUE)
			return;

		if (containsNaNorInf( prhs,ubA_idx, 1 ) == BT_TRUE)
			return;

		/* Check that dimensions are consistent with existing QP instance */
		if (nV != (uint_t) globalQP->getNV () || nC != (uint_t) globalQP->getNC ())
		{
			myMexErrMsgTxt( "ERROR (qpOASES): QP dimensions must be constant during a sequence! Try creating a new QP instance instead." );
			return;
		}

		if ( ( H_idx >= 0 ) && ( ( mxGetN( prhs[ H_idx ] ) != nV ) || ( mxGetM( prhs[ H_idx ] ) != nV ) ) )
		{
			myMexErrMsgTxt( "ERROR (qpOASES): Hessian matrix dimension mismatch!" );
			return;
		}

		if ( ( mxGetN( prhs[ A_idx ] ) != 0 ) && ( mxGetN( prhs[ A_idx ] ) != nV ) )
		{
			myMexErrMsgTxt( "ERROR (qpOASES): Constraint matrix dimension mismatch!" );
			return;
		}

		if ( smartDimensionCheck( &g,nV,1, BT_FALSE,prhs,g_idx ) != SUCCESSFUL_RETURN )
			return;

		if ( smartDimensionCheck( &lb,nV,1, BT_TRUE,prhs,lb_idx ) != SUCCESSFUL_RETURN )
			return;

		if ( smartDimensionCheck( &ub,nV,1, BT_TRUE,prhs,ub_idx ) != SUCCESSFUL_RETURN )
			return;

		if ( smartDimensionCheck( &lbA,nC,1, BT_TRUE,prhs,lbA_idx ) != SUCCESSFUL_RETURN )
			return;

		if ( smartDimensionCheck( &ubA,nC,1, BT_TRUE,prhs,ubA_idx ) != SUCCESSFUL_RETURN )
			return;

		/* default value for nWSR */
		nWSRin = 5*(nV+nC);

		/* Check whether options are specified .*/
		if ( nrhs > 9 )
			if ( ( !mxIsEmpty( prhs[9] ) ) && ( mxIsStruct( prhs[9] ) ) )
				setupOptions( &options,prhs[9],nWSRin,maxCpuTimeIn );

		globalQP->deleteQPMatrices( );

		/* make a deep-copy of the user-specified Hessian matrix (possibly sparse) */
		if ( H_idx >= 0 )
			setupHessianMatrix(	prhs[H_idx],nV, &(globalQP->H),&(globalQP->Hir),&(globalQP->Hjc),&(globalQP->Hv) );

		/* make a deep-copy of the user-specified constraint matrix (possibly sparse) */
		if ( ( nC > 0 ) && ( A_idx >= 0 ) )
			setupConstraintMatrix( prhs[A_idx],nV,nC, &(globalQP->A),&(globalQP->Air),&(globalQP->Ajc),&(globalQP->Av) );

		/* Create output vectors and assign pointers to them. */
		allocateOutputs( nlhs,plhs, nV,nC );

		/* Call qpOASES */
		SQProblem_hotstart(	handle, globalQP->H,g,globalQP->A,
							lb,ub,lbA,ubA,
							nWSRin,maxCpuTimeIn,
							&options,
							nlhs,plhs
							);

		return;
	}

	/* 4) Solve current equality constrained QP. */
	if ( ( strcmp( typeString,"e" ) == 0 ) || ( strcmp( typeString,"E" ) == 0 ) )
	{
		/* consistency checks */
		if ( ( nlhs < 1 ) || ( nlhs > 4 ) )
		{
			myMexErrMsgTxt( "ERROR (qpOASES): Invalid number of output arguments!\nType 'help qpOASES_sequence' for further information." );
			return;
		}

		if ( ( nrhs < 7 ) || ( nrhs > 8 ) )
		{
			myMexErrMsgTxt( "ERROR (qpOASES): Invalid number of input arguments!\nType 'help qpOASES_sequence' for further information." );
			return;
		}

		if ( ( mxIsDouble( prhs[1] ) == false ) || ( mxIsScalar( prhs[1] ) == false ) )
		{
			myMexErrMsgTxt( "ERROR (qpOASES): Expecting a handle to QP object as second argument!\nType 'help qpOASES_sequence' for further information." );
			return;
		}

		/* get QP instance */
		handle = (uint_t)mxGetScalar( prhs[1] );
		globalQP = getQPInstance( handle );
		if ( globalQP == 0 )
		{
			myMexErrMsgTxt( "ERROR (qpOASES): Invalid handle to QP instance!" );
			return;
		}

		/* Check inputs dimensions and assign pointers to inputs. */
		int_t nRHS = (int_t)mxGetN(prhs[2]);
		nV = globalQP->getNV( );
		nC = globalQP->getNC( );
		real_t *x_out, *y_out;

		g_idx = 2;
		lb_idx = 3;
		ub_idx = 4;
		lbA_idx = 5;
		ubA_idx = 6;

		/* check if supplied data contains 'NaN' or 'Inf' */
		if (containsNaNorInf(prhs,g_idx, 0) == BT_TRUE)
			return;

		if (containsNaNorInf( prhs,lb_idx, 1 ) == BT_TRUE)
			return;

		if (containsNaNorInf( prhs,ub_idx, 1 ) == BT_TRUE)
			return;

		if (containsNaNorInf( prhs,lbA_idx, 1 ) == BT_TRUE)
			return;

		if (containsNaNorInf( prhs,ubA_idx, 1 ) == BT_TRUE)
			return;

		if ( smartDimensionCheck( &g,nV,nRHS, BT_FALSE,prhs,g_idx ) != SUCCESSFUL_RETURN )
			return;

		if ( smartDimensionCheck( &lb,nV,nRHS, BT_TRUE,prhs,lb_idx ) != SUCCESSFUL_RETURN )
			return;

		if ( smartDimensionCheck( &ub,nV,nRHS, BT_TRUE,prhs,ub_idx ) != SUCCESSFUL_RETURN )
			return;

		if ( smartDimensionCheck( &lbA,nC,nRHS, BT_TRUE,prhs,lbA_idx ) != SUCCESSFUL_RETURN )
			return;

		if ( smartDimensionCheck( &ubA,nC,nRHS, BT_TRUE,prhs,ubA_idx ) != SUCCESSFUL_RETURN )
			return;

		/* Check whether options are specified .*/
		if ( ( nrhs == 8 ) && ( !mxIsEmpty( prhs[7] ) ) && ( mxIsStruct( prhs[7] ) ) )
		{
			nWSRin = 5*(nV+nC);
			setupOptions( &options,prhs[7],nWSRin,maxCpuTimeIn );
			globalQP->sqp->setOptions( options );
		}

		/* Create output vectors and assign pointers to them. */
		plhs[0] = mxCreateDoubleMatrix( nV, nRHS, mxREAL );
		x_out = mxGetPr(plhs[0]);
		if (nlhs >= 2)
		{
			plhs[1] = mxCreateDoubleMatrix( nV+nC, nRHS, mxREAL );
			y_out = mxGetPr(plhs[1]);

			if (nlhs >= 3)
			{
				plhs[2] = mxCreateDoubleMatrix( nV, nRHS, mxREAL );
				real_t* workingSetB = mxGetPr(plhs[2]);
				globalQP->sqp->getWorkingSetBounds(workingSetB);

				if ( nlhs >= 4 )
				{
					plhs[3] = mxCreateDoubleMatrix( nC, nRHS, mxREAL );
					real_t* workingSetC = mxGetPr(plhs[3]);
					globalQP->sqp->getWorkingSetConstraints(workingSetC);
				}
			}
		}
		else
			y_out = new real_t[nV+nC];

		/* Solve equality constrained QP */
		returnValue returnvalue = globalQP->sqp->solveCurrentEQP( nRHS,g,lb,ub,lbA,ubA, x_out,y_out );

		if (nlhs < 2)
			delete[] y_out;

		if (returnvalue != SUCCESSFUL_RETURN)
		{
			char msg[MAX_STRING_LENGTH];
			snprintf(msg, MAX_STRING_LENGTH, "ERROR (qpOASES): Couldn't solve current EQP (code %d)!", returnvalue);
			myMexErrMsgTxt(msg);
			return;
		}

		return;
	}

	/* 5) Cleanup. */
	if ( ( strcmp( typeString,"c" ) == 0 ) || ( strcmp( typeString,"C" ) == 0 ) )
	{		
		/* consistency checks */
		if ( nlhs != 0 )
		{
			myMexErrMsgTxt( "ERROR (qpOASES): Invalid number of output arguments!\nType 'help qpOASES_sequence' for further information." );
			return;
		}

		if ( nrhs != 2 )
		{
			myMexErrMsgTxt( "ERROR (qpOASES): Invalid number of input arguments!\nType 'help qpOASES_sequence' for further information." );
			return;
		}

		if ( ( mxIsDouble( prhs[1] ) == false ) || ( mxIsScalar( prhs[1] ) == false ) )
		{
			myMexErrMsgTxt( "ERROR (qpOASES): Expecting a handle to QP object as second argument!\nType 'help qpOASES_sequence' for further information." );
			return;
		}

		/* Cleanup SQProblem instance. */
		handle = (uint_t)mxGetScalar( prhs[1] );
		deleteQPInstance( handle );
		
		return;
	}

}
Exemple #14
0
/* Convert an array of MATLAB keypoint structs to a Keypoint_store. mx must be
 * a vector, and each struct element must have type double. */
int mx2kp(const mxArray *const mx, Keypoint_store *const kp) {

        const mwSize *mxDims, *coordsDims, *oriDims;
        mxArray *mxCoords, *mxScale, *mxOri, *mxOctave, *mxLevel;
        mwSize numKp, kpRows, kpCols;
        int i, coordsNum, scaleNum, oriNum, octaveNum, levelNum;

        // Verify the number of dimensions
        if (mxGetNumberOfDimensions(mx) != 2)
                return SIFT3D_FAILURE;

        // Parse the input dimensions
        mxDims = mxGetDimensions(mx);
        kpRows = mxDims[0];
        kpCols = mxDims[1];
        if (kpRows == 1)
                numKp = kpCols; 
        else if (kpCols == 1)
                numKp = kpRows;
        else
                return SIFT3D_FAILURE; 

        // Verify the struct size 
        if (!mxIsStruct(mx) || mxGetNumberOfFields(mx) != kpNFields)
                return SIFT3D_FAILURE;

        // Get the field indices
        if ((coordsNum = mxGetFieldNumber(mx, COORDS_NAME)) < 0 ||
                (scaleNum = mxGetFieldNumber(mx, SCALE_NAME)) < 0 ||
                (oriNum = mxGetFieldNumber(mx, ORI_NAME)) < 0 ||
                (octaveNum = mxGetFieldNumber(mx, OCTAVE_NAME)) < 0 || 
                (levelNum = mxGetFieldNumber(mx, LEVEL_NAME)) < 0)
                return SIFT3D_FAILURE;

        // Get the fields of the first keypoint
        if ((mxCoords = mxGetFieldByNumber(mx, 0, coordsNum)) == NULL ||
                (mxScale = mxGetFieldByNumber(mx, 0, scaleNum)) == NULL ||
                (mxOri = mxGetFieldByNumber(mx, 0, oriNum)) == NULL ||
                (mxOctave = mxGetFieldByNumber(mx, 0, octaveNum)) == NULL ||
                (mxLevel = mxGetFieldByNumber(mx, 0, levelNum)) == NULL)
                return SIFT3D_FAILURE;

        // Verify the type
        if (!isDouble(mxCoords) ||
                !isDouble(mxScale) ||
                !isDouble(mxOri) ||
                !isDouble(mxOctave) ||
                !isDouble(mxLevel))
                return SIFT3D_FAILURE;

        // Verify the number of dimensions
        if (mxGetNumberOfDimensions(mxCoords) != 2 ||
                mxGetNumberOfDimensions(mxScale) != 2 ||
                mxGetNumberOfDimensions(mxOri) != 2 ||
                mxGetNumberOfDimensions(mxOctave) != 2 ||
                mxGetNumberOfDimensions(mxLevel) != 2)
                return SIFT3D_FAILURE;

        // Verify the scalars
        if (!mxIsScalar(mxScale) ||
                !mxIsScalar(mxOctave) ||
                !mxIsScalar(mxLevel))
                return SIFT3D_FAILURE;

        // Verify the coordinate vector dimensions 
        coordsDims = mxGetDimensions(mxCoords);
        if ((coordsDims[0] != 1 && coordsDims[1] != 1) ||
                coordsDims[0] * coordsDims[1] != IM_NDIMS)
                return SIFT3D_FAILURE;


        // Verify the orientation matrix dimensions
        oriDims = mxGetDimensions(mxOri);
        if (oriDims[0] != IM_NDIMS || oriDims[1] != IM_NDIMS)
                return SIFT3D_FAILURE;

        // Allocate space in the keypoint store
        if (resize_Keypoint_store(kp, (size_t) numKp))
                return SIFT3D_FAILURE;

        // Copy the data
        for (i = 0; i < (int) numKp; i++) {

                double *coordsData;

                Keypoint *const key = kp->buf + i;
                const mwIndex idx = (mwIndex) i;

                // Get the matrices
                if ((mxCoords = mxGetFieldByNumber(mx, idx, coordsNum)) == 
                                NULL ||
                        (mxScale = mxGetFieldByNumber(mx, idx, scaleNum)) == 
                                NULL ||
                        (mxOri = mxGetFieldByNumber(mx, idx, oriNum)) == 
                                NULL ||
                        (mxOctave = mxGetFieldByNumber(mx, idx, octaveNum)) == 
                                NULL ||
                        (mxLevel = mxGetFieldByNumber(mx, idx, levelNum)) == 
                                NULL)
                        return SIFT3D_FAILURE;


                // Copy the scalars
                key->sd = mxGetScalar(mxScale);
                key->o = (int) mxGetScalar(mxOctave);
                key->s = (int) mxGetScalar(mxLevel);

                // Get the coordinate data
                if ((coordsData = mxGetData(mxCoords)) == NULL)
                        return SIFT3D_FAILURE;

                // Copy the coordinate vector
                key->xd = coordsData[0];
                key->yd = coordsData[1];
                key->zd = coordsData[2];

		// Initialize the orientation matrix
		if (init_Mat_rm_p(&key->R, key->r_data, IM_NDIMS, IM_NDIMS, 
                        FLOAT, SIFT3D_FALSE))
			return SIFT3D_FAILURE;

                // Copy the orientation matrix
                if (mx2mat(mxOri, &key->R))
                        return SIFT3D_FAILURE;
        }

        return SIFT3D_SUCCESS;
}
Exemple #15
0
void mexFunction (int nlhs, mxArray * plhs[], int nrhs, const mxArray * prhs[]) {
		int rc, enabled;
		UINT32_T masterid = 0;
		time_t timallow = 0;
		UINT64_T memallow = 0;
		UINT64_T rss, vs;

		/* this function will be called upon unloading of the mex file */
		mexAtExit(exitFun);

		if (nrhs<3)
				mexErrMsgTxt ("invalid number of input arguments");

		if (mxIsScalar(prhs[0]))
				masterid = mxGetScalar(prhs[0]);
		else if (mxIsEmpty(prhs[0]))
				masterid = 0;
		else
				mexErrMsgTxt ("invalid input argument #1");

		if (mxIsScalar(prhs[1]))
				timallow = mxGetScalar(prhs[1]);
		else if (mxIsEmpty(prhs[1]))
				timallow = 0;
		else
				mexErrMsgTxt ("invalid input argument #2");

		if (mxIsScalar(prhs[2]))
				memallow = mxGetScalar(prhs[2]);
		else if (mxIsEmpty(prhs[2]))
				memallow = 0;
		else
				mexErrMsgTxt ("invalid input argument #3");

		if (masterid!=0 || timallow!=0 || memallow!=0) {
				enabled = 1;
				/* in this case the mex file is not allowed to be cleared from memory */
				if (!mexIsLocked())
						mexLock(); 
		}

		if (masterid==0 && timallow==0 && memallow==0) {
				enabled = 0;
				/* in this case the mex file is allowed to be cleared from memory */
#ifdef SOLUTION_FOR_UNEXPLAINED_CRASH
				if (mexIsLocked())
						mexUnlock(); 
#endif
		}

		if (!peerInitialized) {
				mexPrintf("watchdog: init\n");
				peerinit(NULL);
				peerInitialized = 1;
		}

		/* start the discover thread */
		pthread_mutex_lock(&mutexstatus);
		if (!discoverStatus) {
				pthread_mutex_unlock(&mutexstatus);
				mexPrintf("watchdog: spawning discover thread\n");
				rc = pthread_create(&discoverThread, NULL, discover, (void *)NULL);
				if (rc)
						mexErrMsgTxt("problem with return code from pthread_create()");
				else {
						/* wait until the thread has properly started */
						pthread_mutex_lock(&mutexstatus);
						if (!discoverStatus)
								pthread_cond_wait(&condstatus, &mutexstatus);
						pthread_mutex_unlock(&mutexstatus);
				}
		}
		else {
				pthread_mutex_unlock(&mutexstatus);
		}

		/* start the expire thread */
		pthread_mutex_lock(&mutexstatus);
		if (!expireStatus) {
				pthread_mutex_unlock(&mutexstatus);
				mexPrintf("watchdog: spawning expire thread\n");
				rc = pthread_create(&expireThread, NULL, expire, (void *)NULL);
				if (rc)
						mexErrMsgTxt("problem with return code from pthread_create()");
				else {
						/* wait until the thread has properly started */
						pthread_mutex_lock(&mutexstatus);
						if (!expireStatus)
								pthread_cond_wait(&condstatus, &mutexstatus);
						pthread_mutex_unlock(&mutexstatus);
				}
		}
		else {
				pthread_mutex_unlock(&mutexstatus);
		}

		if (timallow>0) {
				/* timallow should be relative to now */
				timallow += time(NULL);
		}

		if (memallow>0) {
				/* memallow should be in absolute numbers, add the current memory footprint */
				getmem(&rss, &vs);
				memallow += rss;
		}

		/* enable the watchdog: the expire thread will exit if the master is not seen any more */
		pthread_mutex_lock(&mutexwatchdog);
		watchdog.enabled  = enabled;
		watchdog.evidence = 0;
		watchdog.masterid = masterid;
		watchdog.memory   = memallow;
		watchdog.time     = timallow;
		pthread_mutex_unlock(&mutexwatchdog);

		if (enabled)
				mexPrintf("watchdog: enabled for masterid = %lu, time = %d, memory = %lu\n", masterid, timallow, memallow);
		else
				mexPrintf("watchdog: disabled for masterid = %lu, time = %d, memory = %lu\n", masterid, timallow, memallow);

		return;
} /* main */
Exemple #16
0
static double real_check(int arg, const mxArray *prhs[], const char *argname){
	if(!mxIsNumeric(prhs[arg]) || !mxIsScalar(prhs[arg]) || mxIsComplex(prhs[arg])){
		mexErrMsgIdAndTxt("S4:invalidInput", "Expected number.");
	}
	return *(mxGetPr(prhs[arg]));
}
void mexFunction( int nlhs, mxArray *plhs[] , int nrhs, const mxArray *prhs[] )
{
    int ny = 24 , nx = 24;
	double *scale;
	int Nscale = 0;
	int nF;
	unsigned int *F;
	int *dimsF;

    /* Input 1  */    
    if ((nrhs > 0) && (int)mxIsScalar(prhs[0]) )
    {        
        ny        = (int) mxGetScalar(prhs[0]);
    }
	if(ny < 3)
	{
		mexErrMsgTxt("ny must be >= 3");
	}
    
    /* Input 2  */
    
    if ((nrhs > 1) && (int)mxIsScalar(prhs[1]) )    
    {        
        nx        = (int) mxGetScalar(prhs[1]);
		if(nx < 3)
		{
			mexErrMsgTxt("nx must be >= 3");		
		}
    }
	else
	{	
		nx        = ny;	
	}
    if ((nrhs > 2) && !mxIsEmpty(prhs[2]) )
	{
		if(mxGetM(prhs[2]) !=2)
		{		
			mexErrMsgTxt("scale must be a (2 x Nscale) matrix");	
		}
		scale     = mxGetPr(prhs[2]);
		Nscale    = mxGetN(prhs[2]);
	}

	nF            = number_mblbp_features(ny , nx , scale , Nscale);

    /*------------------------ Output ----------------------------*/

	dimsF         = (int *)mxMalloc(2*sizeof(int));
	dimsF[0]      = 5;
	dimsF[1]      = nF;
	plhs[0]       = mxCreateNumericArray(2 , dimsF , mxUINT32_CLASS , mxREAL);
	F             = (unsigned int *)mxGetPr(plhs[0]);

    /*------------------------ Main Call ----------------------------*/
      
    mblbp_featlist(ny , nx , scale , Nscale, F);
    
	/*----------------- Free Memory --------------------------------*/

	mxFree(dimsF);
}
Exemple #18
0
static void S4mex_Simulation_New(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]){
	S4_real Lr[4] = { 0, 0, 0, 0 };
	int nbases = 1;
	int is1d = 0;
	
	if(3 != nrhs){
		mexErrMsgIdAndTxt("S4:invalidInput", "Expected two additional arguments to 'new'.");
	}
	
	if(!mxIsNumeric(prhs[1]) || mxIsComplex(prhs[1]) || !((is1d = mxIsScalar(prhs[1])) || (2 == mxGetM(prhs[1]) && 2 == mxGetN(prhs[1])))){
		mexErrMsgIdAndTxt("S4:invalidInput", "First additional argument (lattice) must be scalar or a 2x2 matrix.");
	}
	if(is1d){
		Lr[0] = mxGetScalar(prhs[1]);
	}else{
		double *p = mxGetPr(prhs[1]);
		Lr[0] = p[0];
		Lr[1] = p[1];
		Lr[2] = p[2];
		Lr[3] = p[3];
		if(0 == Lr[1] && 0 == Lr[2] && 0 == Lr[3]){
			is1d = 1;
		}
	}
	
	if(!mxIsNumeric(prhs[2]) || mxIsComplex(prhs[2]) || !mxIsScalar(prhs[2])){
		mexErrMsgIdAndTxt("S4:invalidInput", "Second additional argument (#bases) must be a positive real integer.");
	}
	if(mxIsDouble(prhs[2])){
		double *p = mxGetPr(prhs[2]);
		nbases = (int)p[0];
	}else{
		void *p = mxGetData(prhs[2]);
		switch(mxGetClassID(prhs[2])){
		case mxSINGLE_CLASS:
			{ float *pp = (float*)p; nbases = (int)pp[0]; break; }
        case mxINT8_CLASS:
			{ char *pp = (char*)p; nbases = (int)pp[0]; break; }
        case mxUINT8_CLASS:
			{ unsigned char *pp = (unsigned char*)p; nbases = (int)pp[0]; break; }
        case mxINT16_CLASS:
			{ short *pp = (short*)p; nbases = (int)pp[0]; break; }
        case mxUINT16_CLASS:
			{ unsigned short *pp = (unsigned short*)p; nbases = (int)pp[0]; break; }
        case mxINT32_CLASS:
			{ int *pp = (int*)p; nbases = (int)pp[0]; break; }
        case mxUINT32_CLASS:
			{ unsigned int *pp = (unsigned int*)p; nbases = (int)pp[0]; break; }
        case mxINT64_CLASS:
			{ long long *pp = (long long*)p; nbases = (int)pp[0]; break; }
        case mxUINT64_CLASS:
			{ unsigned long long *pp = (unsigned long long*)p; nbases = (int)pp[0]; break; }
		default:
			mexErrMsgIdAndTxt("S4:invalidInput", "Second additional argument (#bases) is an unknown class.");
			break;
		}
	}
	if(nbases < 1){
		mexErrMsgIdAndTxt("S4:invalidInput", "Second additional argument (#bases) must be a positive integer.");
	}
	
	MEX_TRACE("> S4mex_Simulation_New(lattice = [%f, %f; %f, %f], bases = %d)\n", Lr[0], Lr[1], Lr[2], Lr[3], nbases);
	
	{
		int *p;
		S4_Simulation *S = S4_Simulation_New(Lr, nbases, NULL);
		plhs[0] = mxCreateNumericMatrix(1, 1, mxINT32_CLASS, mxREAL);
		p = mxGetData(plhs[0]);
		p[0] = S_add(S);
		SS[p[0]].is1d = is1d;
		
		MEX_TRACE("< S4mex_Simulation_New %d\n", p[0]);
	}
}
void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])  {
    // This function is called like: [tSubsampledAndDoubledUp, ySubsampledAndDoubledUp]=minMaxDownsampleMex(t,y,r)
    //   t a double column vector of length nScans
    //   y a nScans x nChannels matrix of doubles
    //   r a double scalar holding a positive integer value, or empty
    //
    // If r is empty, is means "don't downsample", just return t and y as-is.

    // Load in the arguments, checking them thoroughly

    // prhs[0]: t
    if ( mxIsDouble(prhs[0]) && !mxIsComplex(prhs[0]) && mxGetNumberOfDimensions(prhs[0])==2 && mxGetN(prhs[0])==1 )  {
        // all is well
    }
    else  {
        mexErrMsgIdAndTxt("ws:minMaxDownSample:tNotRight", 
                          "Argument t must be a non-complex double column vector.");
    }
    mwSize nScans = mxGetM(prhs[0]) ;
    double *t = mxGetPr(prhs[0]);  // "Convert" to a C++ array
    
    // prhs[1]: y
    //bool isDouble = mxIsDouble(prhs[1]) ;
    //bool isReal = mxIsComplex(prhs[1]) ;
    //mwSize nDims = mxGetNumberOfDimensions(prhs[1]) ;
    //mwSize nRows = mxGetM(prhs[1]) ;
    if ( mxIsDouble(prhs[1]) && !mxIsComplex(prhs[1]) && mxGetNumberOfDimensions(prhs[1])==2 && mxGetM(prhs[1])==nScans )  {
        // all is well
    }
    else  {
        mexErrMsgIdAndTxt("ws:minMaxDownSample:yNotRight", 
                          "Argument y must be a non-complex double matrix with the same number of rows as t.");
    }
    mwSize nChannels = mxGetN(prhs[1]);
    double *y = mxGetPr(prhs[1]);  // "Convert" to a C++ array (sort of: it's still in col-major order)
    
    // prhs[1]: r
    bool rIsEmpty ;
    if ( mxIsEmpty(prhs[2]) )  {
        // this is always OK
        rIsEmpty = true ;
    } else {
        rIsEmpty = false ;
        // non-empty
        if ( mxIsDouble(prhs[2]) && !mxIsComplex(prhs[2]) && mxIsScalar(prhs[2]) )  {
            // this is ok
        }  
        else  {
            mexErrMsgIdAndTxt("ws:minMaxDownSample:rNotRight", 
                              "Argument r must be empty or be a non-complex double scalar.");
        }
    }

    double rAsDouble = -1.0 ;  // should not be used if rIsEmpty
    mwSize r = 0 ;  // should not be used if rIsEmpty
    if (!rIsEmpty)  {
        rAsDouble = mxGetScalar(prhs[2]) ;
        if ( floor(rAsDouble)!=ceil(rAsDouble) || rAsDouble<=0 )  {
            mexErrMsgIdAndTxt("ws:minMaxDownSample:rNotRight", 
                              "Argument r, if a scalar, must be a positive integer.");
        }
        r = (mwSize) rAsDouble ;
    }

    // At this point, all args have been read and validated

    int effectiveNLHS = (nlhs>1)?nlhs:1 ;  // even if nlhs==0, still safe to assign to plhs[0], and should do this, so ans gets assigned
    double* tSubsampledAndDoubled ;
    double* ySubsampledAndDoubled ;

    double* target ;
    double* targetEnd ;
    double* source ;

    if (rIsEmpty)  {
        // just copy the inputs to the outputs
        if ( effectiveNLHS>=1 )  {
            plhs[0] = mxCreateDoubleMatrix(nScans, (mwSize)1, mxREAL) ;
            tSubsampledAndDoubled = mxGetPr(plhs[0]);
            //for (mwSize i=0 ; i<nScans; ++i)  {
            //    tSubsampledAndDoubled[i] = t[i] ;
            //}
            target = tSubsampledAndDoubled ;
            source = t ;
            targetEnd = tSubsampledAndDoubled + nScans ;
            while (target!=targetEnd)  {
                *target = *source ;
                ++target ;
                ++source ;
            }
        }
        if ( effectiveNLHS>=2 )  {
            plhs[1] = mxCreateDoubleMatrix(nScans, nChannels, mxREAL) ;
            ySubsampledAndDoubled = mxGetPr(plhs[1]);
            target = ySubsampledAndDoubled ;
            source = y ;
            mwSize nElements = nChannels * nScans ;
            targetEnd = ySubsampledAndDoubled + nElements ;
            //for (mwSize i=0 ; i<nElements; ++i, ++source, ++target)  {
            //    *target = *source ;
            //}
            while (target!=targetEnd)  {
                *target = *source ;
                ++target ;
                ++source ;
            }
        }
        return ;
    }

    // If get here, r is a scalar, which is the interesting case

    mwSize nScansSubsampled = (mwSize) (ceil(((double)nScans)/rAsDouble)) ;

    // Create the subsampled timeline, decimating t by the factor r
    mxArray* tSubsampledMxArray = mxCreateDoubleMatrix(nScansSubsampled, (mwSize)1, mxREAL) ;
    double* tSubsampled = mxGetPr(tSubsampledMxArray) ;
    source = t ;
    target = tSubsampled ;
    //for (mwSize i=0 ; i<nScansSubsampled; ++i, source+=r, ++target) {
    //    *target = *source ;
    //}
    targetEnd = tSubsampled + nScansSubsampled ;
    while (target!=targetEnd)  {
        *target = *source ;
        ++target ;
        source+=r ;
    }

    // Now "double-up" time, with two copies of each time point
    mxArray* tSubsampledAndDoubledUpMxArray = mxCreateDoubleMatrix(2*nScansSubsampled, (mwSize)1, mxREAL) ;
    double* tSubsampledAndDoubledUp = mxGetPr(tSubsampledAndDoubledUpMxArray) ;
    target = tSubsampledAndDoubledUp ;
    source = tSubsampled ;
    targetEnd = tSubsampledAndDoubledUp + 2*nScansSubsampled ;
    //for (source = tSubsampled; source!=(tSubsampled+nScansSubsampled); ++source) {
    while (target!=targetEnd)  {
        double tSource = *source ;
        *target = tSource ;
        ++target ;
        *target = tSource ;
        ++target ;
        ++source ;
    }

    // Now set up for return (always assign this one)
    plhs[0] = tSubsampledAndDoubledUpMxArray ;

    // If that's all the return values the user wanted, return now
    if ( effectiveNLHS<2 )  {
        return ;
    }

    // Subsample y at each subsampled scan, getting the max and the min of the r samples for that point in the original y
    mxArray* ySubsampledMaxMxArray = mxCreateDoubleMatrix(nScansSubsampled, nChannels, mxREAL) ;
    double* ySubsampledMax = mxGetPr(ySubsampledMaxMxArray) ;
    mxArray* ySubsampledMinMxArray = mxCreateDoubleMatrix(nScansSubsampled, nChannels, mxREAL) ;
    double* ySubsampledMin = mxGetPr(ySubsampledMinMxArray) ;
    double maxSoFar ;
    double minSoFar ;
    mwSize iWithinTheR ;
    //mwSize iScanSubsampled ;
    double* minTarget ;
    double* maxTarget ;
    double* sourceEnd ;
    source = y ;
    double yThis ;  // for holding the current value of the y array
    for (mwSize iChannel=0 ; iChannel<nChannels; ++iChannel)  {
        maxTarget = ySubsampledMax + iChannel*nScansSubsampled ;
        minTarget = ySubsampledMin + iChannel*nScansSubsampled ;
        source = y + iChannel*nScans ;
        sourceEnd = source + nScans ;
        iWithinTheR = 0 ;
        while (source!=sourceEnd)  { 
            if (iWithinTheR==0)  {
                yThis = *source ;
                maxSoFar = yThis ;
                minSoFar = yThis ;
            } else {
                yThis = *source ;
                if (yThis>maxSoFar)  {
                    maxSoFar = yThis ;
                } else {
                    if (yThis<minSoFar)  {
                        minSoFar = yThis ;
                    }
                }
            }
            if (iWithinTheR+1 == r)  {
                // Write to the elements of the subsampled arrays
                *maxTarget = maxSoFar ;
                *minTarget = minSoFar ;
                ++minTarget ;
                ++maxTarget ;
                iWithinTheR = 0 ;
            } else {
                ++iWithinTheR ;
            }
            ++source ;
        }
        if ( iWithinTheR!=0 && nScansSubsampled>0 )  {  
            // This means r does not evenly divide nScans, so need to write the current minSoFar, maxSoFar to last el of ySubsampledMax, ySampledMin
            *maxTarget = maxSoFar ;
            *minTarget = minSoFar ;
        }
    }

    // Now for the y's, max, min, max, min, etc.
    mwSize nScansSubsampledAndDoubledUp = 2*nScansSubsampled ;
    mxArray* ySubsampledAndDoubledUpMxArray = mxCreateDoubleMatrix(nScansSubsampledAndDoubledUp, (mwSize)nChannels, mxREAL) ;
    double* ySubsampledAndDoubledUp = mxGetPr(ySubsampledAndDoubledUpMxArray) ;
    target = ySubsampledAndDoubledUp ;
    targetEnd = ySubsampledAndDoubledUp + nChannels*nScansSubsampledAndDoubledUp ;
    double* minSource = ySubsampledMin ;
    double* maxSource = ySubsampledMax ;
    //for (mwSize iChannel=0 ; iChannel<nChannels; ++iChannel)  {
    //    for (mwSize iScanSubsampled=0; iScanSubsampled<nScansSubsampled; ++iScanSubsampled)  {
    //        //*(ySubsampledAndDoubledUp+iChannel*nScansSubsampled+2*iScanSubsampled)   = *(ySubsampledMax+iChannel*nScansSubsampled+iScanSubsampled) ;
    //        *target = *maxSource ;
    //        ++maxSource ;
    //        ++target ;
    //        //*(ySubsampledAndDoubledUp+iChannel*nScansSubsampled+2*iScanSubsampled+1) = *(ySubsampledMin+iChannel*nScansSubsampled+iScanSubsampled) ;
    //        *target = *minSource ;
    //        ++minSource ;
    //        ++target ;
    //    }
    //}
    while (target!=targetEnd)  {
        *target = *maxSource ;
        ++maxSource ;
        ++target ;
        *target = *minSource ;
        ++minSource ;
        ++target ;
    }

    // Now set up for return
    plhs[1] = ySubsampledAndDoubledUpMxArray ;
}