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 }