LinearProgram::Result MinNormProblem_Sparse::Solve(Vector& x) { if(norm == 2) { Assert(IsValid()); if(HasInequalities()) { //must use quadratic program for inequalities FatalError("Sparse QP not done yet"); return LinearProgram::Error; } else if(A.m != 0) { //no inequality constraints, just equality FatalError("Sparse LS with equality constraints not done yet"); return LinearProgram::Error; } else { LSQRInterface lsqr; if(!lsqr.Solve(C,d)) { cout<<"Error solving for least squares!!!"<<endl; return LinearProgram::Error; } x = lsqr.x; return LinearProgram::Feasible; } } else { RobustLPSolver lps; lps.verbose = verbose; LinearProgram::Result res= lps.Solve(lp); if(res==LinearProgram::Feasible) { x.resize(C.n); lps.xopt.getSubVectorCopy(0,x); } return res; } }
ConvergenceResult ConstrainedMinimizationProblem::StepTR(Real R) { grad.resize(x.n); fx=(*f)(x); f->Gradient(x,grad); int cn = C->NumDimensions(); int dn = D->NumDimensions(); Vector cx(cn),dx(dn); (*C)(x,cx); (*D)(x,dx); cout<<"******************************************************"<<endl; cout<<"ConstrainedMinimization::StepTR(): objective "<<fx<<", initial distance "<<cx.maxAbsElement()<<", margin "<<dx.minElement()<<endl; //solving the linearized version of the problem for a displacement //y such that C(x+y) ~= C(x)+J(x)*y = 0 //etc if(!sparse) { LinearProgram lp; lp.minimize=true; lp.Resize(cn+dn,x.n); //reserve enough space in A Assert(lp.c.n == grad.n); lp.c = grad; Matrix Ac,Ad; Vector bc,bd; Ac.setRef(lp.A,0,0,1,1,cn,x.n); Ad.setRef(lp.A,cn,0,1,1,dn,x.n); C->Jacobian(x,Ac); D->Jacobian(x,Ad); bc.setRef(lp.p,0,1,cn); bc.setNegative(cx); bc.setRef(lp.q,0,1,cn); bc.setNegative(cx); bd.setRef(lp.q,cn,1,dn); bd.setNegative(dx); lp.l.set(-R); lp.u.set(R); if(bmin.n != 0) { for(int i=0;i<x.n;i++) { if(lp.l(i)+x(i) < bmin(i)) lp.l(i) = bmin(i)-x(i); if(lp.u(i)+x(i) > bmax(i)) lp.u(i) = bmax(i)-x(i); } } RobustLPSolver lps; LinearProgram::Result res=lps.Solve(lp); if(res == LinearProgram::Infeasible) { cout<<"ConstrainedMinimization::StepTR(): trust region radius "<<R<<" too small, doing a feasibility solve step"<<endl; //NOTE: if MaxItersReached is returned, this will return ConvergenceError ConvergenceResult r; SolveFeasiblePoint(x,1,&r); if(r == MaxItersReached || r == ConvergenceF) //if(!SolveFeasiblePoint(x,1)) return ConvergenceError; return LocalMinimum; else if(r == ConvergenceX) return ConvergenceX; else return ConvergenceError; } else if(res == LinearProgram::Unbounded) { cout<<"ConstrainedMinimization::StepTR(): uh... can't be unbounded!"<<endl; Abort(); } else if(res == LinearProgram::Feasible) { //arbitrary merit function... Real fScale=tolf/tolc; Real origMerit = Merit(fx,cx,dx,fScale); Real alpha = One; Real stepNorm = lps.xopt.norm(); Vector xold=x; int numSteps=0; while(alpha > 1e-3) { x=xold; x.madd(lps.xopt,alpha); (*C)(x,cx); (*D)(x,dx); fx = (*f)(x); Real newMerit = Merit(fx,cx,dx,fScale); cout<<"ConstrainedMinimization::StepTR(): step length "<<stepNorm*alpha<<endl; cout<<"ConstrainedMinimization::StepTR(): objective "<<fx<<", distance "<<cx.maxAbsElement()<<", margin "<<dx.minElement()<<endl; if(newMerit < origMerit) { if(numSteps == 0) { if(FuzzyEquals(newMerit,origMerit,tolf)) return ConvergenceF; if(stepNorm < tolx) return ConvergenceX; return MaxItersReached; } else { cout<<"ConstrainedMinimization::StepTR(): trust region radius "<<R<<" too large"<<endl; cout<<"Suggest radius "<<R*alpha<<endl; return Divergence; } } numSteps++; alpha *= 0.5; } cout<<"ConstrainedMinimization::StepTR(): trust region radius "<<R<<" too large"<<endl; x = xold; return Divergence; } else { //error cout<<"ConstrainedMinimization::StepTR(): error solving linear program!"<<endl; return ConvergenceError; } return MaxItersReached; } else { //identical, but sparse LinearProgram_Sparse lp; lp.minimize=true; lp.Resize(cn+dn,x.n); //reserve enough space in A Assert(lp.c.n == grad.n); lp.c = grad; SparseMatrix Ac(cn,x.n),Ad(dn,x.n); Vector bc,bd; SparseVectorFunction* sC,*sD; //Note: dangerous cast here! try { sC=dynamic_cast<SparseVectorFunction*>(C); sD=dynamic_cast<SparseVectorFunction*>(D); } catch(exception& e) { FatalError("Could not cast C or D to sparse functions! exception: %s",e.what()); } cout<<"Evaluating equality jacobian..."; cout.flush(); sC->Jacobian_Sparse(x,Ac); cout<<Ac.numNonZeros()<<" nonzeros"<<endl; cout<<"Evaluating inequality jacobian..."; cout.flush(); sD->Jacobian_Sparse(x,Ad); cout<<Ad.numNonZeros()<<" nonzeros"<<endl; lp.A.copySubMatrix(0,0,Ac); lp.A.copySubMatrix(cn,0,Ad); bc.setRef(lp.p,0,1,cn); bc.setNegative(cx); bc.setRef(lp.q,0,1,cn); bc.setNegative(cx); bd.setRef(lp.q,cn,1,dn); bd.setNegative(dx); lp.l.set(-R); lp.u.set(R); if(bmin.n != 0) { for(int i=0;i<x.n;i++) { if(lp.l(i)+x(i) < bmin(i)) lp.l(i) = bmin(i)-x(i); if(lp.u(i)+x(i) > bmax(i)) lp.u(i) = bmax(i)-x(i); } } RobustLPSolver lps; LinearProgram::Result res=lps.Solve(lp); if(res == LinearProgram::Infeasible) { cout<<"Linear program is infeasible!"<<endl; /* lp.l.set(-Inf); lp.u.set(Inf); LinearProgram::Result res2=lps.Solve(lp); if(res2 == LinearProgram::Infeasible) { cout<<"Seems to be infeasible over entire space!"<<endl; getchar(); //deactivate inequality constraints for(int i=0;i<dn;i++) { lp.q(cn+i)=-Inf; lp.p(cn+i)=Inf; } res2 = lps.Solve(lp); if(res2 == LinearProgram::Infeasible) { cout<<"equalities are infeasible!"<<endl; //cout<<"Equalities: "<<cn<<", variables "<<x.n<<endl; //lp.Print(cout); getchar(); cout<<"Trying LSQR of equalities..."<<endl; LSQRInterface lsqr; lsqr.verbose = 0; if(lsqr.Solve(Ac,cx)) { lsqr.x.inplaceNegative(); cout<<"LSQR solved the problem with residual "<<lsqr.residualNorm<<endl; } else cout<<"LSQR failed with residual "<<lsqr.residualNorm<<endl; return ConvergenceError; } else { } } */ /* lp.A.eraseZeros(); LinearProgram_Sparse lptemp; lptemp = lp; lptemp.A.clear(); lptemp.p.clear(); lptemp.q.clear(); for(int i=1;i<lp.A.m;i++) { lptemp.A.resize(i,lp.A.n); lptemp.p.resize(i); lptemp.q.resize(i); for(int j=0;j<i;j++) { lptemp.A.rows[j] = lp.A.rows[j]; lptemp.p(j) = lp.p(j); lptemp.q(j) = lp.q(j); } if(lps.Solve(lptemp) == LinearProgram::Infeasible) { cout<<"Constraint "<<i<<" caused the LP to become infeasible!"<<endl; SparseVector tmp; tmp.set(lptemp.A.rows[i-1]); cout<<lptemp.q(i-1)<<" < "; tmp.print(cout); cout<<" < "<<lptemp.p(i-1)<<endl; getchar(); break; } } */ cout<<"ConstrainedMinimization::StepTR(): trust region radius "<<R<<" too small, doing a feasibility solve step"<<endl; rootSolver.tolf = tolc; rootSolver.tolmin = tolx; rootSolver.tolx = tolx; rootSolver.bmin.setRef(bmin); rootSolver.bmax.setRef(bmax); rootSolver.verbose = verbose; rootSolver.x.setRef(x); rootSolver.sparse = sparse; cout<<"ConstrainedMinimizationProblem::SolveFeasiblePoint..."<<endl; int iters=1; ConvergenceResult r=rootSolver.SolveConstrained_SLP(iters); if(r == ConvergenceError) { cout<<"ConstrainedMinimization::StepTR(): feasibility could not be solved!"<<endl; return ConvergenceError; } else if(r == ConvergenceX) { cout<<"ConstrainedMinimization::StepTR(): feasibility could not be solved, x tolerance has been reached"<<endl; return ConvergenceX; } /* if(!SolveFeasiblePoint(x,1)) { cout<<"ConstrainedMinimization::StepTR(): feasibility could not be solved!"<<endl; return ConvergenceError; } */ (*C)(x,cx); (*D)(x,dx); fx = (*f)(x); cout<<"ConstrainedMinimization::StepTR(): final objective "<<fx<<", distance "<<cx.maxAbsElement()<<", margin "<<dx.minElement()<<endl; /* //shortcut to SolveFeasiblePoint() -- just solve the least squares problem for(int i=0;i<dn;i++) if(dx(i) > Zero) { //remove from lp.A, lp.b lp.A.rows[i+cn].entries.clear(); lp.q(i+cn)=Zero; } bool res=rootSolver.SolveUnderconstrainedLS(lp.A,lp.q,lps.xopt); if(res) { Real merit = Merit(Zero,cx,dx); Real alpha = One; Vector x0=x; while(alpha > 1e-4) { x = x0; x.madd(lps.xopt,alpha); (*C)(x,cx); (*D)(x,dx); fx = (*f)(x); Real newMerit = Merit(Zero,cx,dx); if(newMerit < merit-1e-4) //sufficient decrease break; alpha *= 0.5; } if(alpha <= 1e-4) { cout<<"ConstrainedMinimization::StepTR(): backtracking failed!"<<endl; x=x0; } else { cout<<"ConstrainedMinimization::StepTR(): final objective "<<fx<<", distance "<<cx.maxAbsElement()<<", margin "<<dx.minElement()<<endl; } } */ return LocalMinimum; } else if(res == LinearProgram::Unbounded) { cout<<"ConstrainedMinimization::StepTR(): uh... can't be unbounded!"<<endl; Abort(); } else if(res == LinearProgram::Feasible) { //arbitrary merit function... Real fScale=tolf/tolc; Real origMerit = Merit(fx,cx,dx,fScale); cout<<"ConstrainedMinimization::StepTR():"<<endl; cout<<" Original merit "<<origMerit<<endl; Real alpha = One; Real stepNorm = lps.xopt.norm(); Vector xold=x; int numSteps=0; while(alpha > 1e-3) { x=xold; x.madd(lps.xopt,alpha); (*C)(x,cx); (*D)(x,dx); fx = (*f)(x); Real newMerit = Merit(fx,cx,dx,fScale); cout<<" Step length "<<stepNorm*alpha<<", merit "<<newMerit<<endl; cout<<" Objective "<<fx<<", distance "<<cx.maxAbsElement()<<", margin "<<dx.minElement()<<endl; if(newMerit < origMerit) { if(numSteps == 0) { if(FuzzyEquals(newMerit,origMerit,tolf)) return ConvergenceF; if(stepNorm < tolx) return ConvergenceX; return MaxItersReached; } else { cout<<"ConstrainedMinimization::StepTR(): trust region radius "<<R<<" too large"<<endl; cout<<"Suggest radius "<<R*alpha<<endl; return Divergence; } } numSteps++; alpha *= 0.5; } cout<<"ConstrainedMinimization::StepTR(): trust region radius "<<R<<" too large"<<endl; x = xold; return Divergence; } else { //error cout<<"ConstrainedMinimization::StepTR(): error solving linear program!"<<endl; return ConvergenceError; } return MaxItersReached; } }