Cleaned up QPSVisitor and fixed KeyVector compile issue on Ubuntu

gtsam_unstable/linear/QPSVisitor.cpp

@@ -12,37 +12,48 @@
 /**
  * @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
+ * This visitor in turn stores what the parser has read in a way that can be
- * QP Constraints and cost. This intermediate representation is required because an expression in the QPS file doesn't
+ * later used to build the Factor Graph for the QP Constraints and cost. This
- * necessarily correspond to a single constraint or term in the cost function.
+ * 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 <algorithm>
 #include <iostream>
+#include <map>
 
 using boost::fusion::at_c;
 using namespace std;
 
+using Chars = std::vector<char>;
+
+// Get a string from a fusion vector of Chars
+template <size_t I, class FusionVector>
+static string fromChars(const FusionVector &vars) {
+  const Chars &chars = at_c<I>(vars);
+  return string(chars.begin(), chars.end());
+}
+
 namespace gtsam {
 
 void QPSVisitor::setName(
-    boost::fusion::vector<vector<char>, vector<char>,
+    boost::fusion::vector<Chars, Chars, Chars> const &name) {
-        vector<char>> const &name) {
+  name_ = fromChars<1>(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>,
+    boost::fusion::vector<Chars, Chars, Chars, Chars, Chars, double,
-        vector<char>, vector<char>, vector<char>, double,
+                          Chars> const &vars) {
-        vector<char>> const &vars) {
+  string var_ = fromChars<1>(vars);
-
+  string row_ = fromChars<3>(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_
@@ -58,13 +69,11 @@ void QPSVisitor::addColumn(
 }
 
 void QPSVisitor::addColumnDouble(
-    boost::fusion::vector<vector<char>, vector<char>,
+    boost::fusion::vector<Chars, Chars, Chars, Chars, double, Chars, Chars,
-        vector<char>, vector<char>, double, vector<char>,
+                          Chars, double> const &vars) {
-        vector<char>, vector<char>, double> const &vars) {
+  string var_ = fromChars<0>(vars);
-
+  string row1_ = fromChars<2>(vars);
-  string var_(at_c < 0 > (vars).begin(), at_c < 0 > (vars).end());
+  string row2_ = fromChars<6>(vars);
-  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_))
@@ -80,27 +89,23 @@ void QPSVisitor::addColumnDouble(
 }
 
 void QPSVisitor::addRangeSingle(
-    boost::fusion::vector<vector<char>, vector<char>,
+    boost::fusion::vector<Chars, Chars, Chars, Chars, Chars, double,
-        vector<char>, vector<char>, vector<char>, double,
+                          Chars> const &vars) {
-        vector<char>> const & vars) {
+  string var_ = fromChars<1>(vars);
-  string var_(at_c < 1 > (vars).begin(), at_c < 1 > (vars).end());
+  string row_ = fromChars<3>(vars);
-  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
+    cout << "VAR:" << var_ << " ROW: " << row_ << " RANGE: " << range << endl;
-        << endl;
   }
-  
 }
 void QPSVisitor::addRangeDouble(
-    boost::fusion::vector<vector<char>, vector<char>,
+    boost::fusion::vector<Chars, Chars, Chars, Chars, Chars, double, Chars,
-        vector<char>, vector<char>, vector<char>, double,
+                          Chars, Chars, double> const &vars) {
-        vector<char>, vector<char>, vector<char>, double> const & vars) {
+  string var_ = fromChars<1>(vars);
-  string var_(at_c < 1 > (vars).begin(), at_c < 1 > (vars).end());
+  string row1_ = fromChars<3>(vars);
-  string row1_(at_c < 3 > (vars).begin(), at_c < 3 > (vars).end());
+  string row2_ = fromChars<7>(vars);
-  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;
@@ -110,16 +115,12 @@ void QPSVisitor::addRangeDouble(
     cout << "VAR: " << var_ << " ROW1: " << row1_ << " RANGE1: " << range1
          << " ROW2: " << row2_ << " RANGE2: " << range2 << endl;
   }
-  
 }
 
-void QPSVisitor::addRHS(
+void QPSVisitor::addRHS(boost::fusion::vector<Chars, Chars, Chars, Chars, Chars,
-    boost::fusion::vector<vector<char>, vector<char>,
+                                              double, Chars> const &vars) {
-        vector<char>, vector<char>, vector<char>, double,
+  string var_ = fromChars<1>(vars);
-        vector<char>> const &vars) {
+  string row_ = fromChars<3>(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;
@@ -133,13 +134,11 @@ void QPSVisitor::addRHS(
 }
 
 void QPSVisitor::addRHSDouble(
-    boost::fusion::vector<vector<char>, vector<char>,
+    boost::fusion::vector<Chars, Chars, Chars, Chars, Chars, double, Chars,
-        vector<char>, vector<char>, vector<char>, double,
+                          Chars, Chars, double> const &vars) {
-        vector<char>, vector<char>, vector<char>, double> const &vars) {
+  string var_ = fromChars<1>(vars);
-
+  string row1_ = fromChars<3>(vars);
-  string var_(at_c < 1 > (vars).begin(), at_c < 1 > (vars).end());
+  string row2_ = fromChars<7>(vars);
-  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)
@@ -155,16 +154,14 @@ void QPSVisitor::addRHSDouble(
   if (debug) {
     cout << "Added RHS for Var: " << var_ << " Row: " << row1_
          << " Coefficient: " << coefficient1 << endl;
-    cout << "                      " << "Row: " << row2_
+    cout << "                      "
-        << " Coefficient: " << coefficient2 << endl;
+         << "Row: " << row2_ << " Coefficient: " << coefficient2 << endl;
   }
 }
 
 void QPSVisitor::addRow(
-    boost::fusion::vector<vector<char>, char, vector<char>,
+    boost::fusion::vector<Chars, char, Chars, Chars, Chars> const &vars) {
-        vector<char>, vector<char>> const &vars) {
+  string name_ = fromChars<3>(vars);
-
-  string name_(at_c < 3 > (vars).begin(), at_c < 3 > (vars).end());
   char type = at_c<1>(vars);
   switch (type) {
     case 'N':
@@ -189,12 +186,10 @@ void QPSVisitor::addRow(
 }
 
 void QPSVisitor::addBound(
-    boost::fusion::vector<vector<char>, vector<char>,
+    boost::fusion::vector<Chars, Chars, Chars, Chars, Chars, Chars, Chars,
-        vector<char>, vector<char>, vector<char>,
+                          double> const &vars) {
-        vector<char>, vector<char>, double> const &vars) {
+  string type_ = fromChars<1>(vars);
-
+  string var_ = fromChars<5>(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;
@@ -212,12 +207,11 @@ void QPSVisitor::addBound(
 }
 
 void QPSVisitor::addFreeBound(
-    boost::fusion::vector<vector<char>, vector<char>,
+    boost::fusion::vector<Chars, Chars, Chars, Chars, Chars, Chars, Chars> const
-        vector<char>, vector<char>, vector<char>,
+        &vars) {
-        vector<char>, vector<char>> const &vars) {
+  string type_ = fromChars<1>(vars);
-  string type_(at_c < 1 > (vars).begin(), at_c < 1 > (vars).end());
+  string var_ = fromChars<5>(vars);
-  string var_(at_c < 5 > (vars).begin(), at_c < 5 > (vars).end());
+  free.push_back(varname_to_key[var_]);
-  Free.push_back(varname_to_key[var_]);
   if (debug) {
     cout << "Added Free Bound Type: " << type_ << " Var: " << var_
          << " Amount: " << endl;
@@ -225,11 +219,10 @@ void QPSVisitor::addFreeBound(
 }
 
 void QPSVisitor::addQuadTerm(
-    boost::fusion::vector<vector<char>, vector<char>,
+    boost::fusion::vector<Chars, Chars, Chars, Chars, Chars, double,
-        vector<char>, vector<char>, vector<char>, double,
+                          Chars> const &vars) {
-        vector<char>> const &vars) {
+  string var1_ = fromChars<1>(vars);
-  string var1_(at_c < 1 > (vars).begin(), at_c < 1 > (vars).end());
+  string var2_ = fromChars<3>(vars);
-  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;
@@ -242,7 +235,7 @@ void QPSVisitor::addQuadTerm(
 
 QP QPSVisitor::makeQP() {
   // Create the keys from the variable names
-  vector < Key > keys;
+  KeyVector keys;
   for (auto kv : varname_to_key) {
     keys.push_back(kv.second);
   }
@@ -251,12 +244,11 @@ QP QPSVisitor::makeQP() {
   vector<Matrix> Gs;
   vector<Vector> gs;
   sort(keys.begin(), keys.end());
-  for (unsigned int i = 0; i < keys.size(); ++i) {
+  for (size_t i = 0; i < keys.size(); ++i) {
-    for (unsigned int j = i; j < keys.size(); ++j) {
+    for (size_t j = i; j < keys.size(); ++j) {
-      if (H.count(keys[i]) > 0 and H[keys[i]].count(keys[j]) > 0){
+      if (H.count(keys[i]) > 0 && H[keys[i]].count(keys[j]) > 0) {
         Gs.emplace_back(H[keys[i]][keys[j]]);
-      }
+      } else {
-      else{
         Gs.emplace_back(Z_1x1);
       }
     }
@@ -264,8 +256,7 @@ QP QPSVisitor::makeQP() {
   for (Key key1 : keys) {
     if (g.count(key1) > 0) {
       gs.emplace_back(-g[key1]);
-    }
+    } else {
-    else{
       gs.emplace_back(Z_1x1);
     }
   }
@@ -289,15 +280,13 @@ QP QPSVisitor::makeQP() {
       if (ranges[kv.first] > 0) {
         madeQP.inequalities.push_back(
             LinearInequality(keyMatrixMapNeg, -b[kv.first], dual_key_num++));
-        madeQP.inequalities.push_back(
+        madeQP.inequalities.push_back(LinearInequality(
-            LinearInequality(keyMatrixMapPos, b[kv.first] + ranges[kv.first],
+            keyMatrixMapPos, b[kv.first] + ranges[kv.first], dual_key_num++));
-                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(
+        madeQP.inequalities.push_back(LinearInequality(
-            LinearInequality(keyMatrixMapNeg, ranges[kv.first] - b[kv.first],
+            keyMatrixMapNeg, ranges[kv.first] - b[kv.first], dual_key_num++));
-                dual_key_num++));
       } else {
         cerr << "ERROR: CANNOT ADD A RANGE OF ZERO" << endl;
         throw;
@@ -323,9 +312,8 @@ QP QPSVisitor::makeQP() {
     madeQP.inequalities.push_back(
         LinearInequality(keyMatrixMapNeg, -b[kv.first], dual_key_num++));
     if (ranges.count(kv.first) == 1) {
-      madeQP.inequalities.push_back(
+      madeQP.inequalities.push_back(LinearInequality(
-          LinearInequality(keyMatrixMapPos, b[kv.first] + ranges[kv.first],
+          keyMatrixMapPos, b[kv.first] + ranges[kv.first], dual_key_num++));
-              dual_key_num++));
     }
   }
 
@@ -340,15 +328,13 @@ QP QPSVisitor::makeQP() {
     madeQP.inequalities.push_back(
         LinearInequality(keyMatrixMapPos, b[kv.first], dual_key_num++));
     if (ranges.count(kv.first) == 1) {
-      madeQP.inequalities.push_back(
+      madeQP.inequalities.push_back(LinearInequality(
-          LinearInequality(keyMatrixMapNeg, ranges[kv.first] - b[kv.first],
+          keyMatrixMapNeg, ranges[kv.first] - b[kv.first], dual_key_num++));
-              dual_key_num++));
     }
   }
 
   for (Key k : keys) {
-    if (find(Free.begin(), Free.end(), k) != Free.end())
+    if (find(free.begin(), free.end(), k) != free.end()) continue;
-      continue;
     if (fx.count(k) == 1)
       madeQP.equalities.push_back(
           LinearEquality(k, I_1x1, fx[k] * I_1x1, dual_key_num++));
@@ -364,5 +350,4 @@ QP QPSVisitor::makeQP() {
   }
   return madeQP;
 }
-}
+}  // namespace gtsam
-

gtsam_unstable/linear/QPSVisitor.h

@@ -18,14 +18,16 @@
 
 #pragma once
 
-#include <gtsam_unstable/linear/QP.h>
-#include <gtsam/inference/Symbol.h>
-#include <gtsam/inference/Key.h>
 #include <gtsam/base/Matrix.h>
+#include <gtsam/inference/Key.h>
+#include <gtsam/inference/Symbol.h>
+#include <gtsam_unstable/linear/QP.h>
+
 #include <boost/fusion/include/vector.hpp>
 #include <boost/fusion/sequence.hpp>
-#include <unordered_map>
+
 #include <string>
+#include <unordered_map>
 #include <vector>
 
 namespace gtsam {
@@ -54,67 +56,50 @@ private:
   std::unordered_map<Key, double> up;  // Upper Bound constraints on variable where X < MAX
   std::unordered_map<Key, double> lo;  // Lower Bound constraints on variable where MIN < X
   std::unordered_map<Key, double> fx;  // Equalities specified as FX in BOUNDS part of QPS
-  std::vector<Key> Free; // Variables can be specified as Free (to which no constraints apply)
+  KeyVector free;  // Variables can be specified as free (to which no constraints apply)
   const bool debug = false;
 
+  using Chars = std::vector<char>;
+
  public:
-  QPSVisitor() : numVariables(1) {
+  QPSVisitor() : numVariables(1) {}
-  }
 
-  void setName(
+  void setName(boost::fusion::vector<Chars, Chars, Chars> const& name);
-      boost::fusion::vector<std::vector<char>, std::vector<char>,
-          std::vector<char>> const & name);
 
-  void addColumn(
+  void addColumn(boost::fusion::vector<Chars, Chars, Chars, Chars, Chars,
-      boost::fusion::vector<std::vector<char>, std::vector<char>,
+                                       double, Chars> const& vars);
-          std::vector<char>, std::vector<char>, std::vector<char>, double,
-          std::vector<char>> const & vars);
 
   void addColumnDouble(
-      boost::fusion::vector<std::vector<char>, std::vector<char>,
+      boost::fusion::vector<Chars, Chars, Chars, Chars, double, Chars, Chars,
-          std::vector<char>, std::vector<char>, double, std::vector<char>,
+                            Chars, double> const& vars);
-          std::vector<char>, std::vector<char>, double> const & vars);
 
-  void addRHS(
+  void addRHS(boost::fusion::vector<Chars, Chars, Chars, Chars, Chars, double,
-      boost::fusion::vector<std::vector<char>, std::vector<char>,
+                                    Chars> const& vars);
-          std::vector<char>, std::vector<char>, std::vector<char>, double,
-          std::vector<char>> const & vars);
 
   void addRHSDouble(
-      boost::fusion::vector<std::vector<char>, std::vector<char>,
+      boost::fusion::vector<Chars, Chars, Chars, Chars, Chars, double, Chars,
-          std::vector<char>, std::vector<char>, std::vector<char>, double,
+                            Chars, Chars, double> const& vars);
-          std::vector<char>, std::vector<char>, std::vector<char>, double> const & vars);
 
-  void addRangeSingle(
+  void addRangeSingle(boost::fusion::vector<Chars, Chars, Chars, Chars, Chars,
-      boost::fusion::vector<std::vector<char>, std::vector<char>,
+                                            double, Chars> const& vars);
-          std::vector<char>, std::vector<char>, std::vector<char>, double,
-          std::vector<char>> const & vars);
 
   void addRangeDouble(
-      boost::fusion::vector<std::vector<char>, std::vector<char>,
+      boost::fusion::vector<Chars, Chars, Chars, Chars, Chars, double, Chars,
-          std::vector<char>, std::vector<char>, std::vector<char>, double,
+                            Chars, Chars, double> const& vars);
-          std::vector<char>, std::vector<char>, std::vector<char>, double> const & vars);
 
   void addRow(
-      boost::fusion::vector<std::vector<char>, char, std::vector<char>,
+      boost::fusion::vector<Chars, char, Chars, Chars, Chars> const& vars);
-          std::vector<char>, std::vector<char>> const & vars);
 
-  void addBound(
+  void addBound(boost::fusion::vector<Chars, Chars, Chars, Chars, Chars, Chars,
-      boost::fusion::vector<std::vector<char>, std::vector<char>,
+                                      Chars, double> const& vars);
-          std::vector<char>, std::vector<char>, std::vector<char>,
-          std::vector<char>, std::vector<char>, double> const & vars);
 
-  void addFreeBound(
+  void addFreeBound(boost::fusion::vector<Chars, Chars, Chars, Chars, Chars,
-      boost::fusion::vector<std::vector<char>, std::vector<char>,
+                                          Chars, Chars> const& vars);
-          std::vector<char>, std::vector<char>, std::vector<char>,
-          std::vector<char>, std::vector<char>> const & vars);
 
-  void addQuadTerm(
+  void addQuadTerm(boost::fusion::vector<Chars, Chars, Chars, Chars, Chars,
-      boost::fusion::vector<std::vector<char>, std::vector<char>,
+                                         double, Chars> const& vars);
-          std::vector<char>, std::vector<char>, std::vector<char>, double,
-          std::vector<char>> const & vars);
 
   QP makeQP();
-}
+};
-;
+
-}
+}  // namespace gtsam