diff --git a/gtsam_unstable/linear/QPSParser.cpp b/gtsam_unstable/linear/QPSParser.cpp index 718227307..3039185f2 100644 --- a/gtsam_unstable/linear/QPSParser.cpp +++ b/gtsam_unstable/linear/QPSParser.cpp @@ -17,64 +17,440 @@ #define BOOST_SPIRIT_USE_PHOENIX_V3 1 +#include +#include +#include +#include #include #include -#include -#include +#include +#include #include #include #include +#include + +#include +#include +#include +#include +#include +#include + +using boost::fusion::at_c; +using namespace std; namespace bf = boost::fusion; namespace qi = boost::spirit::qi; +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 { + +/** + * As the parser reads a file, it call functions in this visitor. This visitor + * in turn stores what the parser has read in a way that can be later used to + * build the full QP problem in the file. + */ +class QPSVisitor { + 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 <= + 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 + KeyVector free; // Variables can be specified as free (to which no + // constraints apply) + const bool debug = false; + + public: + QPSVisitor() : numVariables(1) {} + + void setName(boost::fusion::vector const &name) { + name_ = fromChars<1>(name); + if (debug) { + cout << "Parsing file: " << name_ << endl; + } + } + + void addColumn(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; + } + 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 addColumnDouble( + 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++)}); + 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 addRangeSingle(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; + } + } + void addRangeDouble( + 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; + } + } + + void 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 + b[row_] = coefficient; + + if (debug) { + cout << "Added RHS for Var: " << var_ << " Row: " << row_ + << " Coefficient: " << coefficient << endl; + } + } + + void addRHSDouble( + 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 + 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 addRow( + 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; + } + if (debug) { + cout << "Added Row Type: " << type << " Name: " << name_ << endl; + } + } + + void addBound(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) + 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 addFreeBound(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; + } + } + + void addQuadTerm(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; + } + } + + QP makeQP() { + // Create the keys from the variable names + KeyVector keys; + for (auto kv : varname_to_key) { + keys.push_back(kv.second); + } + + // Fill the G matrices and g vectors from + vector Gs; + vector gs; + sort(keys.begin(), keys.end()); + 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 { + 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); + } + } + + // Construct the quadratic program + QP madeQP; + auto obj = HessianFactor(keys, Gs, gs, 2 * f); + madeQP.cost.push_back(obj); + + // Add equality and inequality constraints into the QP + size_t dual_key_num = keys.size() + 1; + for (auto kv : E) { + map keyMatrixMapPos; + map 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 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 keyMatrixMapNeg; + map 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 keyMatrixMapPos; + map 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; + } +}; + typedef qi::grammar> base_grammar; -struct QPSParser::MPSGrammar: base_grammar { +struct QPSParser::MPSGrammar : base_grammar { typedef std::vector Chars; - QPSVisitor * rqp_; - boost::function const&)> setName; - boost::function const &)> addRow; - boost::function< - void(bf::vector const &)> rhsSingle; - boost::function< - void( - bf::vector)> rhsDouble; - boost::function< - void(bf::vector const &)> rangeSingle; - boost::function< - void( - bf::vector)> rangeDouble; - boost::function< - void(bf::vector)> colSingle; - boost::function< - void( - bf::vector const &)> colDouble; - boost::function< - void(bf::vector const &)> addQuadTerm; - boost::function< - void( - bf::vector const &)> addBound; - boost::function< - void(bf::vector const &)> addFreeBound; - MPSGrammar(QPSVisitor * rqp) : - base_grammar(start), rqp_(rqp), setName( - boost::bind(&QPSVisitor::setName, rqp, ::_1)), addRow( - boost::bind(&QPSVisitor::addRow, rqp, ::_1)), rhsSingle( - boost::bind(&QPSVisitor::addRHS, rqp, ::_1)), rhsDouble( - boost::bind(&QPSVisitor::addRHSDouble, rqp, ::_1)), rangeSingle( - boost::bind(&QPSVisitor::addRangeSingle, rqp, ::_1)), rangeDouble( - boost::bind(&QPSVisitor::addRangeDouble, rqp, ::_1)), colSingle( - boost::bind(&QPSVisitor::addColumn, rqp, ::_1)), colDouble( - boost::bind(&QPSVisitor::addColumnDouble, rqp, ::_1)), addQuadTerm( - boost::bind(&QPSVisitor::addQuadTerm, rqp, ::_1)), addBound( - boost::bind(&QPSVisitor::addBound, rqp, ::_1)), addFreeBound( - boost::bind(&QPSVisitor::addFreeBound, rqp, ::_1)) { + QPSVisitor *rqp_; + boost::function const &)> setName; + boost::function const &)> + addRow; + boost::function const &)> + rhsSingle; + boost::function)> + rhsDouble; + boost::function const &)> + rangeSingle; + boost::function)> + rangeDouble; + boost::function)> + colSingle; + boost::function const &)> + colDouble; + boost::function const &)> + addQuadTerm; + boost::function const &)> + addBound; + boost::function const &)> + addFreeBound; + MPSGrammar(QPSVisitor *rqp) + : base_grammar(start), + rqp_(rqp), + setName(boost::bind(&QPSVisitor::setName, rqp, ::_1)), + addRow(boost::bind(&QPSVisitor::addRow, rqp, ::_1)), + rhsSingle(boost::bind(&QPSVisitor::addRHS, rqp, ::_1)), + rhsDouble(boost::bind(&QPSVisitor::addRHSDouble, rqp, ::_1)), + rangeSingle(boost::bind(&QPSVisitor::addRangeSingle, rqp, ::_1)), + rangeDouble(boost::bind(&QPSVisitor::addRangeDouble, rqp, ::_1)), + colSingle(boost::bind(&QPSVisitor::addColumn, rqp, ::_1)), + colDouble(boost::bind(&QPSVisitor::addColumnDouble, rqp, ::_1)), + addQuadTerm(boost::bind(&QPSVisitor::addQuadTerm, rqp, ::_1)), + addBound(boost::bind(&QPSVisitor::addBound, rqp, ::_1)), + addFreeBound(boost::bind(&QPSVisitor::addFreeBound, rqp, ::_1)) { using namespace boost::spirit; using namespace boost::spirit::qi; character = lexeme[alnum | '_' | '-' | '.']; @@ -82,42 +458,48 @@ struct QPSParser::MPSGrammar: base_grammar { word = lexeme[+character]; name = lexeme[lit("NAME") >> *blank >> title >> +space][setName]; row = lexeme[*blank >> character >> +blank >> word >> *blank][addRow]; - rhs_single = lexeme[*blank >> word >> +blank >> word >> +blank >> double_ - >> *blank][rhsSingle]; - rhs_double = lexeme[(*blank >> word >> +blank >> word >> +blank >> double_ - >> +blank >> word >> +blank >> double_)[rhsDouble] >> *blank]; - range_single = lexeme[*blank >> word >> +blank >> word >> +blank >> double_ - >> *blank][rangeSingle]; - range_double = lexeme[(*blank >> word >> +blank >> word >> +blank >> double_ - >> +blank >> word >> +blank >> double_)[rangeDouble] >> *blank]; - col_single = lexeme[*blank >> word >> +blank >> word >> +blank >> double_ - >> *blank][colSingle]; - col_double = lexeme[*blank - >> (word >> +blank >> word >> +blank >> double_ >> +blank >> word - >> +blank >> double_)[colDouble] >> *blank]; - quad_l = lexeme[*blank >> word >> +blank >> word >> +blank >> double_ - >> *blank][addQuadTerm]; - bound = lexeme[(*blank >> word >> +blank >> word >> +blank >> word >> +blank - >> double_)[addBound] >> *blank]; - bound_fr = lexeme[*blank >> word >> +blank >> word >> +blank >> word - >> *blank][addFreeBound]; + rhs_single = lexeme[*blank >> word >> +blank >> word >> +blank >> double_ >> + *blank][rhsSingle]; + rhs_double = + lexeme[(*blank >> word >> +blank >> word >> +blank >> double_ >> + +blank >> word >> +blank >> double_)[rhsDouble] >> + *blank]; + range_single = lexeme[*blank >> word >> +blank >> word >> +blank >> + double_ >> *blank][rangeSingle]; + range_double = + lexeme[(*blank >> word >> +blank >> word >> +blank >> double_ >> + +blank >> word >> +blank >> double_)[rangeDouble] >> + *blank]; + col_single = lexeme[*blank >> word >> +blank >> word >> +blank >> double_ >> + *blank][colSingle]; + col_double = + lexeme[*blank >> (word >> +blank >> word >> +blank >> double_ >> + +blank >> word >> +blank >> double_)[colDouble] >> + *blank]; + quad_l = lexeme[*blank >> word >> +blank >> word >> +blank >> double_ >> + *blank][addQuadTerm]; + bound = lexeme[(*blank >> word >> +blank >> word >> +blank >> word >> + +blank >> double_)[addBound] >> + *blank]; + bound_fr = lexeme[*blank >> word >> +blank >> word >> +blank >> word >> + *blank][addFreeBound]; 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)]; + 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 - >> *((range_double | range_single) >> eol)]; + ranges = lexeme[lit("RANGES") >> +space >> + *((range_double | range_single) >> eol)]; end = lexeme[lit("ENDATA") >> *space]; - start = lexeme[name >> rows >> cols >> rhs >> -ranges >> bounds >> quad - >> end]; + start = + lexeme[name >> rows >> cols >> rhs >> -ranges >> bounds >> quad >> end]; } qi::rule, char()> character; qi::rule, Chars()> word, title; - qi::rule > row, end, col_single, + qi::rule> row, end, col_single, col_double, rhs_single, rhs_double, range_single, range_double, ranges, bound, bound_fr, bounds, quad, quad_l, rows, cols, rhs, name, start; }; @@ -136,4 +518,4 @@ QP QPSParser::Parse() { return rawData.makeQP(); } -} +} // namespace gtsam diff --git a/gtsam_unstable/linear/QPSVisitor.cpp b/gtsam_unstable/linear/QPSVisitor.cpp deleted file mode 100644 index 88c4ad636..000000000 --- a/gtsam_unstable/linear/QPSVisitor.cpp +++ /dev/null @@ -1,353 +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 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 - -#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 const &name) { - name_ = fromChars<1>(name); - if (debug) { - cout << "Parsing file: " << name_ << endl; - } -} - -void QPSVisitor::addColumn( - 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; - } - 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 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++)}); - 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 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; - } -} -void QPSVisitor::addRangeDouble( - 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; - } -} - -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 - b[row_] = coefficient; - - if (debug) { - cout << "Added RHS for Var: " << var_ << " Row: " << row_ - << " Coefficient: " << coefficient << endl; - } -} - -void QPSVisitor::addRHSDouble( - 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 - 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 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; - } - if (debug) { - cout << "Added Row Type: " << type << " Name: " << name_ << endl; - } -} - -void QPSVisitor::addBound( - 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) - 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::addFreeBound( - 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; - } -} - -void QPSVisitor::addQuadTerm( - 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; - } -} - -QP QPSVisitor::makeQP() { - // Create the keys from the variable names - KeyVector keys; - for (auto kv : varname_to_key) { - keys.push_back(kv.second); - } - - // Fill the G matrices and g vectors from - vector Gs; - vector gs; - sort(keys.begin(), keys.end()); - 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 { - 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); - } - } - - // Construct the quadratic program - QP madeQP; - auto obj = HessianFactor(keys, Gs, gs, 2 * f); - madeQP.cost.push_back(obj); - - // Add equality and inequality constraints into the QP - size_t dual_key_num = keys.size() + 1; - for (auto kv : E) { - map keyMatrixMapPos; - map 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 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 keyMatrixMapNeg; - map 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 keyMatrixMapPos; - map 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; -} -} // namespace gtsam diff --git a/gtsam_unstable/linear/QPSVisitor.h b/gtsam_unstable/linear/QPSVisitor.h deleted file mode 100644 index 3cb7fbc6e..000000000 --- a/gtsam_unstable/linear/QPSVisitor.h +++ /dev/null @@ -1,105 +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 QPSVisitor.h - * @brief - * @author Ivan Dario Jimenez - * @date 3/5/16 - */ - -#pragma once - -#include -#include -#include -#include - -#include -#include - -#include -#include -#include - -namespace gtsam { -/** - * As the parser reads a file, it call functions in this visitor. This visitor in turn stores what the parser has read - * in a way that can be later used to build the full QP problem in the file. - */ -class QPSVisitor { - 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 <= - 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 - KeyVector free; // Variables can be specified as free (to which no constraints apply) - const bool debug = false; - - using Chars = std::vector; - - public: - QPSVisitor() : numVariables(1) {} - - void setName(boost::fusion::vector const& name); - - void addColumn(boost::fusion::vector const& vars); - - void addColumnDouble( - boost::fusion::vector const& vars); - - void addRHS(boost::fusion::vector const& vars); - - void addRHSDouble( - boost::fusion::vector const& vars); - - void addRangeSingle(boost::fusion::vector const& vars); - - void addRangeDouble( - boost::fusion::vector const& vars); - - void addRow( - boost::fusion::vector const& vars); - - void addBound(boost::fusion::vector const& vars); - - void addFreeBound(boost::fusion::vector const& vars); - - void addQuadTerm(boost::fusion::vector const& vars); - - QP makeQP(); -}; - -} // namespace gtsam