diff --git a/examples/Data/HS21.QPS b/examples/Data/HS21.QPS new file mode 100644 index 0000000000..c71305c6e6 --- /dev/null +++ b/examples/Data/HS21.QPS @@ -0,0 +1,20 @@ +NAME HS21 +ROWS + N OBJ.FUNC + G R------1 +COLUMNS + C------1 R------1 0.100000e+02 + C------2 R------1 -.100000e+01 +RHS + RHS OBJ.FUNC 0.100000e+03 + RHS R------1 0.100000e+02 +RANGES +BOUNDS + LO BOUNDS C------1 0.200000e+01 + UP BOUNDS C------1 0.500000e+02 + LO BOUNDS C------2 -.500000e+02 + UP BOUNDS C------2 0.500000e+02 +QUADOBJ + C------1 C------1 0.200000e-01 + C------2 C------2 0.200000e+01 +ENDATA diff --git a/examples/Data/HS268.QPS b/examples/Data/HS268.QPS new file mode 100644 index 0000000000..53de894211 --- /dev/null +++ b/examples/Data/HS268.QPS @@ -0,0 +1,55 @@ +NAME HS268 +ROWS + N OBJ.FUNC + G R------1 + G R------2 + G R------3 + G R------4 + G R------5 +COLUMNS + C------1 OBJ.FUNC 0.183400e+05 R------1 -.100000e+01 + C------1 R------2 0.100000e+02 R------3 -.800000e+01 + C------1 R------4 0.800000e+01 R------5 -.400000e+01 + C------2 OBJ.FUNC -.341980e+05 R------1 -.100000e+01 + C------2 R------2 0.100000e+02 R------3 0.100000e+01 + C------2 R------4 -.100000e+01 R------5 -.200000e+01 + C------3 OBJ.FUNC 0.454200e+04 R------1 -.100000e+01 + C------3 R------2 -.300000e+01 R------3 -.200000e+01 + C------3 R------4 0.200000e+01 R------5 0.300000e+01 + C------4 OBJ.FUNC 0.867200e+04 R------1 -.100000e+01 + C------4 R------2 0.500000e+01 R------3 -.500000e+01 + C------4 R------4 0.500000e+01 R------5 -.500000e+01 + C------5 OBJ.FUNC 0.860000e+02 R------1 -.100000e+01 + C------5 R------2 0.400000e+01 R------3 0.300000e+01 + C------5 R------4 -.300000e+01 R------5 0.100000e+01 +RHS + RHS OBJ.FUNC -.144630e+05 + RHS R------1 -.500000e+01 + RHS R------2 0.200000e+02 + RHS R------3 -.400000e+02 + RHS R------4 0.110000e+02 + RHS R------5 -.300000e+02 +RANGES +BOUNDS + FR BOUNDS C------1 + FR BOUNDS C------2 + FR BOUNDS C------3 + FR BOUNDS C------4 + FR BOUNDS C------5 +QUADOBJ + C------1 C------1 0.203940e+05 + C------1 C------2 -.249080e+05 + C------1 C------3 -.202600e+04 + C------1 C------4 0.389600e+04 + C------1 C------5 0.658000e+03 + C------2 C------2 0.418180e+05 + C------2 C------3 -.346600e+04 + C------2 C------4 -.982800e+04 + C------2 C------5 -.372000e+03 + C------3 C------3 0.351000e+04 + C------3 C------4 0.217800e+04 + C------3 C------5 -.348000e+03 + C------4 C------4 0.303000e+04 + C------4 C------5 -.440000e+02 + C------5 C------5 0.540000e+02 +ENDATA diff --git a/examples/Data/HS35.QPS b/examples/Data/HS35.QPS new file mode 100644 index 0000000000..1ee230e39e --- /dev/null +++ b/examples/Data/HS35.QPS @@ -0,0 +1,20 @@ +NAME HS35 +ROWS + N OBJ.FUNC + G R------1 +COLUMNS + C------1 OBJ.FUNC -.800000e+01 R------1 -.100000e+01 + C------2 OBJ.FUNC -.600000e+01 R------1 -.100000e+01 + C------3 OBJ.FUNC -.400000e+01 R------1 -.200000e+01 +RHS + RHS OBJ.FUNC -.900000e+01 + RHS R------1 -.300000e+01 +RANGES +BOUNDS +QUADOBJ + C------1 C------1 0.400000e+01 + C------1 C------2 0.200000e+01 + C------1 C------3 0.200000e+01 + C------2 C------2 0.400000e+01 + C------3 C------3 0.200000e+01 +ENDATA diff --git a/examples/Data/HS35MOD.QPS b/examples/Data/HS35MOD.QPS new file mode 100644 index 0000000000..2fe75ef96e --- /dev/null +++ b/examples/Data/HS35MOD.QPS @@ -0,0 +1,21 @@ +NAME HS35MOD +ROWS + N OBJ.FUNC + G R------1 +COLUMNS + C------1 OBJ.FUNC -.800000e+01 R------1 -.100000e+01 + C------2 OBJ.FUNC -.600000e+01 R------1 -.100000e+01 + C------3 OBJ.FUNC -.400000e+01 R------1 -.200000e+01 +RHS + RHS OBJ.FUNC -.900000e+01 + RHS R------1 -.300000e+01 +RANGES +BOUNDS + FX BOUNDS C------2 0.500000e+00 +QUADOBJ + C------1 C------1 0.400000e+01 + C------1 C------2 0.200000e+01 + C------1 C------3 0.200000e+01 + C------2 C------2 0.400000e+01 + C------3 C------3 0.200000e+01 +ENDATA diff --git a/examples/Data/HS51.QPS b/examples/Data/HS51.QPS new file mode 100644 index 0000000000..46416af035 --- /dev/null +++ b/examples/Data/HS51.QPS @@ -0,0 +1,33 @@ +NAME HS51 +ROWS + N OBJ.FUNC + E R------1 + E R------2 + E R------3 +COLUMNS + C------1 R------1 0.100000e+01 + C------2 OBJ.FUNC -.400000e+01 R------1 0.300000e+01 + C------2 R------3 0.100000e+01 + C------3 OBJ.FUNC -.400000e+01 R------2 0.100000e+01 + C------4 OBJ.FUNC -.200000e+01 R------2 0.100000e+01 + C------5 OBJ.FUNC -.200000e+01 R------2 -.200000e+01 + C------5 R------3 -.100000e+01 +RHS + RHS OBJ.FUNC -.600000e+01 + RHS R------1 0.400000e+01 +RANGES +BOUNDS + FR BOUNDS C------1 + FR BOUNDS C------2 + FR BOUNDS C------3 + FR BOUNDS C------4 + FR BOUNDS C------5 +QUADOBJ + C------1 C------1 0.200000e+01 + C------1 C------2 -.200000e+01 + C------2 C------2 0.400000e+01 + C------2 C------3 0.200000e+01 + C------3 C------3 0.200000e+01 + C------4 C------4 0.200000e+01 + C------5 C------5 0.200000e+01 +ENDATA diff --git a/examples/Data/HS52.QPS b/examples/Data/HS52.QPS new file mode 100644 index 0000000000..06add5ac37 --- /dev/null +++ b/examples/Data/HS52.QPS @@ -0,0 +1,32 @@ +NAME HS52 +ROWS + N OBJ.FUNC + E R------1 + E R------2 + E R------3 +COLUMNS + C------1 R------1 0.100000e+01 + C------2 OBJ.FUNC -.400000e+01 R------1 0.300000e+01 + C------2 R------3 0.100000e+01 + C------3 OBJ.FUNC -.400000e+01 R------2 0.100000e+01 + C------4 OBJ.FUNC -.200000e+01 R------2 0.100000e+01 + C------5 OBJ.FUNC -.200000e+01 R------2 -.200000e+01 + C------5 R------3 -.100000e+01 +RHS + RHS OBJ.FUNC -.600000e+01 +RANGES +BOUNDS + FR BOUNDS C------1 + FR BOUNDS C------2 + FR BOUNDS C------3 + FR BOUNDS C------4 + FR BOUNDS C------5 +QUADOBJ + C------1 C------1 0.320000e+02 + C------1 C------2 -.800000e+01 + C------2 C------2 0.400000e+01 + C------2 C------3 0.200000e+01 + C------3 C------3 0.200000e+01 + C------4 C------4 0.200000e+01 + C------5 C------5 0.200000e+01 +ENDATA diff --git a/examples/Data/QPTEST.QPS b/examples/Data/QPTEST.QPS new file mode 100644 index 0000000000..8f4f17f057 --- /dev/null +++ b/examples/Data/QPTEST.QPS @@ -0,0 +1,19 @@ +NAME QP example +ROWS + N obj + G r1 + L r2 +COLUMNS + c1 r1 2.0 r2 -1.0 + c1 obj 1.5 + c2 r1 1.0 r2 2.0 + c2 obj -2.0 +RHS + rhs1 r1 2.0 r2 6.0 +BOUNDS + UP bnd1 c1 20.0 +QUADOBJ + c1 c1 8.0 + c1 c2 2.0 + c2 c2 10.0 +ENDATA diff --git a/gtsam/linear/JacobianFactor.cpp b/gtsam/linear/JacobianFactor.cpp index d617ecc156..06f6b3bed2 100644 --- a/gtsam/linear/JacobianFactor.cpp +++ b/gtsam/linear/JacobianFactor.cpp @@ -102,10 +102,9 @@ JacobianFactor::JacobianFactor(const Key i1, const Matrix& A1, Key i2, } /* ************************************************************************* */ -JacobianFactor::JacobianFactor(const HessianFactor& factor) : - Base(factor), Ab_( - VerticalBlockMatrix::LikeActiveViewOf(factor.info(), - factor.rows())) { +JacobianFactor::JacobianFactor(const HessianFactor& factor) + : Base(factor), + Ab_(VerticalBlockMatrix::LikeActiveViewOf(factor.info(), factor.rows())) { // Copy Hessian into our matrix and then do in-place Cholesky Ab_.full() = factor.info().selfadjointView(); @@ -114,16 +113,19 @@ JacobianFactor::JacobianFactor(const HessianFactor& factor) : bool success; boost::tie(maxrank, success) = choleskyCareful(Ab_.matrix()); - // Check for indefinite system - if (!success) + // Check that Cholesky succeeded OR it managed to factor the full Hessian. + // THe latter case occurs with non-positive definite matrices arising from QP. + if (success || maxrank == factor.rows() - 1) { + // Zero out lower triangle + Ab_.matrix().topRows(maxrank).triangularView() = + Matrix::Zero(maxrank, Ab_.matrix().cols()); + // FIXME: replace with triangular system + Ab_.rowEnd() = maxrank; + model_ = SharedDiagonal(); // is equivalent to Unit::Create(maxrank) + } else { + // indefinite system throw IndeterminantLinearSystemException(factor.keys().front()); - - // Zero out lower triangle - Ab_.matrix().topRows(maxrank).triangularView() = - Matrix::Zero(maxrank, Ab_.matrix().cols()); - // FIXME: replace with triangular system - Ab_.rowEnd() = maxrank; - model_ = SharedDiagonal(); // should be same as Unit::Create(maxrank); + } } /* ************************************************************************* */ diff --git a/gtsam/linear/tests/testJacobianFactor.cpp b/gtsam/linear/tests/testJacobianFactor.cpp index f6ab4be73c..110a1223ac 100644 --- a/gtsam/linear/tests/testJacobianFactor.cpp +++ b/gtsam/linear/tests/testJacobianFactor.cpp @@ -146,7 +146,7 @@ TEST(JacobianFactor, constructors_and_accessors) /* ************************************************************************* */ TEST(JabobianFactor, Hessian_conversion) { - HessianFactor hessian(0, (Matrix(4,4) << + HessianFactor hessian(0, (Matrix(4, 4) << 1.57, 2.695, -1.1, -2.35, 2.695, 11.3125, -0.65, -10.225, -1.1, -0.65, 1, 0.5, @@ -154,7 +154,7 @@ TEST(JabobianFactor, Hessian_conversion) { (Vector(4) << -7.885, -28.5175, 2.75, 25.675).finished(), 73.1725); - JacobianFactor expected(0, (Matrix(2,4) << + JacobianFactor expected(0, (Matrix(2, 4) << 1.2530, 2.1508, -0.8779, -1.8755, 0, 2.5858, 0.4789, -2.3943).finished(), Vector2(-6.2929, -5.7941)); @@ -162,6 +162,27 @@ TEST(JabobianFactor, Hessian_conversion) { EXPECT(assert_equal(expected, JacobianFactor(hessian), 1e-3)); } +/* ************************************************************************* */ +TEST(JabobianFactor, Hessian_conversion2) { + JacobianFactor jf(0, (Matrix(3, 3) << + 1, 2, 3, + 0, 2, 3, + 0, 0, 3).finished(), + Vector3(1, 2, 2)); + HessianFactor hessian(jf); + EXPECT(assert_equal(jf, JacobianFactor(hessian), 1e-9)); +} + +/* ************************************************************************* */ +TEST(JabobianFactor, Hessian_conversion3) { + JacobianFactor jf(0, (Matrix(2, 4) << + 1, 2, 3, 0, + 0, 3, 2, 1).finished(), + Vector2(1, 2)); + HessianFactor hessian(jf); + EXPECT(assert_equal(jf, JacobianFactor(hessian), 1e-9)); +} + /* ************************************************************************* */ namespace simple_graph { diff --git a/gtsam_unstable/linear/QPSParser.cpp b/gtsam_unstable/linear/QPSParser.cpp index a6fc072c7f..3039185f24 100644 --- a/gtsam_unstable/linear/QPSParser.cpp +++ b/gtsam_unstable/linear/QPSParser.cpp @@ -17,56 +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; - RawQP * 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)> 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 &)> addBoundFr; - MPSGrammar(RawQP * 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)), 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)) { + 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 | '_' | '-' | '.']; @@ -74,43 +458,54 @@ 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]; - 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][addBoundFr]; + 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]; + bounds = lexeme[lit("BOUNDS") >> +space >> *((bound | bound_fr) >> 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, - col_double, rhs_single, rhs_double, ranges, bound, bound_fr, bounds, quad, - quad_l, rows, cols, rhs, name, start; + 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; }; 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); @@ -123,4 +518,4 @@ QP QPSParser::Parse() { return rawData.makeQP(); } -} +} // namespace gtsam diff --git a/gtsam_unstable/linear/QPSolver.h b/gtsam_unstable/linear/QPSolver.h index 9efc23a67a..3854d2a159 100644 --- a/gtsam_unstable/linear/QPSolver.h +++ b/gtsam_unstable/linear/QPSolver.h @@ -32,9 +32,14 @@ struct QPPolicy { static constexpr double maxAlpha = 1.0; /// Simply the cost of the QP problem - static const GaussianFactorGraph& buildCostFunction( - const QP& qp, const VectorValues& xk = VectorValues()) { - return qp.cost; + static const GaussianFactorGraph buildCostFunction(const QP& qp, + const VectorValues& xk = VectorValues()) { + GaussianFactorGraph no_constant_factor; + for (auto factor : qp.cost) { + HessianFactor hf = static_cast(*factor); + no_constant_factor.push_back(hf); + } + return no_constant_factor; } }; diff --git a/gtsam_unstable/linear/RawQP.cpp b/gtsam_unstable/linear/RawQP.cpp deleted file mode 100644 index 47672a9473..0000000000 --- a/gtsam_unstable/linear/RawQP.cpp +++ /dev/null @@ -1,271 +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 (!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; - if (debug) { - std::cout << "Added Column for Var: " << var_ << " Row: " << row_ - << " Coefficient: " << coefficient << std::endl; - } - -} - -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::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 - 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() { - KeyVector keys; - std::vector Gs; - std::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) { - Gs.push_back(H[keys[i]][keys[j]]); - } - } - for (Key key1 : keys) { - gs.push_back(-g[key1]); - } - int dual_key_num = keys.size() + 1; - QP madeQP; - auto obj = HessianFactor(keys, Gs, gs, f); - - madeQP.cost.push_back(obj); - - for (auto kv : E) { - std::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) { - std::map keyMatrixMap; - for (auto km : kv.second) { - km.second = -km.second; - keyMatrixMap.insert(km); - } - madeQP.inequalities.push_back( - LinearInequality(keyMatrixMap, -b[kv.first], dual_key_num++)); - } - - for (auto kv : IL) { - std::map keyMatrixMap; - for (auto km : kv.second) { - keyMatrixMap.insert(km); - } - madeQP.inequalities.push_back( - LinearInequality(keyMatrixMap, b[kv.first], dual_key_num++)); - } - - for (Key k : keys) { - if (std::find(Free.begin(), Free.end(), k) != Free.end()) - continue; - 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/RawQP.h deleted file mode 100644 index 70b2a9d278..0000000000 --- a/gtsam_unstable/linear/RawQP.h +++ /dev/null @@ -1,106 +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.h - * @brief - * @author Ivan Dario Jimenez - * @date 3/5/16 - */ - -#pragma once - -#include -#include -#include - -#include -#include -#include -#include -#include -#include - -namespace gtsam { -class RawQP { -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 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; - KeyVector Free; - const bool debug = false; - -public: - RawQP() : - row_to_constraint_v(), E(), IG(), IL(), varNumber(1), b(), g(), varname_to_key(), H(), f(), obj_name(), name_(), up(), lo(), Free() { - } - - void setName( - boost::fusion::vector, std::vector, - std::vector> const & name); - - void addColumn( - boost::fusion::vector, std::vector, - std::vector, std::vector, std::vector, double, - std::vector> const & vars); - - void addColumnDouble( - boost::fusion::vector, std::vector, - std::vector, std::vector, double, std::vector, - std::vector, std::vector, double> const & vars); - - void addRHS( - boost::fusion::vector, std::vector, - std::vector, std::vector, std::vector, double, - std::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); - - void addRow( - boost::fusion::vector, char, std::vector, - std::vector, std::vector> const & vars); - - void addBound( - boost::fusion::vector, std::vector, - std::vector, std::vector, std::vector, - std::vector, std::vector, double> const & vars); - - void addBoundFr( - boost::fusion::vector, std::vector, - std::vector, std::vector, std::vector, - std::vector, std::vector> const & vars); - - void addQuadTerm( - boost::fusion::vector, std::vector, - std::vector, std::vector, std::vector, double, - std::vector> const & vars); - - QP makeQP(); -} -; -} diff --git a/gtsam_unstable/linear/tests/testQPSolver.cpp b/gtsam_unstable/linear/tests/testQPSolver.cpp index bc9dd1f98b..2292c63d78 100644 --- a/gtsam_unstable/linear/tests/testQPSolver.cpp +++ b/gtsam_unstable/linear/tests/testQPSolver.cpp @@ -14,6 +14,7 @@ * @brief Test simple QP solver for a linear inequality constraint * @date Apr 10, 2014 * @author Duy-Nguyen Ta + * @author Ivan Dario Jimenez */ #include @@ -218,7 +219,7 @@ pair testParser(QPSParser parser) { // min f(x,y) = 4 + 1.5x -y + 0.58x^2 + 2xy + 2yx + 10y^2 expectedqp.cost.push_back( HessianFactor(X1, X2, 8.0 * I_1x1, 2.0 * I_1x1, -1.5 * kOne, 10.0 * I_1x1, - 2.0 * kOne, 4.0)); + 2.0 * kOne, 8.0)); // 2x + y >= 2 // -x + 2y <= 6 expectedqp.inequalities.push_back( @@ -269,6 +270,66 @@ TEST(QPSolver, QPExampleTest){ CHECK(assert_equal(error_expected, error_actual)) } +TEST(QPSolver, HS21) { + QP problem = QPSParser("HS21.QPS").Parse(); + VectorValues actualSolution; + VectorValues expectedSolution; + expectedSolution.insert(Symbol('X',1), 2.0*I_1x1); + expectedSolution.insert(Symbol('X',2), 0.0*I_1x1); + boost::tie(actualSolution, boost::tuples::ignore) = QPSolver(problem).optimize(); + double error_actual = problem.cost.error(actualSolution); + CHECK(assert_equal(-99.9599999, error_actual, 1e-7)) + CHECK(assert_equal(expectedSolution, actualSolution)) +} + +TEST(QPSolver, HS35) { + QP problem = QPSParser("HS35.QPS").Parse(); + VectorValues actualSolution; + boost::tie(actualSolution, boost::tuples::ignore) = QPSolver(problem).optimize(); + double error_actual = problem.cost.error(actualSolution); + CHECK(assert_equal(1.11111111e-01,error_actual, 1e-7)) +} + +TEST(QPSolver, HS35MOD) { + QP problem = QPSParser("HS35MOD.QPS").Parse(); + VectorValues actualSolution; + boost::tie(actualSolution, boost::tuples::ignore) = QPSolver(problem).optimize(); + double error_actual = problem.cost.error(actualSolution); + CHECK(assert_equal(2.50000001e-01,error_actual, 1e-7)) +} + +TEST(QPSolver, HS51) { + QP problem = QPSParser("HS51.QPS").Parse(); + VectorValues actualSolution; + boost::tie(actualSolution, boost::tuples::ignore) = QPSolver(problem).optimize(); + double error_actual = problem.cost.error(actualSolution); + CHECK(assert_equal(8.88178420e-16,error_actual, 1e-7)) +} + +TEST(QPSolver, HS52) { + QP problem = QPSParser("HS52.QPS").Parse(); + VectorValues actualSolution; + boost::tie(actualSolution, boost::tuples::ignore) = QPSolver(problem).optimize(); + double error_actual = problem.cost.error(actualSolution); + CHECK(assert_equal(5.32664756,error_actual, 1e-7)) +} + +TEST(QPSolver, HS268) { // This test needs an extra order of magnitude of tolerance than the rest + QP problem = QPSParser("HS268.QPS").Parse(); + VectorValues actualSolution; + boost::tie(actualSolution, boost::tuples::ignore) = QPSolver(problem).optimize(); + double error_actual = problem.cost.error(actualSolution); + CHECK(assert_equal(5.73107049e-07,error_actual, 1e-6)) +} + +TEST(QPSolver, QPTEST) { // REQUIRES Jacobian Fix + QP problem = QPSParser("QPTEST.QPS").Parse(); + VectorValues actualSolution; + boost::tie(actualSolution, boost::tuples::ignore) = QPSolver(problem).optimize(); + double error_actual = problem.cost.error(actualSolution); + CHECK(assert_equal(0.437187500e01,error_actual, 1e-7)) +} + /* ************************************************************************* */ // Create Matlab's test graph as in http://www.mathworks.com/help/optim/ug/quadprog.html QP createTestMatlabQPEx() {