diff --git a/gtsam_unstable/linear/QPSParser.cpp b/gtsam_unstable/linear/QPSParser.cpp index a4b04634d..f11116bd7 100644 --- a/gtsam_unstable/linear/QPSParser.cpp +++ b/gtsam_unstable/linear/QPSParser.cpp @@ -19,7 +19,7 @@ #include #include -#include +#include #include #include @@ -34,7 +34,7 @@ typedef qi::grammar> base_grammar; struct QPSParser::MPSGrammar: base_grammar { typedef std::vector Chars; - RawQP * rqp_; + QPSVisitor * rqp_; boost::function const&)> setName; boost::function const &)> addRow; boost::function< @@ -62,19 +62,19 @@ struct QPSParser::MPSGrammar: base_grammar { bf::vector const &)> addBound; boost::function< void(bf::vector const &)> addBoundFr; - MPSGrammar(RawQP * rqp) : + MPSGrammar(QPSVisitor * rqp) : base_grammar(start), rqp_(rqp), setName( - boost::bind(&RawQP::setName, rqp, ::_1)), addRow( - boost::bind(&RawQP::addRow, rqp, ::_1)), rhsSingle( - boost::bind(&RawQP::addRHS, rqp, ::_1)), rhsDouble( - boost::bind(&RawQP::addRHSDouble, rqp, ::_1)), rangeSingle( - boost::bind(&RawQP::addRangeSingle, rqp, ::_1)), rangeDouble( - boost::bind(&RawQP::addRangeDouble, rqp, ::_1)), colSingle( - boost::bind(&RawQP::addColumn, rqp, ::_1)), colDouble( - boost::bind(&RawQP::addColumnDouble, rqp, ::_1)), addQuadTerm( - boost::bind(&RawQP::addQuadTerm, rqp, ::_1)), addBound( - boost::bind(&RawQP::addBound, rqp, ::_1)), addBoundFr( - boost::bind(&RawQP::addBoundFr, rqp, ::_1)) { + 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)), addBoundFr( + 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 begin(stream); diff --git a/gtsam_unstable/linear/QPSVisitor.cpp b/gtsam_unstable/linear/QPSVisitor.cpp new file mode 100644 index 000000000..6f82f1a31 --- /dev/null +++ b/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 +#include + +using boost::fusion::at_c; +using namespace std; + +namespace gtsam { + +void QPSVisitor::setName( + boost::fusion::vector, vector, + vector> 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, + 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; + 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, + 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; + 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, + 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); + 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, + 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); + 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, + 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); + 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, + 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); + 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, char, vector, + vector, vector> 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, + 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); + 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, + 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_]); + if (debug) { + cout << "Added Free Bound Type: " << type_ << " Var: " << var_ + << " 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; + + 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; +} +} + diff --git a/gtsam_unstable/linear/RawQP.h b/gtsam_unstable/linear/QPSVisitor.h similarity index 70% rename from gtsam_unstable/linear/RawQP.h rename to gtsam_unstable/linear/QPSVisitor.h index 75045aa2e..26f4c95dd 100644 --- a/gtsam_unstable/linear/RawQP.h +++ b/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 -#include +#include #include - +#include +#include +#include +#include #include #include -#include -#include -#include -#include namespace gtsam { /** - * This class is responsible for collecting a QP problem as the parser parses a QPS file - * and then generating a QP problem. + * 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 RawQP { +class QPSVisitor { private: typedef std::unordered_map coefficient_v; typedef std::unordered_map constraint_v; - std::unordered_map row_to_constraint_v; - constraint_v E; - constraint_v IG; - constraint_v IL; - unsigned int varNumber; - std::unordered_map b; - std::unordered_map ranges; - std::unordered_map g; - std::unordered_map varname_to_key; - std::unordered_map > H; - double f; - std::string obj_name; - std::string name_; - std::unordered_map up; - std::unordered_map lo; - std::unordered_map fx; - std::vector Free; + 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) const bool debug = false; public: - RawQP() : - row_to_constraint_v(), E(), IG(), IL(), varNumber(1), b(), ranges(), g(), varname_to_key(), H(), f(), obj_name(), name_(), up(), lo(), fx(), Free() { + QPSVisitor() : numVariables(1) { } void setName( diff --git a/gtsam_unstable/linear/RawQP.cpp b/gtsam_unstable/linear/RawQP.cpp deleted file mode 100644 index f25e80e82..000000000 --- a/gtsam_unstable/linear/RawQP.cpp +++ /dev/null @@ -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 -#include - -using boost::fusion::at_c; - -namespace gtsam { - -void RawQP::setName( - boost::fusion::vector, std::vector, - std::vector> 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, - std::vector, std::vector, std::vector, double, - std::vector> 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, - std::vector, std::vector, double, std::vector, - std::vector, std::vector, 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, - std::vector, std::vector, std::vector, double, - std::vector> 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, - std::vector, std::vector, std::vector, double, - std::vector, std::vector, std::vector, 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, - std::vector, std::vector, std::vector, double, - std::vector> 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, - std::vector, std::vector, std::vector, double, - std::vector, std::vector, std::vector, 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, char, std::vector, - std::vector, std::vector> 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, - std::vector, std::vector, std::vector, - std::vector, std::vector, 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, - std::vector, std::vector, std::vector, - std::vector, std::vector> 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, - std::vector, std::vector, std::vector, double, - std::vector> 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; -} -} - diff --git a/gtsam_unstable/linear/tests/testQPSolver.cpp b/gtsam_unstable/linear/tests/testQPSolver.cpp index 946a5198f..7d076748e 100644 --- a/gtsam_unstable/linear/tests/testQPSolver.cpp +++ b/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