MinimumError MnPosDef::operator()(const MinimumError& e, const MnMachinePrecision& prec) const {
MnAlgebraicSymMatrix err(e.invHessian());
if(err.size() == 1 && err(0,0) < prec.eps()) {
err(0,0) = 1.;
return MinimumError(err, MinimumError::MnMadePosDef());
}
if(err.size() == 1 && err(0,0) > prec.eps()) {
return e;
}
// std::cout<<"MnPosDef init matrix= "<<err<<std::endl;
double epspdf = std::max(1.e-6, prec.eps2());
double dgmin = err(0,0);
for(unsigned int i = 0; i < err.nrow(); i++) {
if(err(i,i) < prec.eps2()) //std::cout<<"negative or zero diagonal element "<<i<<" in covariance matrix"<<std::endl;
if(err(i,i) < dgmin) dgmin = err(i,i);
}
double dg = 0.;
if(dgmin < prec.eps2()) {
//dg = 1. + epspdf - dgmin;
dg = 0.5 + epspdf - dgmin;
// dg = 0.5*(1. + epspdf - dgmin);
//std::cout<<"added "<<dg<<" to diagonal of error matrix"<<std::endl;
//std::cout << "Error matrix " << err << std::endl;
}
MnAlgebraicVector s(err.nrow());
MnAlgebraicSymMatrix p(err.nrow());
for(unsigned int i = 0; i < err.nrow(); i++) {
err(i,i) += dg;
if(err(i,i) < 0.) err(i,i) = 1.;
s(i) = 1./sqrt(err(i,i));
for(unsigned int j = 0; j <= i; j++) {
p(i,j) = err(i,j)*s(i)*s(j);
}
}
//std::cout<<"MnPosDef p: "<<p<<std::endl;
MnAlgebraicVector eval = eigenvalues(p);
double pmin = eval(0);
double pmax = eval(eval.size() - 1);
//std::cout<<"pmin= "<<pmin<<" pmax= "<<pmax<<std::endl;
pmax = std::max(fabs(pmax), 1.);
if(pmin > epspdf*pmax) return MinimumError(err, e.dcovar());
double padd = 0.001*pmax - pmin;
//std::cout<<"eigenvalues: "<<std::endl;
for(unsigned int i = 0; i < err.nrow(); i++) {
err(i,i) *= (1. + padd);
//std::cout<<eval(i)<<std::endl;
}
// std::cout<<"MnPosDef final matrix: "<<err<<std::endl;
// std::cout<<"matrix forced pos-def by adding "<<padd<<" to diagonal"<<std::endl;
// std::cout<<"eigenvalues: "<<eval<<std::endl;
return MinimumError(err, MinimumError::MnMadePosDef());
}
MinimumState MnHesse::operator()(const MnFcn& mfcn, const MinimumState& st, const MnUserTransformation& trafo, unsigned int maxcalls) const {
const MnMachinePrecision& prec = trafo.precision();
// make sure starting at the right place
double amin = mfcn(st.vec());
double aimsag = sqrt(prec.eps2())*(fabs(amin)+mfcn.up());
// diagonal elements first
unsigned int n = st.parameters().vec().size();
if(maxcalls == 0) maxcalls = 200 + 100*n + 5*n*n;
MnAlgebraicSymMatrix vhmat(n);
MnAlgebraicVector g2 = st.gradient().g2();
MnAlgebraicVector gst = st.gradient().gstep();
MnAlgebraicVector grd = st.gradient().grad();
MnAlgebraicVector dirin = st.gradient().gstep();
MnAlgebraicVector yy(n);
if(st.gradient().isAnalytical()) {
InitialGradientCalculator igc(mfcn, trafo, theStrategy);
FunctionGradient tmp = igc(st.parameters());
gst = tmp.gstep();
dirin = tmp.gstep();
g2 = tmp.g2();
}
MnAlgebraicVector x = st.parameters().vec();
for(unsigned int i = 0; i < n; i++) {
double xtf = x(i);
double dmin = 8.*prec.eps2()*(fabs(xtf) + prec.eps2());
double d = fabs(gst(i));
if(d < dmin) d = dmin;
for(unsigned int icyc = 0; icyc < ncycles(); icyc++) {
double sag = 0.;
double fs1 = 0.;
double fs2 = 0.;
for(unsigned int multpy = 0; multpy < 5; multpy++) {
x(i) = xtf + d;
fs1 = mfcn(x);
x(i) = xtf - d;
fs2 = mfcn(x);
x(i) = xtf;
sag = 0.5*(fs1+fs2-2.*amin);
if(sag > prec.eps2()) goto L30; // break;
if(trafo.parameter(i).hasLimits()) {
if(d > 0.5) goto L26;
d *= 10.;
if(d > 0.5) d = 0.51;
continue;
}
d *= 10.;
}
L26:
std::cout<<"MnHesse: 2nd derivative zero for parameter "<<i<<std::endl;
std::cout<<"MnHesse fails and will return diagonal matrix "<<std::endl;
for(unsigned int j = 0; j < n; j++) {
double tmp = g2(j) < prec.eps2() ? 1. : 1./g2(j);
vhmat(j,j) = tmp < prec.eps2() ? 1. : tmp;
}
return MinimumState(st.parameters(), MinimumError(vhmat, MinimumError::MnHesseFailed()), st.gradient(), st.edm(), mfcn.numOfCalls());
L30:
double g2bfor = g2(i);
g2(i) = 2.*sag/(d*d);
grd(i) = (fs1-fs2)/(2.*d);
gst(i) = d;
dirin(i) = d;
yy(i) = fs1;
double dlast = d;
d = sqrt(2.*aimsag/fabs(g2(i)));
if(trafo.parameter(i).hasLimits()) d = std::min(0.5, d);
if(d < dmin) d = dmin;
// see if converged
if(fabs((d-dlast)/d) < tolerstp()) break;
if(fabs((g2(i)-g2bfor)/g2(i)) < tolerg2()) break;
d = std::min(d, 10.*dlast);
d = std::max(d, 0.1*dlast);
}
vhmat(i,i) = g2(i);
if(mfcn.numOfCalls() > maxcalls) {
//std::cout<<"maxcalls " << maxcalls << " " << mfcn.numOfCalls() << " " << st.nfcn() << std::endl;
std::cout<<"MnHesse: maximum number of allowed function calls exhausted."<<std::endl;
std::cout<<"MnHesse fails and will return diagonal matrix "<<std::endl;
for(unsigned int j = 0; j < n; j++) {
double tmp = g2(j) < prec.eps2() ? 1. : 1./g2(j);
vhmat(j,j) = tmp < prec.eps2() ? 1. : tmp;
}
return MinimumState(st.parameters(), MinimumError(vhmat, MinimumError::MnHesseFailed()), st.gradient(), st.edm(), mfcn.numOfCalls());
}
}
if(theStrategy.strategy() > 0) {
// refine first derivative
HessianGradientCalculator hgc(mfcn, trafo, theStrategy);
FunctionGradient gr = hgc(st.parameters(), FunctionGradient(grd, g2, gst));
grd = gr.grad();
}
//off-diagonal elements
for(unsigned int i = 0; i < n; i++) {
x(i) += dirin(i);
for(unsigned int j = i+1; j < n; j++) {
x(j) += dirin(j);
double fs1 = mfcn(x);
double elem = (fs1 + amin - yy(i) - yy(j))/(dirin(i)*dirin(j));
vhmat(i,j) = elem;
x(j) -= dirin(j);
}
x(i) -= dirin(i);
}
//verify if matrix pos-def (still 2nd derivative)
MinimumError tmp = MnPosDef()(MinimumError(vhmat,1.), prec);
vhmat = tmp.invHessian();
int ifail = invert(vhmat);
if(ifail != 0) {
std::cout<<"MnHesse: matrix inversion fails!"<<std::endl;
std::cout<<"MnHesse fails and will return diagonal matrix."<<std::endl;
MnAlgebraicSymMatrix tmpsym(vhmat.nrow());
for(unsigned int j = 0; j < n; j++) {
double tmp = g2(j) < prec.eps2() ? 1. : 1./g2(j);
tmpsym(j,j) = tmp < prec.eps2() ? 1. : tmp;
}
return MinimumState(st.parameters(), MinimumError(tmpsym, MinimumError::MnHesseFailed()), st.gradient(), st.edm(), mfcn.numOfCalls());
}
FunctionGradient gr(grd, g2, gst);
// needed this ? (if posdef and inversion ok continue. it is like this in the Fortran version
// if(tmp.isMadePosDef()) {
// std::cout<<"MnHesse: matrix is invalid!"<<std::endl;
// std::cout<<"MnHesse: matrix is not pos. def.!"<<std::endl;
// std::cout<<"MnHesse: matrix was forced pos. def."<<std::endl;
// return MinimumState(st.parameters(), MinimumError(vhmat, MinimumError::MnMadePosDef()), gr, st.edm(), mfcn.numOfCalls());
// }
//calculate edm
MinimumError err(vhmat, 0.);
VariableMetricEDMEstimator estim;
double edm = estim.estimate(gr, err);
return MinimumState(st.parameters(), err, gr, edm, mfcn.numOfCalls());
}
FunctionMinimum VariableMetricBuilder::minimum(const MnFcn& fcn,
const GradientCalculator& gc, const MinimumSeed& seed,
std::vector<MinimumState>& result, unsigned int maxfcn, double edmval) const {
const MnMachinePrecision& prec = seed.precision();
// result.push_back(MinimumState(seed.parameters(), seed.error(), seed.gradient(), edm, fcn.numOfCalls()));
const MinimumState & initialState = result.back();
double edm = initialState.edm();
#ifdef DEBUG
std::cout << "\n\nDEBUG Variable Metric Builder \nInitial State: "
<< " Parameter " << initialState.vec()
<< " Gradient " << initialState.gradient().vec()
<< " Inv Hessian " << initialState.error().invHessian()
<< " edm = " << initialState.edm() << std::endl;
#endif
// iterate until edm is small enough or max # of iterations reached
edm *= (1. + 3.*initialState.error().dcovar());
MnLineSearch lsearch;
MnAlgebraicVector step(initialState.gradient().vec().size());
// keep also prevStep
MnAlgebraicVector prevStep(initialState.gradient().vec().size());
do {
// const MinimumState& s0 = result.back();
MinimumState s0 = result.back();
step = -1.*s0.error().invHessian()*s0.gradient().vec();
#ifdef DEBUG
std::cout << "\n\n---> Iteration - " << result.size()
<< "\nFval = " << s0.fval() << " numOfCall = " << fcn.numOfCalls()
<< "\nInternal Parameter values " << s0.vec()
<< " Newton step " << step << std::endl;
#endif
double gdel = inner_product(step, s0.gradient().grad());
if(gdel > 0.) {
std::cout<<"VariableMetricBuilder: matrix not pos.def."<<std::endl;
std::cout<<"gdel > 0: "<<gdel<<std::endl;
MnPosDef psdf;
s0 = psdf(s0, prec);
step = -1.*s0.error().invHessian()*s0.gradient().vec();
// #ifdef DEBUG
// std::cout << "After MnPosdef - error " << s0.error().invHessian() << " gradient " << s0.gradient().vec() << " step " << step << std::endl;
// #endif
gdel = inner_product(step, s0.gradient().grad());
std::cout<<"gdel: "<<gdel<<std::endl;
if(gdel > 0.) {
result.push_back(s0);
return FunctionMinimum(seed, result, fcn.up());
}
}
MnParabolaPoint pp = lsearch(fcn, s0.parameters(), step, gdel, prec);
if(fabs(pp.y() - s0.fval()) < fabs(s0.fval())*prec.eps() ) {
if(VariableMetricBuilder::print_level>=1){
std::cout<<"VariableMetricBuilder: warning: no improvement in line search " << std::endl;
}
// no improvement exit (is it really needed LM ? in vers. 1.22 tried alternative )
break;
}
#ifdef DEBUG
std::cout << "Result after line search : \nx = " << pp.x()
<< "\nOld Fval = " << s0.fval()
<< "\nNew Fval = " << pp.y()
<< "\nNFcalls = " << fcn.numOfCalls() << std::endl;
#endif
MinimumParameters p(s0.vec() + pp.x()*step, pp.y());
FunctionGradient g = gc(p, s0.gradient());
edm = estimator().estimate(g, s0.error());
if(edm < 0.) {
std::cout<<"VariableMetricBuilder: matrix not pos.def."<<std::endl;
std::cout<<"edm < 0"<<std::endl;
MnPosDef psdf;
s0 = psdf(s0, prec);
edm = estimator().estimate(g, s0.error());
if(edm < 0.) {
result.push_back(s0);
return FunctionMinimum(seed, result, fcn.up());
}
}
MinimumError e = errorUpdator().update(s0, p, g);
#ifdef DEBUG
std::cout << "Updated new point: \n "
<< " Parameter " << p.vec()
<< " Gradient " << g.vec()
<< " InvHessian " << e.matrix()
<< " Hessian " << e.hessian()
<< " edm = " << edm << std::endl << std::endl;
#endif
result.push_back(MinimumState(p, e, g, edm, fcn.numOfCalls()));
// correct edm
edm *= (1. + 3.*e.dcovar());
#ifdef DEBUG
std::cout << "edm corrected = " << edm << std::endl;
#endif
FunctionMinimum tmp(seed, result, fcn.up());
if(VariableMetricBuilder::print_level >= 2){
printProgress(fcn);
tmp.print(true);
}
} while(edm > edmval && fcn.numOfCalls() < maxfcn);
if(fcn.numOfCalls() >= maxfcn) {
std::cout<<"VariableMetricBuilder: call limit exceeded."<<std::endl;
return FunctionMinimum(seed, result, fcn.up(), FunctionMinimum::MnReachedCallLimit());
}
if(edm > edmval) {
if(edm < fabs(prec.eps2()*result.back().fval())) {
std::cout<<"VariableMetricBuilder: machine accuracy limits further improvement."<<std::endl;
return FunctionMinimum(seed, result, fcn.up());
} else if(edm < 10.*edmval) {
return FunctionMinimum(seed, result, fcn.up());
} else {
std::cout<<"VariableMetricBuilder: finishes without convergence."<<std::endl;
std::cout<<"VariableMetricBuilder: edm= "<<edm<<" requested: "<<edmval<<std::endl;
return FunctionMinimum(seed, result, fcn.up(), FunctionMinimum::MnAboveMaxEdm());
}
}
//std::cout<<"result.back().error().dcovar()= "<<result.back().error().dcovar()<<std::endl;
#ifdef DEBUG
std::cout << "Exiting succesfully Variable Metric Builder \n"
<< "NFCalls = " << fcn.numOfCalls()
<< "\nFval = " << result.back().fval()
<< "\nedm = " << edm << " requested = " << edmval << std::endl;
#endif
return FunctionMinimum(seed, result, fcn.up());
}
