/**
* 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;
}
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);
}
/* 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;
}