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!
}
