diff --git a/gtsam_unstable/linear/LPSolver.h b/gtsam_unstable/linear/LPSolver.h index 849be035d..9ff8bd878 100644 --- a/gtsam_unstable/linear/LPSolver.h +++ b/gtsam_unstable/linear/LPSolver.h @@ -11,6 +11,7 @@ #include #include #include +#include #include #include @@ -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 optimize() const { -// -// // Initialize workingSet from the feasible initialValues + pair 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 diff --git a/gtsam_unstable/linear/QPSolver.cpp b/gtsam_unstable/linear/QPSolver.cpp index bb47067cc..cf391f10c 100644 --- a/gtsam_unstable/linear/QPSolver.cpp +++ b/gtsam_unstable/linear/QPSolver.cpp @@ -19,6 +19,7 @@ #include #include #include +#include #include #include @@ -177,4 +178,19 @@ pair QPSolver::optimize( return make_pair(state.values, state.duals); } +pair 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 */ diff --git a/gtsam_unstable/linear/QPSolver.h b/gtsam_unstable/linear/QPSolver.h index 6ff47e580..bd1bb2a17 100644 --- a/gtsam_unstable/linear/QPSolver.h +++ b/gtsam_unstable/linear/QPSolver.h @@ -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 solutions + */ + std::pair optimize() const; + }; } // namespace gtsam diff --git a/gtsam_unstable/linear/SQPSolver.h b/gtsam_unstable/linear/SQPSolver.h deleted file mode 100644 index f6488ac4a..000000000 --- a/gtsam_unstable/linear/SQPSolver.h +++ /dev/null @@ -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 -#include -#include - -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 optimize(const Vector& initialValues) const; - - /* - * This function will attempt to optimize the Non-linear problem without the need - * of initial values. - */ - pair optimize(); -}; diff --git a/gtsam_unstable/linear/SQPState.h b/gtsam_unstable/linear/SQPState.h deleted file mode 100644 index e272e308a..000000000 --- a/gtsam_unstable/linear/SQPState.h +++ /dev/null @@ -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 - -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) {} -}; -} diff --git a/gtsam_unstable/linear/tests/testLPSolver.cpp b/gtsam_unstable/linear/tests/testLPSolver.cpp index 7af7fbd38..adb4cb622 100644 --- a/gtsam_unstable/linear/tests/testLPSolver.cpp +++ b/gtsam_unstable/linear/tests/testLPSolver.cpp @@ -19,7 +19,6 @@ #include #include #include -#include #include #include #include @@ -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 diff --git a/gtsam_unstable/linear/tests/testQPSolver.cpp b/gtsam_unstable/linear/tests/testQPSolver.cpp index 259de3430..0bb884675 100644 --- a/gtsam_unstable/linear/tests/testQPSolver.cpp +++ b/gtsam_unstable/linear/tests/testQPSolver.cpp @@ -22,6 +22,7 @@ #include #include +#include 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; diff --git a/gtsam_unstable/linear/tests/testSQPSolver.cpp b/gtsam_unstable/linear/tests/testSQPSolver.cpp deleted file mode 100644 index c259641ed..000000000 --- a/gtsam_unstable/linear/tests/testSQPSolver.cpp +++ /dev/null @@ -1,11 +0,0 @@ -#include -#include -#include - - -using namespace gtsam; - -int main(){ - TestResult tr; - return TestRegistry::runAllTests(tr); -} \ No newline at end of file