From 31556ff98198da88145dde3141ec81a0a75b65c0 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Wed, 24 Apr 2019 20:10:23 -0400 Subject: [PATCH] Cleaned up QPSVisitor and fixed KeyVector compile issue on Ubuntu --- gtsam_unstable/linear/QPSVisitor.cpp | 267 +++++++++++++-------------- gtsam_unstable/linear/QPSVisitor.h | 115 +++++------- 2 files changed, 176 insertions(+), 206 deletions(-) diff --git a/gtsam_unstable/linear/QPSVisitor.cpp b/gtsam_unstable/linear/QPSVisitor.cpp index 89c4d8766..88c4ad636 100644 --- a/gtsam_unstable/linear/QPSVisitor.cpp +++ b/gtsam_unstable/linear/QPSVisitor.cpp @@ -12,41 +12,52 @@ /** * @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. + * 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 + +#include #include +#include using boost::fusion::at_c; using namespace std; +using Chars = std::vector; + +// Get a string from a fusion vector of Chars +template +static string fromChars(const FusionVector &vars) { + const Chars &chars = at_c(vars); + return string(chars.begin(), chars.end()); +} + namespace gtsam { void QPSVisitor::setName( - boost::fusion::vector, vector, - vector> const &name) { - name_ = string(at_c < 1 > (name).begin(), at_c < 1 > (name).end()); + boost::fusion::vector const &name) { + name_ = fromChars<1>(name); if (debug) { cout << "Parsing file: " << name_ << endl; } } void QPSVisitor::addColumn( - boost::fusion::vector, vector, - vector, vector, vector, double, - vector> 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; + boost::fusion::vector const &vars) { + string var_ = fromChars<1>(vars); + string row_ = fromChars<3>(vars); + Matrix11 coefficient = at_c<5>(vars) * I_1x1; if (debug) { cout << "Added Column for Var: " << var_ << " Row: " << row_ - << " Coefficient: " << coefficient << endl; + << " Coefficient: " << coefficient << endl; } if (!varname_to_key.count(var_)) varname_to_key[var_] = Symbol('X', numVariables++); @@ -58,17 +69,15 @@ void QPSVisitor::addColumn( } void QPSVisitor::addColumnDouble( - boost::fusion::vector, vector, - vector, vector, double, vector, - vector, vector, 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; + boost::fusion::vector const &vars) { + string var_ = fromChars<0>(vars); + string row1_ = fromChars<2>(vars); + string row2_ = fromChars<6>(vars); + 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++) }); + varname_to_key.insert({var_, Symbol('X', numVariables++)}); if (row1_ == obj_name) g[varname_to_key[var_]] = coefficient1; else @@ -80,47 +89,39 @@ void QPSVisitor::addColumnDouble( } void QPSVisitor::addRangeSingle( - boost::fusion::vector, vector, - vector, vector, vector, double, - vector> 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); + boost::fusion::vector const &vars) { + string var_ = fromChars<1>(vars); + string row_ = fromChars<3>(vars); + double range = at_c<5>(vars); ranges[row_] = range; if (debug) { cout << "SINGLE RANGE ADDED" << endl; - cout << "VAR:" << var_ << " ROW: " << row_ << " RANGE: " << range - << endl; + cout << "VAR:" << var_ << " ROW: " << row_ << " RANGE: " << range << endl; } - } void QPSVisitor::addRangeDouble( - boost::fusion::vector, vector, - vector, vector, vector, double, - vector, vector, vector, 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); + boost::fusion::vector const &vars) { + string var_ = fromChars<1>(vars); + string row1_ = fromChars<3>(vars); + string row2_ = fromChars<7>(vars); + 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; + << " ROW2: " << row2_ << " RANGE2: " << range2 << endl; } - } -void QPSVisitor::addRHS( - boost::fusion::vector, vector, - vector, vector, vector, double, - vector> 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); +void QPSVisitor::addRHS(boost::fusion::vector const &vars) { + string var_ = fromChars<1>(vars); + string row_ = fromChars<3>(vars); + double coefficient = at_c<5>(vars); if (row_ == obj_name) f = -coefficient; else @@ -128,20 +129,18 @@ void QPSVisitor::addRHS( if (debug) { cout << "Added RHS for Var: " << var_ << " Row: " << row_ - << " Coefficient: " << coefficient << endl; + << " Coefficient: " << coefficient << endl; } } void QPSVisitor::addRHSDouble( - boost::fusion::vector, vector, - vector, vector, vector, double, - vector, vector, vector, 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); + boost::fusion::vector const &vars) { + string var_ = fromChars<1>(vars); + string row1_ = fromChars<3>(vars); + string row2_ = fromChars<7>(vars); + double coefficient1 = at_c<5>(vars); + double coefficient2 = at_c<9>(vars); if (row1_ == obj_name) f = -coefficient1; else @@ -154,34 +153,32 @@ void QPSVisitor::addRHSDouble( if (debug) { cout << "Added RHS for Var: " << var_ << " Row: " << row1_ - << " Coefficient: " << coefficient1 << endl; - cout << " " << "Row: " << row2_ - << " Coefficient: " << coefficient2 << endl; + << " Coefficient: " << coefficient1 << endl; + cout << " " + << "Row: " << row2_ << " Coefficient: " << coefficient2 << endl; } } void QPSVisitor::addRow( - boost::fusion::vector, char, vector, - vector, vector> const &vars) { - - string name_(at_c < 3 > (vars).begin(), at_c < 3 > (vars).end()); - char type = at_c < 1 > (vars); + boost::fusion::vector const &vars) { + string name_ = fromChars<3>(vars); + 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; + 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; @@ -189,13 +186,11 @@ void QPSVisitor::addRow( } void QPSVisitor::addBound( - boost::fusion::vector, vector, - vector, vector, vector, - vector, vector, 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); + boost::fusion::vector const &vars) { + string type_ = fromChars<1>(vars); + string var_ = fromChars<5>(vars); + 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) @@ -207,65 +202,61 @@ void QPSVisitor::addBound( if (debug) { cout << "Added Bound Type: " << type_ << " Var: " << var_ - << " Amount: " << number << endl; + << " Amount: " << number << endl; } } void QPSVisitor::addFreeBound( - boost::fusion::vector, vector, - vector, vector, vector, - vector, vector> 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_]); + boost::fusion::vector const + &vars) { + string type_ = fromChars<1>(vars); + string var_ = fromChars<5>(vars); + free.push_back(varname_to_key[var_]); if (debug) { cout << "Added Free Bound Type: " << type_ << " Var: " << var_ - << " Amount: " << endl; + << " Amount: " << endl; } } void QPSVisitor::addQuadTerm( - boost::fusion::vector, vector, - vector, vector, vector, double, - vector> 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; + boost::fusion::vector const &vars) { + string var1_ = fromChars<1>(vars); + string var2_ = fromChars<3>(vars); + 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; + << " Coefficient: " << coefficient << endl; } } 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); } - // Fill the G matrices and g vectors from - vector < Matrix > Gs; - vector < Vector > gs; + // Fill the G matrices and g vectors from + vector Gs; + vector gs; 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){ + for (size_t i = 0; i < keys.size(); ++i) { + for (size_t j = i; j < keys.size(); ++j) { + 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); } } } for (Key key1 : keys) { - if(g.count(key1) > 0){ + if (g.count(key1) > 0) { gs.emplace_back(-g[key1]); - } - else{ + } else { gs.emplace_back(Z_1x1); } } @@ -278,8 +269,8 @@ QP QPSVisitor::makeQP() { // Add equality and inequality constraints into the QP size_t dual_key_num = keys.size() + 1; for (auto kv : E) { - map < Key, Matrix11 > keyMatrixMapPos; - map < Key, Matrix11 > keyMatrixMapNeg; + map keyMatrixMapPos; + map keyMatrixMapNeg; if (ranges.count(kv.first) == 1) { for (auto km : kv.second) { keyMatrixMapPos.insert(km); @@ -289,22 +280,20 @@ QP QPSVisitor::makeQP() { 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++)); + 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++)); + 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; + map keyMatrixMap; for (auto km : kv.second) { keyMatrixMap.insert(km); } @@ -313,8 +302,8 @@ QP QPSVisitor::makeQP() { } for (auto kv : IG) { - map < Key, Matrix11 > keyMatrixMapNeg; - map < Key, Matrix11 > keyMatrixMapPos; + map keyMatrixMapNeg; + map keyMatrixMapPos; for (auto km : kv.second) { keyMatrixMapPos.insert(km); km.second = -km.second; @@ -323,15 +312,14 @@ 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( - LinearInequality(keyMatrixMapPos, b[kv.first] + ranges[kv.first], - dual_key_num++)); + 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; + map keyMatrixMapPos; + map keyMatrixMapNeg; for (auto km : kv.second) { keyMatrixMapPos.insert(km); km.second = -km.second; @@ -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( - LinearInequality(keyMatrixMapNeg, ranges[kv.first] - b[kv.first], - dual_key_num++)); + 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 (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++)); @@ -364,5 +350,4 @@ QP QPSVisitor::makeQP() { } return madeQP; } -} - +} // namespace gtsam diff --git a/gtsam_unstable/linear/QPSVisitor.h b/gtsam_unstable/linear/QPSVisitor.h index 8a255c3b1..3cb7fbc6e 100644 --- a/gtsam_unstable/linear/QPSVisitor.h +++ b/gtsam_unstable/linear/QPSVisitor.h @@ -18,14 +18,16 @@ #pragma once -#include -#include -#include #include +#include +#include +#include + #include #include -#include + #include +#include #include namespace gtsam { @@ -34,87 +36,70 @@ namespace gtsam { * in a way that can be later used to build the full QP problem in the file. */ class QPSVisitor { -private: + private: typedef std::unordered_map coefficient_v; typedef std::unordered_map constraint_v; - std::unordered_map row_to_constraint_v; // Maps QPS ROWS to Variable-Matrix pairs - constraint_v E; // Equalities - constraint_v IG;// Inequalities >= - constraint_v IL;// Inequalities <= + std::unordered_map row_to_constraint_v; // Maps QPS ROWS to Variable-Matrix pairs + constraint_v E; // Equalities + constraint_v IG; // Inequalities >= + constraint_v IL; // Inequalities <= unsigned int numVariables; - std::unordered_map b; // maps from constraint name to b value for Ax = b equality constraints - std::unordered_map ranges; // Inequalities can be specified as ranges on a variable - std::unordered_map g; // linear term of quadratic cost - std::unordered_map varname_to_key; // Variable QPS string name to key - std::unordered_map > H; // H from hessian - double f; // Constant term of quadratic cost - std::string obj_name; // the objective function has a name in the QPS - std::string name_; // the quadratic program has a name in the QPS - std::unordered_map up; // Upper Bound constraints on variable where X < MAX - std::unordered_map lo; // Lower Bound constraints on variable where MIN < X - std::unordered_map fx; // Equalities specified as FX in BOUNDS part of QPS - std::vector Free; // Variables can be specified as Free (to which no constraints apply) + std::unordered_map b; // maps from constraint name to b value for Ax = b equality constraints + std::unordered_map ranges; // Inequalities can be specified as ranges on a variable + std::unordered_map g; // linear term of quadratic cost + std::unordered_map varname_to_key; // Variable QPS string name to key + std::unordered_map > H; // H from hessian + double f; // Constant term of quadratic cost + std::string obj_name; // the objective function has a name in the QPS + std::string name_; // the quadratic program has a name in the QPS + std::unordered_map up; // Upper Bound constraints on variable where X < MAX + std::unordered_map lo; // Lower Bound constraints on variable where MIN < X + std::unordered_map fx; // Equalities specified as FX in BOUNDS part of QPS + KeyVector free; // Variables can be specified as free (to which no constraints apply) const bool debug = false; -public: - QPSVisitor() : numVariables(1) { - } + using Chars = std::vector; - void setName( - boost::fusion::vector, std::vector, - std::vector> const & name); + public: + QPSVisitor() : numVariables(1) {} - void addColumn( - boost::fusion::vector, std::vector, - std::vector, std::vector, std::vector, double, - std::vector> const & vars); + void setName(boost::fusion::vector const& name); + + void addColumn(boost::fusion::vector const& vars); void addColumnDouble( - boost::fusion::vector, std::vector, - std::vector, std::vector, double, std::vector, - std::vector, std::vector, double> const & vars); + boost::fusion::vector const& vars); - void addRHS( - boost::fusion::vector, std::vector, - std::vector, std::vector, std::vector, double, - std::vector> const & vars); + void addRHS(boost::fusion::vector const& vars); void addRHSDouble( - boost::fusion::vector, std::vector, - std::vector, std::vector, std::vector, double, - std::vector, std::vector, std::vector, double> const & vars); + boost::fusion::vector const& vars); - void addRangeSingle( - boost::fusion::vector, std::vector, - std::vector, std::vector, std::vector, double, - std::vector> const & vars); + void addRangeSingle(boost::fusion::vector const& vars); void addRangeDouble( - boost::fusion::vector, std::vector, - std::vector, std::vector, std::vector, double, - std::vector, std::vector, std::vector, double> const & vars); + boost::fusion::vector const& vars); void addRow( - boost::fusion::vector, char, std::vector, - std::vector, std::vector> const & vars); + boost::fusion::vector const& vars); - void addBound( - boost::fusion::vector, std::vector, - std::vector, std::vector, std::vector, - std::vector, std::vector, double> const & vars); + void addBound(boost::fusion::vector const& vars); - void addFreeBound( - boost::fusion::vector, std::vector, - std::vector, std::vector, std::vector, - std::vector, std::vector> const & vars); + void addFreeBound(boost::fusion::vector const& vars); - void addQuadTerm( - boost::fusion::vector, std::vector, - std::vector, std::vector, std::vector, double, - std::vector> const & vars); + void addQuadTerm(boost::fusion::vector const& vars); QP makeQP(); -} -; -} +}; + +} // namespace gtsam