diff --git a/gtsam_unstable/linear/LPInitSolver.h b/gtsam_unstable/linear/LPInitSolver.h index 7978c0369..91ad3573e 100644 --- a/gtsam_unstable/linear/LPInitSolver.h +++ b/gtsam_unstable/linear/LPInitSolver.h @@ -1,5 +1,7 @@ #pragma once +#include "QPSolver.h" + namespace gtsam { /** * Abstract class to solve for an initial value of an LP problem @@ -13,6 +15,7 @@ public: LPInitSolver(const LPSolver& lpSolver) : lp_(lpSolver.lp()), lpSolver_(lpSolver) { } + virtual ~LPInitSolver() { } ; diff --git a/gtsam_unstable/linear/LPInitSolverMatlab.h b/gtsam_unstable/linear/LPInitSolverMatlab.h index 3e15f355f..3ddff32ed 100644 --- a/gtsam_unstable/linear/LPInitSolverMatlab.h +++ b/gtsam_unstable/linear/LPInitSolverMatlab.h @@ -9,6 +9,7 @@ #include #include +#include #include namespace gtsam { diff --git a/gtsam_unstable/linear/QPSolver.cpp b/gtsam_unstable/linear/QPSolver.cpp index cf391f10c..85cbb69e7 100644 --- a/gtsam_unstable/linear/QPSolver.cpp +++ b/gtsam_unstable/linear/QPSolver.cpp @@ -22,13 +22,15 @@ #include #include #include +#include "LPInitSolverMatlab.h" using namespace std; namespace gtsam { //****************************************************************************** -QPSolver::QPSolver(const QP& qp) : qp_(qp) { +QPSolver::QPSolver(const QP& qp) : + qp_(qp) { baseGraph_ = qp_.cost; baseGraph_.push_back(qp_.equalities.begin(), qp_.equalities.end()); costVariableIndex_ = VariableIndex(qp_.cost); @@ -43,37 +45,35 @@ VectorValues QPSolver::solveWithCurrentWorkingSet( const InequalityFactorGraph& workingSet) const { GaussianFactorGraph workingGraph = baseGraph_; for (const LinearInequality::shared_ptr& factor : workingSet) { - if (factor->active()) workingGraph.push_back(factor); + if (factor->active()) + workingGraph.push_back(factor); } return workingGraph.optimize(); } //****************************************************************************** -JacobianFactor::shared_ptr QPSolver::createDualFactor( - Key key, const InequalityFactorGraph& workingSet, - const VectorValues& delta) const { +JacobianFactor::shared_ptr QPSolver::createDualFactor(Key key, + const InequalityFactorGraph& workingSet, const VectorValues& delta) const { // Transpose the A matrix of constrained factors to have the jacobian of the // dual key - std::vector > Aterms = - collectDualJacobians(key, qp_.equalities, - equalityVariableIndex_); - std::vector > AtermsInequalities = - collectDualJacobians(key, workingSet, - inequalityVariableIndex_); + std::vector < std::pair > Aterms = collectDualJacobians + < LinearEquality > (key, qp_.equalities, equalityVariableIndex_); + std::vector < std::pair > AtermsInequalities = + collectDualJacobians < LinearInequality + > (key, workingSet, inequalityVariableIndex_); Aterms.insert(Aterms.end(), AtermsInequalities.begin(), - AtermsInequalities.end()); + AtermsInequalities.end()); // Collect the gradients of unconstrained cost factors to the b vector if (Aterms.size() > 0) { Vector b = zero(delta.at(key).size()); if (costVariableIndex_.find(key) != costVariableIndex_.end()) { - for (size_t factorIx: costVariableIndex_[key]) { + for (size_t factorIx : costVariableIndex_[key]) { GaussianFactor::shared_ptr factor = qp_.cost.at(factorIx); b += factor->gradient(key, delta); } } - return boost::make_shared( - Aterms, b); // compute the least-square approximation of dual variables + return boost::make_shared < JacobianFactor > (Aterms, b); // compute the least-square approximation of dual variables } else { return boost::make_shared(); } @@ -97,20 +97,20 @@ QPState QPSolver::iterate(const QPState& state) const { // update is zero. if (newValues.equals(state.values, 1e-7)) { // Compute lambda from the dual graph - GaussianFactorGraph::shared_ptr dualGraph = - buildDualGraph(state.workingSet, newValues); + GaussianFactorGraph::shared_ptr dualGraph = buildDualGraph(state.workingSet, + newValues); VectorValues duals = dualGraph->optimize(); int leavingFactor = identifyLeavingConstraint(state.workingSet, duals); // If all inequality constraints are satisfied: We have the solution!! if (leavingFactor < 0) { return QPState(newValues, duals, state.workingSet, true, - state.iterations + 1); + state.iterations + 1); } else { // Inactivate the leaving constraint InequalityFactorGraph newWorkingSet = state.workingSet; newWorkingSet.at(leavingFactor)->inactivate(); return QPState(newValues, duals, newWorkingSet, false, - state.iterations + 1); + state.iterations + 1); } } else { // If we CAN make some progress, i.e. p_k != 0 @@ -118,15 +118,16 @@ QPState QPSolver::iterate(const QPState& state) const { double alpha; int factorIx; VectorValues p = newValues - state.values; - boost::tie(alpha, factorIx) = // using 16.41 + 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 InequalityFactorGraph newWorkingSet = state.workingSet; - if (factorIx >= 0) newWorkingSet.at(factorIx)->activate(); + if (factorIx >= 0) + newWorkingSet.at(factorIx)->activate(); // step! newValues = state.values + alpha * p; return QPState(newValues, state.duals, newWorkingSet, false, - state.iterations + 1); + state.iterations + 1); } } @@ -136,7 +137,7 @@ InequalityFactorGraph QPSolver::identifyActiveConstraints( const VectorValues& initialValues, const VectorValues& duals, bool useWarmStart) const { InequalityFactorGraph workingSet; - for (const LinearInequality::shared_ptr& factor: inequalities) { + for (const LinearInequality::shared_ptr& factor : inequalities) { LinearInequality::shared_ptr workingFactor(new LinearInequality(*factor)); if (useWarmStart == true && duals.exists(workingFactor->dualKey())) { workingFactor->activate(); @@ -148,7 +149,8 @@ InequalityFactorGraph QPSolver::identifyActiveConstraints( // TODO: find a feasible initial point for QPSolver. // For now, we just throw an exception, since we don't have an LPSolver // to do this yet - if (error > 0) throw InfeasibleInitialValues(); + if (error > 0) + throw InfeasibleInitialValues(); if (fabs(error) < 1e-7) { workingFactor->activate(); @@ -167,8 +169,8 @@ pair QPSolver::optimize( const VectorValues& initialValues, const VectorValues& duals, bool useWarmStart) const { // Initialize workingSet from the feasible initialValues - InequalityFactorGraph workingSet = identifyActiveConstraints( - qp_.inequalities, initialValues, duals, useWarmStart); + InequalityFactorGraph workingSet = identifyActiveConstraints(qp_.inequalities, + initialValues, duals, useWarmStart); QPState state(initialValues, duals, workingSet, false, 0); /// main loop of the solver @@ -179,18 +181,22 @@ pair QPSolver::optimize( } pair QPSolver::optimize() const { + //Make an LP with any linear cost function. It doesn't matter for initialization. LP initProblem; - - initProblem.cost = LinearCost(2,ones(1,1)); // min gamma s.t. - initProblem.equalities = qp_.equalities; // s.t Ax = b from the original problem - //TODO: SLACK Variable needs to be added - initProblem.inequalities = qp_.inequalities; // s.t. Ax - gamma <= b Ax from the original problem - + Key newKey = 0; // make an unrelated key for a random variable cost + BOOST_FOREACH(Key key, qp_.cost.getKeyDimMap() | boost::adaptors::map_keys) + if(newKey < key) + newKey = key; + newKey++; + initProblem.cost = LinearCost(newKey, ones(1)); + initProblem.equalities = qp_.equalities; + initProblem.inequalities = qp_.inequalities; LPSolver solver(initProblem); - VectorValues result, duals; - boost::tie(result, duals) = solver.optimize(); + LPInitSolverMatlab initSolver(solver); + VectorValues initValues = initSolver.solve(); - return optimize(result); -}; + return optimize(initValues); +} +; } /* namespace gtsam */ diff --git a/gtsam_unstable/linear/tests/testQPSolver.cpp b/gtsam_unstable/linear/tests/testQPSolver.cpp index fbb7149c3..c4128cdaf 100644 --- a/gtsam_unstable/linear/tests/testQPSolver.cpp +++ b/gtsam_unstable/linear/tests/testQPSolver.cpp @@ -28,8 +28,7 @@ using namespace std; using namespace gtsam; using namespace gtsam::symbol_shorthand; -const Matrix One = ones(1,1); - +const Matrix One = ones(1, 1); /* ************************************************************************* */ // Create test graph according to Forst10book_pg171Ex5 @@ -47,10 +46,11 @@ QP createTestCase() { 2.0 * ones(1, 1), zero(1), 10)); // Inequality constraints - qp.inequalities.push_back(LinearInequality(X(1), ones(1,1), X(2), ones(1,1), 2, 0)); // x1 + x2 <= 2 --> x1 + x2 -2 <= 0, --> b=2 - qp.inequalities.push_back(LinearInequality(X(1), -ones(1,1), 0, 1)); // -x1 <= 0 - qp.inequalities.push_back(LinearInequality(X(2), -ones(1,1), 0, 2)); // -x2 <= 0 - qp.inequalities.push_back(LinearInequality(X(1), ones(1,1), 1.5, 3)); // x1 <= 3/2 + qp.inequalities.push_back( + LinearInequality(X(1), ones(1, 1), X(2), ones(1, 1), 2, 0)); // x1 + x2 <= 2 --> x1 + x2 -2 <= 0, --> b=2 + qp.inequalities.push_back(LinearInequality(X(1), -ones(1, 1), 0, 1)); // -x1 <= 0 + qp.inequalities.push_back(LinearInequality(X(2), -ones(1, 1), 0, 2)); // -x2 <= 0 + qp.inequalities.push_back(LinearInequality(X(1), ones(1, 1), 1.5, 3)); // x1 <= 3/2 return qp; } @@ -72,15 +72,15 @@ TEST(QPSolver, constraintsAux) { VectorValues lambdas; lambdas.insert(0, (Vector(1) << -0.5).finished()); - lambdas.insert(1, (Vector(1) << 0.0).finished()); - lambdas.insert(2, (Vector(1) << 0.3).finished()); - lambdas.insert(3, (Vector(1) << 0.1).finished()); + lambdas.insert(1, (Vector(1) << 0.0).finished()); + lambdas.insert(2, (Vector(1) << 0.3).finished()); + lambdas.insert(3, (Vector(1) << 0.1).finished()); int factorIx = solver.identifyLeavingConstraint(qp.inequalities, lambdas); LONGS_EQUAL(2, factorIx); VectorValues lambdas2; lambdas2.insert(0, (Vector(1) << -0.5).finished()); - lambdas2.insert(1, (Vector(1) << 0.0).finished()); + lambdas2.insert(1, (Vector(1) << 0.0).finished()); lambdas2.insert(2, (Vector(1) << -0.3).finished()); lambdas2.insert(3, (Vector(1) << -0.1).finished()); int factorIx2 = solver.identifyLeavingConstraint(qp.inequalities, lambdas2); @@ -137,14 +137,14 @@ TEST(QPSolver, indentifyActiveConstraints) { currentSolution.insert(X(2), zero(1)); InequalityFactorGraph workingSet = - solver.identifyActiveConstraints(qp.inequalities, currentSolution); + solver.identifyActiveConstraints(qp.inequalities, currentSolution); - CHECK(!workingSet.at(0)->active()); // inactive - CHECK(workingSet.at(1)->active()); // active - CHECK(workingSet.at(2)->active()); // active - CHECK(!workingSet.at(3)->active()); // inactive + CHECK(!workingSet.at(0)->active()); // inactive + CHECK(workingSet.at(1)->active());// active + CHECK(workingSet.at(2)->active());// active + CHECK(!workingSet.at(3)->active());// inactive - VectorValues solution = solver.solveWithCurrentWorkingSet(workingSet); + VectorValues solution = solver.solveWithCurrentWorkingSet(workingSet); VectorValues expectedSolution; expectedSolution.insert(X(1), (Vector(1) << 0.0).finished()); expectedSolution.insert(X(2), (Vector(1) << 0.0).finished()); @@ -178,7 +178,7 @@ TEST(QPSolver, iterate) { expectedSolutions[3].insert(X(2), (Vector(1) << 0.5).finished()); InequalityFactorGraph workingSet = - solver.identifyActiveConstraints(qp.inequalities, currentSolution); + solver.identifyActiveConstraints(qp.inequalities, currentSolution); QPState state(currentSolution, VectorValues(), workingSet, false, 100); @@ -227,15 +227,16 @@ QP createTestMatlabQPEx() { 2.0 * ones(1, 1), 6 * ones(1), 1000.0)); // Inequality constraints - qp.inequalities.push_back(LinearInequality(X(1), One, X(2), One, 2, 0)); // x1 + x2 <= 2 - qp.inequalities.push_back(LinearInequality(X(1), -One, X(2), 2*One, 2, 1)); //-x1 + 2*x2 <=2 - qp.inequalities.push_back(LinearInequality(X(1), 2*One, X(2), One, 3, 2)); // 2*x1 + x2 <=3 - qp.inequalities.push_back(LinearInequality(X(1), -One, 0, 3)); // -x1 <= 0 - qp.inequalities.push_back(LinearInequality(X(2), -One, 0, 4)); // -x2 <= 0 + qp.inequalities.push_back(LinearInequality(X(1), One, X(2), One, 2, 0)); // x1 + x2 <= 2 + qp.inequalities.push_back(LinearInequality(X(1), -One, X(2), 2 * One, 2, 1)); //-x1 + 2*x2 <=2 + qp.inequalities.push_back(LinearInequality(X(1), 2 * One, X(2), One, 3, 2)); // 2*x1 + x2 <=3 + qp.inequalities.push_back(LinearInequality(X(1), -One, 0, 3)); // -x1 <= 0 + qp.inequalities.push_back(LinearInequality(X(2), -One, 0, 4)); // -x2 <= 0 return qp; } +///* ************************************************************************* */ TEST(QPSolver, optimizeMatlabEx) { QP qp = createTestMatlabQPEx(); QPSolver solver(qp); @@ -252,15 +253,14 @@ TEST(QPSolver, optimizeMatlabEx) { ///* ************************************************************************* */ TEST(QPSolver, optimizeMatlabExNoinitials) { -// TODO: Enable this test when everything is fixed. -// QP qp = createTestMatlabQPEx(); -// QPSolver solver(qp); -// VectorValues solution; -// boost::tie(solution, boost::tuples::ignore) = solver.optimize(); -// VectorValues expectedSolution; -// expectedSolution.insert(X(1), (Vector(1) << 2.0 / 3.0).finished()); -// expectedSolution.insert(X(2), (Vector(1) << 4.0 / 3.0).finished()); -// CHECK(assert_equal(expectedSolution, solution, 1e-7)); + QP qp = createTestMatlabQPEx(); + QPSolver solver(qp); + VectorValues solution; + boost::tie(solution, boost::tuples::ignore) = solver.optimize(); + VectorValues expectedSolution; + expectedSolution.insert(X(1), (Vector(1) << 2.0 / 3.0).finished()); + expectedSolution.insert(X(2), (Vector(1) << 4.0 / 3.0).finished()); + CHECK(assert_equal(expectedSolution, solution, 1e-7)); } /* ************************************************************************* */ @@ -272,9 +272,12 @@ QP createTestNocedal06bookEx16_4() { qp.cost.push_back(JacobianFactor(X(2), ones(1, 1), 2.5 * ones(1))); // Inequality constraints - qp.inequalities.push_back(LinearInequality(X(1), -ones(1, 1), X(2), 2 * ones(1, 1), 2, 0)); - qp.inequalities.push_back(LinearInequality(X(1), ones(1, 1), X(2), 2 * ones(1, 1), 6, 1)); - qp.inequalities.push_back(LinearInequality(X(1), ones(1, 1), X(2), -2 * ones(1, 1), 2, 2)); + qp.inequalities.push_back( + LinearInequality(X(1), -ones(1, 1), X(2), 2 * ones(1, 1), 2, 0)); + qp.inequalities.push_back( + LinearInequality(X(1), ones(1, 1), X(2), 2 * ones(1, 1), 6, 1)); + qp.inequalities.push_back( + LinearInequality(X(1), ones(1, 1), X(2), -2 * ones(1, 1), 2, 2)); qp.inequalities.push_back(LinearInequality(X(1), -ones(1, 1), 0.0, 3)); qp.inequalities.push_back(LinearInequality(X(2), -ones(1, 1), 0.0, 4)); @@ -308,7 +311,7 @@ TEST(QPSolver, failedSubproblem) { expected.insert(X(1), (Vector(2) << 1.0, 0.0).finished()); VectorValues initialValues; - initialValues.insert(X(1), (Vector(2) << 10.0, 100.0).finished()); + initialValues.insert(X(1), (Vector(2) << 10.0, 100.0).finished()); QPSolver solver(qp); VectorValues solution; @@ -329,42 +332,16 @@ TEST(QPSolver, infeasibleInitial) { expected.insert(X(1), (Vector(2) << 1.0, 0.0).finished()); VectorValues initialValues; - initialValues.insert(X(1), (Vector(2) << -10.0, 100.0).finished()); + initialValues.insert(X(1), (Vector(2) << -10.0, 100.0).finished()); QPSolver solver(qp); VectorValues solution; CHECK_EXCEPTION( - solver.optimize(initialValues), - InfeasibleInitialValues + solver.optimize(initialValues), + InfeasibleInitialValues ); } -/* ************************************************************************* */ -TEST(QPSolver, infeasibleConstraints) { - QP qp; - // Objective functions x1^2 - x1*x2 + x2^2 - 3*x1 + 5 - // Note the Hessian encodes: - // 0.5*x1'*G11*x1 + x1'*G12*x2 + 0.5*x2'*G22*x2 - x1'*g1 - x2'*g2 + 0.5*f - // Hence, we have G11=2, G12 = -1, g1 = +3, G22 = 2, g2 = 0, f = 10 - // TODO: Test Fails because we if constraints aren't feasible, the initial point will not - // be feasible either. we need to check for infeasible constraints. - qp.cost.push_back( - HessianFactor(X(1), X(2), 2.0 * ones(1, 1), -ones(1, 1), 3.0 * ones(1), - 2.0 * ones(1, 1), zero(1), 5)); - qp.inequalities.push_back(LinearInequality(X(1), -1.0 * ones(1,1), X(2), -1.0 * ones(1,1), 32, 0)); // x1 + x2 >= 32 - qp.inequalities.push_back(LinearInequality(X(1), ones(1,1), 0, 1)); // x1 <= 0 - qp.inequalities.push_back(LinearInequality(X(2), ones(1,1), 0, 2)); // x2 <= 0 - qp.inequalities.push_back(LinearInequality(X(1), -ones(1,1), 0, 1)); // -x1 <= 0 - qp.inequalities.push_back(LinearInequality(X(2), -ones(1,1), 0, 2)); // -x2 <= 0 - QPSolver solver(qp); - VectorValues init; - init.insert(X(1), (Vector(1) << 0.0).finished()); - init.insert(X(2), (Vector(1) << 0.0).finished()); - CHECK_EXCEPTION( - solver.optimize(init), - InfeasibleOrUnboundedProblem); -} - /* ************************************************************************* */ int main() { TestResult tr;