int lmdif0(void fcn(int,int,double [],double [],int *),int m, int n,double x[],int msk[],
double fvec[],double tol,int *info,int *nfev)
{
int j,maxfev,mode;
int *ipvt;
double ftol,xtol,gtol,epsfcn,factor;
double *diag,**fjac,*qtf,*wa1,*wa2,*wa3,*wa4;
/* Check input parameters */
if (n <= 0 || m < n || tol < 0.0) {
*info = 0;
return(1);
}
/* Allocate memory for working arrays. */
ipvt = (int *)calloc(n,sizeof(int));
diag = (double *)calloc(n,sizeof(double));
qtf = (double *)calloc(n,sizeof(double));
wa1 = (double *)calloc(n,sizeof(double));
wa2 = (double *)calloc(n,sizeof(double));
wa3 = (double *)calloc(n,sizeof(double));
wa4 = (double *)calloc(m,sizeof(double));
/* Create 2d matrix for Jacobian */
fjac = (double **)calloc(n,sizeof(double *));
for (j=0;j<n;j++)
fjac[j] = (double *)calloc(m,sizeof(double));
/* Set convergence tolerances */
ftol = tol;
xtol = tol;
gtol = 0.0;
maxfev = 800;
epsfcn = 0.0;
mode = 1;
factor = 10;
*nfev = 0;
lmdif(fcn,m,n,x,msk,fvec,ftol,xtol,gtol,maxfev,epsfcn,diag,mode,
factor,info,nfev,fjac,ipvt,qtf,wa1,wa2,wa3,wa4);
if (*info == 8) *info = 4;
for (j=0;j<n;j++)
free(fjac[j]);
free(fjac);
free(wa4);
free(wa3);
free(wa2);
free(wa1);
free(qtf);
free(diag);
free(ipvt);
return(0);
}
int SkeletonFitting::minimize(void* p, minpack_func_mn fcn, int m, int n, double* vals, double tol, int iterFac, double factor, double eps)
{
double* fvec = new double[m];
// work array
int* iwa = new int[n];
int lwa = m * n + 5 * n + m;
double* wa = new double[lwa];
double* fvec2 = fvec;
int* iwa2 = iwa;
double* wa2 = wa;
int mp5n, mode, nfev;
double ftol, gtol, xtol;
double epsfcn;
int maxfev, nprint;
int info;
--fvec2;
--iwa2;
--vals;
--wa2;
if (n <= 0 || m < n || tol < 0. || lwa < m * n + n * 5 + m)
throw Exception("invalid data");
maxfev = (n + 1) * iterFac;
ftol = tol;
xtol = tol;
gtol = 0.;
epsfcn = eps;
mode = 1;
nprint = 0;
mp5n = m + n * 5;
info = lmdif(fcn, p, m, n, &vals[1], &fvec2[1], ftol, xtol, gtol, maxfev,
epsfcn, &wa2[1], mode, factor, nprint, &nfev, &wa2[mp5n +
1], m, &iwa2[1], &wa2[n + 1], &wa2[(n << 1) + 1], &wa2[n * 3 + 1],
&wa2[(n << 2) + 1], &wa2[n * 5 + 1]);
if (info == 8) {
info = 4;
}
delete[] fvec;
delete[] iwa;
delete[] wa;
return info;
}
void RunBROptimizer ( OptInfo *o, double minStepWidth)
{
struct LMStruct LM;
int iflag;
// PrintError("RunBROptimizer");
LM.n = o->numVars;
setFcnPanoNperCP(1); // This optimizer does not use direction, don't waste time computing it
if( o->numData*FUNCS_PER_CP < LM.n )
{
LM.m = LM.n;
}
else
{
LM.m = o->numData*FUNCS_PER_CP;
}
fcn = o->fcn;
if( AllocateLMStruct( &LM ) != 0 )
{
PrintError( "Not enough Memory to allocate Data for BR-solver" );
return;
}
// Initialize optimization params
if( o->SetVarsToX( LM.x ) != 0)
{
PrintError("Internal Error");
return;
}
iflag = -100; // reset counter
fcn(LM.m, LM.n, LM.x, LM.fvec, &iflag);
//infoDlg ( _initProgress, "Optimizing Params" );
/* Call lmdif. */
LM.ldfjac = LM.m;
LM.mode = 1;
LM.nprint = 1;
// Set stepwidth to angle corresponding to one pixel in final pano
LM.epsfcn = minStepWidth; // g->pano.hfov / (double)g->pano.width;
LM.info = 0;
LM.factor = 1.0;
#if 0
lmdif( LM.m, LM.n, LM.x, LM.fvec, LM.ftol, LM.xtol,
LM.gtol, LM.maxfev, LM.epsfcn, LM.diag, LM.mode, LM.factor,
LM.nprint, &LM.info, &LM.nfev, LM.fjac, LM.ldfjac, LM.ipvt,
LM.qtf, LM.wa1, LM.wa2, LM.wa3, LM.wa4);
#endif
bracket( &LM );
o->SetXToVars( LM.x );
iflag = -99; //
fcn(LM.m, LM.n, LM.x, LM.fvec, &iflag);
//infoDlg ( _disposeProgress, "" );
FreeLMStruct( &LM );
}
void RunLMOptimizer( OptInfo *o)
{
struct LMStruct LM;
int iflag;
char *warning;
char *infmsg[] = {
"improper input parameters",
"the relative error in the sum of squares is at most tol",
"the relative error between x and the solution is at most tol",
"conditions for info = 1 and info = 2 both hold",
"fvec is orthogonal to the columns of the jacobian to machine precision",
"number of calls to fcn has reached or exceeded 200*(n+1)",
"tol is too small. no further reduction in the sum of squares is possible",
"tol too small. no further improvement in approximate solution x possible",
"Interrupted"
};
int istrat; // strategy
int totalfev; // total function evaluations
int numconstraints; // number of constraints imposed by control points
int i;
int lmInfo;
AlignInfo *g; // obtained from adjust.c
// PrintError("RunLMOptimizer");
// The method used here is a hybrid of two optimization strategies.
// In the first strategy, fcnPano is configured to return one function per
// control point, that function being total distance without regard for
// direction. In the second strategy, fcnPano is configured to return two
// functions per control point, those functions being distance in two
// directions, typically longitude and latitude. The second strategy
// converges much faster, but may be less stable with poor initial estimates.
// So, we use the first method as long as it makes significant progress
// (currently 5% reduction in error per iteration), and then switch to
// the second method to rapidly polish the estimate. Final result
// returned to the user is that of the second method.
//
// Older versions of Panorama Tools used just the first strategy,
// with error tolerances set to make it run to full convergence,
// which often took hundreds or thousands of iterations. The hybrid
// approach typically converges much faster (a few tens of iterations)
// and appears to be equally robust in testing to date. Full convergence
// (to am lmdif ftol of 1.0e-14) is not always achieved faster than the old
// version. However the convergence rate (error reduction per wall-clock
// second) has been significantly better in all cases tested, and the final
// accuracy has been equal or improved.
//
// So, in the interest of behavior that is friendlier to the user, I have
// set an ftol convergence criterion that is looser than before, 1.0e-6
// instead of 1.0e-14. By this point, it is very unlikely that
// significant reductions can be achieved by more iterating, since
// even 10,000 more iterations would be predicted to make at most 1%
// improvement in the total error.
//
// I have also made the diagnosis of too few control points more precise
// and more obvious to the user. The old version complained if the
// number of control points was less than the number of parameters,
// although in fact each normal control point contributes two independent
// constraints (x and y) so the actual critical number is
// 2*controlpoints >= parameters. As a result, the old version often
// complained even when things were fine, and never complained more loudly
// even when things were awful. This version does not complain
// at all unless there are not enough actual constraints, and then it puts
// out an error dialog that must be dismissed by the user.
//
// Rik Littlefield ([email protected]), May 2004.
LM.n = o->numVars;
g = GetGlobalPtr();
numconstraints = 0;
for(i=0; i < g->numPts; i++) {
if (g->cpt[i].type == 0)
numconstraints += 2;
else numconstraints += 1;
}
warning = "";
if( numconstraints < LM.n )
{
char msgx[200];
warning = "Warning: Number of Data Points is smaller than Number of Variables to fit.\n";
sprintf (msgx,"You have too few control points (%d) or too many parameters (%d). Strange values may result!",o->numData,LM.n);
PrintError(msgx);
}
totalfev = 0;
for (istrat=1; istrat <= 2; istrat++) {
setFcnPanoNperCP(istrat);
LM.m = o->numData*FUNCS_PER_CP;
if( LM.m < LM.n ) LM.m = LM.n; // in strategy #1, fcnpano will pad fvec if needed
fcn = o->fcn;
if( AllocateLMStruct( &LM ) != 0 )
{
PrintError( "Not enough Memory" );
return;
}
// Initialize optimization params
if( o->SetVarsToX( LM.x ) != 0)
{
PrintError("Internal Error");
return;
}
iflag = -100; // reset counter and initialize dialog
fcn(LM.m, LM.n, LM.x, LM.fvec, &iflag);
if (istrat == 2) setFcnPanoDoNotInitAvgFov();
// infoDlg ( _initProgress, "Optimizing Variables" );
/* Call lmdif. */
LM.ldfjac = LM.m;
LM.mode = 1;
LM.nprint = 1; // 10
LM.info = 0;
LM.factor = 100.0;
LM.ftol = 1.0e-6; // used to be DBL_EPSILON; //1.0e-14;
if (istrat == 1) {
LM.ftol = 0.05; // for distance-only strategy, bail out when convergence slows
}
lmdif( LM.m, LM.n, LM.x, LM.fvec, LM.ftol, LM.xtol,
LM.gtol, LM.maxfev, LM.epsfcn, LM.diag, LM.mode, LM.factor,
LM.nprint, &LM.info, &LM.nfev, LM.fjac, LM.ldfjac, LM.ipvt,
LM.qtf, LM.wa1, LM.wa2, LM.wa3, LM.wa4);
lmInfo = LM.info;
// At end, one final evaluation to get errors that do not have fov stabilization applied,
// for reporting purposes.
if (istrat == 2) {
forceFcnPanoReinitAvgFov();
iflag = 1;
fcn(LM.m, LM.n, LM.x, LM.fvec, &iflag);
}
o->SetXToVars( LM.x );
iflag = -99; // reset counter and dispose dialog
fcn(LM.m, LM.n, LM.x, LM.fvec, &iflag);
// infoDlg ( _disposeProgress, "" );
// Display solver info
if(LM.info >= 8)
LM.info = 4;
if(LM.info < 0)
LM.info = 8;
totalfev += LM.nfev;
sprintf( (char*) o->message, "# %s%d function evaluations\n# %s\n# final rms error %g units\n",
warning, totalfev, infmsg[LM.info],
sqrt(sumSquared(LM.fvec,LM.m)/LM.m) * sqrt((double)FUNCS_PER_CP));
FreeLMStruct( &LM );
if (lmInfo < 0) break; // to honor user cancel in strategy 1
}
setFcnPanoNperCP(1); // Force back to startegy 1 for backwards compatability
}
