From 3fc9d9870786bf4ef0d72b26373a4c728f680c75 Mon Sep 17 00:00:00 2001 From: ivan Date: Mon, 25 Apr 2016 19:00:22 -0400 Subject: [PATCH] [QP/LP] Error Identified and test sample test case generated but fails. --- gtsam_unstable/linear/LPSolver.cpp | 33 ++- gtsam_unstable/linear/RawQP.cpp | 253 ++++++++++++++++++- gtsam_unstable/linear/RawQP.h | 224 +++------------- gtsam_unstable/linear/tests/testLPSolver.cpp | 51 +++- gtsam_unstable/linear/tests/testQPSolver.cpp | 21 +- 5 files changed, 368 insertions(+), 214 deletions(-) diff --git a/gtsam_unstable/linear/LPSolver.cpp b/gtsam_unstable/linear/LPSolver.cpp index e7d7ac08d..c57291966 100644 --- a/gtsam_unstable/linear/LPSolver.cpp +++ b/gtsam_unstable/linear/LPSolver.cpp @@ -54,7 +54,8 @@ LPState LPSolver::iterate(const LPState &state) const { // to find the direction to move VectorValues newValues = solveWithCurrentWorkingSet(state.values, state.workingSet); - +// GTSAM_PRINT(newValues); +// GTSAM_PRINT(state.values); // If we CAN'T move further // LP: projection on the constraints' nullspace is zero: we are at a vertex if (newValues.equals(state.values, 1e-7)) { @@ -66,6 +67,8 @@ LPState LPSolver::iterate(const LPState &state) const { GaussianFactorGraph::shared_ptr dualGraph = buildDualGraph(state.workingSet, newValues); VectorValues duals = dualGraph->optimize(); +// GTSAM_PRINT(*dualGraph); +// GTSAM_PRINT(duals); // LP: see which inequality constraint has wrong pulling direction, i.e., dual < 0 int leavingFactor = identifyLeavingConstraint(state.workingSet, duals); // If all inequality constraints are satisfied: We have the solution!! @@ -92,6 +95,7 @@ LPState LPSolver::iterate(const LPState &state) const { double alpha; int factorIx; VectorValues p = newValues - state.values; +// GTSAM_PRINT(p); boost::tie(alpha, factorIx) = // using 16.41 computeStepSize(state.workingSet, state.values, p); // also add to the working set the one that complains the most @@ -108,14 +112,31 @@ LPState LPSolver::iterate(const LPState &state) const { GaussianFactorGraph::shared_ptr LPSolver::createLeastSquareFactors( const LinearCost &cost, const VectorValues &xk) const { GaussianFactorGraph::shared_ptr graph(new GaussianFactorGraph()); - KeyVector keys = cost.keys(); + //Something in this function breaks when working with funcions that do not include all + //Variables in the cost function. When adding the missing variables as shown we still don't converge + //to the right answer as we would expect from the iterations. +// GTSAM_PRINT(xk); +// GTSAM_PRINT(cost); for (LinearCost::const_iterator it = cost.begin(); it != cost.end(); ++it) { size_t dim = cost.getDim(it); Vector b = xk.at(*it) - cost.getA(it).transpose(); // b = xk-g graph->push_back(JacobianFactor(*it, eye(dim), b)); } + KeySet allKeys = lp_.inequalities.keys(); + allKeys.merge(lp_.equalities.keys()); + allKeys.merge(KeySet(lp_.cost.keys())); + if (cost.keys().size() != allKeys.size()) { + KeySet difference; + std::set_difference(allKeys.begin(), allKeys.end(), lp_.cost.begin(), + lp_.cost.end(), std::inserter(difference, difference.end())); + for (Key k : difference) { + graph->push_back(JacobianFactor(k, eye(keysDim_.at(k)), xk.at(k))); + } + } + +// GTSAM_PRINT(*graph); return graph; } @@ -123,11 +144,11 @@ VectorValues LPSolver::solveWithCurrentWorkingSet(const VectorValues &xk, const InequalityFactorGraph &workingSet) const { GaussianFactorGraph workingGraph = baseGraph_; // || X - Xk + g ||^2 workingGraph.push_back(*createLeastSquareFactors(lp_.cost, xk)); - for (const LinearInequality::shared_ptr &factor : workingSet) { if (factor->active()) workingGraph.push_back(factor); } +// GTSAM_PRINT(workingGraph); return workingGraph.optimize(); } @@ -186,9 +207,11 @@ std::pair LPSolver::optimize( LPState state(initialValues, duals, workingSet, false, 0); /// main loop of the solver - while (!state.converged) + while (!state.converged) { + if(state.iterations > 10000) // Temporary break to avoid infine loops + break; state = iterate(state); - + } return make_pair(state.values, state.duals); } } diff --git a/gtsam_unstable/linear/RawQP.cpp b/gtsam_unstable/linear/RawQP.cpp index 334ecc177..069b0328d 100644 --- a/gtsam_unstable/linear/RawQP.cpp +++ b/gtsam_unstable/linear/RawQP.cpp @@ -1,2 +1,253 @@ - #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) * ones(1, 1); + + 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) * ones(1, 1); + Matrix11 coefficient2 = at_c < 8 > (vars) * ones(1, 1); + 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) * ones(1, 1); + + 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) { + 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::vector < std::pair > KeyMatPair; + for (auto km : kv.second) { + KeyMatPair.push_back(km); + } + madeQP.equalities.push_back( + LinearEquality(KeyMatPair, b[kv.first] * ones(1, 1), dual_key_num++)); + } + + for (auto kv : IG) { + std::vector < std::pair > KeyMatPair; + for (auto km : kv.second) { + km.second = -km.second; + KeyMatPair.push_back(km); + } + madeQP.inequalities.push_back( + LinearInequality(KeyMatPair, -b[kv.first], dual_key_num++)); + } + + for (auto kv : IL) { + std::vector < std::pair > KeyMatPair; + for (auto km : kv.second) { + KeyMatPair.push_back(km); + } + madeQP.inequalities.push_back( + LinearInequality(KeyMatPair, 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, ones(1, 1), up[k], dual_key_num++)); + if (lo.count(k) == 1) + madeQP.inequalities.push_back( + LinearInequality(k, -ones(1, 1), lo[k], dual_key_num++)); + else + madeQP.inequalities.push_back( + LinearInequality(k, -ones(1, 1), 0, dual_key_num++)); + } + return madeQP; +} +} + diff --git a/gtsam_unstable/linear/RawQP.h b/gtsam_unstable/linear/RawQP.h index aca2fa6a8..2e436e2a8 100644 --- a/gtsam_unstable/linear/RawQP.h +++ b/gtsam_unstable/linear/RawQP.h @@ -4,250 +4,86 @@ #include #include -#include #include #include -#include #include #include -#include #include namespace gtsam { class RawQP { - typedef std::vector > coefficient_v; +private: + typedef std::unordered_map coefficient_v; + typedef std::unordered_map constraint_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; + 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; + std::vector Free; const bool debug = true; + public: RawQP() : - g(), row_to_map(), varname_to_key(), IL(), IG(), E(), b(), H(), varNumber( - 1) { + 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) { + std::vector> const & name); - name_ = std::string(boost::fusion::at_c < 1 > (name).begin(), - boost::fusion::at_c < 1 > (name).end()); - if (debug) { - std::cout << "Parsing file: " << name_ << std::endl; - } - } - -// void addColumn(std::vector var, std::vector row, -// double coefficient) { void addColumn( boost::fusion::vector, std::vector, std::vector, std::vector, std::vector, double, - std::vector> const & vars) { - std::string var_(boost::fusion::at_c < 1 > (vars).begin(), - boost::fusion::at_c < 1 > (vars).end()), row_( - boost::fusion::at_c < 3 > (vars).begin(), - boost::fusion::at_c < 3 > (vars).end()); - double coefficient = boost::fusion::at_c < 5 > (vars); - if (!varname_to_key.count(var_)) - varname_to_key.insert( { var_, Symbol('X', varNumber++) }); + std::vector> const & vars); - 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) { void addColumnDouble( boost::fusion::vector, std::vector, std::vector, std::vector, double, std::vector, - std::vector, std::vector, double> const & vars) { - std::string var_(boost::fusion::at_c < 0 > (vars).begin(), - boost::fusion::at_c < 0 > (vars).end()), row1_( - boost::fusion::at_c < 2 > (vars).begin(), - boost::fusion::at_c < 2 > (vars).end()), row2_( - boost::fusion::at_c < 6 > (vars).begin(), - boost::fusion::at_c < 6 > (vars).end()); - double coefficient1 = boost::fusion::at_c < 4 > (vars); - double coefficient2 = boost::fusion::at_c < 8 > (vars); - 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; - } - } + 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) { - std::string var_(boost::fusion::at_c < 1 > (vars).begin(), - boost::fusion::at_c < 1 > (vars).end()), row_( - boost::fusion::at_c < 3 > (vars).begin(), - boost::fusion::at_c < 3 > (vars).end()); - double coefficient = boost::fusion::at_c < 5 > (vars); - if (row_ == obj_name) - f = -coefficient; - else - b.insert( { row_, coefficient }); + std::vector> const & vars); - 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) { void 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_(boost::fusion::at_c < 1 > (vars).begin(), - boost::fusion::at_c < 1 > (vars).end()), row1_( - boost::fusion::at_c < 3 > (vars).begin(), - boost::fusion::at_c < 3 > (vars).end()), row2_( - boost::fusion::at_c < 7 > (vars).begin(), - boost::fusion::at_c < 7 > (vars).end()); - double coefficient1 = boost::fusion::at_c < 5 > (vars); - double coefficient2 = boost::fusion::at_c < 9 > (vars); - 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; - } - } + std::vector, std::vector, std::vector, double> const & vars); void addRow( boost::fusion::vector, char, std::vector, - std::vector, std::vector> const & vars) { - std::string name_(boost::fusion::at_c < 3 > (vars).begin(), - boost::fusion::at_c < 3 > (vars).end()); - char type = boost::fusion::at_c < 1 > (vars); - 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; - } - } + 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) { - std::string type_(boost::fusion::at_c < 1 > (vars).begin(), - boost::fusion::at_c < 1 > (vars).end()), var_( - boost::fusion::at_c < 5 > (vars).begin(), - boost::fusion::at_c < 5 > (vars).end()); - double number = boost::fusion::at_c < 7 > (vars); + std::vector, std::vector, double> const & vars); - if (debug) { - std::cout << "Added Bound Type: " << type_ << " Var: " << var_ - << " Amount: " << number << std::endl; - } - } - -// void addBound(std::vector type, std::vector var) { void addBoundFr( boost::fusion::vector, std::vector, std::vector, std::vector, std::vector, - std::vector, std::vector> const & vars) { - std::string type_(boost::fusion::at_c < 1 > (vars).begin(), - boost::fusion::at_c < 1 > (vars).end()), var_( - boost::fusion::at_c < 5 > (vars).begin(), - boost::fusion::at_c < 5 > (vars).end()); - if (debug) { - std::cout << "Added Free Bound Type: " << type_ << " Var: " << var_ - << " Amount: " << std::endl; - } - } + std::vector, std::vector> const & vars); -// void addQuadTerm(std::vector var1, std::vector var2, -// double coefficient) { void addQuadTerm( boost::fusion::vector, std::vector, std::vector, std::vector, std::vector, double, - std::vector> const & vars) { - std::string var1_(boost::fusion::at_c < 1 > (vars).begin(), - boost::fusion::at_c < 1 > (vars).end()), var2_( - boost::fusion::at_c < 3 > (vars).begin(), - boost::fusion::at_c < 3 > (vars).end()); - double coefficient = boost::fusion::at_c < 5 > (vars); - if (debug) { - std::cout << "Added QuadTerm for Var: " << var1_ << " Row: " << var2_ - << " Coefficient: " << coefficient << std::endl; - } - } + std::vector> const & vars); - QP makeQP() { - return QP(); - } -}; + QP makeQP(); +} +; } diff --git a/gtsam_unstable/linear/tests/testLPSolver.cpp b/gtsam_unstable/linear/tests/testLPSolver.cpp index fc40f5a36..8ef5a5a14 100644 --- a/gtsam_unstable/linear/tests/testLPSolver.cpp +++ b/gtsam_unstable/linear/tests/testLPSolver.cpp @@ -54,15 +54,51 @@ LP simpleLP1() { return lp; } -LP infeasibleLP() { - LP lp; - - lp.cost = LinearCost(1, Vector3(-1, -1, -2)); - -} - /* ************************************************************************* */ namespace gtsam { +TEST(LPInitSolverMatlab, infinite_loop_multi_var) { + LP initchecker; + Key X = symbol('X',1); + Key Y = symbol('Y',1); + Key Z = symbol('Z',1); + initchecker.cost = LinearCost(Z, ones(1)); //min alpha +// initchecker.cost = LinearCost(Z, ones(1), X, zero(1), Y, zero(1)); //min alpha + initchecker.inequalities.push_back(LinearInequality(X,-2.0*ones(1), Y, -1.0*ones(1), Z, -1.0*ones(1),-2,1));//-2x-y-alpha <= -2 + initchecker.inequalities.push_back(LinearInequality(X, -1.0*ones(1), Y, 2.0*ones(1), Z, -1.0*ones(1), 6, 2));// -x+2y-alpha <= 6 + initchecker.inequalities.push_back(LinearInequality(X, -1.0*ones(1), Z, -1.0*ones(1), 0,3));// -x - alpha <= 0 + initchecker.inequalities.push_back(LinearInequality(X, 1.0*ones(1), Z, -1.0*ones(1), 20, 4));//x - alpha <= 20 + initchecker.inequalities.push_back(LinearInequality(Y, -1.0*ones(1), Z, -1.0*ones(1), 0, 5));// -y - alpha <= 0 + LPSolver solver(initchecker); + VectorValues starter; + starter.insert(X, zero(1)); + starter.insert(Y, zero(1)); + starter.insert(Z, 2*ones(1)); + VectorValues results, duals; + boost::tie(results, duals) = solver.optimize(starter); + VectorValues expected; + expected.insert(X, Vector::Constant(1, 13.5)); + expected.insert(Y, Vector::Constant(1,6.5)); + expected.insert(Z, Vector::Constant(1,-6.5)); + CHECK(assert_equal(results, expected, 1e-7)); +} + +TEST(LPInitSolverMatlab, infinite_loop_single_var) { + LP initchecker; + initchecker.cost = LinearCost(1,Vector3(0,0,1)); //min alpha + initchecker.inequalities.push_back(LinearInequality(1, Vector3(-2,-1,-1),-2,1));//-2x-y-alpha <= -2 + initchecker.inequalities.push_back(LinearInequality(1, Vector3(-1,2,-1), 6, 2));// -x+2y-alpha <= 6 + initchecker.inequalities.push_back(LinearInequality(1, Vector3(-1,0,-1), 0,3));// -x - alpha <= 0 + initchecker.inequalities.push_back(LinearInequality(1, Vector3(1,0,-1), 20, 4));//x - alpha <= 20 + initchecker.inequalities.push_back(LinearInequality(1, Vector3(0,-1,-1),0, 5));// -y - alpha <= 0 + LPSolver solver(initchecker); + VectorValues starter; + starter.insert(1,Vector3(0,0,2)); + VectorValues results, duals; + boost::tie(results, duals) = solver.optimize(starter); + VectorValues expected; + expected.insert(1, Vector3(13.5, 6.5, -6.5)); + CHECK(assert_equal(results, expected, 1e-7)); +} TEST(LPInitSolverMatlab, initialization) { LP lp = simpleLP1(); LPSolver lpSolver(lp); @@ -93,7 +129,6 @@ TEST(LPInitSolverMatlab, initialization) { expectedInitLP.inequalities.push_back( LinearInequality(1, Vector2( -1, 1), 2, Vector::Constant(1, -1), 1, 5));// -x1 + x2 - y <= 1 CHECK(assert_equal(expectedInitLP, *initLP, 1e-10)); - LPSolver lpSolveInit(*initLP); VectorValues xy0(x0); xy0.insert(yKey, Vector::Constant(1, y0)); diff --git a/gtsam_unstable/linear/tests/testQPSolver.cpp b/gtsam_unstable/linear/tests/testQPSolver.cpp index 1b0e6cdb8..c15ccb48c 100644 --- a/gtsam_unstable/linear/tests/testQPSolver.cpp +++ b/gtsam_unstable/linear/tests/testQPSolver.cpp @@ -213,9 +213,8 @@ TEST(QPSolver, optimizeForst10book_pg171Ex5) { CHECK(assert_equal(expectedSolution, solution, 1e-100)); } -void testParser(QPSParser parser) { +pair testParser(QPSParser parser) { QP exampleqp = parser.Parse(); - // QP expectedqp = createExampleQP(); QP expectedqp; // min f(x,y) = 4 + 1.5x -y + 0.58x^2 + 2xy + 2yx + 10y^2 @@ -234,7 +233,8 @@ void testParser(QPSParser parser) { 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)); - + return std::make_pair(expectedqp, exampleqp); +// CHECK(assert_equal(expectedqp.cost, exampleqp.cost, 1e-7)); // CHECK(expectedqp.cost.equals(exampleqp.cost, 1e-7)); // CHECK(expectedqp.inequalities.equals(exampleqp.inequalities, 1e-7)); // CHECK(expectedqp.equalities.equals(exampleqp.equalities, 1e-7)); @@ -242,9 +242,18 @@ void testParser(QPSParser parser) { TEST(QPSolver, QPExampleData) { - testParser(QPSParser("QPExample.QPS")); - testParser(QPSParser("AUG2D.QPS")); - testParser(QPSParser("CONT-050.QPS")); + auto expected_actual = testParser(QPSParser("QPExample.QPS")); + VectorValues actualSolution, expectedSolution; + expected_actual.first.print("EXPECTED GRAPH:"); + expected_actual.second.print("ACTUAL GRAPH"); + boost::tie(expectedSolution, boost::tuples::ignore) = QPSolver(expected_actual.first).optimize(); + std::cout << "Expected Execution Works" << std::endl; + boost::tie(actualSolution, boost::tuples::ignore) = QPSolver(expected_actual.second).optimize(); + std::cout << "Actual Execution Works" << std::endl; + + CHECK(assert_equal(actualSolution, expectedSolution, 1e-7)); +// testParser(QPSParser("AUG2D.QPS")); +// testParser(QPSParser("CONT-050.QPS")); } /* ************************************************************************* */