[QP/LP] LP Added Zeros Error Fixed.

[QP] Now Checks for syntactic Equality by comparing each factor imported .
[QP] Now Checks for semantic Equality by ensuring each imported QP gives the same solution.

gtsam/nonlinear/NonlinearFactor.h

@@ -131,7 +131,8 @@ public:
   }
 
   /**
-   * Creates a shared_ptr clone of the factor with different keys using
+   * Creates a shared_ptr clone of the
+   * factor with different keys using
    * a map from old->new keys
    */
   shared_ptr rekey(const std::map<Key,Key>& rekey_mapping) const;

gtsam_unstable/linear/LPSolver.cpp

@@ -13,7 +13,7 @@
 
 namespace gtsam {
 LPSolver::LPSolver(const LP &lp) :
-    lp_(lp) {
+    lp_(lp), addedZeroPriorsIndex_(){
   // 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
@@ -27,7 +27,11 @@ LPSolver::LPSolver(const LP &lp) :
 
   // Create and push zero priors of constrained variables that do not exist in
   // the cost function
-  // baseGraph_.push_back(*createZeroPriors(lp_.cost.keys(), keysDim_));
+  auto addedZeroPriorsGraph = *createZeroPriors(lp_.cost.keys(), keysDim_);
+  //before we add them we fill an array with the indeces of the added zero priors
+  addedZeroPriorsIndex_.resize(addedZeroPriorsGraph.size());
+  std::iota(addedZeroPriorsIndex_.begin(), addedZeroPriorsIndex_.end(), baseGraph_.size());
+  baseGraph_.push_back(addedZeroPriorsGraph);
 
   // Variable index
   equalityVariableIndex_ = VariableIndex(lp_.equalities);
@@ -54,8 +58,6 @@ LPState LPSolver::iterate(const LPState &state) const {
   // to find the direction to move
   VectorValues newValues = solveWithCurrentWorkingSet(state.values,
       state.workingSet);
-//  GTSAM_PRINT(newValues);
-//  GTSAM_PRINT(state.values);
   // If we CAN'T move further
   // LP: projection on the constraints' nullspace is zero: we are at a vertex
   if (newValues.equals(state.values, 1e-7)) {
@@ -67,8 +69,6 @@ LPState LPSolver::iterate(const LPState &state) const {
     GaussianFactorGraph::shared_ptr dualGraph = buildDualGraph(state.workingSet,
         newValues);
     VectorValues duals = dualGraph->optimize();
-//    GTSAM_PRINT(*dualGraph);
-//    GTSAM_PRINT(duals);
     // LP: see which inequality constraint has wrong pulling direction, i.e., dual < 0
     int leavingFactor = identifyLeavingConstraint(state.workingSet, duals);
     // If all inequality constraints are satisfied: We have the solution!!
@@ -112,44 +112,43 @@ LPState LPSolver::iterate(const LPState &state) const {
 GaussianFactorGraph::shared_ptr LPSolver::createLeastSquareFactors(
     const LinearCost &cost, const VectorValues &xk) const {
   GaussianFactorGraph::shared_ptr graph(new GaussianFactorGraph());
-  //Something in this function breaks when working with funcions that do not include all
-  //Variables in the cost function. When adding the missing variables as shown we still don't converge
-  //to the right answer as we would expect from the iterations.
-//  GTSAM_PRINT(xk);
-//  GTSAM_PRINT(cost);
-
   for (LinearCost::const_iterator it = cost.begin(); it != cost.end(); ++it) {
     size_t dim = cost.getDim(it);
     Vector b = xk.at(*it) - cost.getA(it).transpose(); // b = xk-g
     graph->push_back(JacobianFactor(*it, eye(dim), b));
   }
+
   KeySet allKeys = lp_.inequalities.keys();
   allKeys.merge(lp_.equalities.keys());
   allKeys.merge(KeySet(lp_.cost.keys()));
-
+// add the corresponding factors for all variables that are not explicitly in the cost
-  // for vars that are not in the cost, the cost gradient is zero (g=0), so b=xk
+  //function
+//   for vars that are not in the cost, the cost gradient is zero (g=0), so b=xk
   if (cost.keys().size() != allKeys.size()) {
     KeySet difference;
     std::set_difference(allKeys.begin(), allKeys.end(), lp_.cost.begin(),
-        lp_.cost.end(), std::inserter(difference, difference.end()));
+                        lp_.cost.end(), std::inserter(difference, difference.end()));
     for (Key k : difference) {
       graph->push_back(JacobianFactor(k, eye(keysDim_.at(k)), xk.at(k)));
     }
   }
-
-//  GTSAM_PRINT(*graph);
   return graph;
 }
 
 VectorValues LPSolver::solveWithCurrentWorkingSet(const VectorValues &xk,
     const InequalityFactorGraph &workingSet) const {
   GaussianFactorGraph workingGraph = baseGraph_; // || X - Xk + g ||^2
+//  We remove the old zero priors from the base graph we are going to use to solve
+  //This iteration's problem
+  for (size_t index : addedZeroPriorsIndex_) {
+    workingGraph.remove(index);
+  }
+
   workingGraph.push_back(*createLeastSquareFactors(lp_.cost, xk));
   for (const LinearInequality::shared_ptr &factor : workingSet) {
     if (factor->active())
       workingGraph.push_back(factor);
   }
-//  GTSAM_PRINT(workingGraph);
   return workingGraph.optimize();
 }
 
@@ -209,8 +208,6 @@ std::pair<VectorValues, VectorValues> LPSolver::optimize(
 
     /// main loop of the solver
     while (!state.converged) {
-      // if(state.iterations > 100) // Temporary break to avoid infine loops
-      //   break;
       state = iterate(state);
     }
     return make_pair(state.values, state.duals);

gtsam_unstable/linear/LPSolver.h

@@ -23,7 +23,7 @@ typedef std::map<Key, size_t> KeyDimMap;
 class LPSolver: public ActiveSetSolver {
   const LP &lp_; //!< the linear programming problem
   KeyDimMap keysDim_; //!< key-dim map of all variables in the constraints, used to create zero priors
-
+  std::vector<size_t> addedZeroPriorsIndex_;
 public:
   /// Constructor
   LPSolver(const LP &lp);

gtsam_unstable/linear/QPSParser.cpp

@@ -137,7 +137,7 @@ QP QPSParser::Parse() {
   if (!parse(begin, last, MPSGrammar(&rawData)) || begin != last) {
     throw QPSParserException();
   } else {
-    std::cout << "Parse Successful." << std::endl;
+//    std::cout << "Parse Successful." << std::endl;
   }
 
   return rawData.makeQP();

gtsam_unstable/linear/tests/testLPSolver.cpp

@@ -56,36 +56,11 @@ LP simpleLP1() {
 
 /* ************************************************************************* */
 namespace gtsam {
-TEST(LPInitSolverMatlab, infinite_loop_multi_var) {
-  LP initchecker;
-  Key X = symbol('X',1);
-  Key Y = symbol('Y',1);
-  Key Z = symbol('Z',1);
-  initchecker.cost = LinearCost(Z, ones(1)); //min alpha
-//  initchecker.cost = LinearCost(Z, ones(1), X, zero(1), Y, zero(1)); //min alpha
-  initchecker.inequalities.push_back(LinearInequality(X,-2.0*ones(1), Y, -1.0*ones(1), Z, -1.0*ones(1),-2,1));//-2x-y-alpha <= -2
-  initchecker.inequalities.push_back(LinearInequality(X, -1.0*ones(1), Y, 2.0*ones(1), Z, -1.0*ones(1), 6, 2));// -x+2y-alpha <= 6
-  initchecker.inequalities.push_back(LinearInequality(X, -1.0*ones(1), Z, -1.0*ones(1), 0,3));// -x - alpha <= 0
-  initchecker.inequalities.push_back(LinearInequality(X, 1.0*ones(1), Z, -1.0*ones(1), 20, 4));//x - alpha <= 20
-  initchecker.inequalities.push_back(LinearInequality(Y, -1.0*ones(1), Z, -1.0*ones(1), 0, 5));// -y - alpha <= 0
-  LPSolver solver(initchecker);
-  VectorValues starter;
-  starter.insert(X, zero(1));
-  starter.insert(Y, zero(1));
-  starter.insert(Z, 2*ones(1));
-  VectorValues results, duals;
-  boost::tie(results, duals) = solver.optimize(starter);
-  VectorValues expected;
-  expected.insert(X, Vector::Constant(1, 13.5));
-  expected.insert(Y, Vector::Constant(1,6.5));
-  expected.insert(Z, Vector::Constant(1,-6.5));
-  CHECK(assert_equal(results, expected, 1e-7));
-}
 
 TEST(LPInitSolverMatlab, infinite_loop_single_var) {
   LP initchecker;
   initchecker.cost = LinearCost(1,Vector3(0,0,1)); //min alpha
-  initchecker.inequalities.push_back(LinearInequality(1, Vector3(-2,-1,-1),-2,1));//-2x-y-alpha <= -2 
+  initchecker.inequalities.push_back(LinearInequality(1, Vector3(-2,-1,-1),-2,1));//-2x-y-alpha <= -2
   initchecker.inequalities.push_back(LinearInequality(1, Vector3(-1,2,-1), 6, 2));// -x+2y-alpha <= 6
   initchecker.inequalities.push_back(LinearInequality(1, Vector3(-1,0,-1), 0,3));// -x - alpha <= 0
   initchecker.inequalities.push_back(LinearInequality(1, Vector3(1,0,-1), 20, 4));//x - alpha <= 20
@@ -99,6 +74,34 @@ TEST(LPInitSolverMatlab, infinite_loop_single_var) {
   expected.insert(1, Vector3(13.5, 6.5, -6.5));
   CHECK(assert_equal(results, expected, 1e-7));
 }
+
+TEST(LPInitSolverMatlab, infinite_loop_multi_var) {
+  LP initchecker;
+  Key X = symbol('X', 1);
+  Key Y = symbol('Y', 1);
+  Key Z = symbol('Z', 1);
+  initchecker.cost = LinearCost(Z, ones(1)); //min alpha
+  initchecker.inequalities.push_back(
+      LinearInequality(X, -2.0 * ones(1), Y, -1.0 * ones(1), Z, -1.0 * ones(1), -2, 1));//-2x-y-alpha <= -2
+  initchecker.inequalities.push_back(
+      LinearInequality(X, -1.0 * ones(1), Y, 2.0 * ones(1), Z, -1.0 * ones(1), 6, 2));// -x+2y-alpha <= 6
+  initchecker.inequalities.push_back(LinearInequality(X, -1.0 * ones(1), Z, -1.0 * ones(1), 0, 3));// -x - alpha <= 0
+  initchecker.inequalities.push_back(LinearInequality(X, 1.0 * ones(1), Z, -1.0 * ones(1), 20, 4));//x - alpha <= 20
+  initchecker.inequalities.push_back(LinearInequality(Y, -1.0 * ones(1), Z, -1.0 * ones(1), 0, 5));// -y - alpha <= 0
+  LPSolver solver(initchecker);
+  VectorValues starter;
+  starter.insert(X, zero(1));
+  starter.insert(Y, zero(1));
+  starter.insert(Z, 2 * ones(1));
+  VectorValues results, duals;
+  boost::tie(results, duals) = solver.optimize(starter);
+  VectorValues expected;
+  expected.insert(X, Vector::Constant(1, 13.5));
+  expected.insert(Y, Vector::Constant(1, 6.5));
+  expected.insert(Z, Vector::Constant(1, -6.5));
+  CHECK(assert_equal(results, expected, 1e-7));
+}
+
 TEST(LPInitSolverMatlab, initialization) {
   LP lp = simpleLP1();
   LPSolver lpSolver(lp);

gtsam_unstable/linear/tests/testQPSolver.cpp

@@ -211,48 +211,42 @@ TEST(QPSolver, optimizeForst10book_pg171Ex5) {
   expectedSolution.insert(X(2), (Vector(1) << 0.5).finished());
   CHECK(assert_equal(expectedSolution, solution, 1e-100));
 }
-
 pair<QP, QP> testParser(QPSParser parser) {
   QP exampleqp = parser.Parse();
-//  QP expectedqp = createExampleQP();
   QP expectedqp;
+  Key X1(Symbol('X',1)), X2(Symbol('X',2));
   // min f(x,y) = 4 + 1.5x -y + 0.58x^2 + 2xy + 2yx + 10y^2
   expectedqp.cost.push_back(
-    HessianFactor(X(1), X(2), 8.0 * ones(1, 1), 2.0 * ones(1, 1),
+    HessianFactor(X1, X2, 8.0 * ones(1, 1), 2.0 * ones(1, 1),
                   1.5 * ones(1), 10.0 * ones(1, 1), -2.0 * ones(1), 4.0));
   // 2x + y >= 2
-  expectedqp.inequalities.push_back(
-    LinearInequality(X(1), -2.0 * ones(1, 1), X(2), -ones(1, 1), -2, 0));
   // -x + 2y <= 6
   expectedqp.inequalities.push_back(
-    LinearInequality(X(1), -ones(1, 1), X(2), 2.0 * ones(1, 1), 6, 1));
+    LinearInequality(X2, -ones(1, 1), X1, -2.0 * ones(1, 1), -2, 0));
-  //x >= 0
+  expectedqp.inequalities.push_back(
-  expectedqp.inequalities.push_back(LinearInequality(X(1), -ones(1, 1), 0, 2));
+    LinearInequality(X2, 2.0 * ones(1, 1), X1, -ones(1, 1), 6, 1));
-  // y > = 0
-  expectedqp.inequalities.push_back(LinearInequality(X(2), -ones(1, 1), 0, 3));
   // x<= 20
-  expectedqp.inequalities.push_back(LinearInequality(X(1), ones(1, 1), 20, 4));
+  expectedqp.inequalities.push_back(LinearInequality(X1, ones(1, 1), 20, 4));
+  //x >= 0
+  expectedqp.inequalities.push_back(LinearInequality(X1, -ones(1, 1), 0, 2));
+  // y > = 0
+  expectedqp.inequalities.push_back(LinearInequality(X2, -ones(1, 1), 0, 3));
   return std::make_pair(expectedqp, exampleqp);
-//  CHECK(assert_equal(expectedqp.cost, exampleqp.cost, 1e-7));
-//  CHECK(expectedqp.cost.equals(exampleqp.cost, 1e-7));
-//  CHECK(expectedqp.inequalities.equals(exampleqp.inequalities, 1e-7));
-//  CHECK(expectedqp.equalities.equals(exampleqp.equalities, 1e-7));
 };
 
-TEST(QPSolver, QPExampleData) {
+TEST(QPSolver, ParserSyntaticTest){
+  auto expectedActual = testParser(QPSParser("QPExample.QPS"));
+  CHECK(assert_equal(expectedActual.first.cost, expectedActual.second.cost, 1e-7));
+  CHECK(assert_equal(expectedActual.first.inequalities, expectedActual.second.inequalities, 1e-7));
+  CHECK(assert_equal(expectedActual.first.equalities, expectedActual.second.equalities, 1e-7));
+}
 
+TEST(QPSolver, ParserSemanticTest) {
   auto expected_actual = testParser(QPSParser("QPExample.QPS"));
   VectorValues actualSolution, expectedSolution;
-  expected_actual.first.print("EXPECTED GRAPH:");
-  expected_actual.second.print("ACTUAL GRAPH");
   boost::tie(expectedSolution, boost::tuples::ignore) = QPSolver(expected_actual.first).optimize();
-  std::cout << "Expected Execution Works" << std::endl;
   boost::tie(actualSolution, boost::tuples::ignore) = QPSolver(expected_actual.second).optimize();
-  std::cout << "Actual Execution Works" << std::endl;
+  CHECK(assert_equal(actualSolution, expectedSolution, 1e-7));
-
-    CHECK(assert_equal(actualSolution, expectedSolution, 1e-7));
-//  testParser(QPSParser("AUG2D.QPS"));
-//  testParser(QPSParser("CONT-050.QPS"));
 }
 
 /* ************************************************************************* */