bool MatrixEquation::LeastSquares_SVD(Vector& x) const { //svd automatically does the least-squares Assert(IsValid()); SVDecomposition<Real> svd; if(A.m <= A.n) { if(!svd.set(A)) return false; svd.backSub(b,x); return true; } else { if(!svd.set(A)) return false; svd.backSub(b,x); return true; /* cerr<<"Doing the transpose SVD"<<endl; Matrix At; At.setRefTranspose(A); if(!svd.set(At)) return false; svd.getInverse(At); cout<<"Result"<<endl<<MatrixPrinter(At)<<endl; Matrix Ainv; Ainv.setRefTranspose(At); Ainv.mul(b,x); return true; */ } }
bool MatrixEquation::Solve_SVD(Vector& x) const { Assert(A.isSquare()); Assert(A.n == b.n); SVDecomposition<Real> svd; if(!svd.set(A)) return false; svd.backSub(b,x); return true; }
bool MatrixEquation::AllSolutions_SVD(Vector& x0,Matrix& N) const { if(A.n < A.m) { cout<<"Warning: matrix is overconstrained"<<endl; } SVDecomposition<Real> svd; if(!svd.set(A)) return false; svd.backSub(b,x0); svd.getNullspace(N); return true; }
ConvergenceResult Root_Newton(VectorFieldFunction& f,const Vector& x0, Vector& x, int& iters, Real tolx, Real tolf) { //move in gradient direction to set f to 0 //f(x) ~= f(x0) + df/dx(x0)*(x-x0) + O(h^2) //=> 0 = fx + J*(x-x0) => dx = -J^-1*fx SVDecomposition<Real> svd; Vector fx,p; Matrix fJx; if(&x != &x0) x = x0; int maxIters=iters; for (iters=0;iters<maxIters;iters++) { f.PreEval(x); f.Eval(x,fx); f.Jacobian(x,fJx); if(fx.maxAbsElement() <= tolf) return ConvergenceF; if(!svd.set(fJx)) { return ConvergenceError; } svd.backSub(fx,p); x -= p; if(p.maxAbsElement() <= tolx) return ConvergenceX; } return MaxItersReached; }
bool LP_InteriorPoint::Set(const LinearProgram& lp) { Matrix Aeq; Vector beq; int neq=0,nineq=0; for(int i=0;i<lp.A.m;i++) { if(lp.ConstraintType(i) == LinearProgram::Fixed) neq++; else { if(lp.HasLowerBound(lp.ConstraintType(i))) nineq++; if(lp.HasUpperBound(lp.ConstraintType(i))) nineq++; } } for(int i=0;i<lp.A.n;i++) { if(lp.VariableType(i) == LinearProgram::Fixed) neq++; else { if(lp.HasLowerBound(lp.VariableType(i))) nineq++; if(lp.HasUpperBound(lp.VariableType(i))) nineq++; } } if(neq == 0) { x0.clear(); N.clear(); ((LinearProgram&)solver) = lp; //solver.minimize is ignored by the solver if(!solver.minimize) solver.c.inplaceNegative(); return true; } Aeq.resize(neq,lp.A.n); beq.resize(neq); neq=0; for(int i=0;i<lp.A.m;i++) { if(lp.ConstraintType(i)==LinearProgram::Fixed) { Vector Ai; lp.A.getRowRef(i,Ai); Aeq.copyRow(neq,Ai); beq(neq) = lp.p(i); neq++; } } for (int i=0;i<lp.A.n;i++) { if(lp.VariableType(i)==LinearProgram::Fixed) { Vector Aeqi; Aeq.getRowRef(i,Aeqi); Aeqi.setZero(); Aeqi(i) = One; beq(neq) = lp.l(i); neq++; } } SVDecomposition<Real> svd; if(!svd.set(Aeq)) { if(solver.verbose>=1) cout<<"LP_InteriorPoint: Couldn't set SVD of equality constraints!!!"<<endl; return false; } svd.backSub(beq,x0); svd.getNullspace(N); //Set the solver to use the new variable y if(N.n == 0) { //overconstrained! cout<<"Overconstrained!"<<endl; solver.Resize(0,0); return true; } if(nineq == 0) { cout<<"No inequalities!"<<endl; abort(); return true; } if(solver.verbose >= 1) cout<<"LP_InteriorPoint: Decomposed the problem from "<<lp.A.n<<" to "<<N.n<<" variables"<<endl; solver.Resize(nineq,N.n); //objective foffset = dot(lp.c,x0); //c is such that c'*y = lp.c'*N*y => c = N'*lp.c N.mulTranspose(lp.c,solver.c); solver.minimize = lp.minimize; if(!solver.minimize) solver.c.inplaceNegative(); //inequality constraints //q <= Aineq*x <= p //q <= Aineq*x0 + Aineq*N*y <= p //q - Aineq*x0 <= Aineq*N*y <= p-Aineq*x0 //==> -Aineq*N*y <= -q + Aineq*x0 nineq=0; for(int i=0;i<lp.A.m;i++) { if(lp.ConstraintType(i)==LinearProgram::Fixed) continue; if(lp.HasUpperBound(lp.ConstraintType(i))) { Vector Ai,sAi; lp.A.getRowRef(i,Ai); solver.A.getRowRef(nineq,sAi); N.mulTranspose(Ai,sAi); solver.p(nineq) = lp.p(i) - dot(Ai,x0); nineq++; } if(lp.HasLowerBound(lp.ConstraintType(i))) { Vector Ai,sAi; lp.A.getRowRef(i,Ai); solver.A.getRowRef(nineq,sAi); N.mulTranspose(Ai,sAi); sAi.inplaceNegative(); solver.p(nineq) = dot(Ai,x0) - lp.q(i); nineq++; } } //transform bounds to inequality constraints for(int i=0;i<lp.u.n;i++) { if(lp.VariableType(i)==LinearProgram::Fixed) continue; if(lp.HasLowerBound(lp.VariableType(i))) { //-xi < -li //-ei'*N*y <= -li+ei'*x0 Vector Ni,sAi; N.getRowRef(i,Ni); solver.A.getRowRef(nineq,sAi); sAi.setNegative(Ni); solver.p(nineq) = -lp.l(i) + x0(i); nineq++; } if(lp.HasUpperBound(lp.VariableType(i))) { //xi < ui //ei'*N*y <= ui-ei'*x0 Vector Ni,sAi; N.getRowRef(i,Ni); solver.A.getRowRef(nineq,sAi); sAi.copy(Ni); solver.p(nineq) = lp.u(i) - x0(i); nineq++; } } Assert(solver.IsValid()); return true; }