[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 **

gtsam_unstable/linear/LPSolver.h

@@ -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 {
+  pair<VectorValues, VectorValues> optimize() const {
-//
+
-//    // Initialize workingSet from the feasible initialValues
+    // 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

gtsam_unstable/linear/QPSolver.cpp

@@ -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 */

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 <primal, dual> solutions
+   */
+  std::pair<VectorValues, VectorValues> optimize() const;
+
 };
 
 } // namespace gtsam

gtsam_unstable/linear/SQPSolver.h

@@ -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();
-};

gtsam_unstable/linear/SQPState.h

@@ -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) {}
-};
-}

gtsam_unstable/linear/tests/testLPSolver.cpp

@@ -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

gtsam_unstable/linear/tests/testQPSolver.cpp

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

gtsam_unstable/linear/tests/testSQPSolver.cpp

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