From fc1f5ff6a8c833baaa0888a981da33af7900f934 Mon Sep 17 00:00:00 2001 From: thduynguyen Date: Fri, 2 May 2014 10:18:01 -0400 Subject: [PATCH] unittest for QPSolver without initial point --- gtsam/linear/QPSolver.cpp | 23 +++++++++--------- gtsam/linear/QPSolver.h | 2 +- gtsam/linear/tests/testQPSolver.cpp | 37 +++++++++++++++++++++++------ 3 files changed, 43 insertions(+), 19 deletions(-) diff --git a/gtsam/linear/QPSolver.cpp b/gtsam/linear/QPSolver.cpp index 6ca526631..3cc840f7a 100644 --- a/gtsam/linear/QPSolver.cpp +++ b/gtsam/linear/QPSolver.cpp @@ -194,11 +194,6 @@ GaussianFactorGraph QPSolver::buildDualGraph(const GaussianFactorGraph& graph, else { // Enforce constrained noise model so lambdas are solved with QR // and should exactly satisfy all the equations - for (size_t i = 0; i QPSolver::computeStepSize(const GaussianFactorGraph& workingGraph, const VectorValues& xk, const VectorValues& p) const { - static bool debug = true; + static bool debug = false; double minAlpha = 1.0; int closestFactorIx = -1, closestSigmaIx = -1; @@ -323,7 +318,7 @@ boost::tuple QPSolver::computeStepSize(const GaussianFactorGra /* ************************************************************************* */ bool QPSolver::iterateInPlace(GaussianFactorGraph& workingGraph, VectorValues& currentSolution, VectorValues& lambdas) const { - static bool debug = true; + static bool debug = false; if (debug) workingGraph.print("workingGraph: "); // Obtain the solution from the current working graph VectorValues newSolution = workingGraph.optimize(); @@ -367,7 +362,6 @@ bool QPSolver::iterateInPlace(GaussianFactorGraph& workingGraph, VectorValues& c /* ************************************************************************* */ VectorValues QPSolver::optimize(const VectorValues& initials, boost::optional lambdas) const { - cout << "QP optimize.." << endl; GaussianFactorGraph workingGraph = graph_.clone(); VectorValues currentSolution = initials; VectorValues workingLambdas; @@ -468,7 +462,7 @@ std::pair QPSolver::constraintsLP } /* ************************************************************************* */ -VectorValues QPSolver::findFeasibleInitialValues() const { +std::pair QPSolver::findFeasibleInitialValues() const { // Initial values with slack variables for the LP subproblem, Nocedal06book, pg.473 VectorValues initials; size_t firstSlackKey; @@ -487,7 +481,9 @@ VectorValues QPSolver::findFeasibleInitialValues() const { VectorValues solution = lpSolver.solve(); // Remove slack variables from solution + double slackSum = 0.0; for (Key key = firstSlackKey; key < firstSlackKey+constraintIndices_.size(); ++key) { + slackSum += solution.at(key).cwiseAbs().sum(); solution.erase(key); } @@ -500,12 +496,17 @@ VectorValues QPSolver::findFeasibleInitialValues() const { } } - return solution; + return make_pair(slackSum<1e-5, solution); } /* ************************************************************************* */ VectorValues QPSolver::optimize(boost::optional lambdas) const { - VectorValues initials = findFeasibleInitialValues(); + bool isFeasible; + VectorValues initials; + boost::tie(isFeasible, initials) = findFeasibleInitialValues(); + if (!isFeasible) { + throw std::runtime_error("LP subproblem is infeasible!"); + } return optimize(initials, lambdas); } diff --git a/gtsam/linear/QPSolver.h b/gtsam/linear/QPSolver.h index 316c5295c..21a7e5c7c 100644 --- a/gtsam/linear/QPSolver.h +++ b/gtsam/linear/QPSolver.h @@ -150,7 +150,7 @@ public: std::pair constraintsLP(Key firstSlackKey) const; /// Find a feasible initial point - VectorValues findFeasibleInitialValues() const; + std::pair findFeasibleInitialValues() const; /// Convert a Gaussian factor to a jacobian. return empty shared ptr if failed /// TODO: Move to GaussianFactor? diff --git a/gtsam/linear/tests/testQPSolver.cpp b/gtsam/linear/tests/testQPSolver.cpp index f66e67141..ee3f50cf2 100644 --- a/gtsam/linear/tests/testQPSolver.cpp +++ b/gtsam/linear/tests/testQPSolver.cpp @@ -249,7 +249,7 @@ TEST(QPSolver, optimizeNocedal06bookEx16_4) { /* ************************************************************************* */ // 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 +// x1 - 2 x2 - 1 >=0 // so that the trivial initial point (0,0) is infeasible GaussianFactorGraph modifyNocedal06bookEx16_4() { GaussianFactorGraph graph; @@ -260,7 +260,7 @@ GaussianFactorGraph modifyNocedal06bookEx16_4() { // 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), -1*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)); @@ -284,9 +284,7 @@ TEST(QPSolver, optimizeNocedal06bookEx16_4_findInitialPoint) { 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))); @@ -299,17 +297,42 @@ TEST(QPSolver, optimizeNocedal06bookEx16_4_findInitialPoint) { 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(3), -ones(1,1),-1*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: "); + bool isFeasible; + VectorValues initials; + boost::tie(isFeasible, initials) = solver.findFeasibleInitialValues(); + EXPECT(assert_equal(1.0*ones(1), initials.at(X(1)))); + EXPECT(assert_equal(0.0*ones(1), initials.at(X(2)))); + + VectorValues solution = solver.optimize(); + EXPECT(assert_equal(2.0*ones(1), solution.at(X(1)))); + EXPECT(assert_equal(0.5*ones(1), solution.at(X(2)))); } +TEST(QPSolver, optimizeNocedal06bookEx16_4_2) { + GaussianFactorGraph graph = createTestNocedal06bookEx16_4(); + QPSolver solver(graph); + VectorValues initials; + initials.insert(X(1), (Vector(1)<<0.0)); + initials.insert(X(2), (Vector(1)<<100.0)); + + VectorValues expectedSolution; + expectedSolution.insert(X(1), (Vector(1)<< 1.4)); + expectedSolution.insert(X(2), (Vector(1)<< 1.7)); + + VectorValues solution = solver.optimize(initials); + // THIS should fail because of the bad infeasible initial point!! +// CHECK(assert_equal(expectedSolution, solution, 1e-7)); + + VectorValues solution2 = solver.optimize(); + CHECK(assert_equal(expectedSolution, solution2, 1e-7)); +} /* ************************************************************************* */ int main() {