From 85397223efca171afe6dfe7d227f6b2bc8e7ad67 Mon Sep 17 00:00:00 2001 From: thduynguyen Date: Tue, 9 Dec 2014 16:27:11 -0500 Subject: [PATCH] fix QPSolver unit tests --- .../linear/LinearInequalityFactorGraph.h | 1 - gtsam_unstable/linear/QPSolver.cpp | 58 ++++++++-- gtsam_unstable/linear/QPSolver.h | 29 ++++- gtsam_unstable/linear/tests/testQPSolver.cpp | 100 ++++++++++++------ 4 files changed, 144 insertions(+), 44 deletions(-) diff --git a/gtsam_unstable/linear/LinearInequalityFactorGraph.h b/gtsam_unstable/linear/LinearInequalityFactorGraph.h index bfc64df2a..b72f2cc3c 100644 --- a/gtsam_unstable/linear/LinearInequalityFactorGraph.h +++ b/gtsam_unstable/linear/LinearInequalityFactorGraph.h @@ -18,7 +18,6 @@ #pragma once -#include #include #include diff --git a/gtsam_unstable/linear/QPSolver.cpp b/gtsam_unstable/linear/QPSolver.cpp index 7872ce27e..57c20d897 100644 --- a/gtsam_unstable/linear/QPSolver.cpp +++ b/gtsam_unstable/linear/QPSolver.cpp @@ -14,9 +14,6 @@ using namespace std; -#define ACTIVE 0.0 -#define INACTIVE std::numeric_limits::infinity() - namespace gtsam { //****************************************************************************** @@ -52,11 +49,29 @@ JacobianFactor::shared_ptr QPSolver::createDualFactor(Key key, // Collect the gradients of unconstrained cost factors to the b vector Vector b = zero(delta.at(key).size()); - BOOST_FOREACH(size_t factorIx, costVariableIndex_[key]) { - GaussianFactor::shared_ptr factor = qp_.cost.at(factorIx); - b += factor->gradient(key, delta); + if (costVariableIndex_.find(key) != costVariableIndex_.end()) { + BOOST_FOREACH(size_t factorIx, costVariableIndex_[key]) { + GaussianFactor::shared_ptr factor = qp_.cost.at(factorIx); + b += factor->gradient(key, delta); + } } - return boost::make_shared(Aterms, b); + + return boost::make_shared(Aterms, b, noiseModel::Constrained::All(b.rows())); +} + +//****************************************************************************** +GaussianFactor::shared_ptr QPSolver::createDualPrior( + const LinearInequality::shared_ptr& factor) const { + Vector sigmas = factor->get_model()->sigmas(); + size_t n = sigmas.rows(); + Matrix A = eye(n); + for (size_t i = 0; i(factor->dualKey(), A, b); } //****************************************************************************** @@ -67,6 +82,11 @@ GaussianFactorGraph::shared_ptr QPSolver::buildDualGraph( // Each constrained key becomes a factor in the dual graph dualGraph->push_back(createDualFactor(key, workingSet, delta)); } + + // Add prior for inactive dual variables + BOOST_FOREACH(const LinearInequality::shared_ptr& factor, workingSet) { + dualGraph->push_back(createDualPrior(factor)); + } return dualGraph; } @@ -224,13 +244,33 @@ QPState QPSolver::iterate(const QPState& state) const { } } +//****************************************************************************** +LinearInequalityFactorGraph QPSolver::identifyActiveConstraints( + const LinearInequalityFactorGraph& inequalities, + const VectorValues& initialValues) const { + LinearInequalityFactorGraph workingSet; + BOOST_FOREACH(const LinearInequality::shared_ptr& factor, inequalities){ + LinearInequality::shared_ptr workingFactor(new LinearInequality(*factor)); + Vector e = workingFactor->error_vector(initialValues); + Vector sigmas = zero(e.rows()); + for (int i = 0; i < e.rows(); ++i){ + if (fabs(e[i])>1e-7){ + sigmas[i] = INACTIVE; + } + } + workingFactor->setModel(true, sigmas); + workingSet.push_back(workingFactor); + } + return workingSet; +} + //****************************************************************************** pair QPSolver::optimize( const VectorValues& initialValues) const { // TODO: initialize workingSet from the feasible initialValues - LinearInequalityFactorGraph workingSet(qp_.inequalities); - + LinearInequalityFactorGraph workingSet = + identifyActiveConstraints(qp_.inequalities, initialValues); QPState state(initialValues, VectorValues(), workingSet, false); /// main loop of the solver diff --git a/gtsam_unstable/linear/QPSolver.h b/gtsam_unstable/linear/QPSolver.h index b3c66a21f..85a387140 100644 --- a/gtsam_unstable/linear/QPSolver.h +++ b/gtsam_unstable/linear/QPSolver.h @@ -13,6 +13,9 @@ #include #include +#define ACTIVE 0.0 +#define INACTIVE std::numeric_limits::infinity() + namespace gtsam { /// This struct holds the state of QPSolver at each iteration @@ -67,10 +70,12 @@ public: const FactorGraph& graph, const VariableIndex& variableIndex) const { std::vector > Aterms; - BOOST_FOREACH(size_t factorIx, variableIndex[key]){ - typename FACTOR::shared_ptr factor = graph.at(factorIx); - Matrix Ai = factor->getA(factor->find(key)).transpose(); - Aterms.push_back(std::make_pair(factor->dualKey(), Ai)); + if (variableIndex.find(key) != variableIndex.end()) { + BOOST_FOREACH(size_t factorIx, variableIndex[key]){ + typename FACTOR::shared_ptr factor = graph.at(factorIx); + Matrix Ai = factor->getA(factor->find(key)).transpose(); + Aterms.push_back(std::make_pair(factor->dualKey(), Ai)); + } } return Aterms; } @@ -80,6 +85,15 @@ public: const LinearInequalityFactorGraph& workingSet, const VectorValues& delta) const; + /** + * Create dummy prior for inactive dual variables + * TODO: This might be inefficient but I don't know how to avoid + * the Indeterminant exception due to no information for the duals + * on inactive components of the constraints. + */ + GaussianFactor::shared_ptr createDualPrior( + const LinearInequality::shared_ptr& factor) const; + /** * Build the dual graph to solve for the Lagrange multipliers. * @@ -173,6 +187,13 @@ public: /** Iterate 1 step, return a new state with a new workingSet and values */ QPState iterate(const QPState& state) const; + /** + * Identify active constraints based on initial values. + */ + LinearInequalityFactorGraph identifyActiveConstraints( + const LinearInequalityFactorGraph& inequalities, + const VectorValues& initialValues) const; + /** 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. diff --git a/gtsam_unstable/linear/tests/testQPSolver.cpp b/gtsam_unstable/linear/tests/testQPSolver.cpp index edf64655d..6ac329688 100644 --- a/gtsam_unstable/linear/tests/testQPSolver.cpp +++ b/gtsam_unstable/linear/tests/testQPSolver.cpp @@ -26,9 +26,6 @@ using namespace std; using namespace gtsam; using namespace gtsam::symbol_shorthand; -#define TEST_DISABLED(testGroup, testName)\ - void testGroup##testName##Test(TestResult& result_, const std::string& name_) - /* ************************************************************************* */ // Create test graph according to Forst10book_pg171Ex5 QP createTestCase() { @@ -73,7 +70,7 @@ TEST(QPSolver, constraintsAux) { int factorIx, lambdaIx; boost::tie(factorIx, lambdaIx) = solver.identifyLeavingConstraint( qp.inequalities, lambdas); - LONGS_EQUAL(1, factorIx); + LONGS_EQUAL(0, factorIx); LONGS_EQUAL(2, lambdaIx); VectorValues lambdas2; @@ -122,37 +119,80 @@ TEST(QPSolver, dual) { qp.inequalities, initialValues); VectorValues dual = dualGraph->optimize(); VectorValues expectedDual; - expectedDual.insert(1, (Vector(1) << 2.0)); + expectedDual.insert(0, (Vector(1) << 2.0)); CHECK(assert_equal(expectedDual, dual, 1e-10)); } /* ************************************************************************* */ +TEST(QPSolver, indentifyActiveConstraints) { + QP qp = createTestCase(); + QPSolver solver(qp); + + VectorValues currentSolution; + currentSolution.insert(X(1), zero(1)); + currentSolution.insert(X(2), zero(1)); + + LinearInequalityFactorGraph workingSet = + solver.identifyActiveConstraints(qp.inequalities, currentSolution); + + Vector actualSigmas = workingSet.at(0)->get_model()->sigmas(); + Vector expectedSigmas = (Vector(4) << INACTIVE, ACTIVE, ACTIVE, INACTIVE); + CHECK(assert_equal(expectedSigmas, actualSigmas, 1e-100)); + + VectorValues solution = solver.solveWithCurrentWorkingSet(workingSet); + VectorValues expectedSolution; + expectedSolution.insert(X(1), (Vector(1) << 0.0)); + expectedSolution.insert(X(2), (Vector(1) << 0.0)); + CHECK(assert_equal(expectedSolution, solution, 1e-100)); + +} + +/* ************************************************************************* */ +TEST(QPSolver, iterate) { + QP qp = createTestCase(); + QPSolver solver(qp); + + VectorValues currentSolution; + currentSolution.insert(X(1), zero(1)); + currentSolution.insert(X(2), zero(1)); + + std::vector expectedSolutions(4), expectedDuals(4); + expectedSolutions[0].insert(X(1), (Vector(1) << 0.0)); + expectedSolutions[0].insert(X(2), (Vector(1) << 0.0)); + expectedDuals[0].insert(0, (Vector(4) << 0, 3, 0, 0)); + + expectedSolutions[1].insert(X(1), (Vector(1) << 1.5)); + expectedSolutions[1].insert(X(2), (Vector(1) << 0.0)); + expectedDuals[1].insert(0, (Vector(4) << 0, 0, 1.5, 0)); + + expectedSolutions[2].insert(X(1), (Vector(1) << 1.5)); + expectedSolutions[2].insert(X(2), (Vector(1) << 0.75)); + expectedDuals[2].insert(0, (Vector(4) << 0, 0, 1.5, 0)); + + expectedSolutions[3].insert(X(1), (Vector(1) << 1.5)); + expectedSolutions[3].insert(X(2), (Vector(1) << 0.5)); + expectedDuals[3].insert(0, (Vector(4) << -0.5, 0, 0, 0)); + + LinearInequalityFactorGraph workingSet = + solver.identifyActiveConstraints(qp.inequalities, currentSolution); + + QPState state(currentSolution, VectorValues(), workingSet, false); + + int it = 0; + while (!state.converged) { + state = solver.iterate(state); + // These checks will fail because the expected solutions obtained from + // Forst10book do not follow exactly what we implemented from Nocedal06book. + // Specifically, we do not re-identify active constraints and + // do not recompute dual variables after every step!!! +// CHECK(assert_equal(expectedSolutions[it], state.values, 1e-10)); +// CHECK(assert_equal(expectedDuals[it], state.duals, 1e-10)); + it++; + } + + CHECK(assert_equal(expectedSolutions[3], state.values, 1e-10)); +} -//TEST(QPSolver, iterate) { -// QP qp = createTestCase(); -// QPSolver solver(qp); -// -// VectorValues currentSolution; -// currentSolution.insert(X(1), zero(1)); -// currentSolution.insert(X(2), zero(1)); -// -// std::vector expectedSolutions(3); -// expectedSolutions[0].insert(X(1), (Vector(1) << 4.0 / 3.0)); -// expectedSolutions[0].insert(X(2), (Vector(1) << 2.0 / 3.0)); -// expectedSolutions[1].insert(X(1), (Vector(1) << 1.5)); -// expectedSolutions[1].insert(X(2), (Vector(1) << 0.5)); -// expectedSolutions[2].insert(X(1), (Vector(1) << 1.5)); -// expectedSolutions[2].insert(X(2), (Vector(1) << 0.5)); -// -// bool converged = false; -// int it = 0; -// while (!converged) { -// VectorValues lambdas; -// converged = solver.iterateInPlace(workingGraph, currentSolution, lambdas); -// CHECK(assert_equal(expectedSolutions[it], currentSolution, 1e-100)); -// it++; -// } -//} /* ************************************************************************* */ TEST(QPSolver, optimizeForst10book_pg171Ex5) {