From 23a1382bb22da3207d2c5438ef34d0400408343d Mon Sep 17 00:00:00 2001 From: ivan Date: Mon, 2 May 2016 19:54:58 -0400 Subject: [PATCH] [QP/LP] LP Added Zeros Error Fixed. [QP] Now Checks for syntactic Equality by comparing each factor imported . [QP] Now Checks for semantic Equality by ensuring each imported QP gives the same solution. --- gtsam/nonlinear/NonlinearFactor.h | 3 +- gtsam_unstable/linear/LPSolver.cpp | 37 ++++++------- gtsam_unstable/linear/LPSolver.h | 2 +- gtsam_unstable/linear/QPSParser.cpp | 2 +- gtsam_unstable/linear/tests/testLPSolver.cpp | 55 +++++++++++--------- gtsam_unstable/linear/tests/testQPSolver.cpp | 42 +++++++-------- 6 files changed, 68 insertions(+), 73 deletions(-) diff --git a/gtsam/nonlinear/NonlinearFactor.h b/gtsam/nonlinear/NonlinearFactor.h index 505f58394..dd4c9123a 100644 --- a/gtsam/nonlinear/NonlinearFactor.h +++ b/gtsam/nonlinear/NonlinearFactor.h @@ -131,7 +131,8 @@ public: } /** - * Creates a shared_ptr clone of the factor with different keys using + * Creates a shared_ptr clone of the + * factor with different keys using * a map from old->new keys */ shared_ptr rekey(const std::map& rekey_mapping) const; diff --git a/gtsam_unstable/linear/LPSolver.cpp b/gtsam_unstable/linear/LPSolver.cpp index 92075e416..469555690 100644 --- a/gtsam_unstable/linear/LPSolver.cpp +++ b/gtsam_unstable/linear/LPSolver.cpp @@ -13,7 +13,7 @@ namespace gtsam { LPSolver::LPSolver(const LP &lp) : - lp_(lp) { + lp_(lp), addedZeroPriorsIndex_(){ // Push back factors that are the same in every iteration to the base graph. // Those include the equality constraints and zero priors for keys that are // not in the cost @@ -27,7 +27,11 @@ LPSolver::LPSolver(const LP &lp) : // Create and push zero priors of constrained variables that do not exist in // the cost function - // baseGraph_.push_back(*createZeroPriors(lp_.cost.keys(), keysDim_)); + auto addedZeroPriorsGraph = *createZeroPriors(lp_.cost.keys(), keysDim_); + //before we add them we fill an array with the indeces of the added zero priors + addedZeroPriorsIndex_.resize(addedZeroPriorsGraph.size()); + std::iota(addedZeroPriorsIndex_.begin(), addedZeroPriorsIndex_.end(), baseGraph_.size()); + baseGraph_.push_back(addedZeroPriorsGraph); // Variable index equalityVariableIndex_ = VariableIndex(lp_.equalities); @@ -54,8 +58,6 @@ 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)) { @@ -67,8 +69,6 @@ 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!! @@ -112,44 +112,43 @@ 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()); - //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())); - - // for vars that are not in the cost, the cost gradient is zero (g=0), so b=xk +// add the corresponding factors for all variables that are not explicitly in the cost + //function +// for vars that are not in the cost, the cost gradient is zero (g=0), so b=xk 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())); + 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; } VectorValues LPSolver::solveWithCurrentWorkingSet(const VectorValues &xk, const InequalityFactorGraph &workingSet) const { GaussianFactorGraph workingGraph = baseGraph_; // || X - Xk + g ||^2 +// We remove the old zero priors from the base graph we are going to use to solve + //This iteration's problem + for (size_t index : addedZeroPriorsIndex_) { + workingGraph.remove(index); + } + 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(); } @@ -209,8 +208,6 @@ std::pair LPSolver::optimize( /// main loop of the solver while (!state.converged) { - // if(state.iterations > 100) // Temporary break to avoid infine loops - // break; state = iterate(state); } return make_pair(state.values, state.duals); diff --git a/gtsam_unstable/linear/LPSolver.h b/gtsam_unstable/linear/LPSolver.h index 1d09af5ff..d260e1409 100644 --- a/gtsam_unstable/linear/LPSolver.h +++ b/gtsam_unstable/linear/LPSolver.h @@ -23,7 +23,7 @@ typedef std::map KeyDimMap; class LPSolver: public ActiveSetSolver { const LP &lp_; //!< the linear programming problem KeyDimMap keysDim_; //!< key-dim map of all variables in the constraints, used to create zero priors - + std::vector addedZeroPriorsIndex_; public: /// Constructor LPSolver(const LP &lp); diff --git a/gtsam_unstable/linear/QPSParser.cpp b/gtsam_unstable/linear/QPSParser.cpp index 331081126..5ab0f8728 100644 --- a/gtsam_unstable/linear/QPSParser.cpp +++ b/gtsam_unstable/linear/QPSParser.cpp @@ -137,7 +137,7 @@ QP QPSParser::Parse() { if (!parse(begin, last, MPSGrammar(&rawData)) || begin != last) { throw QPSParserException(); } else { - std::cout << "Parse Successful." << std::endl; +// std::cout << "Parse Successful." << std::endl; } return rawData.makeQP(); diff --git a/gtsam_unstable/linear/tests/testLPSolver.cpp b/gtsam_unstable/linear/tests/testLPSolver.cpp index 8ef5a5a14..8839ed6c7 100644 --- a/gtsam_unstable/linear/tests/testLPSolver.cpp +++ b/gtsam_unstable/linear/tests/testLPSolver.cpp @@ -56,36 +56,11 @@ LP simpleLP1() { /* ************************************************************************* */ 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(-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 @@ -99,6 +74,34 @@ TEST(LPInitSolverMatlab, infinite_loop_single_var) { expected.insert(1, Vector3(13.5, 6.5, -6.5)); CHECK(assert_equal(results, expected, 1e-7)); } + +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.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, initialization) { LP lp = simpleLP1(); LPSolver lpSolver(lp); diff --git a/gtsam_unstable/linear/tests/testQPSolver.cpp b/gtsam_unstable/linear/tests/testQPSolver.cpp index 780262615..4396a8541 100644 --- a/gtsam_unstable/linear/tests/testQPSolver.cpp +++ b/gtsam_unstable/linear/tests/testQPSolver.cpp @@ -211,48 +211,42 @@ TEST(QPSolver, optimizeForst10book_pg171Ex5) { expectedSolution.insert(X(2), (Vector(1) << 0.5).finished()); CHECK(assert_equal(expectedSolution, solution, 1e-100)); } - pair testParser(QPSParser parser) { QP exampleqp = parser.Parse(); -// QP expectedqp = createExampleQP(); QP expectedqp; + Key X1(Symbol('X',1)), X2(Symbol('X',2)); // 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), + HessianFactor(X1, X2, 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)); + LinearInequality(X2, -ones(1, 1), X1, -2.0 * ones(1, 1), -2, 0)); + expectedqp.inequalities.push_back( + LinearInequality(X2, 2.0 * ones(1, 1), X1, -ones(1, 1), 6, 1)); // x<= 20 - expectedqp.inequalities.push_back(LinearInequality(X(1), ones(1, 1), 20, 4)); + expectedqp.inequalities.push_back(LinearInequality(X1, ones(1, 1), 20, 4)); + //x >= 0 + expectedqp.inequalities.push_back(LinearInequality(X1, -ones(1, 1), 0, 2)); + // y > = 0 + expectedqp.inequalities.push_back(LinearInequality(X2, -ones(1, 1), 0, 3)); 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)); }; -TEST(QPSolver, QPExampleData) { +TEST(QPSolver, ParserSyntaticTest){ + auto expectedActual = testParser(QPSParser("QPExample.QPS")); + CHECK(assert_equal(expectedActual.first.cost, expectedActual.second.cost, 1e-7)); + CHECK(assert_equal(expectedActual.first.inequalities, expectedActual.second.inequalities, 1e-7)); + CHECK(assert_equal(expectedActual.first.equalities, expectedActual.second.equalities, 1e-7)); +} +TEST(QPSolver, ParserSemanticTest) { 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")); + CHECK(assert_equal(actualSolution, expectedSolution, 1e-7)); } /* ************************************************************************* */