diff --git a/gtsam_unstable/linear/QPSParser.cpp b/gtsam_unstable/linear/QPSParser.cpp index f11116bd7..718227307 100644 --- a/gtsam_unstable/linear/QPSParser.cpp +++ b/gtsam_unstable/linear/QPSParser.cpp @@ -61,7 +61,7 @@ struct QPSParser::MPSGrammar: base_grammar { void( bf::vector const &)> addBound; boost::function< - void(bf::vector const &)> addBoundFr; + void(bf::vector const &)> addFreeBound; MPSGrammar(QPSVisitor * rqp) : base_grammar(start), rqp_(rqp), setName( boost::bind(&QPSVisitor::setName, rqp, ::_1)), addRow( @@ -73,8 +73,8 @@ struct QPSParser::MPSGrammar: base_grammar { 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)) { + 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 | '_' | '-' | '.']; @@ -100,7 +100,7 @@ struct QPSParser::MPSGrammar: base_grammar { bound = lexeme[(*blank >> word >> +blank >> word >> +blank >> word >> +blank >> double_)[addBound] >> *blank]; bound_fr = lexeme[*blank >> word >> +blank >> word >> +blank >> word - >> *blank][addBoundFr]; + >> *blank][addFreeBound]; rows = lexeme[lit("ROWS") >> *blank >> eol >> +(row >> eol)]; rhs = lexeme[lit("RHS") >> *blank >> eol >> +((rhs_double | rhs_single) >> eol)]; diff --git a/gtsam_unstable/linear/QPSVisitor.cpp b/gtsam_unstable/linear/QPSVisitor.cpp index 6f82f1a31..89c4d8766 100644 --- a/gtsam_unstable/linear/QPSVisitor.cpp +++ b/gtsam_unstable/linear/QPSVisitor.cpp @@ -211,7 +211,7 @@ void QPSVisitor::addBound( } } -void QPSVisitor::addBoundFr( +void QPSVisitor::addFreeBound( boost::fusion::vector, vector, vector, vector, vector, vector, vector> const &vars) { @@ -241,12 +241,15 @@ void QPSVisitor::addQuadTerm( } QP QPSVisitor::makeQP() { + // Create the keys from the variable names vector < Key > keys; - vector < Matrix > Gs; - vector < Vector > gs; 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; sort(keys.begin(), keys.end()); for (unsigned int i = 0; i < keys.size(); ++i) { for (unsigned int j = i; j < keys.size(); ++j) { @@ -266,12 +269,14 @@ QP QPSVisitor::makeQP() { gs.emplace_back(Z_1x1); } } - size_t dual_key_num = keys.size() + 1; + + // 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 < Key, Matrix11 > keyMatrixMapPos; map < Key, Matrix11 > keyMatrixMapNeg; diff --git a/gtsam_unstable/linear/QPSVisitor.h b/gtsam_unstable/linear/QPSVisitor.h index 26f4c95dd..8a255c3b1 100644 --- a/gtsam_unstable/linear/QPSVisitor.h +++ b/gtsam_unstable/linear/QPSVisitor.h @@ -104,7 +104,7 @@ public: std::vector, std::vector, std::vector, std::vector, std::vector, double> const & vars); - void addBoundFr( + void addFreeBound( boost::fusion::vector, std::vector, std::vector, std::vector, std::vector, std::vector, std::vector> const & vars);