int main() { const int n=40, m=3; // 40 measurements, 3 parameters double p[m], x[n], opts[LM_OPTS_SZ], info[LM_INFO_SZ]; register int i; int ret; /* generate some measurement using the exponential model with * parameters (5.0, 0.1, 1.0), corrupted with zero-mean * Gaussian noise of s=0.1 */ INIT_RANDOM(0); for(i=0; i<n; ++i) x[i]=(5.0*exp(-0.1*i) + 1.0) + gNoise(0.0, 0.1); /* initial parameters estimate: (1.0, 0.0, 0.0) */ p[0]=1.0; p[1]=0.0; p[2]=0.0; /* optimization control parameters; passing to levmar NULL instead of opts reverts to defaults */ 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 /* invoke the optimization function */ //ret=dlevmar_der(expfunc, jacexpfunc, p, x, m, n, 1000, opts, info, NULL, NULL, NULL); // with analytic Jacobian ret=dlevmar_dif(expfunc, p, x, m, n, 1000, opts, info, NULL, NULL, NULL); // without Jacobian printf("Levenberg-Marquardt returned in %g iter, reason %g, sumsq %g [%g]\n", info[5], info[6], info[1], info[0]); printf("Best fit parameters: %.7g %.7g %.7g\n", p[0], p[1], p[2]); exit(0); }
extern "C" LEVMARDLL_API void Rhofit(BoxReflSettings* InitStruct, double parameters[], double covariance[], int parametersize, double info[]) { USES_CONVERSION; double ChiSquare = 0; double opts[LM_OPTS_SZ]; double* xvec = new double[InitStruct->ZLength] ; double *work, *covar; 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 RhoCalc Rho; Rho.init(InitStruct); //Allocate a dummy array - Our real calculation is done in Refl.objective memset(xvec, 0, InitStruct->ZLength*sizeof(double)); //Allocate workspace and our covariance matrix work=new double[((LM_DIF_WORKSZ(parametersize, InitStruct->ZLength)+parametersize*InitStruct->ZLength))]; covar=work+LM_DIF_WORKSZ(parametersize, InitStruct->ZLength); dlevmar_dif(Rho.objective, parameters, xvec, parametersize,InitStruct->ZLength, 1000, opts, info, work, covar,(void*)(&Rho)); //Calculate the standard deviations in the parameters for(int i = 0; i< parametersize;i++) { covariance[i] = sqrt(covar[i*(parametersize+1)]); } delete xvec; delete work; }
void binTriangulate(const double K0[9], const double R0[9], const double t0[3], const double K1[9], const double R1[9], const double t1[3], const double m0[2], const double m1[2], double x[3]) { double Q0[9], q0[3], Q1[9], q1[3]; mat33AB(K0, R0, Q0); mat33ProdVec(K0, t0, q0); mat33AB(K1, R1, Q1); mat33ProdVec(K1, t1, q1); double A[12]; double b[4]; A[0] = Q0[0] - m0[0] * Q0[6]; A[1] = Q0[1] - m0[0] * Q0[7]; A[2] = Q0[2] - m0[0] * Q0[8]; A[3] = Q0[3] - m0[1] * Q0[6]; A[4] = Q0[4] - m0[1] * Q0[7]; A[5] = Q0[5] - m0[1] * Q0[8]; A[6] = Q1[0] - m1[0] * Q1[6]; A[7] = Q1[1] - m1[0] * Q1[7]; A[8] = Q1[2] - m1[0] * Q1[8]; A[9] = Q1[3] - m1[1] * Q1[6]; A[10] = Q1[4] - m1[1] * Q1[7]; A[11] = Q1[5] - m1[1] * Q1[8]; b[0] = q0[2] * m0[0] - q0[0]; b[1] = q0[2] * m0[1] - q0[1]; b[2] = q1[2] * m1[0] - q1[0]; b[3] = q1[2] * m1[1] - q1[1]; /* Find the least squares result */ dgelsyFor(4, 3, 1, A, b, x); /* Run a non-linear optimization to refine the result */ CamProjParam param[2]; param[0].set(K0, R0, t0, m0); param[1].set(K1, R1, t1, m1); const int maxIter = 5; double retInfo[LM_INFO_SZ]; dlevmar_dif(residual2View, x, 0, 3, 4, maxIter, 0, retInfo, 0, 0, param); }
void triangulateMultiView(int nview, const double* Rs, const double* ts, const double nms[], double x[]) { double * A = new double[nview * 2 * 3]; double * b = new double[nview * 2]; double* pA = A; double* pb = b; const double* pR = Rs; const double* pT = ts; for (int i = 0; i < nview; i++) { double u = nms[2 * i]; double v = nms[2 * i + 1]; pA[0] = u * pR[6] - pR[0]; pA[1] = u * pR[7] - pR[1]; pA[2] = u * pR[8] - pR[2]; pA[3] = v * pR[6] - pR[3]; pA[4] = v * pR[7] - pR[4]; pA[5] = v * pR[8] - pR[5]; pb[0] = pT[0] - u * pT[2]; pb[1] = pT[1] - v * pT[2]; pA += 6; pb += 2; pR += 9; pT += 3; } /* Find the least squares result */ dgelsyFor(nview * 2, 3, 1, A, b, x); CamProjParam* param = new CamProjParam[nview]; for (int i = 0; i < nview; i++) { param[i].set(0, Rs + i * 9, ts + i * 3, nms + 2 * i); } const int maxIter = 5; double retInfo[LM_INFO_SZ * 2]; dlevmar_dif(residualMultiView, x, 0, 3, nview * 2, maxIter, 0, retInfo, 0, 0, param); delete[] A; delete[] b; delete[] param; }
void stack_levmar_dif( double* ygiven, double* p, const int m, const int n, void* data ) { double opts[LM_OPTS_SZ], info[LM_INFO_SZ]; // printf( "HELLO from stack_levmar_dif\n"); StackData *sdata = (StackData*)data; int x_dim = sdata->x_dim; int x_len = sdata->x_len; // printf( "x_len: %d x_dim: %d\n",x_len,x_dim); // optimization control parameters; passing to levmar NULL instead of opts reverts to defaults 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 // invoke the optimization function dlevmar_dif(stack_func, p, ygiven, m, n, 1000, opts, info, NULL, NULL, data); // without Jacobian }
void triangulateMultiView(int nview, const double* Rs[], const double* ts[], const double* ms[], double x[]) { double * A = new double[nview * 2 * 3]; double * b = new double[nview * 2]; double* pA = A; double* pb = b; for (int i = 0; i < nview; i++) { double u = ms[i][0]; double v = ms[i][1]; pA[0] = u * Rs[i][6] - Rs[i][0]; pA[1] = u * Rs[i][7] - Rs[i][1]; pA[2] = u * Rs[i][8] - Rs[i][2]; pA[3] = v * Rs[i][6] - Rs[i][3]; pA[4] = v * Rs[i][7] - Rs[i][4]; pA[5] = v * Rs[i][8] - Rs[i][5]; pb[0] = ts[i][0] - u * ts[i][2]; pb[1] = ts[i][1] - v * ts[i][2]; pA += 6; pb += 2; } /* Find the least squares result */ dgelsyFor(nview * 2, 3, 1, A, b, x); CamProjParam* param = new CamProjParam[nview]; for (int i = 0; i < nview; i++) { param[i].set(0, Rs[i], ts[i], ms[i]); } const int maxIter = 5; double retInfo[LM_INFO_SZ]; dlevmar_dif(residualMultiView, x, 0, 3, nview * 2, maxIter, 0, retInfo, 0, 0, param); delete[] A; delete[] b; delete[] param; }
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; }
void refineTriangulation(int nview, const double* K, const double* R, const double* t, const double pts2d[], double x[], double* err) { CamProjParam* param = new CamProjParam[nview]; for (int i = 0; i < nview; i++) { param[i].set(K == 0 ? 0 : K + i * 9, R + i * 9, t + i * 3, pts2d + 2 * i); } const int maxIter = 5; double retInfo[LM_INFO_SZ]; dlevmar_dif(residualMultiView, x, 0, 3, nview * 2, maxIter, 0, retInfo, 0, 0, param); if (err) { *err = 0; const double* pK = K; const double* pR = R; const double* pT = t; double pt[2]; for (int i = 0; i < nview; i++) { if (pK) project(pK, pR, pT, x, pt); else project(pR, pT, x, pt); double dx = pts2d[2 * i] - pt[0]; double dy = pts2d[2 * i + 1] - pt[1]; *err += dx * dx + dy * dy; pK += 9; pR += 9; pT += 3; } *err = sqrt(*err / nview); } delete[] param; }
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 PhotometricOptimizer::optimizePhotometric(PanoramaData & pano, const OptimizeVector & vars, const std::vector<vigra_ext::PointPairRGB> & correspondences, AppBase::ProgressReporter & progress, double & error) { OptimizeVector photometricVars; // keep only the photometric variables unsigned int optCount=0; for (OptimizeVector::const_iterator it=vars.begin(); it != vars.end(); ++it) { std::set<std::string> cvars; for (std::set<std::string>::const_iterator itv = (*it).begin(); itv != (*it).end(); ++itv) { if ((*itv)[0] == 'E' || (*itv)[0] == 'R' || (*itv)[0] == 'V') { cvars.insert(*itv); } } photometricVars.push_back(cvars); optCount+=cvars.size(); } //if no variables to optimize return if(optCount==0) { return; }; int nMaxIter = 250; OptimData data(pano, photometricVars, correspondences, 5/255.0, false, nMaxIter, progress); int ret; //double opts[LM_OPTS_SZ]; double info[LM_INFO_SZ]; // parameters int m=data.m_vars.size(); vigra::ArrayVector<double> p(m, 0.0); // vector for errors int n=2*3*correspondences.size()+pano.getNrOfImages(); vigra::ArrayVector<double> x(n, 0.0); data.ToX(p.begin()); #ifdef DEBUG printf("Parameters before optimisation: "); for(int i=0; i<m; ++i) printf("%.7g ", p[i]); printf("\n"); #endif // covariance matrix at solution vigra::DImage cov(m,m); // TODO: setup optimisation options with some good defaults. double optimOpts[5]; optimOpts[0] = 1E-03; // init mu // stop thresholds optimOpts[1] = 1e-5; // ||J^T e||_inf optimOpts[2] = 1e-5; // ||Dp||_2 optimOpts[3] = 1e-1; // ||e||_2 // difference mode optimOpts[4] = LM_DIFF_DELTA; ret=dlevmar_dif(&photometricError, &photometricVis, &(p[0]), &(x[0]), m, n, nMaxIter, optimOpts, info, NULL, &(cov(0,0)), &data); // no jacobian // copy to source images (data.m_imgs) data.FromX(p.begin()); // calculate error at solution data.huberSigma = 0; photometricError(&(p[0]), &(x[0]), m, n, &data); error = 0; for (int i=0; i<n; i++) { error += x[i]*x[i]; } error = sqrt(error/n); #ifdef DEBUG printf("Levenberg-Marquardt returned %d in %g iter, reason %g\nSolution: ", ret, info[5], info[6]); for(int i=0; i<m; ++i) printf("%.7g ", p[i]); printf("\n\nMinimization info:\n"); for(int i=0; i<LM_INFO_SZ; ++i) printf("%g ", info[i]); printf("\n"); #endif // copy settings to panorama for (unsigned i=0; i<pano.getNrOfImages(); i++) { pano.setSrcImage(i, data.m_imgs[i]); } }
bool ccCalibratedImage::computeOrthoRectificationParams(CCLib::GenericIndexedCloud* keypoints3D, std::vector<KeyPoint>& keypointsImage, double a_out[], double b_out[], double c_out[]) const { if (!keypoints3D) return false; unsigned count = (unsigned)keypointsImage.size(); if (count<4) return false; //first guess for X (a0 a1 a2 b0 b1 b2 c1 c2) double norm = (double)std::max(m_width,m_height); double X0[8]; X0[0] = 1.0/sqrt(norm); X0[1] = 1.0/norm; X0[2] = 1.0/norm; X0[3] = 1.0/sqrt(norm); X0[4] = 1.0/norm; X0[5] = 1.0/norm; X0[6] = 1.0/norm; X0[7] = 1.0/norm; #ifndef USE_LEVMAR //compute the A matrix and b vector unsigned Neq = 2*count; //number of equations double *A = new double[8*Neq]; // 8 coefficients: a0 a1 a2 b0 b1 b2 c1 c2 double *b = new double[Neq]; //for all points { double* _A=A; double* _b=b; for (unsigned i=0;i<count;++i) { const KeyPoint& kp = keypointsImage[i]; double kpx = (double)kp.x; double kpy = (double)kp.y; const CCVector3* P = keypoints3D->getPoint(kp.index); *_A++ = 1.0; *_A++ = kpx; *_A++ = kpy; *_A++ = 0.0; *_A++ = 0.0; *_A++ = 0.0; *_A++ = -kpx * (double)P->x; *_A++ = -kpy * (double)P->x; *_b++ = (double)P->x; *_A++ = 0.0; *_A++ = 0.0; *_A++ = 0.0; *_A++ = 1.0; *_A++ = kpx; *_A++ = kpy; *_A++ = -kpx * (double)P->y; *_A++ = -kpy * (double)P->y; *_b++ = (double)P->y; } } //conjugate gradient initialization //we solve tA.A.X=tA.b CCLib::ConjugateGradient<8,double> cg; CCLib::SquareMatrixd& tAA = cg.A(); double* tAb = cg.b(); //compute tA.A and tA.b { for (unsigned i=0; i<8; ++i) { //tA.A part for (unsigned j=i; j<8; ++j) { double sum_prod = 0; const double* _Ai = A+i; const double* _Aj = A+j; for (unsigned k=0; k<Neq; ++k) { //sum_prod += A[(8*2*k)+i]*A[(8*2*k)+j]; sum_prod += (*_Ai) * (*_Aj); _Ai += 8; _Aj += 8; } tAA.m_values[j][i] = tAA.m_values[i][j] = sum_prod; } //tA.b part { double sum_prod = 0; const double* _Ai = A+i; const double* _b = b; for (unsigned k=0; k<Neq; ++k) { //sum_prod += A[(8*2*k)+i]*b[k]; sum_prod += (*_Ai) * (*_b++); _Ai += 8; } tAb[i] = sum_prod; } } } //init. conjugate gradient cg.initConjugateGradient(X0); //conjugate gradient iterations { double convergenceThreshold = 1e-8/* * norm*/; //max. error for convergence for (unsigned i=0; i<1500; ++i) { double lastError = cg.iterConjugateGradient(X0); if (lastError < convergenceThreshold) //converged { #ifdef _DEBUG printf("[computeOrthoRectificationParams] Convergence reached in %i iterations (error: %g)\n",i+1,lastError); #endif break; } } } //inverse normalization //for (unsigned i=0;i<8;++i) // X0[i] *= norm; delete[] A; A=0; delete[] b; b=0; #else double* A = new double[count*4]; double* _A=A; for (unsigned i=0;i<count;++i) { const KeyPoint& kp = keypointsImage[i]; CCVector3 P = *keypoints3D->getPoint(kp.index); //m_trans.apply(P); *_A++ = kp.x; *_A++ = kp.y; *_A++ = P.x; *_A++ = P.y; } //LM minimization double info[LM_INFO_SZ]; s_funcStepIteration=0; dlevmar_dif(func_step, X0, NULL, 8, count*2, 5000, 0, info, 0, 0, A); //numerical derivatives delete[] A; A=0; #endif a_out[0] = X0[0]; a_out[1] = X0[1]; a_out[2] = X0[2]; b_out[0] = X0[3]; b_out[1] = X0[4]; b_out[2] = X0[5]; c_out[0] = 1.0; c_out[1] = X0[6]; c_out[2] = X0[7]; return true; }
qreal CurveFitter::fit(const PointArray<256> &points, PointArray<256> &curve, Transformation transformation) { /* Init input data */ int sz = 2 * points.count(); qreal *pdata = const_cast<qreal*>(points.data()); qreal *x; qreal mean[2] = {.0, .0}; qreal std[2] = {.0, .0}; if (transformation == AFFINE) { x = new qreal[sz]; for (int i = 0; i < sz; i += 2) { mean[0] += pdata[i]; mean[1] += pdata[i + 1]; } mean[0] /= (sz / 2); mean[1] /= (sz / 2); for (int i = 0; i < sz; i += 2) { std[0] += (pdata[i] - mean[0]) * (pdata[i] - mean[0]); std[1] += (pdata[i + 1] - mean[1]) * (pdata[i + 1] - mean[1]); } std[0] = qSqrt(std[0] / (sz / 2)); std[1] = qSqrt(std[1] / (sz / 2)); qreal minStd = qMin(std[0], std[1]); std[0] /= minStd; std[1] /= minStd; for (int i = 0; i < sz; i += 2) { x[i] = (pdata[i] - mean[0]) / std[0]; x[i + 1] = (pdata[i + 1] - mean[1]) / std[1]; } } else { x = pdata; } InternalData data(sz / 2); curve.resize(SPLINE_ORDER); data.pxy = curve.data(); chordLengthParam(sz / 2, x, data.ts, CHORD_LENGTH); qreal segmentLen = (sz / (SPLINE_ORDER - 1)); /* Init middle points of Bezier curve */ for (int i = 2; i < SPLINE_SIZE - 2; i += 2) { int idx = (i / 2) * segmentLen; idx -= idx % 2; data.pxy[i] = x[idx]; data.pxy[i + 1] = x[idx + 1]; } /* Init first point of Bezier curve */ data.pxy[0] = x[0]; data.pxy[1] = x[1]; int idx = (segmentLen - (int)segmentLen % 2); data.pxy[2] = (x[idx] - x[0]) * 2 + x[0]; data.pxy[3] = (x[idx + 1] - x[1]) * 2 + x[1]; data.pxy[SPLINE_SIZE - 4] = (x[sz - 2 - idx] - x[sz - 2]) * 2 + x[sz - 2]; data.pxy[SPLINE_SIZE - 3] = (x[sz - 1 - idx] - x[sz - 1]) * 2 + x[sz - 1]; /* Init last point of Bezier curve */ data.pxy[SPLINE_SIZE - 2] = x[sz - 2]; data.pxy[SPLINE_SIZE - 1] = x[sz - 1]; /* info[0]= ||e||_2 at initial p. * info[1-4]=[ ||e||_2, ||J^T e||_inf, ||Dp||_2, \mu/max[J^T J]_ii ], all computed at estimated p. * info[5]= # iterations, * info[6]=reason for terminating: 1 - stopped by small gradient J^T e * 2 - stopped by small Dp * 3 - stopped by itmax * 4 - singular matrix. Restart from current p with increased \mu * 5 - no further error reduction is possible. Restart with increased mu * 6 - stopped by small ||e||_2 * 7 - stopped by invalid (i.e. NaN or Inf) "func" values; a user error * info[7]= # function evaluations * info[8]= # Jacobian evaluations * info[9]= # linear systems solved, i.e. # attempts for reducing error */ qreal info[LM_INFO_SZ]; qreal *p = data.pxy + 2; int m = SPLINE_SIZE - 2 * 2; int n = sz; qreal fnorm = INT_MAX, fnormPrev; int totalIters = 0; do { /* Optimize spline shape */ dlevmar_dif(CurveFitter::func, p, x, m, n, MAX_ITER, NULL, info, NULL, NULL, &data); /* Residuals */ fnormPrev = fnorm; fnorm = info[1] / sz; qDebug() << "Termination reason" << info[6] << "after" << info[5] << "iterations" << "error" << fnorm; totalIters += info[5]; /* Quit, when improvement is less than 1% */ if ((fnormPrev - fnorm) / fnormPrev < 0.01) break; /* Optimize point parameters */ reparametrizePoints(data.pxy, sz / 2, x, data.ts); } while (true); qDebug() << "Total iterations" << totalIters; if (transformation == AFFINE) { delete [] x; for (int i = 0; i < SPLINE_SIZE; i += 2) { data.pxy[i] = data.pxy[i] * std[0] + mean[0]; data.pxy[i + 1] = data.pxy[i + 1] * std[1] + mean[1]; } } return fnorm; }
// 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); }
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); }
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!"); }
int QxrdFitterPeakPoint::fit() { int niter = -1; if (m_CenterFinder) { double x = m_X0, y = m_Y0; // if (qcepDebug(DEBUG_FITTING) || m_CenterFinder->get_PeakFitDebug()) { // m_CenterFinder -> printMessage(QObject::tr("Fitting i: %1, x0: %2, y0: %3") // .arg(index()).arg(m_X0).arg(m_Y0)); // } int width = 0, height = 0; QcepDoubleImageDataPtr data(m_CenterFinder->data()); if (data) { width = data -> get_Width()+1; height = data -> get_Height()+1; if (x<0 || x>width || y<0 || y>height) { m_Reason = OutsideData; } else { double dr = m_CenterFinder->get_PeakFitRadius(); double bkgd = ( m_CenterFinder->imageValue(x+dr, y) + m_CenterFinder->imageValue(x, y+dr) + m_CenterFinder->imageValue(x-dr, y) + m_CenterFinder->imageValue(x, y-dr) )/4.0; double pkht = m_CenterFinder->imageValue(x,y) - bkgd; double parms[7]; parms[0] = x; parms[1] = y; parms[2] = 2.0; parms[3] = pkht; parms[4] = bkgd; parms[5] = 0; parms[6] = 0; double info[LM_INFO_SZ]; int n = m_CenterFinder->get_PeakFitRadius()*2+1; niter = dlevmar_dif(QxrdFitterPeakPoint::staticEvaluate, parms, NULL, 7, n*n, m_CenterFinder->get_PeakFitIterations(), NULL, info, NULL, NULL, this); if (niter > 0) { m_Reason = Successful; m_FittedX = parms[0]; m_FittedY = parms[1]; m_FittedWidth = parms[2]; m_FittedHeight = parms[3]; m_FittedBkgd = parms[4]; m_FittedBkgdX = parms[5]; m_FittedBkgdY = parms[6]; double dx = m_FittedX - m_X0; double dy = m_FittedY - m_Y0; double dr = sqrt(dx*dx + dy*dy); if ((fabs(m_FittedWidth)<m_CenterFinder->get_FittedWidthMin()) || (fabs(m_FittedWidth)>m_CenterFinder->get_FittedWidthMax())) { m_Reason = BadWidth; } else if ((m_FittedHeight/m_Pkht)<m_CenterFinder->get_FittedHeightMinRatio()) { m_Reason = BadHeight; } else if (dr>m_CenterFinder->get_FittedPositionMaxDistance()) { m_Reason = BadPosition; } } } } } return niter; }
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); }
int main() { register int i, j; int problem, ret; double p[5], // 5 is max(2, 3, 5) x[16]; // 16 is max(2, 3, 5, 6, 16) int m, n; double opts[LM_OPTS_SZ], info[LM_INFO_SZ]; char *probname[]={ "Rosenbrock function", "modified Rosenbrock problem", "Powell's function", "Wood's function", "Meyer's (reformulated) problem", "Osborne's problem", "helical valley function", "Boggs & Tolle's problem #3", "Hock - Schittkowski problem #28", "Hock - Schittkowski problem #48", "Hock - Schittkowski problem #51", "Hock - Schittkowski problem #01", "Hock - Schittkowski modified problem #21", "hatfldb problem", "hatfldc problem", "equilibrium combustion problem", "Hock - Schittkowski modified #1 problem #52", "Schittkowski modified problem #235", "Boggs & Tolle modified problem #7", "Hock - Schittkowski modified #2 problem #52", "Hock - Schittkowski modified problem #76", }; 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 Jacobian is approximated using finite differences; specifies forward differencing //opts[4]=-LM_DIFF_DELTA; // specifies central differencing to approximate Jacobian; more accurate but more expensive to compute! /* uncomment the appropriate line below to select a minimization problem */ problem= //0; // Rosenbrock function //1; // modified Rosenbrock problem //2; // Powell's function //3; // Wood's function 4; // Meyer's (reformulated) problem //5; // Osborne's problem //6; // helical valley function #ifdef HAVE_LAPACK //7; // Boggs & Tolle's problem 3 //8; // Hock - Schittkowski problem 28 //9; // Hock - Schittkowski problem 48 //10; // Hock - Schittkowski problem 51 #else // no LAPACK #ifdef _MSC_VER #pragma message("LAPACK not available, some test problems cannot be used") #else #warning LAPACK not available, some test problems cannot be used #endif // _MSC_VER #endif /* HAVE_LAPACK */ //11; // Hock - Schittkowski problem 01 //12; // Hock - Schittkowski modified problem 21 //13; // hatfldb problem //14; // hatfldc problem //15; // equilibrium combustion problem #ifdef HAVE_LAPACK //16; // Hock - Schittkowski modified #1 problem 52 //17; // Schittkowski modified problem 235 //18; // Boggs & Tolle modified problem #7 //19; // Hock - Schittkowski modified #2 problem 52 //20; // Hock - Schittkowski modified problem #76" #endif /* HAVE_LAPACK */ switch(problem){ default: fprintf(stderr, "unknown problem specified (#%d)! Note that some minimization problems require LAPACK.\n", problem); exit(1); break; case 0: /* Rosenbrock function */ m=2; n=2; p[0]=-1.2; p[1]=1.0; for(i=0; i<n; i++) x[i]=0.0; ret=dlevmar_der(ros, jacros, p, x, m, n, 1000, opts, info, NULL, NULL, NULL); // with analytic Jacobian //ret=dlevmar_dif(ros, p, x, m, n, 1000, opts, info, NULL, NULL, NULL); // no Jacobian break; case 1: /* modified Rosenbrock problem */ m=2; n=3; p[0]=-1.2; p[1]=1.0; for(i=0; i<n; i++) x[i]=0.0; ret=dlevmar_der(modros, jacmodros, p, x, m, n, 1000, opts, info, NULL, NULL, NULL); // with analytic Jacobian //ret=dlevmar_dif(modros, p, x, m, n, 1000, opts, info, NULL, NULL, NULL); // no Jacobian break; case 2: /* Powell's function */ m=2; n=2; p[0]=3.0; p[1]=1.0; for(i=0; i<n; i++) x[i]=0.0; ret=dlevmar_der(powell, jacpowell, p, x, m, n, 1000, opts, info, NULL, NULL, NULL); // with analytic Jacobian //ret=dlevmar_dif(powell, p, x, m, n, 1000, opts, info, NULL, NULL, NULL); // no Jacobian break; case 3: /* Wood's function */ m=4; n=6; p[0]=-3.0; p[1]=-1.0; p[2]=-3.0; p[3]=-1.0; for(i=0; i<n; i++) x[i]=0.0; ret=dlevmar_dif(wood, p, x, m, n, 1000, opts, info, NULL, NULL, NULL); // no Jacobian break; case 4: /* Meyer's data fitting problem */ m=3; n=16; p[0]=8.85; p[1]=4.0; p[2]=2.5; x[0]=34.780; x[1]=28.610; x[2]=23.650; x[3]=19.630; x[4]=16.370; x[5]=13.720; x[6]=11.540; x[7]=9.744; x[8]=8.261; x[9]=7.030; x[10]=6.005; x[11]=5.147; x[12]=4.427; x[13]=3.820; x[14]=3.307; x[15]=2.872; //ret=dlevmar_der(meyer, jacmeyer, p, x, m, n, 1000, opts, info, NULL, NULL, NULL); // with analytic Jacobian { double *work, *covar; work=malloc((LM_DIF_WORKSZ(m, n)+m*m)*sizeof(double)); if(!work){ fprintf(stderr, "memory allocation request failed in main()\n"); exit(1); } covar=work+LM_DIF_WORKSZ(m, n); ret=dlevmar_dif(meyer, p, x, m, n, 1000, opts, info, work, covar, NULL); // no Jacobian, caller allocates work memory, covariance estimated printf("Covariance of the fit:\n"); for(i=0; i<m; ++i){ for(j=0; j<m; ++j) printf("%g ", covar[i*m+j]); printf("\n"); } printf("\n"); free(work); } /* uncomment the following block to verify Jacobian */ /* { double err[16]; dlevmar_chkjac(meyer, jacmeyer, p, m, n, NULL, err); for(i=0; i<n; ++i) printf("gradient %d, err %g\n", i, err[i]); } */ break; case 5: /* Osborne's data fitting problem */ { double x33[]={ 8.44E-1, 9.08E-1, 9.32E-1, 9.36E-1, 9.25E-1, 9.08E-1, 8.81E-1, 8.5E-1, 8.18E-1, 7.84E-1, 7.51E-1, 7.18E-1, 6.85E-1, 6.58E-1, 6.28E-1, 6.03E-1, 5.8E-1, 5.58E-1, 5.38E-1, 5.22E-1, 5.06E-1, 4.9E-1, 4.78E-1, 4.67E-1, 4.57E-1, 4.48E-1, 4.38E-1, 4.31E-1, 4.24E-1, 4.2E-1, 4.14E-1, 4.11E-1, 4.06E-1}; m=5; n=33; p[0]=0.5; p[1]=1.5; p[2]=-1.0; p[3]=1.0E-2; p[4]=2.0E-2; ret=dlevmar_der(osborne, jacosborne, p, x33, m, n, 1000, opts, info, NULL, NULL, NULL); // with analytic Jacobian //ret=dlevmar_dif(osborne, p, x33, m, n, 1000, opts, info, NULL, NULL, NULL); // no Jacobian } break; case 6: /* helical valley function */ m=3; n=3; p[0]=-1.0; p[1]=0.0; p[2]=0.0; for(i=0; i<n; i++) x[i]=0.0; ret=dlevmar_der(helval, jachelval, p, x, m, n, 1000, opts, info, NULL, NULL, NULL); // with analytic Jacobian //ret=dlevmar_dif(helval, p, x, m, n, 1000, opts, info, NULL, NULL, NULL); // no Jacobian break; #ifdef HAVE_LAPACK case 7: /* Boggs-Tolle problem 3 */ m=5; n=5; p[0]=2.0; p[1]=2.0; p[2]=2.0; p[3]=2.0; p[4]=2.0; for(i=0; i<n; i++) x[i]=0.0; { double A[3*5]={1.0, 3.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 1.0, -2.0, 0.0, 1.0, 0.0, 0.0, -1.0}, b[3]={0.0, 0.0, 0.0}; ret=dlevmar_lec_der(bt3, jacbt3, p, x, m, n, A, b, 3, 1000, opts, info, NULL, NULL, NULL); // lin. constraints, analytic Jacobian //ret=dlevmar_lec_dif(bt3, p, x, m, n, A, b, 3, 1000, opts, info, NULL, NULL, NULL); // lin. constraints, no Jacobian } break; case 8: /* Hock - Schittkowski problem 28 */ m=3; n=3; p[0]=-4.0; p[1]=1.0; p[2]=1.0; for(i=0; i<n; i++) x[i]=0.0; { double A[1*3]={1.0, 2.0, 3.0}, b[1]={1.0}; ret=dlevmar_lec_der(hs28, jachs28, p, x, m, n, A, b, 1, 1000, opts, info, NULL, NULL, NULL); // lin. constraints, analytic Jacobian //ret=dlevmar_lec_dif(hs28, p, x, m, n, A, b, 1, 1000, opts, info, NULL, NULL, NULL); // lin. constraints, no Jacobian } break; case 9: /* Hock - Schittkowski problem 48 */ m=5; n=5; p[0]=3.0; p[1]=5.0; p[2]=-3.0; p[3]=2.0; p[4]=-2.0; for(i=0; i<n; i++) x[i]=0.0; { double A[2*5]={1.0, 1.0, 1.0, 1.0, 1.0, 0.0, 0.0, 1.0, -2.0, -2.0}, b[2]={5.0, -3.0}; ret=dlevmar_lec_der(hs48, jachs48, p, x, m, n, A, b, 2, 1000, opts, info, NULL, NULL, NULL); // lin. constraints, analytic Jacobian //ret=dlevmar_lec_dif(hs48, p, x, m, n, A, b, 2, 1000, opts, info, NULL, NULL, NULL); // lin. constraints, no Jacobian } break; case 10: /* Hock - Schittkowski problem 51 */ m=5; n=5; p[0]=2.5; p[1]=0.5; p[2]=2.0; p[3]=-1.0; p[4]=0.5; for(i=0; i<n; i++) x[i]=0.0; { double A[3*5]={1.0, 3.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 1.0, -2.0, 0.0, 1.0, 0.0, 0.0, -1.0}, b[3]={4.0, 0.0, 0.0}; ret=dlevmar_lec_der(hs51, jachs51, p, x, m, n, A, b, 3, 1000, opts, info, NULL, NULL, NULL); // lin. constraints, analytic Jacobian //ret=dlevmar_lec_dif(hs51, p, x, m, n, A, b, 3, 1000, opts, info, NULL, NULL, NULL); // lin. constraints, no Jacobian } break; #endif /* HAVE_LAPACK */ case 11: /* Hock - Schittkowski problem 01 */ m=2; n=2; p[0]=-2.0; p[1]=1.0; for(i=0; i<n; i++) x[i]=0.0; //ret=dlevmar_der(hs01, jachs01, p, x, m, n, 1000, opts, info, NULL, NULL, NULL); // with analytic Jacobian { double lb[2], ub[2]; lb[0]=-DBL_MAX; lb[1]=-1.5; ub[0]=ub[1]=DBL_MAX; ret=dlevmar_bc_der(hs01, jachs01, p, x, m, n, lb, ub, 1000, opts, info, NULL, NULL, NULL); // with analytic Jacobian } break; case 12: /* Hock - Schittkowski (modified) problem 21 */ m=2; n=2; p[0]=-1.0; p[1]=-1.0; for(i=0; i<n; i++) x[i]=0.0; //ret=dlevmar_der(hs21, jachs21, p, x, m, n, 1000, opts, info, NULL, NULL, NULL); // with analytic Jacobian { double lb[2], ub[2]; lb[0]=2.0; lb[1]=-50.0; ub[0]=50.0; ub[1]=50.0; ret=dlevmar_bc_der(hs21, jachs21, p, x, m, n, lb, ub, 1000, opts, info, NULL, NULL, NULL); // with analytic Jacobian } break; case 13: /* hatfldb problem */ m=4; n=4; p[0]=p[1]=p[2]=p[3]=0.1; for(i=0; i<n; i++) x[i]=0.0; //ret=dlevmar_der(hatfldb, jachatfldb, p, x, m, n, 1000, opts, info, NULL, NULL, NULL); // with analytic Jacobian { double lb[4], ub[4]; lb[0]=lb[1]=lb[2]=lb[3]=0.0; ub[0]=ub[2]=ub[3]=DBL_MAX; ub[1]=0.8; ret=dlevmar_bc_der(hatfldb, jachatfldb, p, x, m, n, lb, ub, 1000, opts, info, NULL, NULL, NULL); // with analytic Jacobian } break; case 14: /* hatfldc problem */ m=4; n=4; p[0]=p[1]=p[2]=p[3]=0.9; for(i=0; i<n; i++) x[i]=0.0; //ret=dlevmar_der(hatfldc, jachatfldc, p, x, m, n, 1000, opts, info, NULL, NULL, NULL); // with analytic Jacobian { double lb[4], ub[4]; lb[0]=lb[1]=lb[2]=lb[3]=0.0; ub[0]=ub[1]=ub[2]=ub[3]=10.0; ret=dlevmar_bc_der(hatfldc, jachatfldc, p, x, m, n, lb, ub, 1000, opts, info, NULL, NULL, NULL); // with analytic Jacobian } break; case 15: /* equilibrium combustion problem */ m=5; n=5; p[0]=p[1]=p[2]=p[3]=p[4]=0.0001; for(i=0; i<n; i++) x[i]=0.0; //ret=dlevmar_der(combust, jaccombust, p, x, m, n, 1000, opts, info, NULL, NULL, NULL); // with analytic Jacobian { double lb[5], ub[5]; lb[0]=lb[1]=lb[2]=lb[3]=lb[4]=0.0001; ub[0]=ub[1]=ub[2]=ub[3]=ub[4]=100.0; ret=dlevmar_bc_der(combust, jaccombust, p, x, m, n, lb, ub, 5000, opts, info, NULL, NULL, NULL); // with analytic Jacobian } break; #ifdef HAVE_LAPACK case 16: /* Hock - Schittkowski modified #1 problem 52 */ m=5; n=4; p[0]=2.0; p[1]=2.0; p[2]=2.0; p[3]=2.0; p[4]=2.0; for(i=0; i<n; i++) x[i]=0.0; { double A[3*5]={1.0, 3.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 1.0, -2.0, 0.0, 1.0, 0.0, 0.0, -1.0}, b[3]={0.0, 0.0, 0.0}; double lb[5], ub[5]; double weights[5]={2000.0, 2000.0, 2000.0, 2000.0, 2000.0}; // penalty terms weights lb[0]=-0.09; lb[1]=0.0; lb[2]=-DBL_MAX; lb[3]=-0.2; lb[4]=0.0; ub[0]=DBL_MAX; ub[1]=0.3; ub[2]=0.25; ub[3]=0.3; ub[4]=0.3; ret=dlevmar_blec_der(mod1hs52, jacmod1hs52, p, x, m, n, lb, ub, A, b, 3, weights, 1000, opts, info, NULL, NULL, NULL); // box & lin. constraints, analytic Jacobian //ret=dlevmar_blec_dif(mod1hs52, p, x, m, n, lb, ub, A, b, 3, weights, 1000, opts, info, NULL, NULL, NULL); // box & lin. constraints, no Jacobian } break; case 17: /* Schittkowski modified problem 235 */ m=3; n=2; p[0]=-2.0; p[1]=3.0; p[2]=1.0; for(i=0; i<n; i++) x[i]=0.0; { double A[2*3]={1.0, 0.0, 1.0, 0.0, 1.0, -4.0}, b[2]={-1.0, 0.0}; double lb[3], ub[3]; lb[0]=-DBL_MAX; lb[1]=0.1; lb[2]=0.7; ub[0]=DBL_MAX; ub[1]=2.9; ub[2]=DBL_MAX; ret=dlevmar_blec_der(mods235, jacmods235, p, x, m, n, lb, ub, A, b, 2, NULL, 1000, opts, info, NULL, NULL, NULL); // box & lin. constraints, analytic Jacobian //ret=dlevmar_blec_dif(mods235, p, x, m, n, lb, ub, A, b, 2, NULL, 1000, opts, info, NULL, NULL, NULL); // box & lin. constraints, no Jacobian } break; case 18: /* Boggs & Tolle modified problem 7 */ m=5; n=5; p[0]=-2.0; p[1]=1.0; p[2]=1.0; p[3]=1.0; p[4]=1.0; for(i=0; i<n; i++) x[i]=0.0; { double A[3*5]={1.0, 1.0, -1.0, 0.0, 0.0, 1.0, 1.0, 0.0, -1.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0}, b[3]={1.0, 0.0, 0.5}; double lb[5], ub[5]; lb[0]=-DBL_MAX; lb[1]=-DBL_MAX; lb[2]=-DBL_MAX; lb[3]=-DBL_MAX; lb[4]=-0.3; ub[0]=0.7; ub[1]= DBL_MAX; ub[2]= DBL_MAX; ub[3]= DBL_MAX; ub[4]=DBL_MAX; ret=dlevmar_blec_der(modbt7, jacmodbt7, p, x, m, n, lb, ub, A, b, 3, NULL, 1000, opts, info, NULL, NULL, NULL); // box & lin. constraints, analytic Jacobian //ret=dlevmar_blec_dif(modbt7, p, x, m, n, lb, ub, A, b, 3, NULL, 10000, opts, info, NULL, NULL, NULL); // box & lin. constraints, no Jacobian } break; case 19: /* Hock - Schittkowski modified #2 problem 52 */ m=5; n=5; p[0]=2.0; p[1]=2.0; p[2]=2.0; p[3]=2.0; p[4]=2.0; for(i=0; i<n; i++) x[i]=0.0; { double C[3*5]={1.0, 3.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 1.0, -2.0, 0.0, -1.0, 0.0, 0.0, 1.0}, d[3]={-1.0, -2.0, -7.0}; ret=dlevmar_bleic_der(mod2hs52, jacmod2hs52, p, x, m, n, NULL, NULL, NULL, NULL, 0, C, d, 3, 1000, opts, info, NULL, NULL, NULL); // lin. ineq. constraints, analytic Jacobian //ret=dlevmar_bleic_dif(mod2hs52, p, x, m, n, NULL, NULL, NULL, NULL, 0, C, d, 3, 1000, opts, info, NULL, NULL, NULL); // lin. ineq. constraints, no Jacobian } break; case 20: /* Hock - Schittkowski modified problem 76 */ m=4; n=4; p[0]=0.5; p[1]=0.5; p[2]=0.5; p[3]=0.5; for(i=0; i<n; i++) x[i]=0.0; { double A[1*4]={0.0, 1.0, 4.0, 0.0}, b[1]={1.5}; double C[2*4]={-1.0, -2.0, -1.0, -1.0, -3.0, -1.0, -2.0, 1.0}, d[2]={-5.0, -0.4}; double lb[4]={0.0, 0.0, 0.0, 0.0}; ret=dlevmar_bleic_der(modhs76, jacmodhs76, p, x, m, n, lb, NULL, A, b, 1, C, d, 2, 1000, opts, info, NULL, NULL, NULL); // lin. ineq. constraints, analytic Jacobian //ret=dlevmar_bleic_dif(modhs76, p, x, m, n, lb, NULL, A, b, 1, C, d, 2, 1000, opts, info, NULL, NULL, NULL); // lin. ineq. constraints, no Jacobian /* variations: * if no lb is used, the minimizer is (-0.1135922 0.1330097 0.3417476 0.07572816) * if the rhs of constr2 is 4.0, the minimizer is (0.0, 0.166667, 0.333333, 0.0) */ } break; #endif /* HAVE_LAPACK */ } /* switch */ printf("Results for %s:\n", probname[problem]); printf("Levenberg-Marquardt returned %d in %g iter, reason %g\nSolution: ", ret, info[5], info[6]); for(i=0; i<m; ++i) printf("%.7g ", p[i]); printf("\n\nMinimization info:\n"); for(i=0; i<LM_INFO_SZ; ++i) printf("%g ", info[i]); printf("\n"); return 0; }
int fitPeak(double xCenter,double yCenter,double distance,double energy, double pixelLength,double pixelHeight,double alpha,double beta,double rotation, double * sliceQ,double * sliceVal,int numValues, double qLower,double qUpper,double chi,double allowedStddevRatio,double *x,double *y,double * qFit,double * deviationFit) { double avgVal,maxVal; int index; double p[4]; int status; double cos_beta,sin_beta,cos_alpha,sin_alpha,cos_rotation,sin_rotation; double stddev,sum; int i,num; //double chiSquare,fit; double heightFit,bgFit; double covar[4][4]; int VERBOSE; VERBOSE = 0; // can't fit unless you have several values if (numValues < 10) return 0; avgVal = average(sliceVal,numValues); index = maxIndex(sliceVal,numValues); maxVal = sliceVal[index]; // 'real' parameters p[0]=maxVal-avgVal; p[1]=sliceQ[index]; p[2]= (qUpper-qLower)/15.0; p[3]= avgVal; status=dlevmar_dif(gaussian, p, sliceVal, 4, numValues, 1000,NULL, NULL, NULL,*covar,(void *)sliceQ); // check to see if fit is good. // if not, return 0 to tell the code that no fit was found if (status <= 0) // for successful operation status > 0 return 0; heightFit = p[0]; *qFit = p[1]; *deviationFit = p[2]; // pass pack to caller bgFit = p[3]; sum = 0; num = 0; /* if (sqrt(covar[0][0]) > heightFit) { if (VERBOSE) printf("Uncertanty in height large compared to height.\n"); return 0; } */ /* if (sqrt(covar[1][1]) > *deviationFit) { if (VERBOSE) printf("Uncertanty in postion large compared to width of peak.\n"); return 0; } */ // I am not sure this is a good thing to test, b/c there could be a good fit even with a weird b/g /* if (sqrt(covar[3][3]) > .3*heightFit) { if (VERBOSE) printf("Uncertanty in backgound too much\n"); return 0; } */ // peak must be signifigantly larger then background /* if (heightFit < 0.05*bgFit) { if (VERBOSE) printf("Peak is not tall enough to count\n"); return 0; } */ // make sure fit does not fall too far off the data if (*qFit-2* (*deviationFit) < qLower || *qFit+2* (*deviationFit) > qUpper) { if (VERBOSE) printf("Fit is too far off the data.\n"); return 0; } stddev = 0; sum = 0; num = 0; // calculate variation of background outsize 2 sigma of fit for (i=0;i<numValues;i++) { if (sliceQ[i] > *qFit+2* (*deviationFit) || sliceQ[i] < *qFit-2* (*deviationFit) ) { sum+=(sliceVal[i]-bgFit)*(sliceVal[i]-bgFit); num+=1; } } stddev = sqrt(sum*1.0/num); // make sure height is bigger then deviation of background by user specified ratio if (heightFit < allowedStddevRatio*stddev) { if (VERBOSE) printf("Background stddev is too large.\n"); return 0; } // good fit, calc x,y cordinates of it. cos_beta = cos(beta*PI/180.0); sin_beta = sin(beta*PI/180.0); cos_alpha = cos(alpha*PI/180.0); sin_alpha = sin(alpha*PI/180.0); cos_rotation = cos(rotation*PI/180.0); sin_rotation = sin(rotation*PI/180.0); // get physical pixel cordinates getXY(xCenter,yCenter,distance,energy,*qFit,chi, pixelLength,pixelHeight,rotation,cos_beta,sin_beta, cos_alpha,sin_alpha,cos_rotation,sin_rotation,x,y); return 1; // success! }