12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

[FEATURE] SQP Removed 

[FEATURE] Test SQP for Infeasible
[FEATURE] Test QP for Infeasible
[FEATURE] LP Initialize Without Initial Values
[FEATURE] QP Initialize Without Initial Values
** TESTS FAIL **
release/4.3a0
Ivan Jimenez 2016-02-09 10:45:55 -05:00
parent 8926a1da91
commit f42c4f6a92
8 changed files with 98 additions and 83 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

18
gtsam_unstable/linear/LPSolver.h
View File

@ -11,6 +11,7 @@
#include <gtsam_unstable/linear/LPState.h>
#include <gtsam_unstable/linear/LP.h>
#include <gtsam_unstable/linear/ActiveSetSolver.h>
#include <gtsam_unstable/linear/LinearCost.h>
#include <gtsam/linear/VectorValues.h>
#include <boost/range/adaptor/map.hpp>
@ -113,21 +114,22 @@ public:
/**
* Optimize without initial values
* TODO: Find a feasible initial solution wrt inequality constraints
* TODO: Find a feasible initial solution that doens't involve simplex method
* nor Solving another LP
*/
// pair<VectorValues, VectorValues> optimize() const {
//
// // Initialize workingSet from the feasible initialValues
pair<VectorValues, VectorValues> optimize() const {
// Initialize workingSet from the feasible initialValues
// InequalityFactorGraph workingSet = identifyActiveConstraints(
// lp_.inequalities, initialValues, duals);
// LPState state(initialValues, duals, workingSet, false, 0);
//
// /// main loop of the solver
/// main loop of the solver
// while (!state.converged) {
// state = iterate(state);
// }
//
// return make_pair(state.values, state.duals);
// }
}
};
} // namespace gtsam

16
gtsam_unstable/linear/QPSolver.cpp
View File

@ -19,6 +19,7 @@
#include <gtsam/inference/Symbol.h>
#include <gtsam/inference/FactorGraph-inst.h>
#include <gtsam_unstable/linear/QPSolver.h>
#include <gtsam_unstable/linear/LPSolver.h>
#include <gtsam_unstable/linear/InfeasibleInitialValues.h>
#include <boost/range/adaptor/map.hpp>
@ -177,4 +178,19 @@ pair<VectorValues, VectorValues> QPSolver::optimize(
return make_pair(state.values, state.duals);
}
pair<VectorValues, VectorValues> QPSolver::optimize() const {
LP initProblem;
initProblem.cost = LinearCost(2,ones(1,1)); // min gamma s.t.
initProblem.equalities = qp_.equalities; // s.t Ax = b from the original problem
//TODO: SLACK Variable needs to be added
initProblem.inequalities = qp_.inequalities; // s.t. Ax - gamma <= b Ax from the original problem
LPSolver solver(initProblem);
VectorValues result, duals;
boost::tie(result, duals) = solver.optimize();
return optimize(result);
};
} /* namespace gtsam */

9
gtsam_unstable/linear/QPSolver.h
View File

@ -89,6 +89,15 @@ public:
const VectorValues& initialValues, const VectorValues& duals =
VectorValues(), bool useWarmStart = true) const;
/**
* For this version the caller will not have to provide an initial value
* Uses the matlab strategy for initialization
* See http://www.mathworks.com/help/optim/ug/quadratic-programming-algorithms.html#brrzwpf-22
* For details
* @return a pair of <primal, dual> solutions
*/
std::pair<VectorValues, VectorValues> optimize() const;
};
} // namespace gtsam

32
gtsam_unstable/linear/SQPSolver.h
View File

@ -1,32 +0,0 @@
/**
* @file SQPSolver.h
* @brief This class is used to solve a Non-linear Problem
* (NP) and finds a local solution using SQP. See
* Nocedal Algorithm 18.3 for details.
* @author Ivan Dario Jimenez
* @date 2/2/16
*/
#pragma once
#include <gtsam_unstable/linear/NP.h>
#include <gtsam_unstable/linear/ActiveSetSolver.h>
#include <gtsam_unstable/linear/SQPState.h>
class SQPSolver : public ActiveSetSolver {
const NP& np_; //!< the nonlinear programming problem
public:
SQPSolver(const NP& np) : np_(np){}
NPState iterate(const NPState& state) const;
/*
* This function will optimize the Non-linear problem given a set of initial values
*/
pair<Vector, Vector> optimize(const Vector& initialValues) const;
/*
* This function will attempt to optimize the Non-linear problem without the need
* of initial values.
*/
pair<Vector, Vector> optimize();
};

30
gtsam_unstable/linear/SQPState.h
View File

@ -1,30 +0,0 @@
/**
* @file SQPState.h
* @brief This class is used to store the current state of
* an SQPSolver iteration.
* @author Ivan Dario Jimenez
* @date 2/2/16
*/
#pragma once
#include <gtsam/base/Matrix.h>
namespace gtsam {
class SQPState {
Vector values;
Vector duals;
Vector workingSet;
bool converged;
size_t iterations;
SQPState() :
values(), duals(), workingSet(),converged(false), iterations(0) {}
SQPState(const Vector& initialValues, const Vector& initialDuals,
const Vector& initialWorkingSet, const bool _converged,
const size_t iterations) :
values(initialValues), duals(initialDuals), workingSet(initialWorkingSet),
converged(_converged), iterations(iterations) {}
};
}

22
gtsam_unstable/linear/tests/testLPSolver.cpp
View File

@ -19,7 +19,6 @@
#include <gtsam/base/Testable.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/inference/FactorGraph-inst.h>
#include <gtsam_unstable/linear/LinearCost.h>
#include <gtsam/linear/VectorValues.h>
#include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam_unstable/linear/EqualityFactorGraph.h>
@ -61,6 +60,13 @@ LP simpleLP1() {
return lp;
}
LP infeasibleLP(){
LP lp;
lp.cost = LinearCost(1, Vector3(-1, -1, -2));
}
/* ************************************************************************* */
namespace gtsam {
TEST(LPInitSolverMatlab, initialization) {
@ -159,6 +165,20 @@ TEST(LPSolver, simpleTest1) {
CHECK(assert_equal(expectedResult, result, 1e-10));
}
/* ************************************************************************* */
TEST(LPSolver, testWithoutInitialValues) {
LP lp = simpleLP1();
LPSolver lpSolver(lp);
VectorValues result, duals;
boost::tie(result, duals) = lpSolver.optimize();
VectorValues expectedResult;
expectedResult.insert(1, Vector2(8./3., 2./3.));
CHECK(assert_equal(expectedResult, result, 1e-10));
}
/**
* TODO: More TEST cases:
* - Infeasible

43
gtsam_unstable/linear/tests/testQPSolver.cpp
View File

@ -22,6 +22,7 @@
#include <CppUnitLite/TestHarness.h>
#include <gtsam_unstable/linear/InfeasibleInitialValues.h>
#include <gtsam_unstable/linear/InfeasibleOrUnboundedProblem.h>
using namespace std;
using namespace gtsam;
@ -29,6 +30,7 @@ using namespace gtsam::symbol_shorthand;
const Matrix One = ones(1,1);
/* ************************************************************************* */
// Create test graph according to Forst10book_pg171Ex5
QP createTestCase() {
@ -38,9 +40,11 @@ QP createTestCase() {
// Note the Hessian encodes:
// 0.5*x1'*G11*x1 + x1'*G12*x2 + 0.5*x2'*G22*x2 - x1'*g1 - x2'*g2 + 0.5*f
// Hence, we have G11=2, G12 = -1, g1 = +3, G22 = 2, g2 = 0, f = 10
//TODO: THIS TEST MIGHT BE WRONG : the last parameter might be 5 instead of 10 because the form of the equation
// Should be 0.5x'Gx + gx + f : Nocedal 449
qp.cost.push_back(
HessianFactor(X(1), X(2), 2.0 * ones(1, 1), -ones(1, 1), 3.0 * ones(1),
2.0 * ones(1, 1), zero(1), 10.0));
2.0 * ones(1, 1), zero(1), 10));
// Inequality constraints
qp.inequalities.push_back(LinearInequality(X(1), ones(1,1), X(2), ones(1,1), 2, 0)); // x1 + x2 <= 2 --> x1 + x2 -2 <= 0, --> b=2
@ -246,6 +250,18 @@ TEST(QPSolver, optimizeMatlabEx) {
CHECK(assert_equal(expectedSolution, solution, 1e-7));
}
/* ************************************************************************* */
TEST(QPSolver, optimizeMatlabExNoinitials) {
QP qp = createTestMatlabQPEx();
QPSolver solver(qp);
VectorValues solution;
boost::tie(solution, boost::tuples::ignore) = solver.optimize();
VectorValues expectedSolution;
expectedSolution.insert(X(1), (Vector(1) << 2.0 / 3.0).finished());
expectedSolution.insert(X(2), (Vector(1) << 4.0 / 3.0).finished());
CHECK(assert_equal(expectedSolution, solution, 1e-7));
}
/* ************************************************************************* */
// Create test graph as in Nocedal06book, Ex 16.4, pg. 475
QP createTestNocedal06bookEx16_4() {
@ -322,6 +338,31 @@ TEST(QPSolver, infeasibleInitial) {
);
}
/* ************************************************************************* */
TEST(QPSolver, infeasibleConstraints) {
QP qp;
// Objective functions x1^2 - x1*x2 + x2^2 - 3*x1 + 5
// Note the Hessian encodes:
// 0.5*x1'*G11*x1 + x1'*G12*x2 + 0.5*x2'*G22*x2 - x1'*g1 - x2'*g2 + 0.5*f
// Hence, we have G11=2, G12 = -1, g1 = +3, G22 = 2, g2 = 0, f = 10
qp.cost.push_back(
HessianFactor(X(1), X(2), 2.0 * ones(1, 1), -ones(1, 1), 3.0 * ones(1),
2.0 * ones(1, 1), zero(1), 5));
qp.inequalities.push_back(LinearInequality(X(1), -1.0 * ones(1,1), X(2), -1.0 * ones(1,1), 32, 0)); // x1 + x2 >= 32
qp.inequalities.push_back(LinearInequality(X(1), ones(1,1), 0, 1)); // x1 <= 0
qp.inequalities.push_back(LinearInequality(X(2), ones(1,1), 0, 2)); // x2 <= 0
qp.inequalities.push_back(LinearInequality(X(1), -ones(1,1), 0, 1)); // -x1 <= 0
qp.inequalities.push_back(LinearInequality(X(2), -ones(1,1), 0, 2)); // -x2 <= 0
QPSolver solver(qp);
CHECK_EXCEPTION(
solver.optimize(),
InfeasibleOrUnboundedProblem);
}
/* ************************************************************************* */
int main() {
TestResult tr;

11
gtsam_unstable/linear/tests/testSQPSolver.cpp
View File

@ -1,11 +0,0 @@
#include <gtsam_unstable/linear/NP.h>
#include <gtsam_unstable/linear/SQPSolver.h>
#include <gtsam_unstable/linear/SQPState.h>
using namespace gtsam;
int main(){
TestResult tr;
return TestRegistry::runAllTests(tr);
}