[FEATURE] LPSolver without initial Values.

[REFACTOR] Reformat code with eclipse code formatter.

gtsam_unstable/linear/LPInitSolverMatlab.h

@@ -5,12 +5,11 @@
  * @date     1/24/16
  */
 
-
 #pragma once
 
 #include <gtsam_unstable/linear/LPInitSolver.h>
 #include <gtsam_unstable/linear/InfeasibleOrUnboundedProblem.h>
-
+#include <CppUnitLite/Test.h>
 
 namespace gtsam {
 /**
@@ -42,8 +41,11 @@ namespace gtsam {
 class LPInitSolverMatlab: public LPInitSolver {
   typedef LPInitSolver Base;
 public:
-  LPInitSolverMatlab(const LPSolver& lpSolver) : Base(lpSolver) {}
+  LPInitSolverMatlab(const LPSolver& lpSolver) :
-  virtual ~LPInitSolverMatlab() {}
+      Base(lpSolver) {
+  }
+  virtual ~LPInitSolverMatlab() {
+  }
 
   virtual VectorValues solve() const {
     // Build the graph to solve for the initial value of the initial problem
@@ -74,7 +76,8 @@ private:
     LP::shared_ptr initLP(new LP());
     initLP->cost = LinearCost(yKey, ones(1)); // min y
     initLP->equalities = lp_.equalities; // st. Ax = b
-    initLP->inequalities = addSlackVariableToInequalities(yKey, lp_.inequalities); // Cx-y <= d
+    initLP->inequalities = addSlackVariableToInequalities(yKey,
+        lp_.inequalities); // Cx-y <= d
     return initLP;
   }
 
@@ -93,7 +96,8 @@ private:
    */
   GaussianFactorGraph::shared_ptr buildInitOfInitGraph() const {
     // first add equality constraints Ax = b
-    GaussianFactorGraph::shared_ptr initGraph(new GaussianFactorGraph(lp_.equalities));
+    GaussianFactorGraph::shared_ptr initGraph(
+        new GaussianFactorGraph(lp_.equalities));
 
     // create factor ||x||^2 and add to the graph
     const KeyDimMap& keysDim = lpSolver_.keysDim();
@@ -115,9 +119,9 @@ private:
     return y0;
   }
 
-
   /// Collect all terms of a factor into a container.
-  std::vector<std::pair<Key, Matrix> > collectTerms(const LinearInequality& factor) const {
+  std::vector<std::pair<Key, Matrix> > collectTerms(
+      const LinearInequality& factor) const {
     std::vector < std::pair<Key, Matrix> > terms;
     for (Factor::const_iterator it = factor.begin(); it != factor.end(); it++) {
       terms.push_back(make_pair(*it, factor.getA(it)));

gtsam_unstable/linear/LPSolver.cpp

@@ -9,9 +9,11 @@
 #include <gtsam_unstable/linear/LPSolver.h>
 #include <gtsam_unstable/linear/InfeasibleInitialValues.h>
 #include <gtsam/linear/GaussianFactorGraph.h>
+#include <gtsam_unstable/linear/LPInitSolverMatlab.h>
 
 namespace gtsam {
-LPSolver::LPSolver(const LP &lp) : lp_(lp) {
+LPSolver::LPSolver(const LP &lp) :
+    lp_(lp) {
   // Push back factors that are the same in every iteration to the base graph.
   // Those include the equality constraints and zero priors for keys that are
   // not in the cost
@@ -50,8 +52,8 @@ LPState LPSolver::iterate(const LPState &state) const {
   // Solve with the current working set
   // LP: project the objective neg. gradient to the constraint's null space
   // to find the direction to move
-  VectorValues newValues =
+  VectorValues newValues = solveWithCurrentWorkingSet(state.values,
-      solveWithCurrentWorkingSet(state.values, state.workingSet);
+      state.workingSet);
 
   // If we CAN'T move further
   // LP: projection on the constraints' nullspace is zero: we are at a vertex
@@ -61,8 +63,8 @@ LPState LPSolver::iterate(const LPState &state) const {
     // LP: project the objective's gradient onto each constraint gradient to
     // obtain the dual scaling factors
     //	is it true??
-    GaussianFactorGraph::shared_ptr dualGraph =
+    GaussianFactorGraph::shared_ptr dualGraph = buildDualGraph(state.workingSet,
-        buildDualGraph(state.workingSet, newValues);
+        newValues);
     VectorValues duals = dualGraph->optimize();
     // LP: see which inequality constraint has wrong pulling direction, i.e., dual < 0
     int leavingFactor = identifyLeavingConstraint(state.workingSet, duals);
@@ -71,13 +73,15 @@ LPState LPSolver::iterate(const LPState &state) const {
       // TODO If we still have infeasible equality constraints: the problem is
       // over-constrained. No solution!
       // ...
-      return LPState(newValues, duals, state.workingSet, true, state.iterations + 1);
+      return LPState(newValues, duals, state.workingSet, true,
+          state.iterations + 1);
     } else {
       // Inactivate the leaving constraint
       // LP: remove the bad ineq constraint out of the working set
       InequalityFactorGraph newWorkingSet = state.workingSet;
       newWorkingSet.at(leavingFactor)->inactivate();
-      return LPState(newValues, duals, newWorkingSet, false, state.iterations + 1);
+      return LPState(newValues, duals, newWorkingSet, false,
+          state.iterations + 1);
     }
   } else {
     // If we CAN make some progress, i.e. p_k != 0
@@ -92,10 +96,12 @@ LPState LPSolver::iterate(const LPState &state) const {
         computeStepSize(state.workingSet, state.values, p);
     // also add to the working set the one that complains the most
     InequalityFactorGraph newWorkingSet = state.workingSet;
-    if (factorIx >= 0) newWorkingSet.at(factorIx)->activate();
+    if (factorIx >= 0)
+      newWorkingSet.at(factorIx)->activate();
     // step!
     newValues = state.values + alpha * p;
-    return LPState(newValues, state.duals, newWorkingSet, false, state.iterations + 1);
+    return LPState(newValues, state.duals, newWorkingSet, false,
+        state.iterations + 1);
   }
 }
 
@@ -113,26 +119,26 @@ GaussianFactorGraph::shared_ptr LPSolver::createLeastSquareFactors(
   return graph;
 }
 
-VectorValues LPSolver::solveWithCurrentWorkingSet(
+VectorValues LPSolver::solveWithCurrentWorkingSet(const VectorValues &xk,
-    const VectorValues &xk, const InequalityFactorGraph &workingSet) const {
+    const InequalityFactorGraph &workingSet) const {
   GaussianFactorGraph workingGraph = baseGraph_; // || X - Xk + g ||^2
   workingGraph.push_back(*createLeastSquareFactors(lp_.cost, xk));
 
   for (const LinearInequality::shared_ptr &factor : workingSet) {
-    if (factor->active()) workingGraph.push_back(factor);
+    if (factor->active())
+      workingGraph.push_back(factor);
   }
   return workingGraph.optimize();
 }
 
-boost::shared_ptr<JacobianFactor> LPSolver::createDualFactor(
+boost::shared_ptr<JacobianFactor> LPSolver::createDualFactor(Key key,
-    Key key, const InequalityFactorGraph &workingSet,
+    const InequalityFactorGraph &workingSet, const VectorValues &delta) const {
-    const VectorValues &delta) const {
   // Transpose the A matrix of constrained factors to have the jacobian of the
   // dual key
-  TermsContainer Aterms = collectDualJacobians<LinearEquality>(
+  TermsContainer Aterms = collectDualJacobians < LinearEquality
-      key, lp_.equalities, equalityVariableIndex_);
+      > (key, lp_.equalities, equalityVariableIndex_);
-  TermsContainer AtermsInequalities = collectDualJacobians<LinearInequality>(
+  TermsContainer AtermsInequalities = collectDualJacobians < LinearInequality
-      key, workingSet, inequalityVariableIndex_);
+      > (key, workingSet, inequalityVariableIndex_);
   Aterms.insert(Aterms.end(), AtermsInequalities.begin(),
       AtermsInequalities.end());
 
@@ -140,9 +146,9 @@ boost::shared_ptr<JacobianFactor> LPSolver::createDualFactor(
   if (Aterms.size() > 0) {
     Vector b = zero(delta.at(key).size());
     Factor::const_iterator it = lp_.cost.find(key);
-    if (it != lp_.cost.end()) b = lp_.cost.getA(it).transpose();
+    if (it != lp_.cost.end())
-    return boost::make_shared<JacobianFactor>(
+      b = lp_.cost.getA(it).transpose();
-        Aterms, b);  // compute the least-square approximation of dual variables
+    return boost::make_shared < JacobianFactor > (Aterms, b); // compute the least-square approximation of dual variables
   } else {
     return boost::make_shared<JacobianFactor>();
   }
@@ -158,7 +164,8 @@ InequalityFactorGraph LPSolver::identifyActiveConstraints(
     double error = workingFactor->error(initialValues);
     // TODO: find a feasible initial point for LPSolver.
     // For now, we just throw an exception
-    if (error > 0) throw InfeasibleInitialValues();
+    if (error > 0)
+      throw InfeasibleInitialValues();
 
     if (fabs(error) < 1e-7) {
       workingFactor->activate();
@@ -174,8 +181,8 @@ std::pair<VectorValues, VectorValues> LPSolver::optimize(
     const VectorValues &initialValues, const VectorValues &duals) const {
   {
     // Initialize workingSet from the feasible initialValues
-    InequalityFactorGraph workingSet =
+    InequalityFactorGraph workingSet = identifyActiveConstraints(
-        identifyActiveConstraints(lp_.inequalities, initialValues, duals);
+        lp_.inequalities, initialValues, duals);
     LPState state(initialValues, duals, workingSet, false, 0);
 
     /// main loop of the solver
@@ -189,7 +196,14 @@ std::pair<VectorValues, VectorValues> LPSolver::optimize(
 boost::tuples::tuple<double, int> LPSolver::computeStepSize(
     const InequalityFactorGraph &workingSet, const VectorValues &xk,
     const VectorValues &p) const {
-  return ActiveSetSolver::computeStepSize(
+  return ActiveSetSolver::computeStepSize(workingSet, xk, p,
-      workingSet, xk, p, std::numeric_limits<double>::infinity());
+      std::numeric_limits<double>::infinity());
+}
+
+pair<VectorValues, VectorValues> LPSolver::optimize() const {
+  LPInitSolverMatlab initSolver(*this);
+  VectorValues initValues = initSolver.solve();
+  return optimize(initValues);
 }
 }
+

gtsam_unstable/linear/LPSolver.h

@@ -113,23 +113,8 @@ public:
       const VectorValues &duals = VectorValues()) const;
 
   /**
-   * Optimize without initial values
+   * Optimize without initial values.
-   * TODO: Find a feasible initial solution that doesn't involve simplex method
-   * nor Solving another LP
    */
-  pair<VectorValues, VectorValues> optimize() const {
+  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
-//    while (!state.converged) {
-//      state = iterate(state);
-//    }
-
-//    return make_pair(state.values, state.duals);
-  }
 };
 } // namespace gtsam

gtsam_unstable/linear/tests/testLPSolver.cpp

@@ -31,7 +31,6 @@
 #include <gtsam_unstable/linear/LPSolver.h>
 #include <gtsam_unstable/linear/LPInitSolverMatlab.h>
 
-
 using namespace std;
 using namespace gtsam;
 using namespace gtsam::symbol_shorthand;
@@ -47,16 +46,11 @@ using namespace gtsam::symbol_shorthand;
 LP simpleLP1() {
   LP lp;
   lp.cost = LinearCost(1, Vector2(-1., -1.)); // min -x1-x2 (max x1+x2)
-  lp.inequalities.push_back(
+  lp.inequalities.push_back(LinearInequality(1, Vector2(-1, 0), 0, 1)); // x1 >= 0
-      LinearInequality(1, Vector2( -1, 0), 0, 1)); // x1 >= 0
+  lp.inequalities.push_back(LinearInequality(1, Vector2(0, -1), 0, 2)); //  x2 >= 0
-  lp.inequalities.push_back(
+  lp.inequalities.push_back(LinearInequality(1, Vector2(1, 2), 4, 3)); //  x1 + 2*x2 <= 4
-      LinearInequality(1, Vector2( 0, -1), 0, 2)); //  x2 >= 0
+  lp.inequalities.push_back(LinearInequality(1, Vector2(4, 2), 12, 4)); //  4x1 + 2x2 <= 12
-  lp.inequalities.push_back(
+  lp.inequalities.push_back(LinearInequality(1, Vector2(-1, 1), 1, 5)); //  -x1 + x2 <= 1
-      LinearInequality(1, Vector2( 1, 2), 4, 3)); //  x1 + 2*x2 <= 4
-  lp.inequalities.push_back(
-      LinearInequality(1, Vector2( 4, 2), 12, 4)); //  4x1 + 2x2 <= 12
-  lp.inequalities.push_back(
-      LinearInequality(1, Vector2( -1, 1), 1, 5)); //  -x1 + x2 <= 1
   return lp;
 }
 
@@ -147,7 +141,6 @@ TEST(LPSolver, overConstrainedLinearSystem2) {
 /* ************************************************************************* */
 TEST(LPSolver, simpleTest1) {
 LP lp = simpleLP1();
-
 LPSolver lpSolver(lp);
 VectorValues init;
 init.insert(1, zero(2));
@@ -165,18 +158,14 @@ TEST(LPSolver, simpleTest1) {
 CHECK(assert_equal(expectedResult, result, 1e-10));
 }
 
-
 /* ************************************************************************* */
 TEST(LPSolver, testWithoutInitialValues) {
-//  LP lp = simpleLP1();
+LP lp = simpleLP1();
-//
+LPSolver lpSolver(lp);
-//  LPSolver lpSolver(lp);
+VectorValues result,duals, expectedResult;
-//  VectorValues result, duals;
+expectedResult.insert(1, Vector2(8./3., 2./3.));
-//  boost::tie(result, duals) = lpSolver.optimize();
+boost::tie(result, duals) = lpSolver.optimize();
-//
+CHECK(assert_equal(expectedResult, result));
-//  VectorValues expectedResult;
-//  expectedResult.insert(1, Vector2(8./3., 2./3.));
-//  CHECK(assert_equal(expectedResult, result, 1e-10));
 }
 
 /**