From c97f29be23a78fe229ccc71a178a2443ac235d56 Mon Sep 17 00:00:00 2001 From: thduynguyen Date: Thu, 1 May 2014 23:55:43 -0400 Subject: [PATCH] first unittest finding QP's feasible initial point works --- gtsam/linear/LPSolver.cpp | 2 +- gtsam/linear/QPSolver.cpp | 139 +++++++++++++++++++++++++++- gtsam/linear/QPSolver.h | 30 +++++- gtsam/linear/tests/testQPSolver.cpp | 65 +++++++++++++ 4 files changed, 230 insertions(+), 6 deletions(-) diff --git a/gtsam/linear/LPSolver.cpp b/gtsam/linear/LPSolver.cpp index 9bfeaaddb..02cc919fc 100644 --- a/gtsam/linear/LPSolver.cpp +++ b/gtsam/linear/LPSolver.cpp @@ -87,7 +87,7 @@ void LPSolver::addConstraints(const boost::shared_ptr& lp, vector columnNoCopy(columnNo); if (sigmas[i]>0) { - throw runtime_error("LP can't accept Gaussian noise!"); + cout << "Warning: Ignore Gaussian noise (sigma>0) in LP constraints!" << endl; } int constraintType = (sigmas[i]<0)?LE:EQ; if(!add_constraintex(lp.get(), columnNoCopy.size(), r.data(), columnNoCopy.data(), diff --git a/gtsam/linear/QPSolver.cpp b/gtsam/linear/QPSolver.cpp index e4402c1cd..6ca526631 100644 --- a/gtsam/linear/QPSolver.cpp +++ b/gtsam/linear/QPSolver.cpp @@ -6,8 +6,10 @@ */ #include -#include +#include #include +#include +#include using namespace std; @@ -15,7 +17,7 @@ namespace gtsam { /* ************************************************************************* */ QPSolver::QPSolver(const GaussianFactorGraph& graph) : - graph_(graph), fullFactorIndices_(graph) { + graph_(graph), fullFactorIndices_(graph) { // Split the original graph into unconstrained and constrained part // and collect indices of constrained factors for (size_t i = 0; i < graph.nrFactors(); ++i) { @@ -290,8 +292,8 @@ boost::tuple QPSolver::computeStepSize(const GaussianFactorGra if (debug) cout << "s, ajTp, b[s]: " << s << " " << ajTp << " " << b[s] << endl; // Check if aj'*p >0. Don't care if it's not. -// if (ajTp - b[s]>0) -// throw std::runtime_error("Infeasible point detected. Please choose a feasible initial values!"); + // if (ajTp - b[s]>0) + // throw std::runtime_error("Infeasible point detected. Please choose a feasible initial values!"); if (ajTp<=0) continue; // Compute aj'*xk @@ -377,5 +379,134 @@ VectorValues QPSolver::optimize(const VectorValues& initials, return currentSolution; } +/* ************************************************************************* */ +std::pair QPSolver::initialValuesLP() const { + size_t firstSlackKey = 0; + BOOST_FOREACH(Key key, fullFactorIndices_ | boost::adaptors::map_keys) { + if (firstSlackKey < key) firstSlackKey = key; + } + firstSlackKey += 1; + + VectorValues initials; + // Create zero values for constrained vars + BOOST_FOREACH(size_t iFactor, constraintIndices_) { + JacobianFactor::shared_ptr jacobian = toJacobian(graph_.at(iFactor)); + KeyVector keys = jacobian->keys(); + BOOST_FOREACH(Key key, keys) { + if (!initials.exists(key)) { + size_t dim = jacobian->getDim(jacobian->find(key)); + initials.insert(key, zero(dim)); + } + } + } + + // Insert initial values for slack variables + size_t slackKey = firstSlackKey; + BOOST_FOREACH(size_t iFactor, constraintIndices_) { + JacobianFactor::shared_ptr jacobian = toJacobian(graph_.at(iFactor)); + Vector errorAtZero = jacobian->getb(); + Vector slackInit = zero(errorAtZero.size()); + Vector sigmas = jacobian->get_model()->sigmas(); + for (size_t i = 0; i0 sigma, i.e. normal Gaussian noise, initialize it at 0 + } + initials.insert(slackKey, slackInit); + slackKey++; + } + return make_pair(initials, firstSlackKey); +} + +/* ************************************************************************* */ +VectorValues QPSolver::objectiveCoeffsLP(Key firstSlackKey) const { + VectorValues slackObjective; + for (size_t i = 0; i < constraintIndices_.size(); ++i) { + Key key = firstSlackKey + i; + size_t iFactor = constraintIndices_[i]; + JacobianFactor::shared_ptr jacobian = toJacobian(graph_.at(iFactor)); + size_t dim = jacobian->rows(); + Vector objective = ones(dim); + /* We should not ignore unconstrained slack var dimensions (those rows with sigmas >0) + * because their values might be underdetermined in the LP. Since they will have only + * 1 constraint zi>=0, enforcing them in the min obj function won't harm the other constrained + * slack vars, but also makes them well defined: 0 at the minimum. + */ + slackObjective.insert(key, ones(dim)); + } + return slackObjective; +} + +/* ************************************************************************* */ +std::pair QPSolver::constraintsLP( + Key firstSlackKey) const { + // Create constraints and 0 lower bounds (zi>=0) + GaussianFactorGraph::shared_ptr constraints(new GaussianFactorGraph()); + VectorValues slackLowerBounds; + for (size_t key = firstSlackKey; key > terms; + for (Factor::iterator it = jacobian->begin(); it != jacobian->end(); ++it) { + terms.push_back(make_pair(*it, jacobian->getA(it))); + } + // Add the slack term to the constraint + // Unlike Nocedal06book, pg.473, we want ax-z <= b, since we always assume + // LE constraints ax <= b for sigma < 0. + size_t dim = jacobian->rows(); + terms.push_back(make_pair(key, -eye(dim))); + constraints->push_back(JacobianFactor(terms, jacobian->getb(), jacobian->get_model())); + // Add lower bound for this slack key + slackLowerBounds.insert(key, zero(dim)); + } + return make_pair(constraints, slackLowerBounds); +} + +/* ************************************************************************* */ +VectorValues QPSolver::findFeasibleInitialValues() const { + // Initial values with slack variables for the LP subproblem, Nocedal06book, pg.473 + VectorValues initials; + size_t firstSlackKey; + boost::tie(initials, firstSlackKey) = initialValuesLP(); + + // Coefficients for the LP subproblem objective function, min \sum_i z_i + VectorValues objectiveLP = objectiveCoeffsLP(firstSlackKey); + + // Create constraints and lower bounds of slack variables + GaussianFactorGraph::shared_ptr constraints; + VectorValues slackLowerBounds; + boost::tie(constraints, slackLowerBounds) = constraintsLP(firstSlackKey); + + // Solve the LP subproblem + LPSolver lpSolver(objectiveLP, constraints, slackLowerBounds); + VectorValues solution = lpSolver.solve(); + + // Remove slack variables from solution + for (Key key = firstSlackKey; key < firstSlackKey+constraintIndices_.size(); ++key) { + solution.erase(key); + } + + // Insert zero vectors for free variables that are not in the constraints + BOOST_FOREACH(Key key, fullFactorIndices_ | boost::adaptors::map_keys) { + if (!solution.exists(key)) { + GaussianFactor::shared_ptr factor = graph_.at(*fullFactorIndices_[key].begin()); + size_t dim = factor->getDim(factor->find(key)); + solution.insert(key, zero(dim)); + } + } + + return solution; +} + +/* ************************************************************************* */ +VectorValues QPSolver::optimize(boost::optional lambdas) const { + VectorValues initials = findFeasibleInitialValues(); + return optimize(initials, lambdas); +} } /* namespace gtsam */ diff --git a/gtsam/linear/QPSolver.h b/gtsam/linear/QPSolver.h index 14035461a..316c5295c 100644 --- a/gtsam/linear/QPSolver.h +++ b/gtsam/linear/QPSolver.h @@ -120,10 +120,38 @@ public: bool iterateInPlace(GaussianFactorGraph& workingGraph, VectorValues& currentSolution, VectorValues& lambdas) const; - /** Optimize */ + /** Optimize with a provided initial values + * For this version, it is the responsibility of the caller to provide + * a feasible initial value, otherwise the solution will be wrong. + * If you don't know a feasible initial value, use the other version + * of optimize(). + */ VectorValues optimize(const VectorValues& initials, boost::optional lambdas = boost::none) const; + /** Optimize without an initial value. + * This version of optimize will try to find a feasible initial value by solving + * an LP program before solving this QP graph. + * TODO: If no feasible initial point exists, it should throw an InfeasibilityException! + */ + VectorValues optimize(boost::optional lambdas = boost::none) const; + + + /** + * Create initial values for the LP subproblem + * @return initial values and the key for the first slack variable + */ + std::pair initialValuesLP() const; + + /// Create coefficients for the LP subproblem's objective function as the sum of slack var + VectorValues objectiveCoeffsLP(Key firstSlackKey) const; + + /// Build constraints and slacks' lower bounds for the LP subproblem + std::pair constraintsLP(Key firstSlackKey) const; + + /// Find a feasible initial point + VectorValues findFeasibleInitialValues() const; + /// Convert a Gaussian factor to a jacobian. return empty shared ptr if failed /// TODO: Move to GaussianFactor? static JacobianFactor::shared_ptr toJacobian(const GaussianFactor::shared_ptr& factor) { diff --git a/gtsam/linear/tests/testQPSolver.cpp b/gtsam/linear/tests/testQPSolver.cpp index f3945cd2f..f66e67141 100644 --- a/gtsam/linear/tests/testQPSolver.cpp +++ b/gtsam/linear/tests/testQPSolver.cpp @@ -246,6 +246,71 @@ TEST(QPSolver, optimizeNocedal06bookEx16_4) { CHECK(assert_equal(expectedSolution, solution, 1e-7)); } +/* ************************************************************************* */ +// Create test graph as in Nocedal06book, Ex 16.4, pg. 475 +// with the first constraint (16.49b) is replaced by +// x1 - 2 x2 - 2 >=0 +// so that the trivial initial point (0,0) is infeasible +GaussianFactorGraph modifyNocedal06bookEx16_4() { + GaussianFactorGraph graph; + + graph.push_back(JacobianFactor(X(1), ones(1,1), ones(1))); + graph.push_back(JacobianFactor(X(2), ones(1,1), 2.5*ones(1))); + + // Inequality constraints + noiseModel::Constrained::shared_ptr noise = noiseModel::Constrained::MixedSigmas( + (Vector(1) << -1)); + graph.push_back(JacobianFactor(X(1), -ones(1,1), X(2), 2*ones(1,1), -2*ones(1), noise)); + graph.push_back(JacobianFactor(X(1), ones(1,1), X(2), 2*ones(1,1), 6*ones(1), noise)); + graph.push_back(JacobianFactor(X(1), ones(1,1), X(2),-2*ones(1,1), 2*ones(1), noise)); + graph.push_back(JacobianFactor(X(1), -ones(1,1), zero(1), noise)); + graph.push_back(JacobianFactor(X(2), -ones(1,1), zero(1), noise)); + + return graph; +} + +TEST(QPSolver, optimizeNocedal06bookEx16_4_findInitialPoint) { + GaussianFactorGraph graph = modifyNocedal06bookEx16_4(); + QPSolver solver(graph); + VectorValues initialsLP; + Key firstSlackKey; + boost::tie(initialsLP, firstSlackKey) = solver.initialValuesLP(); + EXPECT(assert_equal(zero(1), initialsLP.at(X(1)))); + EXPECT(assert_equal(zero(1), initialsLP.at(X(2)))); + LONGS_EQUAL(X(2)+1, firstSlackKey); + EXPECT(assert_equal(zero(1), initialsLP.at(firstSlackKey))); + EXPECT(assert_equal(ones(1)*6.0, initialsLP.at(firstSlackKey+1))); + EXPECT(assert_equal(ones(1)*2.0, initialsLP.at(firstSlackKey+2))); + EXPECT(assert_equal(zero(1), initialsLP.at(firstSlackKey+3))); + EXPECT(assert_equal(zero(1), initialsLP.at(firstSlackKey+4))); + + initialsLP.print("initialsLP: "); + VectorValues objCoeffs = solver.objectiveCoeffsLP(firstSlackKey); + cout << "done" << endl; + for (size_t i = 0; i<5; ++i) + EXPECT(assert_equal(ones(1), objCoeffs.at(firstSlackKey+i))); + + GaussianFactorGraph::shared_ptr constraints; + VectorValues lowerBounds; + boost::tie(constraints, lowerBounds) = solver.constraintsLP(firstSlackKey); + for (size_t i = 0; i<5; ++i) + EXPECT(assert_equal(zero(1), lowerBounds.at(firstSlackKey+i))); + + GaussianFactorGraph expectedConstraints; + noiseModel::Constrained::shared_ptr noise = noiseModel::Constrained::MixedSigmas( + (Vector(1) << -1)); + expectedConstraints.push_back(JacobianFactor(X(1), -ones(1,1), X(2), 2*ones(1,1), X(3), -ones(1,1),-2*ones(1), noise)); + expectedConstraints.push_back(JacobianFactor(X(1), ones(1,1), X(2), 2*ones(1,1), X(4), -ones(1,1), 6*ones(1), noise)); + expectedConstraints.push_back(JacobianFactor(X(1), ones(1,1), X(2),-2*ones(1,1), X(5), -ones(1,1), 2*ones(1), noise)); + expectedConstraints.push_back(JacobianFactor(X(1), -ones(1,1), X(6), -ones(1,1), zero(1), noise)); + expectedConstraints.push_back(JacobianFactor(X(2), -ones(1,1), X(7), -ones(1,1), zero(1), noise)); + EXPECT(assert_equal(expectedConstraints, *constraints)); + + VectorValues initials = solver.findFeasibleInitialValues(); + initials.print("Feasible point found: "); +} + + /* ************************************************************************* */ int main() { TestResult tr;