void Fitter::performLevmar(int nbit)
{
int nbParams = fitParameterList.size();
int nbResids = argumentsList.size();
/* levmar specific parameters */
double opts[LM_OPTS_SZ], info[LM_INFO_SZ];
std::unique_ptr<double[]> x_ptr(new double[nbParams]);
std::unique_ptr<double[]> xlb_ptr(new double[nbParams]);
std::unique_ptr<double[]> xub_ptr(new double[nbParams]);
std::unique_ptr<double[]> xdscl_ptr(new double[nbParams]);
std::unique_ptr<double[]> xcovar_ptr(new double[nbParams * nbParams]);
//initialize levmar calc options
opts[0] = LM_INIT_MU;
opts[1] = 1E-15;
opts[2] = 1E-15;
opts[3] = 1E-1;
opts[4] = LM_DIFF_DELTA; // relevant only if the finite difference Jacobian version is used
//initialize levmar parameters
for(size_t i = 0; i < fitParameterList.size(); ++i)
{
x_ptr[i] = fitParameterList[i].value;
xlb_ptr[i] = fitParameterList[i].lbvalue;
xub_ptr[i] = fitParameterList[i].ubvalue;
xdscl_ptr[i]= fitParameterList[i].scvalue;
}
dlevmar_bc_dif(levmarNormFunc, x_ptr.get(), residualsList.get(), nbParams,
nbResids, xlb_ptr.get(), xub_ptr.get(), xdscl_ptr.get(), nbit,
opts, info, NULL, xcovar_ptr.get(), reinterpret_cast<void *>(this));
resetFParameters(x_ptr.get());
printLevmarInfo(info);
}
extern "C" LEVMARDLL_API void FastReflfit(BoxReflSettings* InitStruct, double m_cParamVec[], double covariance[], int paramsize, double info[])
{
USES_CONVERSION;
//Variables
double *work, *covar;
double ChiSquare = 0;
double Qc = 0;
double calcholder = 0;
FastReflcalc Refl;
Refl.init(InitStruct);
//Setup the fit
//opts[4] is relevant only if the finite difference jacobian version is used
double opts[LM_OPTS_SZ];
opts[0]=LM_INIT_MU; opts[1]=1E-15; opts[2]=1E-15; opts[3]=1E-20; opts[4]=-LM_DIFF_DELTA;
//Allocate a dummy array - Our real calculation is done in Refl.objective
double* xvec = new double[InitStruct->QPoints] ;
memset(xvec, 0, InitStruct->QPoints*sizeof(double));
//Allocate workspace and our covariance matrix
work=new double[((LM_DIF_WORKSZ(paramsize, InitStruct->QPoints)+paramsize*InitStruct->QPoints))];
covar=work+LM_DIF_WORKSZ(paramsize, InitStruct->QPoints);
if(InitStruct->UL == NULL)
dlevmar_dif(Refl.objective,m_cParamVec, xvec, paramsize,InitStruct->QPoints, 1000, opts, info, work, covar,(void*)(&Refl));
else
dlevmar_bc_dif(Refl.objective, m_cParamVec, xvec, paramsize,InitStruct->QPoints, InitStruct->LL,InitStruct->UL,1000, opts, info, work, covar,(void*)(&Refl));
for(int i = 0; i< paramsize;i++)
{
covariance[i] = sqrt(covar[i*(paramsize+1)]);
}
delete[] xvec;
delete[] work;
}
extern "C" LEVMARDLL_API void StochFit(BoxReflSettings* InitStruct, double parameters[], double covararray[], int paramsize,
double info[], double ParamArray[], double chisquarearray[], int* paramarraysize)
{
FastReflcalc Refl;
Refl.init(InitStruct);
double* Reflectivity = InitStruct->Refl;
int QSize = InitStruct->QPoints;
double* parampercs = InitStruct->ParamPercs;
//Setup the fit
double opts[LM_OPTS_SZ];
opts[0]=LM_INIT_MU; opts[1]=1E-15; opts[2]=1E-15; opts[3]=1E-20;
opts[4]=-LM_DIFF_DELTA; // relevant only if the finite difference jacobian version is used
//Allocate a dummy array - Our real calculation is done in Refl.objective
double* xvec = new double[InitStruct->QPoints] ;
for(int i = 0; i < InitStruct->QPoints; i++)
{
xvec[i] = 0;
}
//Copy starting solution
double* origguess = new double[paramsize];
memcpy(origguess, parameters, sizeof(double)*paramsize);
if(InitStruct->OneSigma)
Refl.mkdensityonesigma(parameters, paramsize);
else
Refl.mkdensity(parameters, paramsize);
Refl.myrfdispatch();
double bestchisquare = 0;
for(int i = 0; i < InitStruct->QPoints; i++)
{
bestchisquare += (log(Refl.reflpt[i])-log(Reflectivity[i]))*(log(Refl.reflpt[i])-log(Reflectivity[i]));
}
double tempinfoarray[9];
tempinfoarray[1] = bestchisquare;
double* tempcovararray = new double[paramsize*paramsize];
memset(tempcovararray,0.0, sizeof(double)*paramsize*paramsize);
ParameterContainer original(parameters, tempcovararray, paramsize,InitStruct->OneSigma,
tempinfoarray, parampercs[6]);
delete[] tempcovararray;
vector<ParameterContainer> temp;
temp.reserve(6000);
omp_set_num_threads(omp_get_num_procs());
#pragma omp parallel
{
FastReflcalc locRefl;
locRefl.init(InitStruct);
//Initialize random number generator
int seed = time_seed();
CRandomMersenne randgen(time_seed()+omp_get_thread_num());
ParameterContainer localanswer;
double locparameters[20];
double locbestchisquare = bestchisquare;
double bestparam[20];
int vecsize = 1000;
int veccount = 0;
ParameterContainer* vec = (ParameterContainer*)malloc(vecsize*sizeof(ParameterContainer));
double locinfo[9];
//Allocate workspace - these will be private to each thread
double* work, *covar;
work=(double*)malloc((LM_DIF_WORKSZ(paramsize, QSize)+paramsize*QSize)*sizeof(double));
covar=work+LM_DIF_WORKSZ(paramsize, QSize);
#pragma omp for schedule(runtime)
for(int i = 0; i < InitStruct->Iterations;i++)
{
locparameters[0] = randgen.IRandom(origguess[0]*parampercs[4], origguess[0]*parampercs[5]);
for(int k = 0; k< InitStruct->Boxes; k++)
{
if(InitStruct->OneSigma)
{
locparameters[2*k+1] = randgen.IRandom(origguess[2*k+1]*parampercs[0], origguess[2*k+1]*parampercs[1]);
locparameters[2*k+2] = randgen.IRandom(origguess[2*k+2]*parampercs[2], origguess[2*k+2]*parampercs[3]);
}
else
{
locparameters[3*k+1] = randgen.IRandom(origguess[3*k+1]*parampercs[0], origguess[3*k+1]*parampercs[1]);
locparameters[3*k+2] = randgen.IRandom(origguess[3*k+2]*parampercs[2], origguess[3*k+2]*parampercs[3]);
locparameters[3*k+3] = randgen.IRandom(origguess[3*k+3]*parampercs[4], origguess[3*k+3]*parampercs[5]);
}
}
locparameters[paramsize-1] = origguess[paramsize-1];
if(InitStruct->UL == NULL)
dlevmar_dif(locRefl.objective, locparameters, xvec, paramsize, InitStruct->QPoints, 500, opts, locinfo, work,covar,(void*)(&locRefl));
else
dlevmar_bc_dif(locRefl.objective, locparameters, xvec, paramsize, InitStruct->QPoints, InitStruct->LL, InitStruct->UL,
500, opts, locinfo, work,covar,(void*)(&locRefl));
localanswer.SetContainer(locparameters,covar,paramsize,InitStruct->OneSigma,locinfo, parampercs[6]);
if(locinfo[1] < bestchisquare && localanswer.IsReasonable())
{
//Resize the private arrays if we need the space
if(veccount+2 == vecsize)
{
vecsize += 1000;
vec = (ParameterContainer*)realloc(vec,vecsize*sizeof(ParameterContainer));
}
bool unique = true;
int arraysize = veccount;
//Check if the answer already exists
for(int i = 0; i < arraysize; i++)
{
if(localanswer == vec[i])
{
unique = false;
i = arraysize;
}
}
//If the answer is unique add it to our set of answers
if(unique)
{
vec[veccount] = localanswer;
veccount++;
}
}
}
#pragma omp critical (AddVecs)
{
for(int i = 0; i < veccount; i++)
{
temp.push_back(vec[i]);
}
}
free(vec);
free(work);
}
//
delete[] xvec;
delete[] origguess;
//Sort the answers
//Get the total number of answers
temp.push_back(original);
vector<ParameterContainer> allsolutions;
allsolutions.reserve(6000);
int tempsize = temp.size();
allsolutions.push_back(temp[0]);
for(int i = 1; i < tempsize; i++)
{
int allsolutionssize = allsolutions.size();
for(int j = 0; j < allsolutionssize;j++)
{
if(temp[i] == allsolutions[j])
{
break;
}
if(j == allsolutionssize-1)
{
allsolutions.push_back(temp[i]);
}
}
}
if(allsolutions.size() > 0)
{
sort(allsolutions.begin(), allsolutions.end());
}
for(int i = 0; i < allsolutions.size() && i < 1000 && allsolutions.size() > 0; i++)
{
for(int j = 0; j < paramsize; j++)
{
ParamArray[(i)*paramsize+j] = (allsolutions.at(i).GetParamArray())[j];
covararray[(i)*paramsize+j] = (allsolutions.at(i).GetCovarArray())[j];
}
memcpy(info, allsolutions.at(i).GetInfoArray(), 9* sizeof(double));
info += 9;
chisquarearray[i] = (allsolutions.at(i).GetScore());
}
*paramarraysize = min(allsolutions.size(),999);
}
static PyObject *
_pylm_dlevmar_generic(PyObject *mod, PyObject *args, PyObject *kwds,
char *argstring, char *kwlist[],
int jacobian, int bounds) {
PyObject *func = NULL;
PyObject *jacf = NULL;
PyObject *initial = NULL, *initial_npy = NULL;
PyObject *measurements = NULL, *measurements_npy = NULL;
PyObject *lower = NULL, *lower_npy = NULL;
PyObject *upper = NULL, *upper_npy = NULL;
PyObject *opts = NULL, *opts_npy = NULL;
PyObject *covar = NULL;
PyObject *retval = NULL;
PyObject *info = NULL;
pylm_callback_data *pydata = NULL;
double *c_initial = NULL;
double *c_measurements = NULL;
double *c_opts = NULL;
double *c_lower = NULL;
double *c_upper = NULL;
double *c_covar = NULL;
int max_iter = 0;
int run_iter = 0;
int m = 0, n = 0;
double c_info[LM_INFO_SZ];
int nopts;
// If finite-difference approximate Jacobians are used, we
// need 5 optional params; otherwise 4.
if (jacobian){
nopts = 4;
} else {
nopts = 5;
}
// parse arguments
if (!bounds) {
if (jacobian) {
if (!PyArg_ParseTupleAndKeywords(args, kwds, argstring, kwlist,
&func, &jacf, &initial,
&measurements, &max_iter,
&opts, &covar)){
return NULL;
}
} else {
if (!PyArg_ParseTupleAndKeywords(args, kwds, argstring, kwlist,
&func, &initial,
&measurements, &max_iter,
&opts, &covar)){
return NULL;
}
}
} else {
if (jacobian) {
if (!PyArg_ParseTupleAndKeywords(args, kwds, argstring, kwlist,
&func, &jacf, &initial,
&measurements, &lower, &upper, &max_iter,
&opts, &covar)){
return NULL;
}
} else {
if (!PyArg_ParseTupleAndKeywords(args, kwds, argstring, kwlist,
&func, &initial,
&measurements, &lower, &upper, &max_iter,
&opts, &covar)){
return NULL;
}
}
}
// Check each variable type
if (!PyCallable_Check(func)) {
PyErr_SetString(PyExc_TypeError, "func must be a callable object");
return NULL;
}
if (!PyArray_Check(initial)) {
PyErr_SetString(PyExc_TypeError, "initial must be a numpy array");
return NULL;
}
if (!PyArray_Check(measurements)) {
PyErr_SetString(PyExc_TypeError, "measurements must be a numpy array");
return NULL;
}
if (jacobian && !PyCallable_Check(jacf)) {
PyErr_SetString(PyExc_TypeError, "jacf must be a callable object");
return NULL;
}
if (lower && !PyArray_Check(lower)) {
PyErr_SetString(PyExc_TypeError, "lower bounds must be a numpy array");
return NULL;
}
if (upper && !PyArray_Check(upper)) {
PyErr_SetString(PyExc_TypeError, "upper bounds must be a numpy array");
return NULL;
}
if (opts && !PyArray_Check(opts) && (PyArray_Size(opts) != nopts)) {
if (nopts == 4){
PyErr_SetString(PyExc_TypeError,
"opts must be a numpy vector of length 4.");
} else {
PyErr_SetString(PyExc_TypeError,
"opts must be a numpy vector of length 5.");
}
return NULL;
}
// convert python types into C
pydata = PyMem_Malloc(sizeof(pydata));
if(!pydata){
PyErr_SetString(PyExc_RuntimeError,
"Error in allocating memory for data.");
return NULL;
}
pydata->func = func;
pydata->jacf = jacf;
initial_npy = PyArray_FROMANY(initial, NPY_DOUBLE, 0, 0, NPY_INOUT_ARRAY);
measurements_npy = PyArray_FROMANY(measurements, NPY_DOUBLE, 0, 0, NPY_IN_ARRAY);
if(!initial_npy || !measurements_npy){
// Cannot create array
PyErr_SetString(PyExc_RuntimeError,
"Error in creating arrays from input data.");
//Py_XDECREF(initial_npy);
//Py_XDECREF(measurements_npy);
return NULL;
}
c_initial = (double *)PyArray_DATA(initial_npy);
c_measurements = (double *)PyArray_DATA(measurements_npy);
m = PyArray_SIZE(initial_npy);
n = PyArray_SIZE(measurements_npy);
npy_intp dims[2] = {m, m};
covar = PyArray_SimpleNew(2, dims, NPY_DOUBLE);
c_covar = PyArray_DATA(covar);
if (lower){
lower_npy = PyArray_FROMANY(lower, PyArray_DOUBLE, 0, 0, NPY_IN_ARRAY);
c_lower = PyArray_DATA(lower_npy);
// TODO check dims
}
if (upper){
upper_npy = PyArray_FROMANY(upper, PyArray_DOUBLE, 0, 0, NPY_IN_ARRAY);
c_upper = PyArray_DATA(upper_npy);
// TODO check dims
}
if (opts) {
opts_npy = PyArray_FROMANY(opts, PyArray_DOUBLE, 0, 0, NPY_IN_ARRAY);
c_opts = PyArray_DATA(opts_npy);
// TODO check dims
}
// call function to do the fitting
if (!bounds) {
if (jacobian) {
run_iter = dlevmar_der(_pylm_func_callback, _pylm_jacf_callback,
c_initial, c_measurements, m, n,
max_iter, c_opts, c_info, NULL, c_covar, pydata);
} else {
run_iter = dlevmar_dif(_pylm_func_callback, c_initial, c_measurements,
m, n, max_iter, c_opts, c_info, NULL, c_covar, pydata);
}
} else {
if (jacobian) {
run_iter = dlevmar_bc_der(_pylm_func_callback, _pylm_jacf_callback,
c_initial, c_measurements, m, n,
c_lower, c_upper,
max_iter, c_opts, c_info, NULL, c_covar, pydata);
} else {
run_iter = dlevmar_bc_dif(_pylm_func_callback, c_initial, c_measurements,
m, n, c_lower, c_upper,
max_iter, c_opts, c_info, NULL, c_covar, pydata);
}
}
// convert results back into python
if (run_iter > 0) {
npy_intp dims[1] = {m};
retval = PyArray_SimpleNewFromData(1, dims, PyArray_DOUBLE, c_initial);
} else {
retval = Py_None;
Py_INCREF(Py_None);
}
if (pydata) {
PyMem_Free(pydata);
}
// convert additional information into python
info = Py_BuildValue("{s:d,s:d,s:d,s:d,s:d,s:d,s:d,s:d,s:d}",
"initial_e2", c_info[0],
"estimate_e2", c_info[1],
"estimate_Jt", c_info[2],
"estimate_Dp2", c_info[3],
"estimate_mu", c_info[4],
"iterations", c_info[5],
"termination", c_info[6],
"function_evaluations", c_info[7],
"jacobian_evaluations", c_info[8]);
//Py_XDECREF(measurements_npy);
//Py_XDECREF(initial_npy);
//Py_XDECREF(lower_npy);
//Py_XDECREF(upper_npy);
//Py_XDECREF(opts_npy);
return Py_BuildValue("(OOiO)", retval, covar, run_iter, info, NULL);
}
// Function definitions.
// -----------------------------------------------------------------
void mexFunction (int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
{
//Input Args
user_function_data fun;
double *x0, *ydata = NULL, *lb = NULL, *ub = NULL, *A = NULL, *b = NULL, *Aeq = NULL, *beq = NULL;
//Options
int maxIter = 500;
double info[LM_INFO_SZ];
double opts[LM_OPTS_SZ]={J_INIT_MU, J_STOP_THRESH, J_STOP_THRESH, J_STOP_THRESH, LM_DIFF_DELTA};
//Outputs Args
double *x, *fval, *exitflag, *iter, *feval;
double *pcovar = NULL;
//Internal Vars
size_t ndec, ndat;
int i, status, havJac = 0, conMode = 0;
int nineq=0, neq=0;
double *covar = NULL;
double *Apr, *bpr;
double *llb, *lub;
citer = 1;
iterF.enabled = false;
if (nrhs < 1) {
if(nlhs < 1)
printSolverInfo();
else
plhs[0] = mxCreateString(LM_VERSION);
return;
}
//Check user inputs & get constraint information
checkInputs(prhs,nrhs,&conMode);
//Get Sizes
ndec = mxGetNumberOfElements(prhs[2]);
ndat = mxGetNumberOfElements(prhs[3]);
//Get Objective Function Handle
if (mxIsChar(prhs[0])) {
CHECK(mxGetString(prhs[0], fun.f, FLEN) == 0,"error reading objective name string");
fun.nrhs = 1;
fun.xrhs = 0;
} else {
fun.prhs[0] = (mxArray*)prhs[0];
strcpy(fun.f, "feval");
fun.nrhs = 2;
fun.xrhs = 1;
}
fun.prhs[fun.xrhs] = mxCreateDoubleMatrix(ndec, 1, mxREAL); //x0
fun.print = 0;
//Check and Get Gradient Function Handle
if(!mxIsEmpty(prhs[1])) {
havJac = 1;
if (mxIsChar(prhs[1])) {
CHECK(mxGetString(prhs[1], fun.g, FLEN) == 0,"error reading gradient name string");
fun.nrhs_g = 1;
fun.xrhs_g = 0;
} else {
fun.prhs_g[0] = (mxArray*)prhs[1];
strcpy(fun.g, "feval");
fun.nrhs_g = 2;
fun.xrhs_g = 1;
}
fun.prhs_g[fun.xrhs_g] = mxCreateDoubleMatrix(ndec, 1, mxREAL); //x0
}
//Get x0 + data
x0 = mxGetPr(prhs[2]);
ydata = mxGetPr(prhs[3]);
fun.ydata = ydata;
//Get Bounds
if(conMode & 1) {
//LB
if(!mxIsEmpty(prhs[4])){
llb = mxGetPr(prhs[4]);
lb = mxCalloc(ndec,sizeof(double));
memcpy(lb,llb,ndec*sizeof(double));
for(i=0;i<ndec;i++) {
if(mxIsInf(lb[i]))
lb[i] = -DBL_MAX;
}
}
else {
lb = mxCalloc(ndec,sizeof(double));
for(i=0;i<ndec;i++)
lb[i] = -DBL_MAX;
}
//UB
if(nrhs > 5 && !mxIsEmpty(prhs[5])){
lub = mxGetPr(prhs[5]);
ub = mxCalloc(ndec,sizeof(double));
memcpy(ub,lub,ndec*sizeof(double));
for(i=0;i<ndec;i++) {
if(mxIsInf(ub[i]))
ub[i] = DBL_MAX;
}
}
else {
ub = mxCalloc(ndec,sizeof(double));
for(i=0;i<ndec;i++)
ub[i] = DBL_MAX;
}
}
//Get Linear Inequality Constraints
if(conMode & 2) {
nineq = (int)mxGetM(prhs[7]);
Apr = mxGetPr(prhs[6]);
bpr = mxGetPr(prhs[7]);
//Need to flip >= to <=
A = mxCalloc(ndec*nineq,sizeof(double));
b = mxCalloc(nineq,sizeof(double));
for(i=0;i<ndec*nineq;i++)
A[i] = -Apr[i];
for(i=0;i<nineq;i++)
b[i] = -bpr[i];
}
//Get Linear Equality Constraints
if(conMode & 4) {
Aeq = mxGetPr(prhs[8]);
beq = mxGetPr(prhs[9]);
neq = (int)mxGetM(prhs[9]);
}
//Get Options if specified
if(nrhs > 10) {
if(mxGetField(prhs[10],0,"maxiter"))
maxIter = (int)*mxGetPr(mxGetField(prhs[10],0,"maxiter"));
if(mxGetField(prhs[10],0,"display"))
fun.print = (int)*mxGetPr(mxGetField(prhs[10],0,"display"));
if(mxGetField(prhs[10],0,"iterfun") && !mxIsEmpty(mxGetField(prhs[10],0,"iterfun")))
{
iterF.prhs[0] = (mxArray*)mxGetField(prhs[10],0,"iterfun");
strcpy(iterF.f, "feval");
iterF.enabled = true;
iterF.prhs[1] = mxCreateNumericMatrix(1,1,mxINT32_CLASS,mxREAL);
iterF.prhs[2] = mxCreateDoubleMatrix(1,1,mxREAL);
iterF.prhs[3] = mxCreateDoubleMatrix(ndec,1,mxREAL);
}
}
//Create Outputs
plhs[0] = mxCreateDoubleMatrix(ndec,1, mxREAL);
plhs[1] = mxCreateDoubleMatrix(1,1, mxREAL);
plhs[2] = mxCreateDoubleMatrix(1,1, mxREAL);
plhs[3] = mxCreateDoubleMatrix(1,1, mxREAL);
plhs[4] = mxCreateDoubleMatrix(1,1, mxREAL);
x = mxGetPr(plhs[0]);
fval = mxGetPr(plhs[1]);
exitflag = mxGetPr(plhs[2]);
iter = mxGetPr(plhs[3]);
feval = mxGetPr(plhs[4]);
//Copy initial guess to x
memcpy(x,x0,ndec*sizeof(double));
//Create Covariance Matrix if Required
if(nlhs>4)
covar=mxCalloc(ndec*ndec,sizeof(double));
//Print Header
if(fun.print) {
mexPrintf("\n------------------------------------------------------------------\n");
mexPrintf(" This is LEVMAR v2.5\n");
mexPrintf(" Author: Manolis Lourakis\n MEX Interface J. Currie 2011\n\n");
mexPrintf(" Problem Properties:\n");
mexPrintf(" # Decision Variables: %4d\n",ndec);
mexPrintf(" # Data Points: %4d\n",ndat);
mexPrintf("------------------------------------------------------------------\n");
}
//Solve based on constraints
switch(conMode)
{
case MIN_UNCONSTRAINED:
//mexPrintf("Unc Problem\n");
if(havJac)
status = dlevmar_der(func, jac, x, ydata, (int)ndec, (int)ndat, maxIter, opts, info, NULL, covar, &fun);
else
status = dlevmar_dif(func, x, ydata, (int)ndec, (int)ndat, maxIter, opts, info, NULL, covar, &fun);
break;
case MIN_CONSTRAINED_BC:
//mexPrintf("Box Constrained Problem\n");
if(havJac)
status = dlevmar_bc_der(func, jac, x, ydata, (int)ndec, (int)ndat, lb, ub, NULL, maxIter, opts, info, NULL, covar, &fun);
else
status = dlevmar_bc_dif(func, x, ydata, (int)ndec, (int)ndat, lb, ub, NULL, maxIter, opts, info, NULL, covar, &fun);
break;
case MIN_CONSTRAINED_LIC:
//mexPrintf("Linear Inequality Problem\n");
if(havJac)
status = dlevmar_lic_der(func, jac, x, ydata, (int)ndec, (int)ndat, A, b, nineq, maxIter, opts, info, NULL, covar, &fun);
else
status = dlevmar_lic_dif(func, x, ydata, (int)ndec, (int)ndat, A, b, nineq, maxIter, opts, info, NULL, covar, &fun);
break;
case MIN_CONSTRAINED_BLIC:
//mexPrintf("Boxed Linear Inequality Problem\n");
if(havJac)
status = dlevmar_blic_der(func, jac, x, ydata, (int)ndec, (int)ndat, lb, ub, A, b, nineq, maxIter, opts, info, NULL, covar, &fun);
else
status = dlevmar_blic_dif(func, x, ydata, (int)ndec, (int)ndat, lb, ub, A, b, nineq, maxIter, opts, info, NULL, covar, &fun);
break;
case MIN_CONSTRAINED_LEC:
//mexPrintf("Linear Equality Problem\n");
if(havJac)
status = dlevmar_lec_der(func, jac, x, ydata, (int)ndec, (int)ndat, Aeq, beq, neq, maxIter, opts, info, NULL, covar, &fun);
else
status = dlevmar_lec_dif(func, x, ydata, (int)ndec, (int)ndat, Aeq, beq, neq, maxIter, opts, info, NULL, covar, &fun);
break;
case MIN_CONSTRAINED_BLEC:
//mexPrintf("Boxed Linear Equality Problem\n");
if(havJac)
status = dlevmar_blec_der(func, jac, x, ydata, (int)ndec, (int)ndat, lb, ub, Aeq, beq, neq, NULL, maxIter, opts, info, NULL, covar, &fun);
else
status = dlevmar_blec_dif(func, x, ydata, (int)ndec, (int)ndat, lb, ub, Aeq, beq, neq, NULL, maxIter, opts, info, NULL, covar, &fun);
break;
case MIN_CONSTRAINED_LEIC:
//mexPrintf("Linear Inequality + Equality Problem\n");
if(havJac)
status = dlevmar_leic_der(func, jac, x, ydata, (int)ndec, (int)ndat, Aeq, beq, neq, A, b, nineq, maxIter, opts, info, NULL, covar, &fun);
else
status = dlevmar_leic_dif(func, x, ydata, (int)ndec, (int)ndat, Aeq, beq, neq, A, b, nineq, maxIter, opts, info, NULL, covar, &fun);
break;
case MIN_CONSTRAINED_BLEIC:
//mexPrintf("Boxed Linear Inequality + Equality Problem\n");
if(havJac)
status = dlevmar_bleic_der(func, jac, x, ydata, (int)ndec, (int)ndat, lb, ub, Aeq, beq, neq, A, b, nineq, maxIter, opts, info, NULL, covar, &fun);
else
status = dlevmar_bleic_dif(func, x, ydata, (int)ndec, (int)ndat, lb, ub, Aeq, beq, neq, A, b, nineq, maxIter, opts, info, NULL, covar, &fun);
break;
default:
mexErrMsgTxt("Unknown constraint configuration");
}
//Save Status & Iterations
*fval = info[1];
*exitflag = getStatus(info[6]);
*iter = (double)status;
*feval = (double)citer;
//Save Covariance if Required
if(nlhs > 5) {
plhs[5] = mxCreateDoubleMatrix(ndec, ndec, mxREAL);
pcovar = mxGetPr(plhs[5]);
memcpy(pcovar,covar,ndec*ndec*sizeof(double));
}
//Print Header
if(fun.print){
//Termination Detected
if(*exitflag == 1)
mexPrintf("\n *** SUCCESSFUL TERMINATION ***\n");
else if(*exitflag == 0)
mexPrintf("\n *** MAXIMUM ITERATIONS REACHED ***\n");
else if(*exitflag == -1)
mexPrintf("\n *** TERMINATION: TOLERANCE TOO SMALL ***\n");
else if(*exitflag == -2)
mexPrintf("\n *** TERMINATION: ROUTINE ERROR ***\n");
if(*exitflag==1)
mexPrintf(" Final SSE: %12.5g\n In %3.0f iterations\n",*fval,*iter);
mexPrintf("------------------------------------------------------------------\n\n");
}
//Clean Up
if(lb) mxFree(lb);
if(ub) mxFree(ub);
if(covar) mxFree(covar);
if(A) mxFree(A);
if(b) mxFree(b);
}
void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *Prhs[])
{
register int i;
register double *pdbl;
mxArray **prhs=(mxArray **)&Prhs[0], *At, *Ct;
struct mexdata mdata;
int len, status;
double *p, *p0, *ret, *x;
int m, n, havejac, Arows, Crows, itmax, nopts, mintype, nextra;
double opts[LM_OPTS_SZ]={LM_INIT_MU, LM_STOP_THRESH, LM_STOP_THRESH, LM_STOP_THRESH, LM_DIFF_DELTA};
double info[LM_INFO_SZ];
double *lb=NULL, *ub=NULL, *A=NULL, *b=NULL, *wghts=NULL, *C=NULL, *d=NULL, *covar=NULL;
/* parse input args; start by checking their number */
if((nrhs<5))
matlabFmtdErrMsgTxt("levmar: at least 5 input arguments required (got %d).", nrhs);
if(nlhs>4)
matlabFmtdErrMsgTxt("levmar: too many output arguments (max. 4, got %d).", nlhs);
else if(nlhs<2)
matlabFmtdErrMsgTxt("levmar: too few output arguments (min. 2, got %d).", nlhs);
/* note that in order to accommodate optional args, prhs & nrhs are adjusted accordingly below */
/** func **/
/* first argument must be a string , i.e. a char row vector */
if(mxIsChar(prhs[0])!=1)
mexErrMsgTxt("levmar: first argument must be a string.");
if(mxGetM(prhs[0])!=1)
mexErrMsgTxt("levmar: first argument must be a string (i.e. char row vector).");
/* store supplied name */
len=mxGetN(prhs[0])+1;
mdata.fname=mxCalloc(len, sizeof(char));
status=mxGetString(prhs[0], mdata.fname, len);
if(status!=0)
mexErrMsgTxt("levmar: not enough space. String is truncated.");
/** jac (optional) **/
/* check whether second argument is a string */
if(mxIsChar(prhs[1])==1){
if(mxGetM(prhs[1])!=1)
mexErrMsgTxt("levmar: second argument must be a string (i.e. row vector).");
/* store supplied name */
len=mxGetN(prhs[1])+1;
mdata.jacname=mxCalloc(len, sizeof(char));
status=mxGetString(prhs[1], mdata.jacname, len);
if(status!=0)
mexErrMsgTxt("levmar: not enough space. String is truncated.");
havejac=1;
++prhs;
--nrhs;
}
else{
mdata.jacname=NULL;
havejac=0;
}
#ifdef DEBUG
fflush(stderr);
fprintf(stderr, "LEVMAR: %s analytic Jacobian\n", havejac? "with" : "no");
#endif /* DEBUG */
/* CHECK
if(!mxIsDouble(prhs[1]) || mxIsComplex(prhs[1]) || !(mxGetM(prhs[1])==1 && mxGetN(prhs[1])==1))
*/
/** p0 **/
/* the second required argument must be a real row or column vector */
if(!mxIsDouble(prhs[1]) || mxIsComplex(prhs[1]) || !(mxGetM(prhs[1])==1 || mxGetN(prhs[1])==1))
mexErrMsgTxt("levmar: p0 must be a real vector.");
p0=mxGetPr(prhs[1]);
/* determine if we have a row or column vector and retrieve its
* size, i.e. the number of parameters
*/
if(mxGetM(prhs[1])==1){
m=mxGetN(prhs[1]);
mdata.isrow_p0=1;
}
else{
m=mxGetM(prhs[1]);
mdata.isrow_p0=0;
}
/* copy input parameter vector to avoid destroying it */
p=mxMalloc(m*sizeof(double));
for(i=0; i<m; ++i)
p[i]=p0[i];
/** x **/
/* the third required argument must be a real row or column vector */
if(!mxIsDouble(prhs[2]) || mxIsComplex(prhs[2]) || !(mxGetM(prhs[2])==1 || mxGetN(prhs[2])==1))
mexErrMsgTxt("levmar: x must be a real vector.");
x=mxGetPr(prhs[2]);
n=__MAX__(mxGetM(prhs[2]), mxGetN(prhs[2]));
/** itmax **/
/* the fourth required argument must be a scalar */
if(!mxIsDouble(prhs[3]) || mxIsComplex(prhs[3]) || mxGetM(prhs[3])!=1 || mxGetN(prhs[3])!=1)
mexErrMsgTxt("levmar: itmax must be a scalar.");
itmax=(int)mxGetScalar(prhs[3]);
/** opts **/
/* the fifth required argument must be a real row or column vector */
if(!mxIsDouble(prhs[4]) || mxIsComplex(prhs[4]) || (!(mxGetM(prhs[4])==1 || mxGetN(prhs[4])==1) &&
!(mxGetM(prhs[4])==0 && mxGetN(prhs[4])==0)))
mexErrMsgTxt("levmar: opts must be a real vector.");
pdbl=mxGetPr(prhs[4]);
nopts=__MAX__(mxGetM(prhs[4]), mxGetN(prhs[4]));
if(nopts!=0){ /* if opts==[], nothing needs to be done and the defaults are used */
if(nopts>LM_OPTS_SZ)
matlabFmtdErrMsgTxt("levmar: opts must have at most %d elements, got %d.", LM_OPTS_SZ, nopts);
else if(nopts<((havejac)? LM_OPTS_SZ-1 : LM_OPTS_SZ))
matlabFmtdWarnMsgTxt("levmar: only the %d first elements of opts specified, remaining set to defaults.", nopts);
for(i=0; i<nopts; ++i)
opts[i]=pdbl[i];
}
#ifdef DEBUG
else{
fflush(stderr);
fprintf(stderr, "LEVMAR: empty options vector, using defaults\n");
}
#endif /* DEBUG */
/** mintype (optional) **/
/* check whether sixth argument is a string */
if(nrhs>=6 && mxIsChar(prhs[5])==1 && mxGetM(prhs[5])==1){
char *minhowto;
/* examine supplied name */
len=mxGetN(prhs[5])+1;
minhowto=mxCalloc(len, sizeof(char));
status=mxGetString(prhs[5], minhowto, len);
if(status!=0)
mexErrMsgTxt("levmar: not enough space. String is truncated.");
for(i=0; minhowto[i]; ++i)
minhowto[i]=tolower(minhowto[i]);
if(!strncmp(minhowto, "unc", 3)) mintype=MIN_UNCONSTRAINED;
else if(!strncmp(minhowto, "bc", 2)) mintype=MIN_CONSTRAINED_BC;
else if(!strncmp(minhowto, "lec", 3)) mintype=MIN_CONSTRAINED_LEC;
else if(!strncmp(minhowto, "blec", 4)) mintype=MIN_CONSTRAINED_BLEC;
else if(!strncmp(minhowto, "bleic", 5)) mintype=MIN_CONSTRAINED_BLEIC;
else if(!strncmp(minhowto, "blic", 4)) mintype=MIN_CONSTRAINED_BLIC;
else if(!strncmp(minhowto, "leic", 4)) mintype=MIN_CONSTRAINED_LEIC;
else if(!strncmp(minhowto, "lic", 3)) mintype=MIN_CONSTRAINED_BLIC;
else matlabFmtdErrMsgTxt("levmar: unknown minimization type '%s'.", minhowto);
mxFree(minhowto);
++prhs;
--nrhs;
}
else
mintype=MIN_UNCONSTRAINED;
if(mintype==MIN_UNCONSTRAINED) goto extraargs;
/* arguments below this point are optional and their presence depends
* upon the minimization type determined above
*/
/** lb, ub **/
if(nrhs>=7 && (mintype==MIN_CONSTRAINED_BC || mintype==MIN_CONSTRAINED_BLEC || mintype==MIN_CONSTRAINED_BLIC || mintype==MIN_CONSTRAINED_BLEIC)){
/* check if the next two arguments are real row or column vectors */
if(mxIsDouble(prhs[5]) && !mxIsComplex(prhs[5]) && (mxGetM(prhs[5])==1 || mxGetN(prhs[5])==1)){
if(mxIsDouble(prhs[6]) && !mxIsComplex(prhs[6]) && (mxGetM(prhs[6])==1 || mxGetN(prhs[6])==1)){
if((i=__MAX__(mxGetM(prhs[5]), mxGetN(prhs[5])))!=m)
matlabFmtdErrMsgTxt("levmar: lb must have %d elements, got %d.", m, i);
if((i=__MAX__(mxGetM(prhs[6]), mxGetN(prhs[6])))!=m)
matlabFmtdErrMsgTxt("levmar: ub must have %d elements, got %d.", m, i);
lb=mxGetPr(prhs[5]);
ub=mxGetPr(prhs[6]);
prhs+=2;
nrhs-=2;
}
}
}
/** A, b **/
if(nrhs>=7 && (mintype==MIN_CONSTRAINED_LEC || mintype==MIN_CONSTRAINED_BLEC || mintype==MIN_CONSTRAINED_LEIC || mintype==MIN_CONSTRAINED_BLEIC)){
/* check if the next two arguments are a real matrix and a real row or column vector */
if(mxIsDouble(prhs[5]) && !mxIsComplex(prhs[5]) && mxGetM(prhs[5])>=1 && mxGetN(prhs[5])>=1){
if(mxIsDouble(prhs[6]) && !mxIsComplex(prhs[6]) && (mxGetM(prhs[6])==1 || mxGetN(prhs[6])==1)){
if((i=mxGetN(prhs[5]))!=m)
matlabFmtdErrMsgTxt("levmar: A must have %d columns, got %d.", m, i);
if((i=__MAX__(mxGetM(prhs[6]), mxGetN(prhs[6])))!=(Arows=mxGetM(prhs[5])))
matlabFmtdErrMsgTxt("levmar: b must have %d elements, got %d.", Arows, i);
At=prhs[5];
b=mxGetPr(prhs[6]);
A=getTranspose(At);
prhs+=2;
nrhs-=2;
}
}
}
/* wghts */
/* check if we have a weights vector */
if(nrhs>=6 && mintype==MIN_CONSTRAINED_BLEC){ /* only check if we have seen both box & linear constraints */
if(mxIsDouble(prhs[5]) && !mxIsComplex(prhs[5]) && (mxGetM(prhs[5])==1 || mxGetN(prhs[5])==1)){
if(__MAX__(mxGetM(prhs[5]), mxGetN(prhs[5]))==m){
wghts=mxGetPr(prhs[5]);
++prhs;
--nrhs;
}
}
}
/** C, d **/
if(nrhs>=7 && (mintype==MIN_CONSTRAINED_BLEIC || mintype==MIN_CONSTRAINED_BLIC || mintype==MIN_CONSTRAINED_LEIC || mintype==MIN_CONSTRAINED_LIC)){
/* check if the next two arguments are a real matrix and a real row or column vector */
if(mxIsDouble(prhs[5]) && !mxIsComplex(prhs[5]) && mxGetM(prhs[5])>=1 && mxGetN(prhs[5])>=1){
if(mxIsDouble(prhs[6]) && !mxIsComplex(prhs[6]) && (mxGetM(prhs[6])==1 || mxGetN(prhs[6])==1)){
if((i=mxGetN(prhs[5]))!=m)
matlabFmtdErrMsgTxt("levmar: C must have %d columns, got %d.", m, i);
if((i=__MAX__(mxGetM(prhs[6]), mxGetN(prhs[6])))!=(Crows=mxGetM(prhs[5])))
matlabFmtdErrMsgTxt("levmar: d must have %d elements, got %d.", Crows, i);
Ct=prhs[5];
d=mxGetPr(prhs[6]);
C=getTranspose(Ct);
prhs+=2;
nrhs-=2;
}
}
}
/* arguments below this point are assumed to be extra arguments passed
* to every invocation of the fitting function and its Jacobian
*/
extraargs:
/* handle any extra args and allocate memory for
* passing the current parameter estimate to matlab
*/
nextra=nrhs-5;
mdata.nrhs=nextra+1;
mdata.rhs=(mxArray **)mxMalloc(mdata.nrhs*sizeof(mxArray *));
for(i=0; i<nextra; ++i)
mdata.rhs[i+1]=(mxArray *)prhs[nrhs-nextra+i]; /* discard 'const' modifier */
#ifdef DEBUG
fflush(stderr);
fprintf(stderr, "LEVMAR: %d extra args\n", nextra);
#endif /* DEBUG */
if(mdata.isrow_p0){ /* row vector */
mdata.rhs[0]=mxCreateDoubleMatrix(1, m, mxREAL);
/*
mxSetM(mdata.rhs[0], 1);
mxSetN(mdata.rhs[0], m);
*/
}
else{ /* column vector */
mdata.rhs[0]=mxCreateDoubleMatrix(m, 1, mxREAL);
/*
mxSetM(mdata.rhs[0], m);
mxSetN(mdata.rhs[0], 1);
*/
}
/* ensure that the supplied function & Jacobian are as expected */
if(checkFuncAndJacobian(p, m, n, havejac, &mdata)){
status=LM_ERROR;
goto cleanup;
}
if(nlhs>3) /* covariance output required */
covar=mxMalloc(m*m*sizeof(double));
/* invoke levmar */
switch(mintype){
case MIN_UNCONSTRAINED: /* no constraints */
if(havejac)
status=dlevmar_der(func, jacfunc, p, x, m, n, itmax, opts, info, NULL, covar, (void *)&mdata);
else
status=dlevmar_dif(func, p, x, m, n, itmax, opts, info, NULL, covar, (void *)&mdata);
#ifdef DEBUG
fflush(stderr);
fprintf(stderr, "LEVMAR: calling dlevmar_der()/dlevmar_dif()\n");
#endif /* DEBUG */
break;
case MIN_CONSTRAINED_BC: /* box constraints */
if(havejac)
status=dlevmar_bc_der(func, jacfunc, p, x, m, n, lb, ub, itmax, opts, info, NULL, covar, (void *)&mdata);
else
status=dlevmar_bc_dif(func, p, x, m, n, lb, ub, itmax, opts, info, NULL, covar, (void *)&mdata);
#ifdef DEBUG
fflush(stderr);
fprintf(stderr, "LEVMAR: calling dlevmar_bc_der()/dlevmar_bc_dif()\n");
#endif /* DEBUG */
break;
case MIN_CONSTRAINED_LEC: /* linear equation constraints */
#ifdef HAVE_LAPACK
if(havejac)
status=dlevmar_lec_der(func, jacfunc, p, x, m, n, A, b, Arows, itmax, opts, info, NULL, covar, (void *)&mdata);
else
status=dlevmar_lec_dif(func, p, x, m, n, A, b, Arows, itmax, opts, info, NULL, covar, (void *)&mdata);
#else
mexErrMsgTxt("levmar: no linear constraints support, HAVE_LAPACK was not defined during MEX-file compilation.");
#endif /* HAVE_LAPACK */
#ifdef DEBUG
fflush(stderr);
fprintf(stderr, "LEVMAR: calling dlevmar_lec_der()/dlevmar_lec_dif()\n");
#endif /* DEBUG */
break;
case MIN_CONSTRAINED_BLEC: /* box & linear equation constraints */
#ifdef HAVE_LAPACK
if(havejac)
status=dlevmar_blec_der(func, jacfunc, p, x, m, n, lb, ub, A, b, Arows, wghts, itmax, opts, info, NULL, covar, (void *)&mdata);
else
status=dlevmar_blec_dif(func, p, x, m, n, lb, ub, A, b, Arows, wghts, itmax, opts, info, NULL, covar, (void *)&mdata);
#else
mexErrMsgTxt("levmar: no box & linear constraints support, HAVE_LAPACK was not defined during MEX-file compilation.");
#endif /* HAVE_LAPACK */
#ifdef DEBUG
fflush(stderr);
fprintf(stderr, "LEVMAR: calling dlevmar_blec_der()/dlevmar_blec_dif()\n");
#endif /* DEBUG */
break;
case MIN_CONSTRAINED_BLEIC: /* box, linear equation & inequalities constraints */
#ifdef HAVE_LAPACK
if(havejac)
status=dlevmar_bleic_der(func, jacfunc, p, x, m, n, lb, ub, A, b, Arows, C, d, Crows, itmax, opts, info, NULL, covar, (void *)&mdata);
else
status=dlevmar_bleic_dif(func, p, x, m, n, lb, ub, A, b, Arows, C, d, Crows, itmax, opts, info, NULL, covar, (void *)&mdata);
#else
mexErrMsgTxt("levmar: no box, linear equation & inequality constraints support, HAVE_LAPACK was not defined during MEX-file compilation.");
#endif /* HAVE_LAPACK */
#ifdef DEBUG
fflush(stderr);
fprintf(stderr, "LEVMAR: calling dlevmar_bleic_der()/dlevmar_bleic_dif()\n");
#endif /* DEBUG */
break;
case MIN_CONSTRAINED_BLIC: /* box, linear inequalities constraints */
#ifdef HAVE_LAPACK
if(havejac)
status=dlevmar_bleic_der(func, jacfunc, p, x, m, n, lb, ub, NULL, NULL, 0, C, d, Crows, itmax, opts, info, NULL, covar, (void *)&mdata);
else
status=dlevmar_bleic_dif(func, p, x, m, n, lb, ub, NULL, NULL, 0, C, d, Crows, itmax, opts, info, NULL, covar, (void *)&mdata);
#else
mexErrMsgTxt("levmar: no box & linear inequality constraints support, HAVE_LAPACK was not defined during MEX-file compilation.");
#endif /* HAVE_LAPACK */
#ifdef DEBUG
fflush(stderr);
fprintf(stderr, "LEVMAR: calling dlevmar_blic_der()/dlevmar_blic_dif()\n");
#endif /* DEBUG */
break;
case MIN_CONSTRAINED_LEIC: /* linear equation & inequalities constraints */
#ifdef HAVE_LAPACK
if(havejac)
status=dlevmar_bleic_der(func, jacfunc, p, x, m, n, NULL, NULL, A, b, Arows, C, d, Crows, itmax, opts, info, NULL, covar, (void *)&mdata);
else
status=dlevmar_bleic_dif(func, p, x, m, n, NULL, NULL, A, b, Arows, C, d, Crows, itmax, opts, info, NULL, covar, (void *)&mdata);
#else
mexErrMsgTxt("levmar: no linear equation & inequality constraints support, HAVE_LAPACK was not defined during MEX-file compilation.");
#endif /* HAVE_LAPACK */
#ifdef DEBUG
fflush(stderr);
fprintf(stderr, "LEVMAR: calling dlevmar_leic_der()/dlevmar_leic_dif()\n");
#endif /* DEBUG */
break;
case MIN_CONSTRAINED_LIC: /* linear inequalities constraints */
#ifdef HAVE_LAPACK
if(havejac)
status=dlevmar_bleic_der(func, jacfunc, p, x, m, n, NULL, NULL, NULL, NULL, 0, C, d, Crows, itmax, opts, info, NULL, covar, (void *)&mdata);
else
status=dlevmar_bleic_dif(func, p, x, m, n, NULL, NULL, NULL, NULL, 0, C, d, Crows, itmax, opts, info, NULL, covar, (void *)&mdata);
#else
mexErrMsgTxt("levmar: no linear equation & inequality constraints support, HAVE_LAPACK was not defined during MEX-file compilation.");
#endif /* HAVE_LAPACK */
#ifdef DEBUG
fflush(stderr);
fprintf(stderr, "LEVMAR: calling dlevmar_lic_der()/dlevmar_lic_dif()\n");
#endif /* DEBUG */
break;
default:
mexErrMsgTxt("levmar: unexpected internal error.");
}
#ifdef DEBUG
fflush(stderr);
printf("LEVMAR: minimization returned %d in %g iter, reason %g\n\tSolution: ", status, info[5], info[6]);
for(i=0; i<m; ++i)
printf("%.7g ", p[i]);
printf("\n\n\tMinimization info:\n\t");
for(i=0; i<LM_INFO_SZ; ++i)
printf("%g ", info[i]);
printf("\n");
#endif /* DEBUG */
/* copy back return results */
/** ret **/
plhs[0]=mxCreateDoubleMatrix(1, 1, mxREAL);
ret=mxGetPr(plhs[0]);
ret[0]=(double)status;
/** popt **/
plhs[1]=(mdata.isrow_p0==1)? mxCreateDoubleMatrix(1, m, mxREAL) : mxCreateDoubleMatrix(m, 1, mxREAL);
pdbl=mxGetPr(plhs[1]);
for(i=0; i<m; ++i)
pdbl[i]=p[i];
/** info **/
if(nlhs>2){
plhs[2]=mxCreateDoubleMatrix(1, LM_INFO_SZ, mxREAL);
pdbl=mxGetPr(plhs[2]);
for(i=0; i<LM_INFO_SZ; ++i)
pdbl[i]=info[i];
}
/** covar **/
if(nlhs>3){
plhs[3]=mxCreateDoubleMatrix(m, m, mxREAL);
pdbl=mxGetPr(plhs[3]);
for(i=0; i<m*m; ++i) /* covariance matrices are symmetric, thus no need to transpose! */
pdbl[i]=covar[i];
}
cleanup:
/* cleanup */
mxDestroyArray(mdata.rhs[0]);
if(A) mxFree(A);
if(C) mxFree(C);
mxFree(mdata.fname);
if(havejac) mxFree(mdata.jacname);
mxFree(p);
mxFree(mdata.rhs);
if(covar) mxFree(covar);
if(status==LM_ERROR)
mexWarnMsgTxt("levmar: optimization returned with an error!");
}
double stfnum::lmFit( const Vector_double& data, double dt,
const stfnum::storedFunc& fitFunc, const Vector_double& opts,
bool use_scaling,
Vector_double& p, std::string& info, int& warning )
{
// Basic range checking:
if (fitFunc.pInfo.size()!=p.size()) {
std::string msg("Error in stfnum::lmFit()\n"
"function parameters (p_fit) and parameters entered (p) have different sizes");
throw std::runtime_error(msg);
}
if ( opts.size() != 6 ) {
std::string msg("Error in stfnum::lmFit()\n"
"wrong number of options");
throw std::runtime_error(msg);
}
bool constrained = false;
std::vector< double > constrains_lm_lb( fitFunc.pInfo.size() );
std::vector< double > constrains_lm_ub( fitFunc.pInfo.size() );
bool can_scale = use_scaling;
for ( unsigned n_p=0; n_p < fitFunc.pInfo.size(); ++n_p ) {
if ( fitFunc.pInfo[n_p].constrained ) {
constrained = true;
constrains_lm_lb[n_p] = fitFunc.pInfo[n_p].constr_lb;
constrains_lm_ub[n_p] = fitFunc.pInfo[n_p].constr_ub;
} else {
constrains_lm_lb[n_p] = -DBL_MAX;
constrains_lm_ub[n_p] = DBL_MAX;
}
if ( can_scale ) {
if (fitFunc.pInfo[n_p].scale == stfnum::noscale) {
can_scale = false;
}
}
}
// Store the functions at global scope:
saveFunc(fitFunc.func);
saveJac(fitFunc.jac);
double info_id[LM_INFO_SZ];
Vector_double data_ptr(data);
Vector_double xyscale(4);
if (can_scale) {
xyscale = get_scale(data_ptr, dt);
}
// The parameters need to be separated into two parts:
// Those that are to be fitted and those that the client wants
// to keep constant. Since there is no native support to
// do so in Lourakis' routines, the workaround is a little
// tricky, making (ab)use of the *void pointer:
// number of parameters that need to be fitted:
int n_fitted=0;
for ( unsigned n_p=0; n_p < fitFunc.pInfo.size(); ++n_p ) {
n_fitted += fitFunc.pInfo[n_p].toFit;
}
// parameters that need to be fitted:
Vector_double p_toFit(n_fitted);
std::deque<bool> p_fit_bool( fitFunc.pInfo.size() );
// parameters that are held constant:
Vector_double p_const( fitFunc.pInfo.size()-n_fitted );
for ( unsigned n_p=0, n_c=0, n_f=0; n_p < fitFunc.pInfo.size(); ++n_p ) {
if (fitFunc.pInfo[n_p].toFit) {
p_toFit[n_f++] = p[n_p];
if (can_scale) {
p_toFit[n_f-1] = fitFunc.pInfo[n_p].scale(p_toFit[n_f-1], xyscale[0],
xyscale[1], xyscale[2], xyscale[3]);
}
} else {
p_const[n_c++] = p[n_p];
if (can_scale) {
p_const[n_c-1] = fitFunc.pInfo[n_p].scale(p_const[n_c-1], xyscale[0],
xyscale[1], xyscale[2], xyscale[3]);
}
}
p_fit_bool[n_p] = fitFunc.pInfo[n_p].toFit;
}
// size * dt_new = 1 -> dt_new = 1.0/size
double dt_finfo = dt;
if (can_scale)
dt_finfo = 1.0/data_ptr.size();
fitInfo fInfo( p_fit_bool, p_const, dt_finfo );
// make l-value of opts:
Vector_double opts_l(5);
for (std::size_t n=0; n < 4; ++n) opts_l[n] = opts[n];
opts_l[4] = -1e-6;
int it = 0;
if (p_toFit.size()!=0 && data_ptr.size()!=0) {
double old_info_id[LM_INFO_SZ];
// initialize with initial parameter guess:
Vector_double old_p_toFit(p_toFit);
#ifdef _DEBUG
std::ostringstream optsMsg;
optsMsg << "\nopts: ";
for (std::size_t n_p=0; n_p < opts.size(); ++n_p)
optsMsg << opts[n_p] << "\t";
optsMsg << "\n" << "data_ptr[" << data_ptr.size()-1 << "]=" << data_ptr[data_ptr.size()-1] << "\n";
optsMsg << "constrains_lm_lb: ";
for (std::size_t n_p=0; n_p < constrains_lm_lb.size(); ++n_p)
optsMsg << constrains_lm_lb[n_p] << "\t";
optsMsg << "\n" << "constrains_lm_ub: ";
for (std::size_t n_p=0; n_p < constrains_lm_ub.size(); ++n_p)
optsMsg << constrains_lm_ub[n_p] << "\t";
optsMsg << "\n\n";
std::cout << optsMsg;
#endif
while ( 1 ) {
#ifdef _DEBUG
std::ostringstream paramMsg;
paramMsg << "Pass: "******"\t";
paramMsg << "p_toFit: ";
for (std::size_t n_p=0; n_p < p_toFit.size(); ++n_p)
paramMsg << p_toFit[n_p] << "\t";
paramMsg << "\n";
std::cout << paramMsg.str().c_str();
#endif
if ( !fitFunc.hasJac ) {
if ( !constrained ) {
dlevmar_dif( c_func_lour, &p_toFit[0], &data_ptr[0], n_fitted,
(int)data.size(), (int)opts[4], &opts_l[0], info_id,
NULL, NULL, &fInfo );
} else {
dlevmar_bc_dif( c_func_lour, &p_toFit[0], &data_ptr[0], n_fitted,
(int)data.size(), &constrains_lm_lb[0], &constrains_lm_ub[0], NULL,
(int)opts[4], &opts_l[0], info_id, NULL, NULL, &fInfo );
}
} else {
if ( !constrained ) {
dlevmar_der( c_func_lour, c_jac_lour, &p_toFit[0], &data_ptr[0],
n_fitted, (int)data.size(), (int)opts[4], &opts_l[0], info_id,
NULL, NULL, &fInfo );
} else {
dlevmar_bc_der( c_func_lour, c_jac_lour, &p_toFit[0],
&data_ptr[0], n_fitted, (int)data.size(), &constrains_lm_lb[0],
&constrains_lm_ub[0], NULL, (int)opts[4], &opts_l[0], info_id,
NULL, NULL, &fInfo );
}
}
it++;
if ( info_id[1] != info_id[1] ) {
// restore previous parameters if new chisqr is NaN:
p_toFit = old_p_toFit;
} else {
double dchisqr = (info_id[0] - info_id[1]) / info_id[1]; // (old chisqr - new chisqr) / new_chisqr
if ( dchisqr < 0 ) {
// restore previous results and exit if new chisqr is larger:
for ( int n_i = 0; n_i < LM_INFO_SZ; ++n_i ) info_id[n_i] = old_info_id[n_i];
p_toFit = old_p_toFit;
break;
}
if ( dchisqr < 1e-5 ) {
// Keep current results and exit if change in chisqr is below threshold
break;
}
// otherwise, store results and continue iterating:
for ( int n_i = 0; n_i < LM_INFO_SZ; ++n_i ) old_info_id[n_i] = info_id[n_i];
old_p_toFit = p_toFit;
}
if ( it >= opts[5] )
// Exit if maximal number of iterations is reached
break;
// decrease initial step size for next iteration:
opts_l[0] *= 1e-4;
}
} else {
std::runtime_error e("Array of size zero in lmFit");
throw e;
}
// copy back the fitted parameters to p:
for ( unsigned n_p=0, n_f=0, n_c=0; n_p<fitFunc.pInfo.size(); ++n_p ) {
if (fitFunc.pInfo[n_p].toFit) {
p[n_p] = p_toFit[n_f++];
} else {
p[n_p] = p_const[n_c++];
}
if (can_scale) {
p[n_p] = fitFunc.pInfo[n_p].unscale(p[n_p], xyscale[0],
xyscale[1], xyscale[2], xyscale[3]);
}
}
std::ostringstream str_info;
str_info << "Passes: " << it;
str_info << "\nIterations during last pass: "******"\nStopping reason during last pass:"******"\nStopped by small gradient of squared error.";
warning = 0;
break;
case 2:
str_info << "\nStopped by small rel. parameter change.";
warning = 0;
break;
case 3:
str_info << "\nReached max. number of iterations. Restart\n"
<< "with smarter initial parameters and / or with\n"
<< "increased initial scaling factor and / or with\n"
<< "increased max. number of iterations.";
warning = 3;
break;
case 4:
str_info << "\nSingular matrix. Restart from current parameters\n"
<< "with increased initial scaling factor.";
warning = 4;
break;
case 5:
str_info << "\nNo further error reduction is possible.\n"
<< "Restart with increased initial scaling factor.";
warning = 5;
break;
case 6:
str_info << "\nStopped by small squared error.";
warning = 0;
break;
case 7:
str_info << "\nStopped by invalid (i.e. NaN or Inf) \"func\" values.\n";
str_info << "This is a user error.";
warning = 7;
break;
default:
str_info << "\nUnknown reason for stopping the fit.";
warning = -1;
}
if (use_scaling && !can_scale) {
str_info << "\nCouldn't use scaling because one or more "
<< "of the parameters don't allow it.";
}
info=str_info.str();
return info_id[1];
}
//--------------------------------------------------------------------------------------------------
//
//--------------------------------------------------------------------------------------------------
void ScreenSpaceHandler::computeTransform(osg::Vec3 &T, osg::Quat &R)
{
double q[6] = {
0.0, // Tx
0.0, // Ty
0.0, // Tz
0.0, // Rx
0.0, // Ry
0.0, // Rz
};
int numTouchPoints = this->cursors.size();
if (numTouchPoints >= 3)
{
double clo[] = { -DBL_MAX, -DBL_MAX, -DBL_MAX, -1.0, -1.0, -1.0 };
double chi[] = { DBL_MAX, DBL_MAX, DBL_MAX, 1.0, 1.0, 1.0 };
dlevmar_bc_dif(
&this->errorFunction,
q,
NULL,
6,
2 * numTouchPoints,
clo,
chi,
MAX_ITERATIONS,
NULL,
NULL,
NULL,
NULL,
this);
}
else if (numTouchPoints == 2)
{
double clo[] = { -DBL_MAX, -DBL_MAX, -DBL_MAX, -1.0, -1.0, -1.0 };
double chi[] = { DBL_MAX, DBL_MAX, DBL_MAX, 1.0, 1.0, 1.0 };
dlevmar_bc_dif(
&this->errorFunction,
q,
NULL,
6,
6,
clo,
chi,
MAX_ITERATIONS,
NULL,
NULL,
NULL,
NULL,
this);
}
else // numTouchPoints == 1
{
double clo[] = { -DBL_MAX, -DBL_MAX, -DBL_MAX, -1.0, -1.0, -1.0 };
double chi[] = { DBL_MAX, DBL_MAX, DBL_MAX, 1.0, 1.0, 1.0 };
dlevmar_bc_dif(
&this->errorFunction,
q,
NULL,
6,
6,
clo,
chi,
MAX_ITERATIONS,
NULL,
NULL,
NULL,
NULL,
this);
}
this->getTransformsFromState(q, T, R);
}
void
_tenDwiGageAnswer(gageContext *ctx, gagePerVolume *pvl) {
char me[]="_tenDwiGageAnswer";
unsigned int dwiIdx;
tenDwiGageKindData *kindData;
tenDwiGagePvlData *pvlData;
double *dwiAll, dwiMean=0, tentmp[7];
kindData = AIR_CAST(tenDwiGageKindData *, pvl->kind->data);
pvlData = AIR_CAST(tenDwiGagePvlData *, pvl->data);
dwiAll = pvl->directAnswer[tenDwiGageAll];
if (GAGE_QUERY_ITEM_TEST(pvl->query, tenDwiGageAll)) {
/* done if doV */
if (ctx->verbose) {
for (dwiIdx=0; dwiIdx<pvl->kind->valLen; dwiIdx++) {
fprintf(stderr, "%s(%d+%g,%d+%g,%d+%g): dwi[%u] = %g\n", me,
ctx->point.xi, ctx->point.xf,
ctx->point.yi, ctx->point.yf,
ctx->point.zi, ctx->point.zf,
dwiIdx, dwiAll[dwiIdx]);
}
fprintf(stderr, "%s: type(ngrad) = %d = %s\n", me,
kindData->ngrad->type,
airEnumStr(nrrdType, kindData->ngrad->type));
}
}
/*
if (GAGE_QUERY_ITEM_TEST(pvl->query, tenDwiGageB0)) {
if (GAGE_QUERY_ITEM_TEST(pvl->query, tenDwiGageJustDWI)) {
done if doV
}
*/
/* HEY this isn't valid for multiple b-values */
if (GAGE_QUERY_ITEM_TEST(pvl->query, tenDwiGageADC)) {
double logdwi, logb0;
logb0 = log(AIR_MAX(kindData->valueMin,
pvl->directAnswer[tenDwiGageB0][0]));
for (dwiIdx=1; dwiIdx<pvl->kind->valLen; dwiIdx++) {
logdwi = log(AIR_MAX(kindData->valueMin,
pvl->directAnswer[tenDwiGageJustDWI][dwiIdx-1]));
pvl->directAnswer[tenDwiGageADC][dwiIdx-1]
= (logb0 - logdwi)/kindData->bval;
}
}
if (GAGE_QUERY_ITEM_TEST(pvl->query, tenDwiGageMeanDWIValue)) {
dwiMean = 0;
for (dwiIdx=1; dwiIdx<pvl->kind->valLen; dwiIdx++) {
dwiMean += dwiAll[dwiIdx];
}
dwiMean /= pvl->kind->valLen;
pvl->directAnswer[tenDwiGageMeanDWIValue][0] = dwiMean;
}
/* note: the gage interface to tenEstimate functionality
allows you exactly one kind of tensor estimation (per kind),
so the function call to do the estimation is actually
repeated over and over again; the copy into the answer
buffer is what changes... */
if (GAGE_QUERY_ITEM_TEST(pvl->query, tenDwiGageTensorLLS)) {
tenEstimate1TensorSingle_d(pvlData->tec1, tentmp, dwiAll);
TEN_T_COPY(pvl->directAnswer[tenDwiGageTensorLLS], tentmp);
}
if (GAGE_QUERY_ITEM_TEST(pvl->query, tenDwiGageTensorLLSError)) {
pvl->directAnswer[tenDwiGageTensorLLSError][0] = pvlData->tec1->errorDwi;
}
if (GAGE_QUERY_ITEM_TEST(pvl->query, tenDwiGageTensorLLSErrorLog)) {
pvl->directAnswer[tenDwiGageTensorLLSErrorLog][0]
= pvlData->tec1->errorLogDwi;
}
if (GAGE_QUERY_ITEM_TEST(pvl->query, tenDwiGageTensorWLS)) {
tenEstimate1TensorSingle_d(pvlData->tec1, tentmp, dwiAll);
TEN_T_COPY(pvl->directAnswer[tenDwiGageTensorWLS], tentmp);
}
if (GAGE_QUERY_ITEM_TEST(pvl->query, tenDwiGageTensorNLS)) {
tenEstimate1TensorSingle_d(pvlData->tec1, tentmp, dwiAll);
TEN_T_COPY(pvl->directAnswer[tenDwiGageTensorNLS], tentmp);
}
if (GAGE_QUERY_ITEM_TEST(pvl->query, tenDwiGageTensorMLE)) {
tenEstimate1TensorSingle_d(pvlData->tec1, tentmp, dwiAll);
TEN_T_COPY(pvl->directAnswer[tenDwiGageTensorMLE], tentmp);
}
/* HEY: have to implement all the different kinds of errors */
/* BEGIN sneakiness ........ */
if (GAGE_QUERY_ITEM_TEST(pvl->query, tenDwiGageTensor)) {
gageItemEntry *item;
item = pvl->kind->table + tenDwiGageTensor;
TEN_T_COPY(pvl->directAnswer[tenDwiGageTensor],
pvl->directAnswer[item->prereq[0]]);
}
if (GAGE_QUERY_ITEM_TEST(pvl->query, tenDwiGageTensorError)) {
gageItemEntry *item;
item = pvl->kind->table + tenDwiGageTensorError;
pvl->directAnswer[tenDwiGageTensorError][0]
= pvl->directAnswer[item->prereq[0]][0];
}
if (GAGE_QUERY_ITEM_TEST(pvl->query, tenDwiGageTensorErrorLog)) {
gageItemEntry *item;
item = pvl->kind->table + tenDwiGageTensorErrorLog;
pvl->directAnswer[tenDwiGageTensorErrorLog][0]
= pvl->directAnswer[item->prereq[0]][0];
}
if (GAGE_QUERY_ITEM_TEST(pvl->query, tenDwiGageTensorLikelihood)) {
gageItemEntry *item;
item = pvl->kind->table + tenDwiGageTensorLikelihood;
pvl->directAnswer[tenDwiGageTensorLikelihood][0]
= pvl->directAnswer[item->prereq[0]][0];
}
/* END sneakiness ........ */
if (GAGE_QUERY_ITEM_TEST(pvl->query, tenDwiGageFA)) {
pvl->directAnswer[tenDwiGageFA][0]
= pvl->directAnswer[tenDwiGageTensor][0]
* tenAnisoTen_d(pvl->directAnswer[tenDwiGageTensor],
tenAniso_FA);
}
if (GAGE_QUERY_ITEM_TEST(pvl->query, tenDwiGageTensorAllDWIError)) {
const double *grads;
int gradcount;
double *ten, d;
int i;
/* HEY: should switch to tenEstimate-based DWI simulation */
ten = pvl->directAnswer[tenDwiGageTensor];
gradcount = pvl->kind->valLen -1; /* Dont count b0 */
grads = ((const double*) kindData->ngrad->data) +3; /* Ignore b0 grad */
for( i=0; i < gradcount; i++ ) {
d = dwiAll[0]*exp(- pvlData->tec1->bValue
* TEN_T3V_CONTR(ten, grads + 3*i));
pvl->directAnswer[tenDwiGageTensorAllDWIError][i] = dwiAll[i+1] - d;
}
}
if (GAGE_QUERY_ITEM_TEST(pvl->query, tenDwiGage2TensorQSeg)) {
const double *grads;
int gradcount;
double *twoten;
unsigned int valIdx, E;
twoten = pvl->directAnswer[tenDwiGage2TensorQSeg];
gradcount = pvl->kind->valLen -1; /* Dont count b0 */
grads = ((const double*) kindData->ngrad->data) +3; /* Ignore b0 grad */
if (dwiAll[0] != 0) { /* S0 = 0 */
_tenQball(pvlData->tec2->bValue, gradcount, dwiAll, grads,
pvlData->qvals);
_tenQvals2points(gradcount, pvlData->qvals, grads, pvlData->qpoints);
_tenSegsamp2(gradcount, pvlData->qvals, grads, pvlData->qpoints,
pvlData->wght + 1, pvlData->dists );
} else {
/* stupid; should really return right here since data is garbage */
for (valIdx=1; valIdx < AIR_CAST(unsigned int, gradcount+1); valIdx++) {
pvlData->wght[valIdx] = valIdx % 2;
}
}
E = 0;
for (valIdx=1; valIdx<pvl->kind->valLen; valIdx++) {
if (!E) E |= tenEstimateSkipSet(pvlData->tec2, valIdx,
pvlData->wght[valIdx]);
}
if (!E) E |= tenEstimateUpdate(pvlData->tec2);
if (!E) E |= tenEstimate1TensorSingle_d(pvlData->tec2,
twoten + 0, dwiAll);
for (valIdx=1; valIdx<pvl->kind->valLen; valIdx++) {
if (!E) E |= tenEstimateSkipSet(pvlData->tec2, valIdx,
1 - pvlData->wght[valIdx]);
}
if (!E) E |= tenEstimateUpdate(pvlData->tec2);
if (!E) E |= tenEstimate1TensorSingle_d(pvlData->tec2,
twoten + 7, dwiAll);
if (E) {
fprintf(stderr, "!%s: (trouble) %s\n", me, biffGetDone(TEN));
}
/* hack: confidence for two-tensor fit */
twoten[0] = (twoten[0] + twoten[7])/2;
twoten[7] = 0.5; /* fraction that is the first tensor (initial value) */
/* twoten[1 .. 6] = first tensor */
/* twoten[8 .. 13] = second tensor */
/* Compute fraction between tensors if not garbage in this voxel */
if (twoten[0] > 0.5) {
double exp0,exp1,d,e=0,g=0, a=0,b=0;
int i;
for( i=0; i < gradcount; i++ ) {
exp0 = exp(-pvlData->tec2->bValue * TEN_T3V_CONTR(twoten + 0,
grads + 3*i));
exp1 = exp(-pvlData->tec2->bValue * TEN_T3V_CONTR(twoten + 7,
grads + 3*i));
d = dwiAll[i+1] / dwiAll[0];
e = exp0 - exp1;
g = d - exp1;
a += .5*e*e;
b += e*g;
}
twoten[7] = AIR_CLAMP(0, 0.5*(b/a), 1);
}
}
if (GAGE_QUERY_ITEM_TEST(pvl->query, tenDwiGage2TensorQSegError)) {
const double *grads;
int gradcount;
double *twoten, d;
int i;
/* HEY: should switch to tenEstimate-based DWI simulation */
if (dwiAll[0] != 0) { /* S0 = 0 */
twoten = pvl->directAnswer[tenDwiGage2TensorQSeg];
gradcount = pvl->kind->valLen -1; /* Dont count b0 */
grads = ((const double*) kindData->ngrad->data) +3; /* Ignore b0 grad */
pvl->directAnswer[tenDwiGage2TensorQSegError][0] = 0;
for( i=0; i < gradcount; i++ ) {
d = twoten[7]*exp(-pvlData->tec2->bValue * TEN_T3V_CONTR(twoten + 0,
grads + 3*i));
d += (1 - twoten[7])*exp(-pvlData->tec2->bValue
*TEN_T3V_CONTR(twoten + 7, grads + 3*i));
d = dwiAll[i+1]/dwiAll[0] - d;
pvl->directAnswer[tenDwiGage2TensorQSegError][0] += d*d;
}
pvl->directAnswer[tenDwiGage2TensorQSegError][0] =
sqrt( pvl->directAnswer[tenDwiGage2TensorQSegError][0] );
} else {
/* HEY: COMPLETELY WRONG!! An error is not defined! */
pvl->directAnswer[tenDwiGage2TensorQSegError][0] = 0;
}
/* printf("%f\n",pvl->directAnswer[tenDwiGage2TensorQSegError][0]); */
}
if (GAGE_QUERY_ITEM_TEST(pvl->query, tenDwiGage2TensorQSegAndError)) {
double *twoten, *err, *twotenerr;
twoten = pvl->directAnswer[tenDwiGage2TensorQSeg];
err = pvl->directAnswer[tenDwiGage2TensorQSegError];
twotenerr = pvl->directAnswer[tenDwiGage2TensorQSegAndError];
TEN_T_COPY(twotenerr + 0, twoten + 0);
TEN_T_COPY(twotenerr + 7, twoten + 7);
twotenerr[14] = err[0];
}
if (GAGE_QUERY_ITEM_TEST(pvl->query, tenDwiGage2TensorPeled)) {
#if TEEM_LEVMAR
#define PARAMS 4
double *twoTen, Cp /* , residual, AICSingFit, AICTwoFit */;
/* Vars for the NLLS */
double guess[PARAMS], loBnd[PARAMS], upBnd[PARAMS],
opts[LM_OPTS_SZ], *grad, *egrad, tenA[7], tenB[7],
matA[9], matB[9], matTmp[9], rott[9];
unsigned int gi;
int lmret;
/* Pointer to the location where the two tensor will be written */
twoTen = pvl->directAnswer[tenDwiGage2TensorPeled];
/* Estimate the DWI error, error is given as standard deviation */
pvlData->tec2->recordErrorDwi = AIR_FALSE;
/* Estimate the single tensor */
tenEstimate1TensorSingle_d(pvlData->tec2, pvlData->ten1, dwiAll);
/* Get the eigenValues and eigen vectors for this tensor */
tenEigensolve_d(pvlData->ten1Eval, pvlData->ten1Evec, pvlData->ten1);
/* Get westins Cp */
Cp = tenAnisoEval_d(pvlData->ten1Eval, tenAniso_Cp1);
/* Calculate the residual, need the variance to sqr it */
/* residual = pvlData->tec2->errorDwi*pvlData->tec2->errorDwi; */
/* Calculate the AIC for single tensor fit */
/* AICSingFit = _tenComputeAIC(residual, pvlData->tec2->dwiNum, 6); */
/* the CP-based test is gone; caller's responsibility */
/* rotate DW gradients by inverse of eigenvector column matrix
and place into pvlData->nten1EigenGrads (which has been
allocated by _tenDwiGagePvlDataNew()) */
grad = AIR_CAST(double *, kindData->ngrad->data);
egrad = AIR_CAST(double *, pvlData->nten1EigenGrads->data);
for (gi=0; gi<kindData->ngrad->axis[1].size; gi++) {
/* yes, this is also transforming some zero-length (B0) gradients;
that's harmless */
ELL_3MV_MUL(egrad, pvlData->ten1Evec, grad);
grad += 3;
egrad += 3;
}
/* Lower and upper bounds for the NLLS routine */
loBnd[0] = 0.0;
loBnd[1] = 0.0;
loBnd[2] = -AIR_PI/2;
loBnd[3] = -AIR_PI/2;
upBnd[0] = pvlData->ten1Eval[0]*5;
upBnd[1] = 1.0;
upBnd[2] = AIR_PI/2;
upBnd[3] = AIR_PI/2;
/* Starting point for the NLLS */
guess[0] = pvlData->ten1Eval[0];
guess[1] = 0.5;
guess[2] = AIR_PI/4;
guess[3] = -AIR_PI/4;
/*
guess[2] = AIR_AFFINE(0, airDrandMT_r(pvlData->randState), 1,
AIR_PI/6, AIR_PI/3);
guess[3] = AIR_AFFINE(0, airDrandMT_r(pvlData->randState), 1,
-AIR_PI/6, -AIR_PI/3);
*/
/* Fill in the constraints for the LM optimization,
the threshold of error difference */
opts[0] = pvlData->levmarTau;
opts[1] = pvlData->levmarEps1;
opts[2] = pvlData->levmarEps2;
opts[3] = pvlData->levmarEps3;
/* Very imp to set this opt, note that only forward
differences are used to approx Jacobian */
opts[4] = pvlData->levmarDelta;
/* run NLLS, results are stored back into guess[] */
pvlData->levmarUseFastExp = AIR_FALSE;
lmret = dlevmar_bc_dif(_tenLevmarPeledCB, guess, pvlData->tec2->dwi,
PARAMS, pvlData->tec2->dwiNum, loBnd, upBnd,
pvlData->levmarMaxIter, opts,
pvlData->levmarInfo,
NULL, NULL, pvlData);
if (-1 == lmret) {
ctx->errNum = 1;
sprintf(ctx->errStr, "%s: dlevmar_bc_dif() failed!", me);
} else {
/* Get the AIC for the two tensor fit, use the levmarinfo
to get the residual */
/*
residual = pvlData->levmarInfo[1]/pvlData->tec2->dwiNum;
AICTwoFit = _tenComputeAIC(residual, pvlData->tec2->dwiNum, 12);
*/
/* Form the tensors using the estimated pp, returned in guess */
_tenPeledRotate2D(tenA, guess[0], pvlData->ten1Eval[2], guess[2]);
_tenPeledRotate2D(tenB, guess[0], pvlData->ten1Eval[2], guess[3]);
TEN_T2M(matA, tenA);
TEN_T2M(matB, tenB);
ELL_3M_TRANSPOSE(rott, pvlData->ten1Evec);
ELL_3M_MUL(matTmp, matA, pvlData->ten1Evec);
ELL_3M_MUL(matA, rott, matTmp);
ELL_3M_MUL(matTmp, matB, pvlData->ten1Evec);
ELL_3M_MUL(matB, rott, matTmp);
/* Copy two two tensors */
/* guess[1] is population fraction of first tensor */
if (guess[1] > 0.5) {
twoTen[7] = guess[1];
TEN_M2T(twoTen + 0, matA);
TEN_M2T(twoTen + 7, matB);
} else {
twoTen[7] = 1 - guess[1];
TEN_M2T(twoTen + 0, matB);
TEN_M2T(twoTen + 7, matA);
}
twoTen[0] = 1;
}
#undef PARAMS
#else
double *twoTen;
twoTen = pvl->directAnswer[tenDwiGage2TensorPeled];
TEN_T_SET(twoTen + 0, AIR_NAN, AIR_NAN, AIR_NAN, AIR_NAN,
AIR_NAN, AIR_NAN, AIR_NAN);
TEN_T_SET(twoTen + 7, AIR_NAN, AIR_NAN, AIR_NAN, AIR_NAN,
AIR_NAN, AIR_NAN, AIR_NAN);
fprintf(stderr, "%s: sorry, not compiled with TEEM_LEVMAR\n", me);
#endif
}
static PyObject * FitWrap_fitCalibrationParameters(PyObject *self, PyObject *args) {
PyArrayObject * xValues;
PyArrayObject * yValues;
PyArrayObject * qReal;
PyArrayObject * intensity;
int covarianceMatrixDimensions[2];
PyArrayObject * covarianceMatrix;
double centerX,centerY,distance,energy,alpha,beta,rotation;
int fixedCenterX,fixedCenterY,fixedDistance,fixedEnergy,fixedAlpha,fixedBeta,fixedRotation;
double pixelLength,pixelHeight;
struct everything * useful;
double p[7]; // the parameters
double lb[7]; // lower bounds
double ub[7]; // upper bounds
int status;
int length;
double info[LM_INFO_SZ];
PyArg_ParseTuple(args,"O!O!O!O!didididididididd",
&PyArray_Type,&xValues,
&PyArray_Type,&yValues,
&PyArray_Type,&qReal,
&PyArray_Type,&intensity,
¢erX,&fixedCenterX,¢erY,&fixedCenterY,
&distance,&fixedDistance,&energy,&fixedEnergy,
&alpha,&fixedAlpha,&beta,&fixedBeta,
&rotation,&fixedRotation,&pixelLength,&pixelHeight);
if (xValues->nd != 1 || xValues->descr->type_num != PyArray_DOUBLE) {
PyErr_SetString(PyExc_ValueError,
"First array to function Fit must be a 1 dimensional array of doubles.");
return 0;
}
if (yValues->nd != 1 || yValues->descr->type_num != PyArray_DOUBLE) {
PyErr_SetString(PyExc_ValueError,
"Second array to function Fit must be a 1 dimensional array of doubles.");
return 0;
}
if (qReal->nd != 1 || qReal->descr->type_num != PyArray_DOUBLE) {
PyErr_SetString(PyExc_ValueError,
"Third array to function Fit must be a 1 dimensional array of doubles.");
return 0;
}
if (intensity->nd != 1 || intensity->descr->type_num != PyArray_DOUBLE) {
PyErr_SetString(PyExc_ValueError,
"Fourth array to function Fit must be a 1 dimensional array of doubles.");
return 0;
}
if (intensity->dimensions[0] <= 7) {
PyErr_SetString(PyExc_ValueError,
"Cannot fit calibration data because the calibration parameters must be fit to at least 8 peaks.");
return 0;
}
covarianceMatrixDimensions[0]=7;
covarianceMatrixDimensions[1]=7;
covarianceMatrix = (PyArrayObject *)PyArray_FromDims(2,covarianceMatrixDimensions,PyArray_DOUBLE);
length = xValues->dimensions[0];
useful = malloc( sizeof(struct everything) );
length = xValues->dimensions[0];
useful->x= (double *)xValues->data;
useful->y= (double *)yValues->data;
useful->intensity = (double *)intensity->data;
useful->pixelLength = pixelLength;
useful->pixelHeight = pixelHeight;
// initial guesses
p[0] = centerX;
p[1] = centerY;
p[2] = distance;
p[3] = energy;
p[4] = alpha;
p[5] = beta;
p[6] = rotation;
// set the bounds on parameters
if (fixedCenterX) {
lb[0] = centerX;
ub[0] = centerY;
} else {
lb[0] = -FLT_MAX;
ub[0] = FLT_MAX;
}
if (fixedCenterY) {
lb[1] = centerY;
ub[1] = centerY;
} else {
lb[1] = -FLT_MAX;
ub[1] = FLT_MAX;
}
if (fixedDistance) {
lb[2] = distance;
ub[2] = distance;
} else {
lb[2] = 0;
ub[2] = FLT_MAX;
}
if (fixedEnergy) {
lb[3] = energy;
ub[3] = energy;
} else {
lb[3] = 0;
ub[3] = FLT_MAX;
}
if (fixedAlpha) {
lb[4] = alpha;
ub[4] = alpha;
} else {
lb[4] = -90;
ub[4] = 90;
}
if (fixedBeta) {
lb[5] = beta;
ub[5] = beta;
} else {
lb[5] = -90;
ub[5] = 90;
}
if (fixedRotation) {
lb[6] = rotation;
ub[6] = rotation;
} else {
lb[6] = -360;
ub[6] = 360;
}
printf(" - Before fitting, the calculated residual is %e\n",
getTotalResidual(p,(double *)qReal->data, 7, length, (void *)useful));
printf(" - Doing the fitting\n");
status=dlevmar_bc_dif(residual, p, (double *)qReal->data, 7, length, lb, ub,
10000,NULL, info, NULL,(double *)covarianceMatrix->data,(void *)useful);
printf(" - After fitting, the calculated residual is %e\n",
getTotalResidual(p,(double *)qReal->data, 7, length, (void *)useful));
// the fit gets stored in p, so we can get rid of useful
free(useful);
printf(" - Reason for quitting the fit: %d-",(int)info[6]);
if (info[6] == 2) {
printf("stopped by small gradient J^T e\n");
} else if (info[6] == 2) {
printf("stopped by small Dp\n");
} else if (info[6] == 3) {
printf("stopped by itmax\n");
} else if (info[6] == 4) {
printf("singular matrix. Restart from current p with increased \\mu\n");
} else if (info[6] == 5) {
printf("no further error reduction is possible. Restart with increased mu\n");
} else if (info[6] == 6) {
printf("stopped by small ||e||_2\n");
} else {
printf("\n");
}
if (status <= 0) {
// When unsuccessful operation, return None
printf(" - Status = %d\n",status);
PyErr_SetString(PyExc_Exception,"Fit unsuccessful.");
return 0;
}
// the reason I have to do the N is described at
// http://mail.python.org/pipermail/python-list/2002-October/167549.html
return Py_BuildValue("dddddddN",p[0],p[1],p[2],p[3],p[4],p[5],p[6],covarianceMatrix);
// return the data to the user
}
