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;
}
}
LinearProgram::Result RegularizedLinearProgram::Solve(Vector& x)
{
RobustLPSolver lps;
lps.verbose = verbose;
lps.UpdateGLPK(lp);
LinearProgram::Result res= lps.SolveGLPK();
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;
}
}
LinearProgram::Result MinNormProblem::Solve(Vector& x)
{
if(norm == 2) {
Assert(IsValid());
if(HasInequalities()) { //must use quadratic program for inequalities
Assert(qp.Pobj.m == C.n);
Assert(qp.A.isRef());
/*
QPActiveSetSolver solver(qp);
solver.verbose = verbose;
ConvergenceResult res=solver.Solve();
if(res == ConvergenceError) {
if(verbose >= 1)
cerr<<"Quadratic program unable to solve constrained least-squares problem"<<endl;
getchar();
return LinearProgram::Infeasible;
}
else if(res == MaxItersReached) {
x = solver.x;
return LinearProgram::Error;
}
else {
x = solver.x;
return LinearProgram::Feasible;
}
*/
FatalError("TODO: QP Solve");
}
else if(HasBounds()) {
if(A.m != 0) {
//Equalities done yet
FatalError("Equalities and bounds not done yet");
return LinearProgram::Error;
}
else {
//solve a bounded least squares problem
BoundedLSQRSolver lsqr(C,d,l,u);
LinearProgram::Result res=lsqr.Solve(x);
return res;
}
}
else if(A.m != 0) { //no inequality constraints, just equality
//just transform the problem to an equivalent one
//Ax=p => x = A#*p + N*y = x0 + N*y
//where A# is pseudoinverse, N is nullspace
//so lsq problem becomes min |C*N*y - (d-C*x0)|
Assert(A.m <= A.n); //otherwise overconstrained
Matrix C_new, N;
Vector d_new, x0, y;
MatrixEquation eq(A,p);
if(!eq.AllSolutions(x0,N)) {
if(verbose >= 1)
cerr<<"MinNormProblem (norm 2): Error solving for all solutions to equality constraints"<<endl;
if(verbose >= 2) {
cerr<<"Press any key to continue"<<endl;
getchar();
}
return LinearProgram::Error;
}
if(verbose >= 2) {
Vector r;
eq.Residual(x0,r);
if(r.norm() > 1e-4) {
cout<<"Residual of Aeq*x0=beq: "<<VectorPrinter(r)<<endl;
cout<<"Norm is "<<r.norm()<<endl;
if(r.norm() > 1e-2) {
cout<<MatrixPrinter(A)<<endl;
cout<<"Press any key to continue"<<endl;
getchar();
return LinearProgram::Error;
}
cout<<"Press any key to continue"<<endl;
getchar();
}
}
if(verbose >= 1) {
cout<<"Projecting problem on equality constraints"<<endl;
cout<<"Original dimension "<<A.n<<", nullspace dimension "<<N.n<<endl;
}
//set bnew
C.mul(x0,d_new); d_new-=d; d_new.inplaceNegative();
//set Cnew
C_new.mul(C,N);
if(verbose >= 2) {
cout<<"x0: "<<VectorPrinter(x0)<<endl;
cout<<"N: "<<endl<<MatrixPrinter(N)<<endl;
}
if(verbose >=1) cout<<"Solving transformed problem..."<<endl;
MatrixEquation ls(C_new,d_new);
if(!ls.LeastSquares(y)) {
cerr<<"LeastSquares: Error solving transformed least squares!!!"<<endl;
if(verbose >=1) {
cerr<<"Press any key to continue"<<endl;
getchar();
}
return LinearProgram::Error;
}
//go back to x
x = x0;
N.madd(y,x);
return LinearProgram::Feasible;
}
else {
MatrixEquation ls(C,d);
if(!ls.LeastSquares(x)) {
cout<<"Error solving for least squares!!!"<<endl;
return LinearProgram::Error;
}
return LinearProgram::Feasible;
}
}
else {
RobustLPSolver lps;
lps.verbose = verbose;
lps.UpdateGLPK(lp);
LinearProgram::Result res= lps.SolveGLPK();
if(res==LinearProgram::Feasible) {
x.resize(C.n);
lps.xopt.getSubVectorCopy(0,x);
}
return res;
}
cout<<"Not sure how we got here..."<<endl;
return LinearProgram::Error;
}
