void lmcurve_fit( int n_par, double *par, int m_dat,
const double *t, const double *y,
double (*f)( double t, const double *par ),
const lm_control_struct *control, lm_status_struct *status )
{
lmcurve_data_struct data = { t, y, f };
lmmin( n_par, par, m_dat, (const void*) &data,
lmcurve_evaluate, control, status, lm_printout_std );
}
//! Calls lmmin with the given data, with callback function \c f.
static void gmx_lmcurve(
const int n_par, double* par, const int m_dat,
const double* t, const double* y, const double *dy,
double (*f)(double t, const double* par),
const lm_control_struct* control, lm_status_struct* status)
{
lmcurve_data_struct data = { t, y, dy, f };
lmmin(n_par, par, m_dat, nullptr, &data, lmcurve_evaluate,
control, status);
}
int main()
{
/* parameter vector */
int n_par = 3; // number of parameters in model function f
double par[3] = { -1, 0, 1 }; // arbitrary starting value
/* data points */
int m_dat = 4;
double tx[4] = { -1, -1, 1, 1 };
double tz[4] = { -1, 1, -1, 1 };
double y[4] = { 0, 1, 1, 2 };
data_struct data = { tx, tz, y, f };
/* auxiliary parameters */
lm_status_struct status;
lm_control_struct control = lm_control_double;
control.printflags = 3; // monitor status (+1) and parameters (+2)
/* perform the fit */
printf( "Fitting:\n" );
lmmin( n_par, par, m_dat, (const void*) &data,
evaluate_surface, &control, &status, lm_printout_std );
/* print results */
printf( "\nResults:\n" );
printf( "status after %d function evaluations:\n %s\n",
status.nfev, lm_infmsg[status.info] );
printf("obtained parameters:\n");
int i;
for ( i=0; i<n_par; ++i )
printf(" par[%i] = %12g\n", i, par[i]);
printf("obtained norm:\n %12g\n", status.fnorm );
printf("fitting data as follows:\n");
double ff;
for ( i=0; i<m_dat; ++i ){
ff = f(tx[i], tz[i], par);
printf( " t[%2d]=%12g,%12g y=%12g fit=%12g residue=%12g\n",
i, tx[i], tz[i], y[i], ff, y[i] - ff );
}
return 0;
}
int main( int argc, char **argv )
{
/* parameter lambda */
if( argc!=2 ){
fprintf( stderr, "usage: morobropro lambda\n" );
exit(-1);
}
double lambda = atof( argv[1] );
/* parameter vector */
int n_par = 2; // number of parameters in model function f
double par[2] = { -1.2, 1 }; // arbitrary starting value
/* data points */
int m_dat = 3;
/* auxiliary parameters */
lm_status_struct status;
lm_control_struct control = lm_control_double;
lm_princon_struct princon = lm_princon_std;
princon.flags = 3; // monitor status (+1) and parameters (+2)
control.scale_diag = 1;
control.pivot = 0;
/* perform the fit */
printf( "demo morobropro starting\n" );
lmmin( n_par, par, m_dat, (const void*) &lambda,
evaluate_morobropro, lm_printout_std,
&control, &princon, &status );
/* print results */
printf( "\n" );
printf( "demo morobropro results:\n" );
printf( " after %d function evaluations: status: %s\n",
status.nfev, lm_infmsg[status.info] );
printf(" obtained parameters:\n");
int i;
for ( i=0; i<n_par; ++i )
printf(" par[%i] = %19.11f\n", i, par[i]);
printf(" and norm: %19.11f\n", status.fnorm );
return 0;
}
bool fit_arc(const int m_dat, const arc_data_struct data,
const int n_par, double *par, double sq_error,
Vector2d &result_center, double &result_radiussq)
{
lm_status_struct status;
lm_control_struct control = lm_control_double;
control.printflags = 0; // 3 = monitor status (+1) and parameters (+2)
control.maxcall = 200;
control.ftol = sq_error; // max square error sum
// printf( "Fitting:\n" );
lmmin( n_par, par, m_dat, (const void*) &data,
evaluate_arcfit, &control, &status, lm_printout_std );
result_center.x() = par[0];
result_center.y() = par[1];
result_radiussq = par[2];
double fvec[m_dat];
int info;
evaluate_arcfit(par, m_dat, (const void*) &data, fvec, &info);
double totres = 0;
for (int i=0; i<m_dat; ++i ) totres+=fvec[i];
return (totres < sq_error);
#if 0
printf( "\nResults:\n" );
printf( "status after %d function evaluations:\n %s\n",
status.nfev, lm_infmsg[status.info] );
printf("obtained parameters:\n");
int i;
for ( i=0; i<n_par; ++i )
printf(" par[%i] = %12g\n", i, par[i]);
printf("obtained norm:\n %12g\n", status.fnorm );
#endif
// printf("fitting data as follows:\n");
// double ff;
// for ( i=0; i<m_dat; ++i ){
// ff = f(tx[i], tz[i], par);
// printf( " t[%2d]=%12g,%12g y=%12g fit=%12g residue=%12g\n",
// i, tx[i], tz[i], y[i], ff, y[i] - ff );
// }
// free(data.px);
// free(data.py);
}
void lmcurve_fit2( int n_par, double *par, int m_dat,
const double *t, const double *y, void* userData,
double (*f)( double t, const double *par, void* userData ),
const lm_control_struct *control, lm_status_struct *status )
{
// lmcurve_data_struct data = { t, y, f };
lmcurve_data_struct2 data;
data.t = t;
data.y = y;
data.userData = userData;
data.f = f;
lmmin( n_par, par, m_dat, (const void*) &data,
lmcurve_evaluate, control, status, lm_printout_std );
}
int main( int argc, char **argv )
{
int n = 2; /* dimension of the problem */
double p[2]; /* parameter vector p=(x,y) */
/* auxiliary parameters */
lm_control_struct control = lm_control_double;
lm_status_struct status;
control.verbosity = 31;
/* get start values from command line */
if( argc!=3 ){
fprintf( stderr, "usage: nonlin1 x_start y_start\n" );
exit(-1);
}
p[0] = atof( argv[1] );
p[1] = atof( argv[2] );
/* the minimization */
printf( "Minimization:\n" );
lmmin( n, p, n, NULL, evaluate_nonlin1, &control, &status );
/* print results */
printf( "\n" );
printf( "lmmin status after %d function evaluations:\n %s\n",
status.nfev, lm_infmsg[status.outcome] );
printf( "\n" );
printf("Solution:\n");
printf(" x = %19.11f\n", p[0]);
printf(" y = %19.11f\n", p[1]);
printf(" d = %19.11f => ", status.fnorm);
/* convergence of lmfit is not enough to ensure validity of the solution */
if( status.fnorm >= control.ftol )
printf( "not a valid solution, try other starting values\n" );
else
printf( "valid, though not the only solution: "
"try other starting values\n" );
return 0;
}
QFFitAlgorithm::FitResult QFFitAlgorithmLMFit::intFit(double* paramsOut, double* paramErrorsOut, const double* initialParams, QFFitAlgorithm::Functor* model, const double* paramsMin, const double* paramsMax) {
QFFitAlgorithm::FitResult result;
qDebug()<<"QFFitAlgorithmLMFit::intFit 1";
int paramCount=model->get_paramcount(); // number of parameters
memcpy(paramsOut, initialParams, paramCount*sizeof(double));
qDebug()<<"QFFitAlgorithmLMFit::intFit 2";
//double param=getParameter("param_name").toDouble();
lm_status_struct status;
lm_control_struct control = lm_control_double;
control.verbosity = 0; // monitor status (+1) and parameters (+2)
control.ftol=getParameter("ftol").toDouble();
control.xtol=getParameter("xtol").toDouble();
control.gtol=getParameter("gtol").toDouble();
control.epsilon=getParameter("epsilon").toDouble();
control.stepbound=getParameter("stepbound").toDouble();
control.patience=getParameter("max_iterations").toInt();
qDebug()<<"QFFitAlgorithmLMFit::intFit 3";
QFFItAlgorithmGSL_evalData d;
d.model=model;
d.paramsMin=paramsMin;
d.paramsMax=paramsMax;
d.pcount=paramCount;
d.p=(double*)qfMalloc(paramCount*sizeof(double));
qDebug()<<"QFFitAlgorithmLMFit::intFit 4";
lmmin( paramCount, paramsOut, model->get_evalout(), &d, lmfit_eval, &control, &status );
qDebug()<<"QFFitAlgorithmLMFit::intFit 5";
if ( paramsMin && paramsMax) {
for (int i=0; i<paramCount; i++) {
const double& par=paramsOut[i];
if (par>paramsMax[i]) paramsOut[i]=paramsMax[i];
if (par<paramsMin[i]) paramsOut[i]=paramsMin[i];
}
}
result.addNumber("error_sum", status.fnorm);
result.addNumber("iterations", status.nfev);
qDebug()<<"QFFitAlgorithmLMFit::intFit 6";
QVector<double> J(model->get_evalout()*model->get_paramcount());
QVector<double> COV(model->get_paramcount()*model->get_paramcount());
qDebug()<<"QFFitAlgorithmLMFit::intFit 7";
model->evaluateJacobian(J.data(), paramsOut);
qDebug()<<"QFFitAlgorithmLMFit::intFit 8";
double chi2=status.fnorm;
qDebug()<<"QFFitAlgorithmLMFit::intFit 9";
if (QFFitAlgorithm::functorHasWeights(model) && !QFFitAlgorithm::functorAllWeightsOne(model)) statisticsGetFitProblemCovMatrix(COV.data(), J.data(), model->get_evalout(), model->get_paramcount());
else statisticsGetFitProblemVarCovMatrix(COV.data(), J.data(), model->get_evalout(), model->get_paramcount(), chi2);
qDebug()<<"QFFitAlgorithmLMFit::intFit 10";
result.addNumberMatrix("covariance_matrix", COV.data(), model->get_paramcount(), model->get_paramcount());
qDebug()<<"QFFitAlgorithmLMFit::intFit 11";
for (int i=0; i<model->get_paramcount(); i++) {
qDebug()<<"QFFitAlgorithmLMFit::intFit 12: "<<i;
paramErrorsOut[i]=statisticsGetFitProblemParamErrors(i, COV.data(), model->get_paramcount());
}
qDebug()<<"QFFitAlgorithmLMFit::intFit 13";
if (status.outcome>=0) {
result.fitOK=QString(lm_infmsg[status.outcome]).contains("success") || QString(lm_infmsg[status.outcome]).contains("converged");
result.message=QString(lm_infmsg[status.outcome]);
result.messageSimple=QString(lm_infmsg[status.outcome]);
} else {
result.fitOK=true;
result.message="";
result.messageSimple="";
}
qDebug()<<"QFFitAlgorithmLMFit::intFit 14";
if (d.p) qfFree(d.p);
qDebug()<<"QFFitAlgorithmLMFit::intFit 15 ... DONE";
return result;
}
void testFromInitialData( double alpha, double gamma, double focus, double* &parResult, int &n_par, bool &isPassed, Array& left_points, Array &right_points,
double imageWidth,
double imageHeight,
ModelEvaluatorType modelType,
double &fnorm,
double &fmark
)
{
/*Array left_points, right_points;
parseYAML("image_accepted_points_left.txt", left_points);
parseYAML("image_accepted_points_right.txt", right_points);*/
/* parameter vector */
n_par = -1;
int m_dat = min( 30, left_points.rows);
switch(modelType)
{
case PAR_6_PROJ:
n_par = 6;
break;
case PAR_7_PROJ:
n_par = 7;
break;
case PAR_6_AND_LINE_TIMES_PROJ:
n_par = 6 + m_dat;
break;
case PAR_7_AND_LINE_TIMES_PROJ:
n_par = 7 + m_dat;
break;
}
double* par = new double[n_par];
if(modelType == PAR_6_PROJ || modelType == PAR_6_AND_LINE_TIMES_PROJ)
{
par[0] = alpha;
par[1] = gamma;
par[2] = focus;
par[3] = 0;
par[4] = 0;
par[5] = 0;
}
if(modelType == PAR_6_AND_LINE_TIMES_PROJ)
{
for(int i = 6; i < n_par; ++i)
{
par[i] = 3;
}
}
if(modelType == PAR_7_PROJ || modelType == PAR_7_AND_LINE_TIMES_PROJ)
{
double sh_x, sh_y, sh_z;
GenerateTranslationCoordinates(alpha, gamma, sh_x, sh_y, sh_z);
par[0] = sh_x;
par[1] = sh_y;
par[2] = sh_z;
par[3] = focus;
par[4] = 0;
par[5] = 0;
par[6] = 0;
if(modelType == PAR_7_AND_LINE_TIMES_PROJ)
{
for(int i = 7; i < n_par; ++i)
{
par[i] = 3;
}
}
}
/* arbitrary starting value */
/* data points */
data_struct data = { &left_points, &right_points, imageWidth / 2, imageHeight / 2, n_par};
/* auxiliary parameters */
lm_status_struct status;
lm_control_struct control = lm_control_double;
control.patience = 200;
//control.verbosity = 3;
/* perform the fit */
//printf( "Fitting:\n" );
switch(modelType)
{
case PAR_6_PROJ: case PAR_7_PROJ:
lmmin( n_par, par, m_dat * 2, (const void*) &data, evaluateModelWithPojectionDifferences,
&control, &status );
break;
case PAR_6_AND_LINE_TIMES_PROJ:
lmmin( n_par, par, m_dat * 2, (const void*) &data, evaluateModelWithPojectionDifferencesPointsOnLines,
&control, &status );
break;
case PAR_7_AND_LINE_TIMES_PROJ:
lmmin( n_par, par, m_dat * 2, (const void*) &data, evaluateModelPar7_WithProjectionDifferencesPointsOnLines,
&control, &status );
break;
}
/* print results */
/*printf( "\nResults:\n" );
printf( "status after %d function evaluations:\n %s\n",
status.nfev, lm_infmsg[status.outcome] );*/
//printf("obtained parameters:\n");
/*int i;
for ( i=0; i<n_par; ++i )
printf(" par[%i] = %12g\n", i, par[i]);*/
//printf("obtained norm:\n %12g\n", status.fnorm );
//printf("fitting data as follows:\n");
int userBreak = 0;
int mark = 0;
if(parResult != 0)
{
delete[] parResult;
}
parResult = new double[n_par];
for(int i = 0 ; i < n_par; ++i)
{
parResult[i] = par[i];
}
switch(modelType)
{
case PAR_6_AND_LINE_TIMES_PROJ:
data.n_par = 6;
break;
case PAR_7_AND_LINE_TIMES_PROJ:
data.n_par = 7;
break;
}
mark = getMarkForModel(par, m_dat, &data, &userBreak);
fmark = ((double)mark) / m_dat;
fnorm = status.fnorm;
//printf("obtained mark:\n %12g =(%d / %d)\n", fmark, mark, m_dat );
isPassed = false;
cout<< "fnorm: "<< status.fnorm << " fmark: " << fmark << endl ;
if( (status.fnorm <= 25 * m_dat) && (fmark >= 0.7))
{
++passed;
if( modelType == PAR_6_PROJ || modelType == PAR_6_AND_LINE_TIMES_PROJ)
{
cout <<" alpha: " << par[0] << " gamma: "<< par[1] << endl ;
cout << " f: " << par[2] << endl << " r_x: " << par[3] << endl << " r_y: " << par[4] << endl << " r_z: " << par[5] << endl;
if(modelType == PAR_6_AND_LINE_TIMES_PROJ)
{
cout<< "Line Times:"<<endl;
for(int k = 6; k < n_par; k++)
{
cout <<"t["<< k - 6 <<"]= " << par[k] << "; ";
}
cout << endl;
}
}
if( modelType == PAR_7_PROJ || modelType == PAR_7_AND_LINE_TIMES_PROJ)
{
cout <<" shift x: " << par[0] << " shift y: "<< par[1] << " shift z: "<< par[2] << endl ;
cout << " f: " << par[3] << endl << " r_x: " << par[4] << endl << " r_y: " << par[5] << endl << " r_z: " << par[6] << endl;
if(modelType == PAR_7_AND_LINE_TIMES_PROJ)
{
cout<< "Line Times:"<<endl;
for(int k = 7; k < n_par; k++)
{
cout <<"t["<< k - 7 <<"]= " << par[k] << "; ";
}
cout << endl;
}
}
isPassed = true;
}
//left_points.dispose();
//right_points.dispose();
delete[] par;
}
QFFitAlgorithm::FitResult QFFitAlgorithmLMFitBox::intFit(double* paramsOut, double* paramErrorsOut, const double* initialParams, QFFitAlgorithm::Functor* model, const double* paramsMin, const double* paramsMax) {
QFFitAlgorithm::FitResult result;
//qDebug()<<"QFFitAlgorithmLMFitBox::intFit 1";
int paramCount=model->get_paramcount(); // number of parameters
memcpy(paramsOut, initialParams, paramCount*sizeof(double));
//qDebug()<<"QFFitAlgorithmLMFitBox::intFit 2";
//double param=getParameter("param_name").toDouble();
lm_status_struct status;
lm_control_struct control = lm_control_double;
control.verbosity = 0; // monitor status (+1) and parameters (+2)
control.ftol=getParameter("ftol").toDouble();
control.xtol=getParameter("xtol").toDouble();
control.gtol=getParameter("gtol").toDouble();
control.epsilon=getParameter("epsilon").toDouble();
control.stepbound=getParameter("stepbound").toDouble();
control.patience=getParameter("max_iterations").toInt();
//qDebug()<<"QFFitAlgorithmLMFitBox::intFit 3";
QFFItAlgorithmGSL_evalData d;
d.model=model;
d.paramsMin=paramsMin;
d.paramsMax=paramsMax;
d.pcount=paramCount;
d.p=(double*)qfMalloc(paramCount*sizeof(double));
//qDebug()<<"QFFitAlgorithmLMFitBox::intFit 4";
bool transformParams=paramsMin&¶msMax;
//qDebug()<<"QFFitAlgorithmLMFitBox::intFit 5";
lmmin( paramCount, paramsOut, model->get_evalout(), &d, lmfit_evalboxlimits, &control, &status );
//qDebug()<<"QFFitAlgorithmLMFitBox::intFit 6";
if (paramsMin && paramsMax) {
bool atbound=false;
for (int i=0; i<paramCount; i++) {
const double mi=paramsMin[i];
const double ma=paramsMax[i];
if (qFuzzyCompare(mi,paramsOut[i])) {
atbound=true;
paramsOut[i]=1.05*mi;
} else if (qFuzzyCompare(ma,paramsOut[i])) {
atbound=true;
paramsOut[i]=0.95*ma;
} else paramsOut[i]=qBound(mi, paramsOut[i], ma);
}
if (atbound) {
//qDebug()<<"QFFitAlgorithmLMFitBox::intFit 6.2";
lmmin( paramCount, paramsOut, model->get_evalout(), &d, lmfit_evalboxlimits, &control, &status );
//qDebug()<<"QFFitAlgorithmLMFitBox::intFit 6.3";
for (int i=0; i<paramCount; i++) {
const double mi=paramsMin[i];
const double ma=paramsMax[i];
paramsOut[i]=qBound(mi, paramsOut[i], ma);
}
}
}
//qDebug()<<"QFFitAlgorithmLMFitBox::intFit 7";
double chi2=status.fnorm;
if (true) {
const long long nout=model->get_evalout();
const long long np=model->get_paramcount();
double* J=(double*)qfCalloc(nout*np*2*sizeof(double));
double* COV=(double*)qfCalloc(np*np*2*sizeof(double));
// qDebug()<<"QFFitAlgorithmLMFitBox::intFit 7: test J";
// for (long long i=nout*np; i<nout*np*2; i++) {
// if (J[i]!=0) qDebug()<<"!!! J["<<i<<"] != 0 (="<<J[i]<<") !!!";
// }
// qDebug()<<"QFFitAlgorithmLMFitBox::intFit 7: test COV";
// for (long long i=np*np; i<np*np*2; i++) {
// if (COV[i]!=0) qDebug()<<"!!! COV["<<i<<"] != 0 !!!";
// }
// qDebug()<<"QFFitAlgorithmLMFitBox::intFit 8";
// qDebug()<<" calling evalJac: J.size="<<nout*np<<" p.size="<<np<<" nout="<<nout<<" J="<<J<<" COV="<<COV<<" model="<<typeid(model).name();
model->evaluateJacobian(J, paramsOut);
// qDebug()<<"QFFitAlgorithmLMFitBox::intFit 8: test J";
// for (long long i=nout*np; i<nout*np*2; i++) {
// if (J[i]!=0) qDebug()<<"!!! J["<<i<<"] != 0 (="<<J[i]<<") !!!";
// }
// qDebug()<<"QFFitAlgorithmLMFitBox::intFit 9";
// qDebug()<<"QFFitAlgorithmLMFitBox::intFit 10";
// bool b=model->isWeightedLSQ();
// qDebug()<<"QFFitAlgorithmLMFitBox::intFit 10-isWLSQ"<<model->isWeightedLSQ();
// b=model->areAllWeightsOne();
// qDebug()<<"QFFitAlgorithmLMFitBox::intFit 10-allOne"<<model->areAllWeightsOne();
// qDebug()<<"QFFitAlgorithmLMFitBox::intFit 10-allOne: test J";
// for (long long i=nout*np; i<nout*np*2; i++) {
// if (J[i]!=0) qDebug()<<"!!! J["<<i<<"] != 0 (="<<J[i]<<") !!!";
// }
// qDebug()<<"QFFitAlgorithmLMFitBox::intFit 10-allOne: test COV";
// for (long long i=np*np; i<np*np*2; i++) {
// if (COV[i]!=0) qDebug()<<"!!! COV["<<i<<"] != 0 !!!";
// }
if (model->isWeightedLSQ() && !model->areAllWeightsOne()) statisticsGetFitProblemCovMatrix(COV, J, nout, np);
else statisticsGetFitProblemVarCovMatrix(COV, J/*.constData()*/, nout, np, chi2);
// qDebug()<<"QFFitAlgorithmLMFitBox::intFit 10: test J";
// for (long long i=nout*np; i<nout*np*2; i++) {
// if (J[i]!=0) qDebug()<<"!!! J["<<i<<"] != 0 (="<<J[i]<<") !!!";
// }
// qDebug()<<"QFFitAlgorithmLMFitBox::intFit 10: test COV";
// for (long long i=np*np; i<np*np*2; i++) {
// if (COV[i]!=0) qDebug()<<"!!! COV["<<i<<"] != 0 !!!";
// }
// qDebug()<<"QFFitAlgorithmLMFitBox::intFit 11";
result.addNumberMatrix("covariance_matrix", COV, np,np);
// qDebug()<<"QFFitAlgorithmLMFitBox::intFit 11: test COV";
// for (long long i=np*np; i<np*np*2; i++) {
// if (COV[i]!=0) qDebug()<<"!!! COV["<<i<<"] != 0 !!!";
// }
for (long long i=0; i<model->get_paramcount(); i++) {
// qDebug()<<"QFFitAlgorithmLMFitBox::intFit 12."<<i;
paramErrorsOut[i]=statisticsGetFitProblemParamErrors(i, COV, model->get_paramcount());
}
// qDebug()<<"QFFitAlgorithmLMFitBox::intFit 12: test COV";
// for (long long i=np*np; i<np*np*2; i++) {
// if (COV[i]!=0) qDebug()<<"!!! COV["<<i<<"] != 0 !!!";
// }
// qDebug()<<"QFFitAlgorithmLMFitBox::intFit 12-end1";
qfFree(COV);
// qDebug()<<"QFFitAlgorithmLMFitBox::intFit 12-end2";
qfFree(J);
}
// qDebug()<<"QFFitAlgorithmLMFitBox::intFit 13";
result.addNumber("error_sum", chi2);
result.addNumber("iterations", status.nfev);
if (status.outcome>=0) {
result.fitOK=QString(lm_infmsg[status.outcome]).contains("success") || QString(lm_infmsg[status.outcome]).contains("converged");
result.message=QString(lm_infmsg[status.outcome]);
result.messageSimple=QString(lm_infmsg[status.outcome]);
} else {
result.fitOK=true;
result.message="";
result.messageSimple="";
}
// qDebug()<<"QFFitAlgorithmLMFitBox::intFit 14";
if (d.p) qfFree(d.p);
// qDebug()<<"QFFitAlgorithmLMFitBox::intFit 15";
return result;
}
