Style improvements and comments.

gtsam_unstable/linear/QPSParser.cpp

@@ -19,7 +19,7 @@
 
 #include <gtsam_unstable/linear/QPSParser.h>
 #include <gtsam_unstable/linear/QPSParserException.h>
-#include <gtsam_unstable/linear/RawQP.h>
+#include <gtsam_unstable/linear/QPSVisitor.h>
 
 #include <boost/spirit/include/qi.hpp>
 #include <boost/lambda/lambda.hpp>
@@ -34,7 +34,7 @@ typedef qi::grammar<boost::spirit::basic_istream_iterator<char>> base_grammar;
 
 struct QPSParser::MPSGrammar: base_grammar {
   typedef std::vector<char> Chars;
-  RawQP * rqp_;
+  QPSVisitor * rqp_;
   boost::function<void(bf::vector<Chars, Chars, Chars> const&)> setName;
   boost::function<void(bf::vector<Chars, char, Chars, Chars, Chars> const &)> addRow;
   boost::function<
@@ -62,19 +62,19 @@ struct QPSParser::MPSGrammar: base_grammar {
           bf::vector<Chars, Chars, Chars, Chars, Chars, Chars, Chars, double> const &)> addBound;
   boost::function<
       void(bf::vector<Chars, Chars, Chars, Chars, Chars, Chars, Chars> const &)> addBoundFr;
-  MPSGrammar(RawQP * rqp) :
+  MPSGrammar(QPSVisitor * rqp) :
       base_grammar(start), rqp_(rqp), setName(
-          boost::bind(&RawQP::setName, rqp, ::_1)), addRow(
+          boost::bind(&QPSVisitor::setName, rqp, ::_1)), addRow(
-          boost::bind(&RawQP::addRow, rqp, ::_1)), rhsSingle(
+          boost::bind(&QPSVisitor::addRow, rqp, ::_1)), rhsSingle(
-          boost::bind(&RawQP::addRHS, rqp, ::_1)), rhsDouble(
+          boost::bind(&QPSVisitor::addRHS, rqp, ::_1)), rhsDouble(
-          boost::bind(&RawQP::addRHSDouble, rqp, ::_1)), rangeSingle(
+          boost::bind(&QPSVisitor::addRHSDouble, rqp, ::_1)), rangeSingle(
-          boost::bind(&RawQP::addRangeSingle, rqp, ::_1)), rangeDouble(
+          boost::bind(&QPSVisitor::addRangeSingle, rqp, ::_1)), rangeDouble(
-          boost::bind(&RawQP::addRangeDouble, rqp, ::_1)), colSingle(
+          boost::bind(&QPSVisitor::addRangeDouble, rqp, ::_1)), colSingle(
-          boost::bind(&RawQP::addColumn, rqp, ::_1)), colDouble(
+          boost::bind(&QPSVisitor::addColumn, rqp, ::_1)), colDouble(
-          boost::bind(&RawQP::addColumnDouble, rqp, ::_1)), addQuadTerm(
+          boost::bind(&QPSVisitor::addColumnDouble, rqp, ::_1)), addQuadTerm(
-          boost::bind(&RawQP::addQuadTerm, rqp, ::_1)), addBound(
+          boost::bind(&QPSVisitor::addQuadTerm, rqp, ::_1)), addBound(
-          boost::bind(&RawQP::addBound, rqp, ::_1)), addBoundFr(
+          boost::bind(&QPSVisitor::addBound, rqp, ::_1)), addBoundFr(
-          boost::bind(&RawQP::addBoundFr, rqp, ::_1)) {
+          boost::bind(&QPSVisitor::addBoundFr, rqp, ::_1)) {
     using namespace boost::spirit;
     using namespace boost::spirit::qi;
     character = lexeme[alnum | '_' | '-' | '.'];
@@ -123,7 +123,7 @@ struct QPSParser::MPSGrammar: base_grammar {
 };
 
 QP QPSParser::Parse() {
-  RawQP rawData;
+  QPSVisitor rawData;
   std::fstream stream(fileName_.c_str());
   stream.unsetf(std::ios::skipws);
   boost::spirit::basic_istream_iterator<char> begin(stream);

gtsam_unstable/linear/QPSVisitor.cpp

@@ -0,0 +1,363 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ * @file     QPSVisitor.cpp
+ * @brief    As the QPS parser reads a file, it call functions in QPSVistor.
+ * This visitor in turn stores what the parser has read in a way that can be later used to build the Factor Graph for the
+ * QP Constraints and cost. This intermediate representation is required because an expression in the QPS file doesn't
+ * necessarily correspond to a single constraint or term in the cost function.
+ * @author   Ivan Dario Jimenez
+ * @date     3/5/16
+ */
+
+#include <gtsam_unstable/linear/QPSVisitor.h>
+#include <iostream>
+
+using boost::fusion::at_c;
+using namespace std;
+
+namespace gtsam {
+
+void QPSVisitor::setName(
+    boost::fusion::vector<vector<char>, vector<char>,
+        vector<char>> const &name) {
+  name_ = string(at_c < 1 > (name).begin(), at_c < 1 > (name).end());
+  if (debug) {
+    cout << "Parsing file: " << name_ << endl;
+  }
+}
+
+void QPSVisitor::addColumn(
+    boost::fusion::vector<vector<char>, vector<char>,
+        vector<char>, vector<char>, vector<char>, double,
+        vector<char>> const &vars) {
+
+  string var_(at_c < 1 > (vars).begin(), at_c < 1 > (vars).end());
+  string row_(at_c < 3 > (vars).begin(), at_c < 3 > (vars).end());
+  Matrix11 coefficient = at_c < 5 > (vars) * I_1x1;
+  if (debug) {
+    cout << "Added Column for Var: " << var_ << " Row: " << row_
+        << " Coefficient: " << coefficient << endl;
+  }
+  if (!varname_to_key.count(var_))
+    varname_to_key[var_] = Symbol('X', numVariables++);
+  if (row_ == obj_name) {
+    g[varname_to_key[var_]] = coefficient;
+    return;
+  }
+  (*row_to_constraint_v[row_])[row_][varname_to_key[var_]] = coefficient;
+}
+
+void QPSVisitor::addColumnDouble(
+    boost::fusion::vector<vector<char>, vector<char>,
+        vector<char>, vector<char>, double, vector<char>,
+        vector<char>, vector<char>, double> const &vars) {
+
+  string var_(at_c < 0 > (vars).begin(), at_c < 0 > (vars).end());
+  string row1_(at_c < 2 > (vars).begin(), at_c < 2 > (vars).end());
+  string row2_(at_c < 6 > (vars).begin(), at_c < 6 > (vars).end());
+  Matrix11 coefficient1 = at_c < 4 > (vars) * I_1x1;
+  Matrix11 coefficient2 = at_c < 8 > (vars) * I_1x1;
+  if (!varname_to_key.count(var_))
+    varname_to_key.insert( { var_, Symbol('X', numVariables++) });
+  if (row1_ == obj_name)
+    g[varname_to_key[var_]] = coefficient1;
+  else
+    (*row_to_constraint_v[row1_])[row1_][varname_to_key[var_]] = coefficient1;
+  if (row2_ == obj_name)
+    g[varname_to_key[var_]] = coefficient2;
+  else
+    (*row_to_constraint_v[row2_])[row2_][varname_to_key[var_]] = coefficient2;
+}
+
+void QPSVisitor::addRangeSingle(
+    boost::fusion::vector<vector<char>, vector<char>,
+        vector<char>, vector<char>, vector<char>, double,
+        vector<char>> const & vars) {
+  string var_(at_c < 1 > (vars).begin(), at_c < 1 > (vars).end());
+  string row_(at_c < 3 > (vars).begin(), at_c < 3 > (vars).end());
+  double range = at_c < 5 > (vars);
+  ranges[row_] = range;
+  if (debug) {
+    cout << "SINGLE RANGE ADDED" << endl;
+    cout << "VAR:" << var_ << " ROW: " << row_ << " RANGE: " << range
+        << endl;
+  }
+  
+}
+void QPSVisitor::addRangeDouble(
+    boost::fusion::vector<vector<char>, vector<char>,
+        vector<char>, vector<char>, vector<char>, double,
+        vector<char>, vector<char>, vector<char>, double> const & vars) {
+  string var_(at_c < 1 > (vars).begin(), at_c < 1 > (vars).end());
+  string row1_(at_c < 3 > (vars).begin(), at_c < 3 > (vars).end());
+  string row2_(at_c < 7 > (vars).begin(), at_c < 7 > (vars).end());
+  double range1 = at_c < 5 > (vars);
+  double range2 = at_c < 9 > (vars);
+  ranges[row1_] = range1;
+  ranges[row2_] = range2;
+  if (debug) {
+    cout << "DOUBLE RANGE ADDED" << endl;
+    cout << "VAR: " << var_ << " ROW1: " << row1_ << " RANGE1: " << range1
+        << " ROW2: " << row2_ << " RANGE2: " << range2 << endl;
+  }
+  
+}
+
+void QPSVisitor::addRHS(
+    boost::fusion::vector<vector<char>, vector<char>,
+        vector<char>, vector<char>, vector<char>, double,
+        vector<char>> const &vars) {
+
+  string var_(at_c < 1 > (vars).begin(), at_c < 1 > (vars).end());
+  string row_(at_c < 3 > (vars).begin(), at_c < 3 > (vars).end());
+  double coefficient = at_c < 5 > (vars);
+  if (row_ == obj_name)
+    f = -coefficient;
+  else
+    b[row_] = coefficient;
+
+  if (debug) {
+    cout << "Added RHS for Var: " << var_ << " Row: " << row_
+        << " Coefficient: " << coefficient << endl;
+  }
+}
+
+void QPSVisitor::addRHSDouble(
+    boost::fusion::vector<vector<char>, vector<char>,
+        vector<char>, vector<char>, vector<char>, double,
+        vector<char>, vector<char>, vector<char>, double> const &vars) {
+
+  string var_(at_c < 1 > (vars).begin(), at_c < 1 > (vars).end());
+  string row1_(at_c < 3 > (vars).begin(), at_c < 3 > (vars).end());
+  string row2_(at_c < 7 > (vars).begin(), at_c < 7 > (vars).end());
+  double coefficient1 = at_c < 5 > (vars);
+  double coefficient2 = at_c < 9 > (vars);
+  if (row1_ == obj_name)
+    f = -coefficient1;
+  else
+    b[row1_] = coefficient1;
+
+  if (row2_ == obj_name)
+    f = -coefficient2;
+  else
+    b[row2_] = coefficient2;
+
+  if (debug) {
+    cout << "Added RHS for Var: " << var_ << " Row: " << row1_
+        << " Coefficient: " << coefficient1 << endl;
+    cout << "                      " << "Row: " << row2_
+        << " Coefficient: " << coefficient2 << endl;
+  }
+}
+
+void QPSVisitor::addRow(
+    boost::fusion::vector<vector<char>, char, vector<char>,
+        vector<char>, vector<char>> const &vars) {
+
+  string name_(at_c < 3 > (vars).begin(), at_c < 3 > (vars).end());
+  char type = at_c < 1 > (vars);
+  switch (type) {
+  case 'N':
+    obj_name = name_;
+    break;
+  case 'L':
+    row_to_constraint_v[name_] = &IL;
+    break;
+  case 'G':
+    row_to_constraint_v[name_] = &IG;
+    break;
+  case 'E':
+    row_to_constraint_v[name_] = &E;
+    break;
+  default:
+    cout << "invalid type: " << type << endl;
+    break;
+  }
+  if (debug) {
+    cout << "Added Row Type: " << type << " Name: " << name_ << endl;
+  }
+}
+
+void QPSVisitor::addBound(
+    boost::fusion::vector<vector<char>, vector<char>,
+        vector<char>, vector<char>, vector<char>,
+        vector<char>, vector<char>, double> const &vars) {
+
+  string type_(at_c < 1 > (vars).begin(), at_c < 1 > (vars).end());
+  string var_(at_c < 5 > (vars).begin(), at_c < 5 > (vars).end());
+  double number = at_c < 7 > (vars);
+  if (type_.compare(string("UP")) == 0)
+    up[varname_to_key[var_]] = number;
+  else if (type_.compare(string("LO")) == 0)
+    lo[varname_to_key[var_]] = number;
+  else if (type_.compare(string("FX")) == 0)
+    fx[varname_to_key[var_]] = number;
+  else
+    cout << "Invalid Bound Type: " << type_ << endl;
+
+  if (debug) {
+    cout << "Added Bound Type: " << type_ << " Var: " << var_
+        << " Amount: " << number << endl;
+  }
+}
+
+void QPSVisitor::addBoundFr(
+    boost::fusion::vector<vector<char>, vector<char>,
+        vector<char>, vector<char>, vector<char>,
+        vector<char>, vector<char>> const &vars) {
+  string type_(at_c < 1 > (vars).begin(), at_c < 1 > (vars).end());
+  string var_(at_c < 5 > (vars).begin(), at_c < 5 > (vars).end());
+  Free.push_back(varname_to_key[var_]);
+  if (debug) {
+    cout << "Added Free Bound Type: " << type_ << " Var: " << var_
+        << " Amount: " << endl;
+  }
+}
+
+void QPSVisitor::addQuadTerm(
+    boost::fusion::vector<vector<char>, vector<char>,
+        vector<char>, vector<char>, vector<char>, double,
+        vector<char>> const &vars) {
+  string var1_(at_c < 1 > (vars).begin(), at_c < 1 > (vars).end());
+  string var2_(at_c < 3 > (vars).begin(), at_c < 3 > (vars).end());
+  Matrix11 coefficient = at_c < 5 > (vars) * I_1x1;
+
+  H[varname_to_key[var1_]][varname_to_key[var2_]] = coefficient;
+  H[varname_to_key[var2_]][varname_to_key[var1_]] = coefficient;
+  if (debug) {
+    cout << "Added QuadTerm for Var: " << var1_ << " Row: " << var2_
+        << " Coefficient: " << coefficient << endl;
+  }
+}
+
+QP QPSVisitor::makeQP() {
+  vector < Key > keys;
+  vector < Matrix > Gs;
+  vector < Vector > gs;
+  for (auto kv : varname_to_key) {
+    keys.push_back(kv.second);
+  }
+  sort(keys.begin(), keys.end());
+  for (unsigned int i = 0; i < keys.size(); ++i) {
+    for (unsigned int j = i; j < keys.size(); ++j) {
+      if (H.count(keys[i]) > 0 and H[keys[i]].count(keys[j]) > 0){
+        Gs.emplace_back(H[keys[i]][keys[j]]);
+      }
+      else{
+        Gs.emplace_back(Z_1x1);
+      }
+    }
+  }
+  for (Key key1 : keys) {
+    if(g.count(key1) > 0){
+      gs.emplace_back(-g[key1]);
+    }
+    else{
+      gs.emplace_back(Z_1x1);
+    }
+  }
+  size_t dual_key_num = keys.size() + 1;
+  QP madeQP;
+  auto obj = HessianFactor(keys, Gs, gs, 2 * f);
+
+  madeQP.cost.push_back(obj);
+
+  for (auto kv : E) {
+    map < Key, Matrix11 > keyMatrixMapPos;
+    map < Key, Matrix11 > keyMatrixMapNeg;
+    if (ranges.count(kv.first) == 1) {
+      for (auto km : kv.second) {
+        keyMatrixMapPos.insert(km);
+        km.second = -km.second;
+        keyMatrixMapNeg.insert(km);
+      }
+      if (ranges[kv.first] > 0) {
+        madeQP.inequalities.push_back(
+            LinearInequality(keyMatrixMapNeg, -b[kv.first], dual_key_num++));
+        madeQP.inequalities.push_back(
+            LinearInequality(keyMatrixMapPos, b[kv.first] + ranges[kv.first],
+                dual_key_num++));
+      } else if (ranges[kv.first] < 0) {
+        madeQP.inequalities.push_back(
+            LinearInequality(keyMatrixMapPos, b[kv.first], dual_key_num++));
+        madeQP.inequalities.push_back(
+            LinearInequality(keyMatrixMapNeg, ranges[kv.first] - b[kv.first],
+                dual_key_num++));
+      } else {
+        cerr << "ERROR: CANNOT ADD A RANGE OF ZERO" << endl;
+        throw;
+      }
+      continue;
+    }
+    map < Key, Matrix11 > keyMatrixMap;
+    for (auto km : kv.second) {
+      keyMatrixMap.insert(km);
+    }
+    madeQP.equalities.push_back(
+        LinearEquality(keyMatrixMap, b[kv.first] * I_1x1, dual_key_num++));
+  }
+
+  for (auto kv : IG) {
+    map < Key, Matrix11 > keyMatrixMapNeg;
+    map < Key, Matrix11 > keyMatrixMapPos;
+    for (auto km : kv.second) {
+      keyMatrixMapPos.insert(km);
+      km.second = -km.second;
+      keyMatrixMapNeg.insert(km);
+    }
+    madeQP.inequalities.push_back(
+        LinearInequality(keyMatrixMapNeg, -b[kv.first], dual_key_num++));
+    if (ranges.count(kv.first) == 1) {
+      madeQP.inequalities.push_back(
+          LinearInequality(keyMatrixMapPos, b[kv.first] + ranges[kv.first],
+              dual_key_num++));
+    }
+  }
+
+  for (auto kv : IL) {
+    map < Key, Matrix11 > keyMatrixMapPos;
+    map < Key, Matrix11 > keyMatrixMapNeg;
+    for (auto km : kv.second) {
+      keyMatrixMapPos.insert(km);
+      km.second = -km.second;
+      keyMatrixMapNeg.insert(km);
+    }
+    madeQP.inequalities.push_back(
+        LinearInequality(keyMatrixMapPos, b[kv.first], dual_key_num++));
+    if (ranges.count(kv.first) == 1) {
+      madeQP.inequalities.push_back(
+          LinearInequality(keyMatrixMapNeg, ranges[kv.first] - b[kv.first],
+              dual_key_num++));
+    }
+  }
+
+  for (Key k : keys) {
+    if (find(Free.begin(), Free.end(), k) != Free.end())
+      continue;
+    if (fx.count(k) == 1)
+      madeQP.equalities.push_back(
+          LinearEquality(k, I_1x1, fx[k] * I_1x1, dual_key_num++));
+    if (up.count(k) == 1)
+      madeQP.inequalities.push_back(
+          LinearInequality(k, I_1x1, up[k], dual_key_num++));
+    if (lo.count(k) == 1)
+      madeQP.inequalities.push_back(
+          LinearInequality(k, -I_1x1, -lo[k], dual_key_num++));
+    else
+      madeQP.inequalities.push_back(
+          LinearInequality(k, -I_1x1, 0, dual_key_num++));
+  }
+  return madeQP;
+}
+}
+

gtsam_unstable/linear/QPSVisitor.h

@@ -10,7 +10,7 @@
  * -------------------------------------------------------------------------- */
 
 /**
- * @file     RawQP.h
+ * @file     QPSVisitor.h
  * @brief    
  * @author   Ivan Dario Jimenez
  * @date     3/5/16
@@ -19,48 +19,46 @@
 #pragma once
 
 #include <gtsam_unstable/linear/QP.h>
-#include <gtsam/base/Matrix.h>
+#include <gtsam/inference/Symbol.h>
 #include <gtsam/inference/Key.h>
-
+#include <gtsam/base/Matrix.h>
+#include <boost/fusion/include/vector.hpp>
+#include <boost/fusion/sequence.hpp>
+#include <unordered_map>
 #include <string>
 #include <vector>
-#include <unordered_map>
-#include <gtsam/inference/Symbol.h>
-#include <boost/fusion/sequence.hpp>
-#include <boost/fusion/include/vector.hpp>
 
 namespace gtsam {
 /**
- * This class is responsible for collecting a QP problem as the parser parses a QPS file
+ * As the parser reads a file, it call functions in this visitor. This visitor in turn stores what the parser has read
- * and then generating a QP problem.
+ * in a way that can be later used to build the full QP problem in the file.
  */
-class RawQP {
+class QPSVisitor {
 private:
   typedef std::unordered_map<Key, Matrix11> coefficient_v;
   typedef std::unordered_map<std::string, coefficient_v> constraint_v;
 
-  std::unordered_map<std::string, constraint_v*> row_to_constraint_v;
+  std::unordered_map<std::string, constraint_v*> row_to_constraint_v; // Maps QPS ROWS to Variable-Matrix pairs
-  constraint_v E;
+  constraint_v E; // Equalities
-  constraint_v IG;
+  constraint_v IG;// Inequalities >=
-  constraint_v IL;
+  constraint_v IL;// Inequalities <=
-  unsigned int varNumber;
+  unsigned int numVariables;
-  std::unordered_map<std::string, double> b;
+  std::unordered_map<std::string, double> b; // maps from constraint name to b value for Ax = b equality constraints
-  std::unordered_map<std::string, double> ranges;
+  std::unordered_map<std::string, double> ranges; // Inequalities can be specified as ranges on a variable
-  std::unordered_map<Key, Vector1> g;
+  std::unordered_map<Key, Vector1> g; // linear term of quadratic cost
-  std::unordered_map<std::string, Key> varname_to_key;
+  std::unordered_map<std::string, Key> varname_to_key; // Variable QPS string name to key
-  std::unordered_map<Key, std::unordered_map<Key, Matrix11> > H;
+  std::unordered_map<Key, std::unordered_map<Key, Matrix11> > H; // H from hessian
-  double f;
+  double f; // Constant term of quadratic cost
-  std::string obj_name;
+  std::string obj_name; // the objective function has a name in the QPS
-  std::string name_;
+  std::string name_; // the quadratic program has a name in the QPS
-  std::unordered_map<Key, double> up;
+  std::unordered_map<Key, double> up; // Upper Bound constraints on variable where X < MAX
-  std::unordered_map<Key, double> lo;
+  std::unordered_map<Key, double> lo; // Lower Bound constraints on variable where MIN < X
-  std::unordered_map<Key, double> fx;
+  std::unordered_map<Key, double> fx; // Equalities specified as FX in BOUNDS part of QPS
-  std::vector<Key> Free;
+  std::vector<Key> Free; // Variables can be specified as Free (to which no constraints apply)
   const bool debug = false;
 
 public:
-  RawQP() :
+  QPSVisitor() : numVariables(1) {
-      row_to_constraint_v(), E(), IG(), IL(), varNumber(1), b(), ranges(), g(), varname_to_key(), H(), f(), obj_name(), name_(), up(), lo(), fx(), Free() {
   }
 
   void setName(

gtsam_unstable/linear/RawQP.cpp

@@ -1,359 +0,0 @@
-/* ----------------------------------------------------------------------------
-
- * GTSAM Copyright 2010, Georgia Tech Research Corporation,
- * Atlanta, Georgia 30332-0415
- * All Rights Reserved
- * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
-
- * See LICENSE for the license information
-
- * -------------------------------------------------------------------------- */
-
-/**
- * @file     RawQP.cpp
- * @brief    
- * @author   Ivan Dario Jimenez
- * @date     3/5/16
- */
-
-#include <gtsam_unstable/linear/RawQP.h>
-#include <iostream>
-
-using boost::fusion::at_c;
-
-namespace gtsam {
-
-void RawQP::setName(
-    boost::fusion::vector<std::vector<char>, std::vector<char>,
-        std::vector<char>> const &name) {
-  name_ = std::string(at_c < 1 > (name).begin(), at_c < 1 > (name).end());
-  if (debug) {
-    std::cout << "Parsing file: " << name_ << std::endl;
-  }
-}
-
-void RawQP::addColumn(
-    boost::fusion::vector<std::vector<char>, std::vector<char>,
-        std::vector<char>, std::vector<char>, std::vector<char>, double,
-        std::vector<char>> const &vars) {
-
-  std::string var_(at_c < 1 > (vars).begin(), at_c < 1 > (vars).end());
-  std::string row_(at_c < 3 > (vars).begin(), at_c < 3 > (vars).end());
-  Matrix11 coefficient = at_c < 5 > (vars) * I_1x1;
-  if (debug) {
-    std::cout << "Added Column for Var: " << var_ << " Row: " << row_
-        << " Coefficient: " << coefficient << std::endl;
-  }
-  if (!varname_to_key.count(var_))
-    varname_to_key[var_] = Symbol('X', varNumber++);
-  if (row_ == obj_name) {
-    g[varname_to_key[var_]] = coefficient;
-    return;
-  }
-  (*row_to_constraint_v[row_])[row_][varname_to_key[var_]] = coefficient;
-}
-
-void RawQP::addColumnDouble(
-    boost::fusion::vector<std::vector<char>, std::vector<char>,
-        std::vector<char>, std::vector<char>, double, std::vector<char>,
-        std::vector<char>, std::vector<char>, double> const &vars) {
-
-  std::string var_(at_c < 0 > (vars).begin(), at_c < 0 > (vars).end());
-  std::string row1_(at_c < 2 > (vars).begin(), at_c < 2 > (vars).end());
-  std::string row2_(at_c < 6 > (vars).begin(), at_c < 6 > (vars).end());
-  Matrix11 coefficient1 = at_c < 4 > (vars) * I_1x1;
-  Matrix11 coefficient2 = at_c < 8 > (vars) * I_1x1;
-  if (!varname_to_key.count(var_))
-    varname_to_key.insert( { var_, Symbol('X', varNumber++) });
-  if (row1_ == obj_name)
-    g[varname_to_key[var_]] = coefficient1;
-  else
-    (*row_to_constraint_v[row1_])[row1_][varname_to_key[var_]] = coefficient1;
-  if (row2_ == obj_name)
-    g[varname_to_key[var_]] = coefficient2;
-  else
-    (*row_to_constraint_v[row2_])[row2_][varname_to_key[var_]] = coefficient2;
-}
-
-void RawQP::addRangeSingle(
-    boost::fusion::vector<std::vector<char>, std::vector<char>,
-        std::vector<char>, std::vector<char>, std::vector<char>, double,
-        std::vector<char>> const & vars) {
-  std::string var_(at_c < 1 > (vars).begin(), at_c < 1 > (vars).end());
-  std::string row_(at_c < 3 > (vars).begin(), at_c < 3 > (vars).end());
-  double range = at_c < 5 > (vars);
-  ranges[row_] = range;
-  if (debug) {
-    std::cout << "SINGLE RANGE ADDED" << std::endl;
-    std::cout << "VAR:" << var_ << " ROW: " << row_ << " RANGE: " << range
-        << std::endl;
-  }
-  
-}
-void RawQP::addRangeDouble(
-    boost::fusion::vector<std::vector<char>, std::vector<char>,
-        std::vector<char>, std::vector<char>, std::vector<char>, double,
-        std::vector<char>, std::vector<char>, std::vector<char>, double> const & vars) {
-  std::string var_(at_c < 1 > (vars).begin(), at_c < 1 > (vars).end());
-  std::string row1_(at_c < 3 > (vars).begin(), at_c < 3 > (vars).end());
-  std::string row2_(at_c < 7 > (vars).begin(), at_c < 7 > (vars).end());
-  double range1 = at_c < 5 > (vars);
-  double range2 = at_c < 9 > (vars);
-  ranges[row1_] = range1;
-  ranges[row2_] = range2;
-  if (debug) {
-    std::cout << "DOUBLE RANGE ADDED" << std::endl;
-    std::cout << "VAR: " << var_ << " ROW1: " << row1_ << " RANGE1: " << range1
-        << " ROW2: " << row2_ << " RANGE2: " << range2 << std::endl;
-  }
-  
-}
-
-void RawQP::addRHS(
-    boost::fusion::vector<std::vector<char>, std::vector<char>,
-        std::vector<char>, std::vector<char>, std::vector<char>, double,
-        std::vector<char>> const &vars) {
-
-  std::string var_(at_c < 1 > (vars).begin(), at_c < 1 > (vars).end());
-  std::string row_(at_c < 3 > (vars).begin(), at_c < 3 > (vars).end());
-  double coefficient = at_c < 5 > (vars);
-  if (row_ == obj_name)
-    f = -coefficient;
-  else
-    b[row_] = coefficient;
-
-  if (debug) {
-    std::cout << "Added RHS for Var: " << var_ << " Row: " << row_
-        << " Coefficient: " << coefficient << std::endl;
-  }
-}
-
-void RawQP::addRHSDouble(
-    boost::fusion::vector<std::vector<char>, std::vector<char>,
-        std::vector<char>, std::vector<char>, std::vector<char>, double,
-        std::vector<char>, std::vector<char>, std::vector<char>, double> const &vars) {
-
-  std::string var_(at_c < 1 > (vars).begin(), at_c < 1 > (vars).end());
-  std::string row1_(at_c < 3 > (vars).begin(), at_c < 3 > (vars).end());
-  std::string row2_(at_c < 7 > (vars).begin(), at_c < 7 > (vars).end());
-  double coefficient1 = at_c < 5 > (vars);
-  double coefficient2 = at_c < 9 > (vars);
-  if (row1_ == obj_name)
-    f = -coefficient1;
-  else
-    b[row1_] = coefficient1;
-
-  if (row2_ == obj_name)
-    f = -coefficient2;
-  else
-    b[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 RawQP::addRow(
-    boost::fusion::vector<std::vector<char>, char, std::vector<char>,
-        std::vector<char>, std::vector<char>> const &vars) {
-
-  std::string name_(at_c < 3 > (vars).begin(), at_c < 3 > (vars).end());
-  char type = at_c < 1 > (vars);
-  switch (type) {
-  case 'N':
-    obj_name = name_;
-    break;
-  case 'L':
-    row_to_constraint_v[name_] = &IL;
-    break;
-  case 'G':
-    row_to_constraint_v[name_] = &IG;
-    break;
-  case 'E':
-    row_to_constraint_v[name_] = &E;
-    break;
-  default:
-    std::cout << "invalid type: " << type << std::endl;
-    break;
-  }
-  if (debug) {
-    std::cout << "Added Row Type: " << type << " Name: " << name_ << std::endl;
-  }
-}
-
-void RawQP::addBound(
-    boost::fusion::vector<std::vector<char>, std::vector<char>,
-        std::vector<char>, std::vector<char>, std::vector<char>,
-        std::vector<char>, std::vector<char>, double> const &vars) {
-
-  std::string type_(at_c < 1 > (vars).begin(), at_c < 1 > (vars).end());
-  std::string var_(at_c < 5 > (vars).begin(), at_c < 5 > (vars).end());
-  double number = at_c < 7 > (vars);
-  if (type_.compare(std::string("UP")) == 0)
-    up[varname_to_key[var_]] = number;
-  else if (type_.compare(std::string("LO")) == 0)
-    lo[varname_to_key[var_]] = number;
-  else if (type_.compare(std::string("FX")) == 0)
-    fx[varname_to_key[var_]] = number;
-  else
-    std::cout << "Invalid Bound Type: " << type_ << std::endl;
-
-  if (debug) {
-    std::cout << "Added Bound Type: " << type_ << " Var: " << var_
-        << " Amount: " << number << std::endl;
-  }
-}
-
-void RawQP::addBoundFr(
-    boost::fusion::vector<std::vector<char>, std::vector<char>,
-        std::vector<char>, std::vector<char>, std::vector<char>,
-        std::vector<char>, std::vector<char>> const &vars) {
-  std::string type_(at_c < 1 > (vars).begin(), at_c < 1 > (vars).end());
-  std::string var_(at_c < 5 > (vars).begin(), at_c < 5 > (vars).end());
-  Free.push_back(varname_to_key[var_]);
-  if (debug) {
-    std::cout << "Added Free Bound Type: " << type_ << " Var: " << var_
-        << " Amount: " << std::endl;
-  }
-}
-
-void RawQP::addQuadTerm(
-    boost::fusion::vector<std::vector<char>, std::vector<char>,
-        std::vector<char>, std::vector<char>, std::vector<char>, double,
-        std::vector<char>> const &vars) {
-  std::string var1_(at_c < 1 > (vars).begin(), at_c < 1 > (vars).end());
-  std::string var2_(at_c < 3 > (vars).begin(), at_c < 3 > (vars).end());
-  Matrix11 coefficient = at_c < 5 > (vars) * I_1x1;
-
-  H[varname_to_key[var1_]][varname_to_key[var2_]] = coefficient;
-  H[varname_to_key[var2_]][varname_to_key[var1_]] = coefficient;
-  if (debug) {
-    std::cout << "Added QuadTerm for Var: " << var1_ << " Row: " << var2_
-        << " Coefficient: " << coefficient << std::endl;
-  }
-}
-
-QP RawQP::makeQP() {
-  std::vector < Key > keys;
-  std::vector < Matrix > Gs;
-  std::vector < Vector > gs;
-  for (auto kv : varname_to_key) {
-    keys.push_back(kv.second);
-  }
-  std::sort(keys.begin(), keys.end());
-  for (unsigned int i = 0; i < keys.size(); ++i) {
-    for (unsigned int j = i; j < keys.size(); ++j) {
-      if (H.count(keys[i]) > 0 and H[keys[i]].count(keys[j]) > 0){
-        Gs.emplace_back(H[keys[i]][keys[j]]);
-      }
-      else{
-        Gs.emplace_back(Z_1x1);
-      }
-    }
-  }
-  for (Key key1 : keys) {
-    if(g.count(key1) > 0){
-      gs.emplace_back(-g[key1]);
-    }
-    else{
-      gs.emplace_back(Z_1x1);
-    }
-  }
-  size_t dual_key_num = keys.size() + 1;
-  QP madeQP;
-  auto obj = HessianFactor(keys, Gs, gs, 2 * f);
-
-  madeQP.cost.push_back(obj);
-
-  for (auto kv : E) {
-    std::map < Key, Matrix11 > keyMatrixMapPos;
-    std::map < Key, Matrix11 > keyMatrixMapNeg;
-    if (ranges.count(kv.first) == 1) {
-      for (auto km : kv.second) {
-        keyMatrixMapPos.insert(km);
-        km.second = -km.second;
-        keyMatrixMapNeg.insert(km);
-      }
-      if (ranges[kv.first] > 0) {
-        madeQP.inequalities.push_back(
-            LinearInequality(keyMatrixMapNeg, -b[kv.first], dual_key_num++));
-        madeQP.inequalities.push_back(
-            LinearInequality(keyMatrixMapPos, b[kv.first] + ranges[kv.first],
-                dual_key_num++));
-      } else if (ranges[kv.first] < 0) {
-        madeQP.inequalities.push_back(
-            LinearInequality(keyMatrixMapPos, b[kv.first], dual_key_num++));
-        madeQP.inequalities.push_back(
-            LinearInequality(keyMatrixMapNeg, ranges[kv.first] - b[kv.first],
-                dual_key_num++));
-      } else {
-        std::cerr << "ERROR: CANNOT ADD A RANGE OF ZERO" << std::endl;
-        throw;
-      }
-      continue;
-    }
-    std::map < Key, Matrix11 > keyMatrixMap;
-    for (auto km : kv.second) {
-      keyMatrixMap.insert(km);
-    }
-    madeQP.equalities.push_back(
-        LinearEquality(keyMatrixMap, b[kv.first] * I_1x1, dual_key_num++));
-  }
-
-  for (auto kv : IG) {
-    std::map < Key, Matrix11 > keyMatrixMapNeg;
-    std::map < Key, Matrix11 > keyMatrixMapPos;
-    for (auto km : kv.second) {
-      keyMatrixMapPos.insert(km);
-      km.second = -km.second;
-      keyMatrixMapNeg.insert(km);
-    }
-    madeQP.inequalities.push_back(
-        LinearInequality(keyMatrixMapNeg, -b[kv.first], dual_key_num++));
-    if (ranges.count(kv.first) == 1) {
-      madeQP.inequalities.push_back(
-          LinearInequality(keyMatrixMapPos, b[kv.first] + ranges[kv.first],
-              dual_key_num++));
-    }
-  }
-
-  for (auto kv : IL) {
-    std::map < Key, Matrix11 > keyMatrixMapPos;
-    std::map < Key, Matrix11 > keyMatrixMapNeg;
-    for (auto km : kv.second) {
-      keyMatrixMapPos.insert(km);
-      km.second = -km.second;
-      keyMatrixMapNeg.insert(km);
-    }
-    madeQP.inequalities.push_back(
-        LinearInequality(keyMatrixMapPos, b[kv.first], dual_key_num++));
-    if (ranges.count(kv.first) == 1) {
-      madeQP.inequalities.push_back(
-          LinearInequality(keyMatrixMapNeg, ranges[kv.first] - b[kv.first],
-              dual_key_num++));
-    }
-  }
-
-  for (Key k : keys) {
-    if (std::find(Free.begin(), Free.end(), k) != Free.end())
-      continue;
-    if (fx.count(k) == 1)
-      madeQP.equalities.push_back(
-          LinearEquality(k, I_1x1, fx[k] * I_1x1, dual_key_num++));
-    if (up.count(k) == 1)
-      madeQP.inequalities.push_back(
-          LinearInequality(k, I_1x1, up[k], dual_key_num++));
-    if (lo.count(k) == 1)
-      madeQP.inequalities.push_back(
-          LinearInequality(k, -I_1x1, -lo[k], dual_key_num++));
-    else
-      madeQP.inequalities.push_back(
-          LinearInequality(k, -I_1x1, 0, dual_key_num++));
-  }
-  return madeQP;
-}
-}
-

gtsam_unstable/linear/tests/testQPSolver.cpp

@@ -13,7 +13,8 @@
  * @file testQPSolver.cpp
  * @brief Test simple QP solver for a linear inequality constraint
  * @date Apr 10, 2014
- * @author Duy-Nguyen Ta, Ivan Dario Jimenez
+ * @author Duy-Nguyen Ta
+ * @author Ivan Dario Jimenez
  */
 
 #include <gtsam/base/Testable.h>