diff --git a/gtsam_unstable/linear/QP.h b/gtsam_unstable/linear/QP.h index 34971aff7..17e3347ce 100644 --- a/gtsam_unstable/linear/QP.h +++ b/gtsam_unstable/linear/QP.h @@ -21,6 +21,7 @@ #include #include #include +#include namespace gtsam { @@ -41,14 +42,9 @@ struct QP { QP(const GaussianFactorGraph& _cost, const EqualityFactorGraph& _linearEqualities, const InequalityFactorGraph& _linearInequalities) : - cost(_cost), equalities(_linearEqualities), inequalities( - _linearInequalities) { + cost(_cost), equalities(_linearEqualities), inequalities(_linearInequalities) { } - QP(std::string MPS_FileName): - cost(), equalities(), inequalities() {} - - /** print */ void print(const std::string& s = "") { std::cout << s << std::endl; diff --git a/gtsam_unstable/linear/QPSParser.cpp b/gtsam_unstable/linear/QPSParser.cpp new file mode 100644 index 000000000..ec8307212 --- /dev/null +++ b/gtsam_unstable/linear/QPSParser.cpp @@ -0,0 +1,103 @@ +// +// Created by ivan on 3/5/16. +// + +#include +#include +#include +#include + +#define BOOST_SPIRIT_USE_PHOENIX_V3 1 + +using namespace boost::spirit; +using namespace boost::spirit::qi; +using namespace boost::spirit::qi::labels; + +namespace gtsam { + +struct QPSParser::MPSGrammar: grammar> { + MPSGrammar(RawQP * rqp) : + MPSGrammar::base_type(start), rqp_(rqp) { + character = lexeme[char_("a-zA-Z") | char_('_')]; + title = lexeme[+character + >> *(blank | character | char_('-') | char_('.') | char_("0-9"))]; + word = lexeme[(character + >> *(char_("0-9") | char_('-') | char_('.') | character))]; + name = + lexeme[lit("NAME") >> *blank + >> title[boost::phoenix::bind(&RawQP::setName, rqp_, qi::_1)] + >> +space]; + row = lexeme[*blank + >> (character >> +blank >> word)[boost::phoenix::bind(&RawQP::addRow, + rqp_, qi::_1, qi::_3)] >> *blank]; + rhs_single = lexeme[*blank + >> (word >> +blank >> word >> +blank >> double_)[boost::phoenix::bind( + &RawQP::addRHS, rqp_, qi::_1, qi::_3, qi::_5)] >> *blank]; + rhs_double = lexeme[*blank + >> (word >> +blank >> word >> +blank >> double_ >> +blank >> word + >> +blank >> double_)[boost::phoenix::bind(&RawQP::addRHS, rqp_, + qi::_1, qi::_3, qi::_5, qi::_7, qi::_9)] >> *blank]; + col_single = lexeme[*blank + >> (word >> +blank >> word >> +blank >> double_)[boost::phoenix::bind( + &RawQP::addColumn, rqp_, qi::_1, qi::_3, qi::_5)] >> *blank]; + col_double = lexeme[*blank + >> (word >> +blank >> word >> +blank >> double_ >> +blank >> word + >> +blank >> double_)[boost::phoenix::bind(&RawQP::addColumn, rqp_, + qi::_1, qi::_3, qi::_5, qi::_7, qi::_9)] >> *blank]; + quad_l = lexeme[*blank + >> (word >> +blank >> word >> +blank >> double_)[boost::phoenix::bind( + &RawQP::addQuadTerm, rqp_, qi::_1, qi::_3, qi::_5)] >> *blank]; + bound = + lexeme[*blank + >> (word >> +blank >> word >> +blank >> word >> +blank >> double_)[boost::phoenix::bind( + &RawQP::addBound, rqp_, qi::_1, qi::_5, qi::_7)] >> *blank]; + bound_fr = lexeme[*blank + >> (word >> +blank >> word >> +blank >> word)[boost::phoenix::bind( + &RawQP::addBound, rqp_, qi::_1, qi::_5)] >> *blank]; + 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)]; + quad = lexeme[lit("QUADOBJ") >> *blank >> eol >> +(quad_l >> eol)]; + bounds = lexeme[lit("BOUNDS") >> +space >> +((bound | bound_fr) >> eol)]; + ranges = lexeme[lit("RANGES") >> +space]; + start = lexeme[name >> rows >> cols >> rhs >> ranges >> bounds >> quad + >> lit("ENDATA") >> +space]; + } + RawQP * rqp_; + + rule, char()> character; + rule, std::vector()> word; + rule, std::vector()> title; + rule > row; + rule > col_single; + rule > col_double; + rule > rhs_single; + rule > rhs_double; + rule > ranges; + rule > bound; + rule > bound_fr; + rule > bounds; + rule > quad; + rule > quad_l; + rule > rows; + rule > cols; + rule > rhs; + rule > name; + rule > start; +}; + +QP QPSParser::Parse() { + RawQP rawData; + boost::spirit::basic_istream_iterator begin(stream); + boost::spirit::basic_istream_iterator last; + + if (!parse(begin, last, MPSGrammar(&rawData)) && begin == last) { + throw QPSParserException(); + } + + return rawData.makeQP(); +} + +} diff --git a/gtsam_unstable/linear/QPSParser.h b/gtsam_unstable/linear/QPSParser.h new file mode 100644 index 000000000..9cb795b3c --- /dev/null +++ b/gtsam_unstable/linear/QPSParser.h @@ -0,0 +1,32 @@ +/** + * @file LPSolver.cpp + * @brief QPS parser implementation + * @author Ivan Dario Jimenez + * @date 1/26/16 + */ + +#pragma once + +#include +#include +#include +#include + +namespace gtsam { + +class QPSParser { + +private: + std::fstream stream; + struct MPSGrammar; +public: + + QPSParser(const std::string& fileName) : + stream(findExampleDataFile(fileName).c_str()) { + stream.unsetf(std::ios::skipws); + } + + QP Parse(); +}; +} + diff --git a/gtsam_unstable/linear/QPSParserException.h b/gtsam_unstable/linear/QPSParserException.h new file mode 100644 index 000000000..4210eb6e3 --- /dev/null +++ b/gtsam_unstable/linear/QPSParserException.h @@ -0,0 +1,31 @@ +/** + * @file QPSParserException + * @brief Exception thrown if there is an error parsing a QPS file + * @date jan 24, 2015 + * @author Duy-Nguyen Ta + */ + +#pragma once + +namespace gtsam { + +class QPSParserException: public ThreadsafeException { +public: + QPSParserException() { + } + + virtual ~QPSParserException() throw () { + } + + virtual const char *what() const throw () { + if (description_.empty()) + description_ = "There is a problem parsing the QPS file.\n"; + return description_.c_str(); + } + +private: + mutable std::string description_; +}; + +} + diff --git a/gtsam_unstable/linear/RawQP.cpp b/gtsam_unstable/linear/RawQP.cpp new file mode 100644 index 000000000..334ecc177 --- /dev/null +++ b/gtsam_unstable/linear/RawQP.cpp @@ -0,0 +1,2 @@ + +#include diff --git a/gtsam_unstable/linear/RawQP.h b/gtsam_unstable/linear/RawQP.h new file mode 100644 index 000000000..b97363fdb --- /dev/null +++ b/gtsam_unstable/linear/RawQP.h @@ -0,0 +1,184 @@ +#pragma once + +#include +#include +#include + +#include +#include +#include +#include +#include +#include + + +namespace gtsam { +class RawQP { + typedef std::vector > coefficient_v; + + std::vector > g; + std::unordered_map > row_to_map; + std::unordered_map varname_to_key; + std::unordered_map IL; + std::unordered_map IG; + std::unordered_map E; + std::unordered_map b; + std::unordered_map > H; + unsigned int varNumber; + double f; + std::string obj_name; + std::string name_; + const bool debug = true; +public: + RawQP() : g(), row_to_map(), varname_to_key(), IL(), IG(), E(), b(), H(), varNumber(1){ + } + + void setName(std::vector name) { + name_ = std::string(name.begin(), name.end()); + if (debug) { + std::cout << "Parsing file: " << name_ << std::endl; + } + } + + void addColumn(std::vector var, std::vector row, + double coefficient) { + std::string var_(var.begin(), var.end()), row_(row.begin(), row.end()); + + if(!varname_to_key.count(var_)) + varname_to_key.insert({var_, Symbol('X', varNumber++)}); + + if(row_ == obj_name){ + g.push_back(std::make_pair(varname_to_key[var_], coefficient * ones(1,1))); + return; + } + row_to_map[row_][row_].push_back({varname_to_key[var_], coefficient * ones(1,1)}); + + if (debug) { + std::cout << "Added Column for Var: " << var_ << " Row: " << row_ + << " Coefficient: " << coefficient << std::endl; + } + } + + void addColumn(std::vector var, std::vector row1, + double coefficient1, std::vector row2, double coefficient2) { + std::string var_(var.begin(), var.end()), row1_(row1.begin(), row1.end()), + row2_(row2.begin(), row2.end()); + if(!varname_to_key.count(var_)) + varname_to_key.insert({var_, Symbol('X', varNumber++)}); + + if(row1_ == obj_name) + row_to_map[row1_][row2_].push_back({varname_to_key[var_], coefficient1 * ones(1,1)}); + else + row_to_map[row1_][row1_].push_back({varname_to_key[var_], coefficient1 * ones(1,1)}); + + if(row2_ == obj_name) + row_to_map[row2_][row2_].push_back({varname_to_key[var_], coefficient2 * ones(1,1)}); + else + row_to_map[row2_][row2_].push_back({varname_to_key[var_], coefficient2 * ones(1,1)}); + + if (debug) { + std::cout << "Added Column for Var: " << var_ << " Row: " << row1_ + << " Coefficient: " << coefficient1 << std::endl; + std::cout << " " << "Row: " << row2_ + << " Coefficient: " << coefficient2 << std::endl; + } + } + + void addRHS(std::vector var, std::vector row, + double coefficient) { + std::string var_(var.begin(), var.end()), row_(row.begin(), row.end()); + if(row_ == obj_name) + f = -coefficient; + else + b.insert({row_, coefficient}); + + if (debug) { + std::cout << "Added RHS for Var: " << var_ << " Row: " << row_ + << " Coefficient: " << coefficient << std::endl; + } + } + + void addRHS(std::vector var, std::vector row1, + double coefficient1, std::vector row2, double coefficient2) { + std::string var_(var.begin(), var.end()), row1_(row1.begin(), row1.end()), + row2_(row2.begin(), row2.end()); + + if(row1_ == obj_name) + f = -coefficient1; + else + b.insert({row1_, coefficient1}); + + if(row2_ == obj_name) + f = -coefficient2; + else + b.insert({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 addRow(char type, std::vector name) { + std::string name_(name.begin(), name.end()); + switch(type){ + case 'N': + obj_name = name_; + break; + case 'L': + row_to_map.insert({name_, IL}); + IL.insert({name_, coefficient_v()}); + break; + case 'G': + row_to_map.insert({name_, IG}); + IG.insert({name_, coefficient_v()}); + break; + case 'E': + row_to_map.insert({name_, E}); + E.insert({name_, coefficient_v()}); + break; + default: + std::cout << "invalid type: " << type << std::endl; + break; + } + if (debug) { + std::cout << "Added Row Type: " << type << " Name: " << name_ + << std::endl; + } + } + + void addBound(std::vector type, std::vector var, double number) { + std::string type_(type.begin(), type.end()), var_(var.begin(), var.end()); + + + if (debug) { + std::cout << "Added Bound Type: " << type_ << " Var: " << var_ + << " Amount: " << number << std::endl; + } + } + + void addBound(std::vector type, std::vector var) { + std::string type_(type.begin(), type.end()), var_(var.begin(), var.end()); + if (debug) { + std::cout << "Added Free Bound Type: " << type_ << " Var: " << var_ + << " Amount: " << std::endl; + } + } + + void addQuadTerm(std::vector var1, std::vector var2, + double coefficient) { + std::string var1_(var1.begin(), var1.end()), var2_(var2.begin(), + var2.end()); + if (debug) { + std::cout << "Added QuadTerm for Var: " << var1_ << " Row: " << var2_ + << " Coefficient: " << coefficient << std::endl; + } + } + + QP makeQP() { + return QP(); + } +}; +} diff --git a/gtsam_unstable/linear/tests/testQPSolver.cpp b/gtsam_unstable/linear/tests/testQPSolver.cpp index dd5b5c155..9818dccc8 100644 --- a/gtsam_unstable/linear/tests/testQPSolver.cpp +++ b/gtsam_unstable/linear/tests/testQPSolver.cpp @@ -19,7 +19,7 @@ #include #include #include - +#include #include #include #include @@ -213,39 +213,42 @@ TEST(QPSolver, optimizeForst10book_pg171Ex5) { CHECK(assert_equal(expectedSolution, solution, 1e-100)); } -QP createExampleQP(){ +QP createExampleQP() { QP exampleqp; - exampleqp.cost.push_back( - HessianFactor(X(1),Y(1), - 8.0 *ones(1,1), 2.0 * ones(1,1), 1.5*ones(1), - 10.0 *ones(1,1), -2.0 *ones(1), 4.0)); - // 2x + y >= 2 - exampleqp.inequalities.push_back( - LinearInequality(X(1), -2.0*ones(1,1), Y(1), -ones(1,1), -2, 0)); - // -x + 2y <= 6 - exampleqp.inequalities.push_back( - LinearInequality(X(1), -ones(1,1), Y(1), 2.0* ones(1,1), 6, 1)); - //x >= 0 - exampleqp.inequalities.push_back( - LinearInequality(X(1), -ones(1,1), 0, 2)); - // y > = 0 - exampleqp.inequalities.push_back( - LinearInequality(Y(1), -ones(1,1), 0, 3)); - // x<= 20 - exampleqp.inequalities.push_back( - LinearInequality(X(1), ones(1,1), 20, 4)); + return exampleqp; -}; +} +; -TEST(QPSolver, QPExampleData){ - QP exampleqp("QPExample.QPS"); +TEST(QPSolver, QPExampleData) { + QPSParser parser("QPExample.QPS"); +// QPSParser parser("AUG2D.QPS"); +// QPSParser parser("CONT-050.QPS"); - QP expectedqp = createExampleQP(); - // min f(x,y) = 4 + 1.5x -y + 0.5(8x^2 + 2xy + 2yx + 10y^2 + QP exampleqp = parser.Parse(); -// CHECK(expectedqp.cost.equals(exampleqp.cost, 1e-7)); -// CHECK(expectedqp.inequalities.equals(exampleqp.inequalities, 1e-7)); -// CHECK(expectedqp.equalities.equals(exampleqp.equalities, 1e-7)); +// QP expectedqp = createExampleQP(); + QP expectedqp; + // min f(x,y) = 4 + 1.5x -y + 0.58x^2 + 2xy + 2yx + 10y^2 + expectedqp.cost.push_back( + HessianFactor(X(1), X(2), 8.0 * ones(1, 1), 2.0 * ones(1, 1), + 1.5 * ones(1), 10.0 * ones(1, 1), -2.0 * ones(1), 4.0)); + // 2x + y >= 2 + expectedqp.inequalities.push_back( + LinearInequality(X(1), -2.0 * ones(1, 1), X(2), -ones(1, 1), -2, 0)); + // -x + 2y <= 6 + expectedqp.inequalities.push_back( + LinearInequality(X(1), -ones(1, 1), X(2), 2.0 * ones(1, 1), 6, 1)); + //x >= 0 + expectedqp.inequalities.push_back(LinearInequality(X(1), -ones(1, 1), 0, 2)); + // y > = 0 + expectedqp.inequalities.push_back(LinearInequality(X(2), -ones(1, 1), 0, 3)); + // x<= 20 + expectedqp.inequalities.push_back(LinearInequality(X(1), ones(1, 1), 20, 4)); + + CHECK(expectedqp.cost.equals(exampleqp.cost, 1e-7)); + CHECK(expectedqp.inequalities.equals(exampleqp.inequalities, 1e-7)); + CHECK(expectedqp.equalities.equals(exampleqp.equalities, 1e-7)); } /* ************************************************************************* */