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;
}
