/**
 * Maximum full pseudolikelihood method to estimate parameters.
 *
 * Minimization done by gradient L-BFGS with wolfe
 *
 * @param [in] p Number of variables
 * @param [in,out] mu pdim Real vector. Mu parameter of the mv-vm dist
 * @param [in,out] kappa pdim Real vector. Kappa parameter of the mv-vm dist
 * @param [in,out] lambda pxp Real matrix on row-leading order. Lambda parameter of the mv-vm dist
 * @param [in] n Number of samples
 * @param [in] samples
 * @param [in] phi Prior matrix
 * @param [in] H Confidence matrix
 * @param [in] verbose
 * @param [in] prec
 * @param [in] tol 
 * @param [in] mprec
 * @param [in] lower
 * @param [in] upper
 * @param [in] bounded
 *
 * @returns Natural logarithm of the pseudolikelihood of the fitted distribution (aprox)
 */
double mvvonmises_lbfgs_fit(int p, double* mu, double* kappa, double* lambda, int n, double *samples, double* phi, double *H,
                            int verbose, double prec, double tol, int mprec, double *lower, double *upper, int *bounded ){

    uint_fast16_t i,j,k;

    /* Von Mises computation parameters*/
    long double *S,*C,*ro;
    double *d_kappa,*d_lambda;

    S = malloc(sizeof(long double)*n*p*3);
    C = S + (n*p);
    ro = C + (n*p);

    /* Set up instance */
    multiCircularMean(n,p,samples,mu);

    // Do the theta transformation
    mv_theta_cos_sinTransform(n,p,samples,mu,S,C);

    /* Derivatives */
    d_kappa = malloc(sizeof(double)*(p+(p*p)));
    d_lambda = d_kappa + p;

#ifdef DEBUG
    double *d_kappa_fd,*d_lambda_fd;

    d_kappa_fd = malloc(sizeof(double)*(p+(p*p)));
    d_lambda_fd = d_kappa_fd + p;
#endif


    /* LBFGS variables */
    /* integer and logical types come from lbfgsb.h */
    integer iprint = verbose; // No output
    double  factr = prec; // Moderate prec
    double  pgtol = tol; // Gradient tolerance
    integer m = mprec;       // Number of corrections

    /* Fixd workspaces */
    integer taskValue;
    integer *task = &taskValue;
    integer csaveValue;
    integer *csave = &csaveValue;
    integer isave[44];
    double  dsave[29];
    logical lsave[4];

    /* Dynamic parameters (Given, but we might have to cast)*/
    integer nvar = ((p*p)-p)/2 + p;  // Number of variables

    double  f = DBL_MAX;   // Eval value
    double *g = calloc(sizeof(double),nvar*4); // Gradient value
    double *l = g+nvar; // lower bounds
    double *u = l+nvar; // upper bounds
    double *x = u+nvar; // Point value
    integer *nbd = calloc(sizeof(integer),nvar);

    /* Dynamic Workspaces*/
    double  *wa  = calloc(sizeof(double),( (2*m + 5)*nvar + 11 * m * m + 8 * m));
    integer *iwa = calloc(sizeof(integer) , 3 * nvar );

    /* Copy parameters (this way so casting is done)*/
    for(i=0;i<nvar; i++){
        l[i]   = lower[i] ;
        u[i]   = upper[i];
        nbd[i] = bounded[i];
    }


    /* Copy initial values to X*/
    // Kappa values
    for(i=0;i<p;i++){
        x[i] = kappa[i];
    }

    // Lambda values
    for(i=0, k=0; i < (p-1) ; i++) {
        for( j=(i+1) ; j < p ; j++, k++){
            x[p+k] = lambda[ i*p + j];
        }
    }

    /* Set task to START*/
    *task = (integer)START;

    do{
        setulb(&nvar,&m,x,l,u,nbd,&f,g,&factr,&pgtol,wa,iwa,task,&iprint,csave,lsave,isave,dsave);

        // If F and G comp. is requiered
        if( IS_FG(*task) ){

            // Copy kappa
            memcpy(kappa,x,sizeof(double)*p);

            // Copy lambda
            matrixUpperToFull(p,x+p,lambda);

            // Call loss function with current x
            f = mv_vonmises_lossFunction(n,p,kappa,lambda,S,C,ro,d_kappa,d_lambda);
/*******************************************************/
#ifdef DEBUG
            // Approx DF for kappa
            for(i=0;i<p;i++){
                kappa[i]+=1E-9;
                d_kappa_fd[i] = (mv_vonmises_lossFunction(n,p,kappa,lambda,S,C,ro,NULL,NULL)-f)/1E-9  ;
                kappa[i]-=1E-9;
                printf("KAPPA(%d):  calc %f \t  fd approx: %f \t DIFF: %f \n",i,d_kappa[i],d_kappa_fd[i],fabs(d_kappa[i]-d_kappa_fd[i]));
            }

            // Approx DF for lambda
            for(i=0;i<(p-1);i++){
                    for(j=i+1;j<p;j++){
                        lambda[i*p + j] += 1E-9 ; 
                        lambda[j*p + i] += 1E-9 ; 
                        d_lambda_fd[i*p + j] = (mv_vonmises_lossFunction(n,p,kappa,lambda,S,C,ro,NULL,NULL)-f)/1E-9  ;
                        d_lambda_fd[j*p + i] = d_lambda_fd[i*p + j];
                        printf("Lambda(%d,%d):  calc %f \t  fd approx: %f \t DIFF: %f \n",i,j,d_lambda[i*p + j],d_lambda_fd[i*p +j],fabs(d_lambda[i*p +j ]-d_lambda_fd[i*p + j]));
                        lambda[i*p + j] -= 1E-9 ; 
                        lambda[j*p + i] -= 1E-9 ; 
                    }
            }

#endif
/**************************************************************/

            // Kappa partials
            memcpy(g,d_kappa,sizeof(double)*p);

            // Apply penalization
            if( (phi!=NULL) && (H != NULL) ){

                double fnorm = 0;
                double fnorm_term;

                // Compute f norm
                 for (i=0; i < (p-1); i++) {
                    for (j=i+1; j < p; j++){
                        // RECALL: P is actually diag(kappa) - lambda
                        fnorm_term = (-lambda[ i*p + j] - phi[ i*p + j]) * H[ i*p + j];
                        fnorm += fnorm_term * fnorm_term;
                    }
                 }
            
                 // We also need to include kappa
                 for (i=0; i<p; i++){
                        fnorm_term = (kappa[ i ] - phi[ i*p + i]) * H[ i*p + i];
                        fnorm += fnorm_term * fnorm_term;
                 }
                 // Add norm to F
                 fnorm = sqrtl(fnorm);
                 //f += log(n)*fnorm; // What is this logn doing here???
                 f += fnorm;
#ifdef DEBUG
                printf("MATRIX P:\n");
                for(i=0; i<p ; i++){
                    for(j = 0 ; j<p ; j++){
                        if( j == i ) printf( "%f\t", kappa[j]);
                        else printf( "%f\t", -lambda[i*p + j]);
                    }
                    printf("\n");
                }
                
                printf("MATRIX Phi:\n");
                for(i=0; i<p ; i++){
                    for(j = 0 ; j<p ; j++){
                        printf( "%f\t", phi[i*p + j]);
                    }
                    printf("\n");
                }
                printf("MATRIX H:\n");
                for(i=0; i<p ; i++){
                    for(j = 0 ; j<p ; j++){
                        printf( "%f\t", H[i*p + j]);
                    }
                    printf("\n");
                }
                printf("FNORM: %f\n", fnorm);

#endif
                if(fnorm != 0){
                    // Modify lambda partials
                    for (i=0; i < (p-1); i++) {
                        for (j=i+1; j < p; j++){
                            // RECALL: P is actually diag(kappa) - lambda
                            // d_lambda[i*p + j] += log(n) * (-H[i*p + j] * H[i*p + j] * ( (-lambda[i*p + j]) - phi[ i*p + j] ) / fnorm) ;
                            d_lambda[i*p + j] += (-H[i*p + j] * H[i*p + j] * ( (-lambda[i*p + j]) - phi[ i*p + j] ) / fnorm) ;
                            d_lambda[j*p + i] = d_lambda[i*p + j];
                        }
                    }
    
                    // Modify kappa partials
                    for (i=0; i<p; i++){
                            //d_kappa[i]+= log(n) * H[i*p + i] * H[i*p + i] * ( kappa[i] - phi[ i*p + i] ) / fnorm ;
                            d_kappa[i]+= H[i*p + i] * H[i*p + i] * ( kappa[i] - phi[ i*p + i] ) / fnorm ;
                     }
                }
            }

            // Lambda partials (to vector)
            for(i=0, k=0; i < (p-1) ; i++) {
                for( j=(i+1) ; j < p ; j++, k++){
                    g[p+k] = d_lambda[ i*p + j];
                }
            }
        }

        // Additional stopping criteria to avoid infinite looping

        else if( *task == NEW_X ){

            /* Control number of iterations */
            if(isave[33] >= 100 ){
                *task = STOP_ITER;
            }
            /* Terminate if |proj g| / (1+|f|) < 1E-10 */
            else if ( dsave[12] <= (fabs(f) + 1) * 1E-10 ){
                *task = STOP_GRAD;
            }
        }

    }while( IS_FG(*task) || *task==NEW_X);


    /* Copy back x */
    for(i=0;i<p;i++) kappa[i]=x[i];
    matrixUpperToFull(p,x+p,lambda);

    /** FreE*/

#ifdef DEBUG
            free(d_kappa_fd);
#endif
    free(S); free(d_kappa); free(g);
    free(wa); free(iwa); free(nbd);

    if( IS_ERROR(*task) || IS_WARNING(*task) )
        return NAN;
    else
        return (double)f;
}
Example #2
0
void lbfgsb(int n, int m, double *x, double *l, double *u, int *nbd,
	    double *Fmin, optimfn fminfn, optimgr fmingr, int *fail,
	    void *ex, double factr, double pgtol,
	    int *fncount, int *grcount, int maxit, char *msg,
	    int trace, int nREPORT)
{
    char task[60];
    double f, *g, dsave[29], *wa;
    int tr = -1, iter = 0, *iwa, isave[44], lsave[4];

    /* shut up gcc -Wall in 4.6.x */

    for(int i = 0; i < 4; i++) lsave[i] = 0;

    if(n == 0) { /* not handled in setulb */
	*fncount = 1;
	*grcount = 0;
	*Fmin = fminfn(n, u, ex);
	strcpy(msg, "NOTHING TO DO");
	*fail = 0;
	return;
    }
    if (nREPORT <= 0)
	error(_("REPORT must be > 0 (method = \"L-BFGS-B\")"));
    switch(trace) {
    case 2: tr = 0; break;
    case 3: tr = nREPORT; break;
    case 4: tr = 99; break;
    case 5: tr = 100; break;
    case 6: tr = 101; break;
    default: tr = -1; break;
    }

    *fail = 0;
    g = vect(n);
    /* this needs to be zeroed for snd in mainlb to be zeroed */
    wa = (double *) S_alloc(2*m*n+4*n+11*m*m+8*m, sizeof(double));
    iwa = (int *) R_alloc(3*n, sizeof(int));
    strcpy(task, "START");
    while(1) {
	setulb(n, m, x, l, u, nbd, &f, g, factr, &pgtol, wa, iwa, task,
	       tr, lsave, isave, dsave);
/*	Rprintf("in lbfgsb - %s\n", task);*/
	if (strncmp(task, "FG", 2) == 0) {
	    f = fminfn(n, x, ex);
	    if (!R_FINITE(f))
		error(_("L-BFGS-B needs finite values of 'fn'"));
	    fmingr(n, x, g, ex);
	} else if (strncmp(task, "NEW_X", 5) == 0) {
	    iter++;
	    if(trace == 1 && (iter % nREPORT == 0)) {
		Rprintf("iter %4d value %f\n", iter, f);
	    }
	    if (iter > maxit) {
		*fail = 1;
		break;
	    }
	} else if (strncmp(task, "WARN", 4) == 0) {
	    *fail = 51;
	    break;
	} else if (strncmp(task, "CONV", 4) == 0) {
	    break;
	} else if (strncmp(task, "ERROR", 5) == 0) {
	    *fail = 52;
	    break;
	} else { /* some other condition that is not supposed to happen */
	    *fail = 52;
	    break;
	}
    }
    *Fmin = f;
    *fncount = *grcount = isave[33];
    if (trace) {
	Rprintf("final  value %f \n", *Fmin);
	if (iter < maxit && *fail == 0) Rprintf("converged\n");
	else Rprintf("stopped after %i iterations\n", iter);
    }
    strcpy(msg, task);
}
Example #3
0
/* Main mex gateway routine */
void mexFunction( int nlhs, mxArray *plhs[], int nrhs, const mxArray*prhs[] )   { 
    
    integer iprint = (integer)1;
    integer task=(integer)START, csave=(integer)1;
    integer iterations = 0;
    integer total_iterations = 0;

    int  iterMax = 100;
    int  total_iterMax = 200;


    integer   n, m, *nbd=NULL, *iwa=NULL; 
    double  f=0, factr, pgtol, *x, *l, *u, *g, *wa=NULL;
    int     i;
    mxLogical FREE_nbd=false;

    int ndim = 2; /* for lcc compiler, must declare these here, not later ... */
    mwSize dims[2] = { LENGTH_ISAVE, 1 };

    logical lsave[LENGTH_LSAVE];
    integer isave[LENGTH_ISAVE];
    double  dsave[LENGTH_DSAVE];
    
    double *nbd_dbl=NULL;
    long long *nbd_long=NULL;
    
    mxArray *LHS[2];
    mxArray *RHS[3];
    double *tempX, *tempG, *tempIter;
    
    /* Parse inputs. Quite boring */
    
    if (nrhs < 5 ) mexErrMsgTxt("Needs at least 5 input arguments");
    m       = (int)*mxGetPr( prhs[N_m] );
    n       = (integer)mxGetM( prhs[N_x] );
    if ( mxGetN(prhs[N_x]) != 1 ) mexErrMsgTxt("x must be a column vector");
    if ( mxGetM(prhs[N_l]) != n ) mexErrMsgTxt("l must have same size as x");
    if ( mxGetM(prhs[N_u]) != n ) mexErrMsgTxt("u must have same size as x");
    if ( mxGetM(prhs[N_nbd]) != n ) mexErrMsgTxt("nbd must have same size as x");


    if (nlhs < 2 )  mexErrMsgTxt("Should have 2 or 3 output arguments");
    if (!mxIsDouble(prhs[N_x]))
            mexErrMsgTxt("x should be of type double!\n");
    plhs[1] = mxDuplicateArray( prhs[N_x] );
    x       = mxGetPr( plhs[1] );


    l       = mxGetPr( prhs[N_l] );
    u       = mxGetPr( prhs[N_u] );
    if ( isInt( prhs[N_nbd] ) ) {
        nbd     = (integer *)mxGetData( prhs[N_nbd] ); 
    } else {
        debugPrintf("Converting nbd array to integers\n" );
        if (!mxIsDouble(prhs[N_nbd])){
            if (mxIsInt64(prhs[N_nbd])){
                nbd_long = mxGetData( prhs[N_nbd] );
                nbd     = (integer *)mxMalloc( n * sizeof(integer) );
                assert( nbd != NULL );
                FREE_nbd = true;
                /* convert nbd_dbl (in double format) to integers */
                for (i=0;i<n;i++)
                    nbd[i]  = (integer)nbd_long[i];
            } else {
                debugPrintf("Sizeof(int) is %d bits, sizeof(integer) is %d bits\n",
                        CHAR_BIT*sizeof(int),CHAR_BIT*sizeof(integer) );
                /* integer is aliased to 'long int' and should be at least
                 * 32 bits. 'long long' should be at least 64 bits.
                 * On 64-bit Windows, it seems 'long int' is exactly 32 bits,
                 * while on 64-bit linux and Mac, it is 67 bits */
                debugPrintf("Nbd is of type %s\n", mxGetClassName( prhs[N_nbd] ) );
                mexErrMsgTxt("Nbd array not doubles or type int64!\n");
            }
        } else {
            nbd_dbl = mxGetPr( prhs[N_nbd] );
            nbd     = (integer *)mxMalloc( n * sizeof(integer) );
            assert( nbd != NULL );
            FREE_nbd = true;
            /* convert nbd_dbl (in double format) to integers */
            for (i=0;i<n;i++)
                nbd[i]  = (integer)nbd_dbl[i];
        }
    }


    /* some scalar parameters */
    if ( nrhs < N_factr+1 ) 
        factr   = 1.0e7;
    else if (mxGetNumberOfElements( prhs[N_factr] )!=1)
        factr   = 1.0e7;
    else {
        factr   = (double)mxGetScalar( prhs[N_factr] );
        if (factr < 0 )
            mexErrMsgTxt("factr must be >= 0\n");
    }

    if ( nrhs < N_pgtol+1 ) 
        pgtol   = 1.0e-5;
    else if (mxGetNumberOfElements( prhs[N_pgtol] )!=1)
        pgtol   = 1.0e-5;
    else {
        pgtol   = (double)mxGetScalar( prhs[N_pgtol] );
        if (pgtol < 0)
            mexErrMsgTxt("pgtol must be >= 0\n");
    }
    if ( nrhs < N_iprint+1 ) {
        iprint  = (integer)1;
    } else if (mxGetNumberOfElements( prhs[N_iprint] )!=1) {
        iprint  = (integer)1;
    } else {
        iprint = (integer)mxGetScalar( prhs[N_iprint] );
    }
    
    if ( nrhs >= N_iterMax+1 ) 
        iterMax = (int)mxGetScalar( prhs[N_iterMax] );
    if ( nrhs >= N_total_iterMax+1 ) 
        total_iterMax = (int)mxGetScalar( prhs[N_total_iterMax] );
    
    /* allocate memory for arrays */
    g   = (double *)mxMalloc( n * sizeof(double) );
    assert( g != NULL );
    wa      = (double *)mxMalloc( (2*m*n + 5*n + 11*m*m + 8*m ) * sizeof(double) );
    assert( wa != NULL );
    iwa     = (integer *)mxMalloc( (3*n)*sizeof(integer) );
    assert( iwa != NULL );
    

            
    /* -- Finally, done with parsing inputs. Now, call lbfgsb fortran routine */
    
    /* Be careful! This modifies many variables in-place! 
     * Basically, anything without a '&' before it will be changed in the Matlab
     * workspace */
    
    if ( nrhs < N_fcn - 1 )
        mexErrMsgTxt("For this f(x) feature, need more input aguments\n");
    RHS[0] = mxDuplicateArray( prhs[N_fcn] );
    RHS[1] = mxCreateDoubleMatrix(n,1,mxREAL);
    RHS[2] = mxCreateDoubleScalar( 0.0 ); /* The iterations counter */
    tempX = (double*)mxGetPr( RHS[1] );
    if (!mxIsDouble(RHS[2]))
        mexErrMsgTxt("Error trying to create RHS[2]\n");
    tempIter = (double*)mxGetPr( RHS[2] );

    while ( (iterations < iterMax) && (total_iterations < total_iterMax ) ){
        total_iterations++;

        setulb(&n,&m,x,l,u,nbd,&f,g,&factr,&pgtol,wa,iwa,&task,&iprint,
                &csave,lsave,isave,dsave); /* (ftnlen) TASK_LEN, (ftnlen) CSAVE_LEN); */


        if ( IS_FG(task) ) {

            /* copy data from x to RHS[1] or just set pointer with mxSetPr */
            for (i=0;i<n;i++)
                tempX[i] = x[i];
            /*Try being bold: */
            /*mxSetPr( RHS[1], x ); */

            *tempIter = (double)iterations;
            mexCallMATLAB(2,LHS,3,RHS,"feval");
            f = mxGetScalar( LHS[0] );
            if (mxGetM(LHS[1]) != n )
                mexErrMsgTxt("Error with [f,g]=fcn(x) : g wrong size\n");
            if (mxGetN(LHS[1]) != 1 )
                mexErrMsgTxt("Error with [f,g]=fcn(x) : g wrong size (should be column vector)\n");

            /* could use memcpy, or just do it by hand... */
            if (!mxIsDouble(LHS[1]))
                mexErrMsgTxt("[f,g]=fcn(x) did not return g as type double\n");
            tempG = mxGetPr( LHS[1] );
            for (i=0;i<n;i++)
                g[i] = tempG[i];
            /* Or, be a bit bolder: */
            /*g = tempG; // Hmm, crashed */

            continue;
        }
        if ( task==NEW_X ) {
            iterations++;
            continue;
        } else
            break;

    }

    mxDestroyArray( LHS[0] );
    mxDestroyArray( LHS[1] );
    mxDestroyArray( RHS[0] );
    mxDestroyArray( RHS[1] );
            

    
    plhs[0] = mxCreateDoubleScalar( f );
    if ( nlhs >= 3 )
        plhs[2] = mxCreateDoubleScalar( task );
    if ( nlhs >= 4 )
        plhs[3] = mxCreateDoubleScalar( iterations );
    if ( nlhs >= 5 )
        plhs[4] = mxCreateDoubleScalar( total_iterations );
    if ( nlhs >= 6 )
        mexErrMsgTxt("Did not expect more than 5 outputs\n");

    if (FREE_nbd)
        mxFree(nbd);
    mxFree(g);
    mxFree(wa);
    mxFree(iwa);

    return;
}