[FEATURE] QPS Parser Preliminary Commit.

Notes:
- Correctly parses MPS files and makes appropiate calls to RawQP
- Containts Debug Code
- Does not generate raw bounds from parse
- Does not generate Hessian from parse
- Does not generate Actual QP from raw QP

gtsam_unstable/linear/QP.h

@@ -21,6 +21,7 @@
 #include <gtsam/linear/GaussianFactorGraph.h>
 #include <gtsam_unstable/linear/EqualityFactorGraph.h>
 #include <gtsam_unstable/linear/InequalityFactorGraph.h>
+#include <gtsam/slam/dataset.h>
 
 namespace gtsam {
 
@@ -41,14 +42,9 @@ struct QP {
   QP(const GaussianFactorGraph& _cost,
       const EqualityFactorGraph& _linearEqualities,
       const InequalityFactorGraph& _linearInequalities) :
-      cost(_cost), equalities(_linearEqualities), inequalities(
+      cost(_cost), equalities(_linearEqualities), inequalities(_linearInequalities) {
-          _linearInequalities) {
   }
 
-  QP(std::string MPS_FileName):
-    cost(), equalities(), inequalities() {}
-
-
   /** print */
   void print(const std::string& s = "") {
     std::cout << s << std::endl;

gtsam_unstable/linear/QPSParser.cpp

@@ -0,0 +1,103 @@
+//
+// Created by ivan on 3/5/16.
+//
+
+#include <gtsam_unstable/linear/QPSParser.h>
+#include <boost/spirit/include/qi.hpp>
+#include <boost/lambda/lambda.hpp>
+#include <boost/phoenix/bind.hpp>
+
+#define BOOST_SPIRIT_USE_PHOENIX_V3 1
+
+using namespace boost::spirit;
+using namespace boost::spirit::qi;
+using namespace boost::spirit::qi::labels;
+
+namespace gtsam {
+
+struct QPSParser::MPSGrammar: grammar<basic_istream_iterator<char>> {
+  MPSGrammar(RawQP * rqp) :
+      MPSGrammar::base_type(start), rqp_(rqp) {
+    character = lexeme[char_("a-zA-Z") | char_('_')];
+    title = lexeme[+character
+        >> *(blank | character | char_('-') | char_('.') | char_("0-9"))];
+    word = lexeme[(character
+        >> *(char_("0-9") | char_('-') | char_('.') | character))];
+    name =
+        lexeme[lit("NAME") >> *blank
+            >> title[boost::phoenix::bind(&RawQP::setName, rqp_, qi::_1)]
+            >> +space];
+    row = lexeme[*blank
+        >> (character >> +blank >> word)[boost::phoenix::bind(&RawQP::addRow,
+            rqp_, qi::_1, qi::_3)] >> *blank];
+    rhs_single = lexeme[*blank
+        >> (word >> +blank >> word >> +blank >> double_)[boost::phoenix::bind(
+            &RawQP::addRHS, rqp_, qi::_1, qi::_3, qi::_5)] >> *blank];
+    rhs_double = lexeme[*blank
+        >> (word >> +blank >> word >> +blank >> double_ >> +blank >> word
+            >> +blank >> double_)[boost::phoenix::bind(&RawQP::addRHS, rqp_,
+            qi::_1, qi::_3, qi::_5, qi::_7, qi::_9)] >> *blank];
+    col_single = lexeme[*blank
+        >> (word >> +blank >> word >> +blank >> double_)[boost::phoenix::bind(
+            &RawQP::addColumn, rqp_, qi::_1, qi::_3, qi::_5)] >> *blank];
+    col_double = lexeme[*blank
+        >> (word >> +blank >> word >> +blank >> double_ >> +blank >> word
+            >> +blank >> double_)[boost::phoenix::bind(&RawQP::addColumn, rqp_,
+            qi::_1, qi::_3, qi::_5, qi::_7, qi::_9)] >> *blank];
+    quad_l = lexeme[*blank
+        >> (word >> +blank >> word >> +blank >> double_)[boost::phoenix::bind(
+            &RawQP::addQuadTerm, rqp_, qi::_1, qi::_3, qi::_5)] >> *blank];
+    bound =
+        lexeme[*blank
+            >> (word >> +blank >> word >> +blank >> word >> +blank >> double_)[boost::phoenix::bind(
+                &RawQP::addBound, rqp_, qi::_1, qi::_5, qi::_7)] >> *blank];
+    bound_fr = lexeme[*blank
+        >> (word >> +blank >> word >> +blank >> word)[boost::phoenix::bind(
+            &RawQP::addBound, rqp_, qi::_1, qi::_5)] >> *blank];
+    rows = lexeme[lit("ROWS") >> *blank >> eol >> +(row >> eol)];
+    rhs = lexeme[lit("RHS") >> *blank >> eol
+        >> +((rhs_double | rhs_single) >> eol)];
+    cols = lexeme[lit("COLUMNS") >> *blank >> eol
+        >> +((col_double | col_single) >> eol)];
+    quad = lexeme[lit("QUADOBJ") >> *blank >> eol >> +(quad_l >> eol)];
+    bounds = lexeme[lit("BOUNDS") >> +space >> +((bound | bound_fr) >> eol)];
+    ranges = lexeme[lit("RANGES") >> +space];
+    start = lexeme[name >> rows >> cols >> rhs >> ranges >> bounds >> quad
+        >> lit("ENDATA") >> +space];
+  }
+  RawQP * rqp_;
+
+  rule<basic_istream_iterator<char>, char()> character;
+  rule<basic_istream_iterator<char>, std::vector<char>()> word;
+  rule<basic_istream_iterator<char>, std::vector<char>()> title;
+  rule<basic_istream_iterator<char> > row;
+  rule<basic_istream_iterator<char> > col_single;
+  rule<basic_istream_iterator<char> > col_double;
+  rule<basic_istream_iterator<char> > rhs_single;
+  rule<basic_istream_iterator<char> > rhs_double;
+  rule<basic_istream_iterator<char> > ranges;
+  rule<basic_istream_iterator<char> > bound;
+  rule<basic_istream_iterator<char> > bound_fr;
+  rule<basic_istream_iterator<char> > bounds;
+  rule<basic_istream_iterator<char> > quad;
+  rule<basic_istream_iterator<char> > quad_l;
+  rule<basic_istream_iterator<char> > rows;
+  rule<basic_istream_iterator<char> > cols;
+  rule<basic_istream_iterator<char> > rhs;
+  rule<basic_istream_iterator<char> > name;
+  rule<basic_istream_iterator<char> > start;
+};
+
+QP QPSParser::Parse() {
+  RawQP rawData;
+  boost::spirit::basic_istream_iterator<char> begin(stream);
+  boost::spirit::basic_istream_iterator<char> last;
+
+  if (!parse(begin, last, MPSGrammar(&rawData)) && begin == last) {
+    throw QPSParserException();
+  }
+
+  return rawData.makeQP();
+}
+
+}

gtsam_unstable/linear/QPSParser.h

@@ -0,0 +1,32 @@
+/**
+ * @file     LPSolver.cpp
+ * @brief    QPS parser implementation
+ * @author   Ivan Dario Jimenez
+ * @date     1/26/16
+ */
+
+#pragma once
+
+#include <gtsam_unstable/linear/QP.h>
+#include <gtsam_unstable/linear/QPSParserException.h>
+#include <gtsam_unstable/linear/RawQP.h>
+#include <fstream>
+
+namespace gtsam {
+
+class QPSParser {
+
+private:
+  std::fstream stream;
+  struct MPSGrammar;
+public:
+
+  QPSParser(const std::string& fileName) :
+      stream(findExampleDataFile(fileName).c_str()) {
+    stream.unsetf(std::ios::skipws);
+  }
+
+  QP Parse();
+};
+}
+

gtsam_unstable/linear/QPSParserException.h

@@ -0,0 +1,31 @@
+/**
+ * @file QPSParserException
+ * @brief Exception thrown if there is an error parsing a QPS file
+ * @date jan 24, 2015
+ * @author Duy-Nguyen Ta
+ */
+
+#pragma once
+
+namespace gtsam {
+
+class QPSParserException: public ThreadsafeException<QPSParserException> {
+public:
+  QPSParserException() {
+  }
+
+  virtual ~QPSParserException() throw () {
+  }
+
+  virtual const char *what() const throw () {
+    if (description_.empty())
+      description_ = "There is a problem parsing the QPS file.\n";
+    return description_.c_str();
+  }
+
+private:
+  mutable std::string description_;
+};
+
+}
+

gtsam_unstable/linear/RawQP.cpp

@@ -0,0 +1,2 @@
+
+#include <gtsam_unstable/linear/RawQP.h>

gtsam_unstable/linear/RawQP.h

@@ -0,0 +1,184 @@
+#pragma once
+
+#include <gtsam_unstable/linear/QP.h>
+#include <gtsam/base/Matrix.h>
+#include <gtsam/inference/Key.h>
+
+#include <iostream>
+#include <string>
+#include <vector>
+#include <utility>
+#include <unordered_map>
+#include <gtsam/inference/Symbol.h>
+
+
+namespace gtsam {
+class RawQP {
+  typedef std::vector<std::pair<Key, Matrix> > coefficient_v;
+
+  std::vector<std::pair<Key, Matrix> > g;
+  std::unordered_map<std::string, std::unordered_map<std::string, coefficient_v > > row_to_map;
+  std::unordered_map<std::string, Key> varname_to_key;
+  std::unordered_map<std::string, coefficient_v > IL;
+  std::unordered_map<std::string, coefficient_v > IG;
+  std::unordered_map<std::string, coefficient_v > E;
+  std::unordered_map<std::string, double> b;
+  std::unordered_map<std::string, std::unordered_map<std::string, double> > H;
+  unsigned int varNumber;
+  double f;
+  std::string obj_name;
+  std::string name_;
+  const bool debug = true;
+public:
+  RawQP() : g(), row_to_map(), varname_to_key(), IL(), IG(), E(), b(), H(), varNumber(1){
+  }
+
+  void setName(std::vector<char> name) {
+    name_ = std::string(name.begin(), name.end());
+    if (debug) {
+      std::cout << "Parsing file: " << name_ << std::endl;
+    }
+  }
+
+  void addColumn(std::vector<char> var, std::vector<char> row,
+      double coefficient) {
+    std::string var_(var.begin(), var.end()), row_(row.begin(), row.end());
+
+    if(!varname_to_key.count(var_))
+      varname_to_key.insert({var_, Symbol('X', varNumber++)});
+
+    if(row_ == obj_name){
+      g.push_back(std::make_pair(varname_to_key[var_], coefficient * ones(1,1)));
+      return;
+    }
+    row_to_map[row_][row_].push_back({varname_to_key[var_], coefficient * ones(1,1)});
+
+    if (debug) {
+      std::cout << "Added Column for Var: " << var_ << " Row: " << row_
+          << " Coefficient: " << coefficient << std::endl;
+    }
+  }
+
+  void addColumn(std::vector<char> var, std::vector<char> row1,
+      double coefficient1, std::vector<char> row2, double coefficient2) {
+    std::string var_(var.begin(), var.end()), row1_(row1.begin(), row1.end()),
+        row2_(row2.begin(), row2.end());
+    if(!varname_to_key.count(var_))
+      varname_to_key.insert({var_, Symbol('X', varNumber++)});
+
+    if(row1_ == obj_name)
+      row_to_map[row1_][row2_].push_back({varname_to_key[var_], coefficient1 * ones(1,1)});
+    else
+      row_to_map[row1_][row1_].push_back({varname_to_key[var_], coefficient1 * ones(1,1)});
+
+    if(row2_ == obj_name)
+      row_to_map[row2_][row2_].push_back({varname_to_key[var_], coefficient2 * ones(1,1)});
+    else
+      row_to_map[row2_][row2_].push_back({varname_to_key[var_], coefficient2 * ones(1,1)});
+
+    if (debug) {
+      std::cout << "Added Column for Var: " << var_ << " Row: " << row1_
+          << " Coefficient: " << coefficient1 << std::endl;
+      std::cout << "                      " << "Row: " << row2_
+          << " Coefficient: " << coefficient2 << std::endl;
+    }
+  }
+
+  void addRHS(std::vector<char> var, std::vector<char> row,
+      double coefficient) {
+    std::string var_(var.begin(), var.end()), row_(row.begin(), row.end());
+    if(row_ == obj_name)
+      f = -coefficient;
+    else
+      b.insert({row_, coefficient});
+
+    if (debug) {
+      std::cout << "Added RHS for Var: " << var_ << " Row: " << row_
+          << " Coefficient: " << coefficient << std::endl;
+    }
+  }
+
+  void addRHS(std::vector<char> var, std::vector<char> row1,
+      double coefficient1, std::vector<char> row2, double coefficient2) {
+    std::string var_(var.begin(), var.end()), row1_(row1.begin(), row1.end()),
+        row2_(row2.begin(), row2.end());
+
+    if(row1_ == obj_name)
+      f = -coefficient1;
+    else
+      b.insert({row1_, coefficient1});
+
+    if(row2_ == obj_name)
+      f = -coefficient2;
+    else
+      b.insert({row2_, coefficient2});
+
+    if (debug) {
+      std::cout << "Added RHS for Var: " << var_ << " Row: " << row1_
+          << " Coefficient: " << coefficient1 << std::endl;
+      std::cout << "                      " << "Row: " << row2_
+          << " Coefficient: " << coefficient2 << std::endl;
+    }
+  }
+
+  void addRow(char type, std::vector<char> name) {
+    std::string name_(name.begin(), name.end());
+    switch(type){
+      case 'N':
+        obj_name = name_;
+        break;
+      case 'L':
+        row_to_map.insert({name_, IL});
+        IL.insert({name_, coefficient_v()});
+        break;
+      case 'G':
+        row_to_map.insert({name_, IG});
+        IG.insert({name_, coefficient_v()});
+        break;
+      case 'E':
+        row_to_map.insert({name_, E});
+        E.insert({name_, coefficient_v()});
+        break;
+      default:
+        std::cout << "invalid type: " << type << std::endl;
+        break;
+    }
+    if (debug) {
+      std::cout << "Added Row Type: " << type << " Name: " << name_
+          << std::endl;
+    }
+  }
+
+  void addBound(std::vector<char> type, std::vector<char> var, double number) {
+    std::string type_(type.begin(), type.end()), var_(var.begin(), var.end());
+
+
+    if (debug) {
+      std::cout << "Added Bound Type: " << type_ << " Var: " << var_
+          << " Amount: " << number << std::endl;
+    }
+  }
+
+  void addBound(std::vector<char> type, std::vector<char> var) {
+    std::string type_(type.begin(), type.end()), var_(var.begin(), var.end());
+    if (debug) {
+      std::cout << "Added Free Bound Type: " << type_ << " Var: " << var_
+          << " Amount: " << std::endl;
+    }
+  }
+
+  void addQuadTerm(std::vector<char> var1, std::vector<char> var2,
+      double coefficient) {
+    std::string var1_(var1.begin(), var1.end()), var2_(var2.begin(),
+        var2.end());
+    if (debug) {
+      std::cout << "Added QuadTerm for Var: " << var1_ << " Row: " << var2_
+          << " Coefficient: " << coefficient << std::endl;
+    }
+  }
+
+  QP makeQP() {
+    return QP();
+  }
+};
+}

gtsam_unstable/linear/tests/testQPSolver.cpp

@@ -19,7 +19,7 @@
 #include <gtsam/base/Testable.h>
 #include <gtsam/inference/Symbol.h>
 #include <gtsam_unstable/linear/QPSolver.h>
-
+#include <gtsam_unstable/linear/QPSParser.h>
 #include <CppUnitLite/TestHarness.h>
 #include <gtsam_unstable/linear/InfeasibleInitialValues.h>
 #include <gtsam_unstable/linear/InfeasibleOrUnboundedProblem.h>
@@ -213,39 +213,42 @@ TEST(QPSolver, optimizeForst10book_pg171Ex5) {
   CHECK(assert_equal(expectedSolution, solution, 1e-100));
 }
 
-QP createExampleQP(){
+QP createExampleQP() {
   QP exampleqp;
-  exampleqp.cost.push_back(
+
-  HessianFactor(X(1),Y(1),
-  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
-  exampleqp.inequalities.push_back(
-  LinearInequality(X(1), -2.0*ones(1,1), Y(1), -ones(1,1), -2, 0));
-  // -x + 2y <= 6
-  exampleqp.inequalities.push_back(
-  LinearInequality(X(1), -ones(1,1), Y(1), 2.0* ones(1,1), 6, 1));
-   //x >= 0
-  exampleqp.inequalities.push_back(
-  LinearInequality(X(1), -ones(1,1), 0, 2));
-  // y > = 0
-  exampleqp.inequalities.push_back(
-  LinearInequality(Y(1), -ones(1,1), 0, 3));
-  // x<= 20
-  exampleqp.inequalities.push_back(
-  LinearInequality(X(1), ones(1,1), 20, 4));
   return exampleqp;
-};
+}
+;
 
-TEST(QPSolver, QPExampleData){
+TEST(QPSolver, QPExampleData) {
-  QP exampleqp("QPExample.QPS");
+  QPSParser parser("QPExample.QPS");
+//  QPSParser parser("AUG2D.QPS");
+//  QPSParser parser("CONT-050.QPS");
 
-  QP expectedqp = createExampleQP();
+  QP exampleqp = parser.Parse();
-  // min f(x,y) = 4 + 1.5x -y + 0.5(8x^2 + 2xy + 2yx + 10y^2
 
-//  CHECK(expectedqp.cost.equals(exampleqp.cost, 1e-7));
+//  QP expectedqp = createExampleQP();
-//  CHECK(expectedqp.inequalities.equals(exampleqp.inequalities, 1e-7));
+  QP expectedqp;
-//  CHECK(expectedqp.equalities.equals(exampleqp.equalities, 1e-7));
+  // 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),
+          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));
+  //x >= 0
+  expectedqp.inequalities.push_back(LinearInequality(X(1), -ones(1, 1), 0, 2));
+  // 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));
+
+  CHECK(expectedqp.cost.equals(exampleqp.cost, 1e-7));
+  CHECK(expectedqp.inequalities.equals(exampleqp.inequalities, 1e-7));
+  CHECK(expectedqp.equalities.equals(exampleqp.equalities, 1e-7));
 }
 
 /* ************************************************************************* */