From bb7522c947c1334a76a9c517abc883464cbeb8cb Mon Sep 17 00:00:00 2001 From: thduynguyen Date: Tue, 15 Apr 2014 17:28:23 -0400 Subject: [PATCH] Fix gtsam's old segfault bug in JacobianFactor::isConstrained: return false if it has no noisemodel. Test Nocedal06book, example 16.4, pg 475 passed. --- gtsam/linear/JacobianFactor.h | 2 +- gtsam/linear/QPSolver.cpp | 7 +++++ gtsam/linear/tests/testQPSolver.cpp | 48 ++++++++++++++++++++++++----- 3 files changed, 49 insertions(+), 8 deletions(-) diff --git a/gtsam/linear/JacobianFactor.h b/gtsam/linear/JacobianFactor.h index 9d94a7343..e3c55feb2 100644 --- a/gtsam/linear/JacobianFactor.h +++ b/gtsam/linear/JacobianFactor.h @@ -228,7 +228,7 @@ namespace gtsam { virtual bool empty() const { return size() == 0 /*|| rows() == 0*/; } /** is noise model constrained ? */ - bool isConstrained() const { return model_->isConstrained(); } + bool isConstrained() const { return model_ && model_->isConstrained(); } /** Return the dimension of the variable pointed to by the given key iterator * todo: Remove this in favor of keeping track of dimensions with variables? diff --git a/gtsam/linear/QPSolver.cpp b/gtsam/linear/QPSolver.cpp index 58e8c119e..12eb604d8 100644 --- a/gtsam/linear/QPSolver.cpp +++ b/gtsam/linear/QPSolver.cpp @@ -96,6 +96,8 @@ GaussianFactorGraph::shared_ptr QPSolver::unconstrainedHessiansOfConstrainedVars /* ************************************************************************* */ GaussianFactorGraph QPSolver::buildDualGraph(const GaussianFactorGraph& graph, const VectorValues& x0) const { + static const bool debug = false; + // The dual graph to return GaussianFactorGraph dualGraph; @@ -110,6 +112,7 @@ GaussianFactorGraph QPSolver::buildDualGraph(const GaussianFactorGraph& graph, if (xiFactors.size() == 0) continue; GaussianFactor::shared_ptr xiFactor0 = freeHessians_->at(0); size_t xiDim = xiFactor0->getDim(xiFactor0->find(xiKey)); + if (debug) cout << "xiDim: " << xiDim << endl; // Compute gradf(xi) = \frac{\partial f}{\partial xi}' = \sum_j G_ij*xj - gi Vector gradf_xi = zero(xiDim); @@ -146,6 +149,7 @@ GaussianFactorGraph QPSolver::buildDualGraph(const GaussianFactorGraph& graph, // Gradient is the transpose of the Jacobian: A_k = gradc_k(xi) = \frac{\partial c_k}{\partial xi}' // Each column for each lambda_k corresponds to [the transpose of] each constrained row factor Matrix A_k = factor->getA(factor->find(xiKey)).transpose(); + if (debug) gtsam::print(A_k, "A_k = "); // Deal with mixed sigmas: no information if sigma != 0 Vector sigmas = factor->get_model()->sigmas(); for (size_t sigmaIx = 0; sigmaIx QPSolver::computeStepSize(const GaussianFactorGra /* ************************************************************************* */ bool QPSolver::iterateInPlace(GaussianFactorGraph& workingGraph, VectorValues& currentSolution) const { static bool debug = false; + if (debug) workingGraph.print("workingGraph: "); // Obtain the solution from the current working graph VectorValues newSolution = workingGraph.optimize(); if (debug) newSolution.print("New solution:"); @@ -268,6 +273,7 @@ bool QPSolver::iterateInPlace(GaussianFactorGraph& workingGraph, VectorValues& c // If we CAN'T move further if (newSolution.equals(currentSolution, 1e-5)) { // Compute lambda from the dual graph + if (debug) cout << "Building dual graph..." << endl; GaussianFactorGraph dualGraph = buildDualGraph(workingGraph, newSolution); if (debug) dualGraph.print("Dual graph: "); VectorValues lambdas = dualGraph.optimize(); @@ -284,6 +290,7 @@ bool QPSolver::iterateInPlace(GaussianFactorGraph& workingGraph, VectorValues& c else { // If we CAN make some progress // Adapt stepsize if some inactive inequality constraints complain about this move + if (debug) cout << "Computing stepsize..." << endl; double alpha; int factorIx, sigmaIx; VectorValues p = newSolution - currentSolution; diff --git a/gtsam/linear/tests/testQPSolver.cpp b/gtsam/linear/tests/testQPSolver.cpp index dde5e2fb5..d7ec97a24 100644 --- a/gtsam/linear/tests/testQPSolver.cpp +++ b/gtsam/linear/tests/testQPSolver.cpp @@ -136,8 +136,8 @@ TEST(QPSolver, iterate) { GaussianFactorGraph workingGraph = graph.clone(); VectorValues currentSolution; - currentSolution.insert(X(1), zeros(1,1)); - currentSolution.insert(X(2), zeros(1,1)); + 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)); @@ -162,8 +162,8 @@ TEST(QPSolver, optimizeForst10book_pg171Ex5) { GaussianFactorGraph graph = createTestCase(); QPSolver solver(graph); VectorValues initials; - initials.insert(X(1), zeros(1,1)); - initials.insert(X(2), zeros(1,1)); + initials.insert(X(1), zero(1)); + initials.insert(X(2), zero(1)); VectorValues solution = solver.optimize(initials); VectorValues expectedSolution; expectedSolution.insert(X(1), (Vector(1)<< 1.5)); @@ -172,7 +172,7 @@ TEST(QPSolver, optimizeForst10book_pg171Ex5) { } /* ************************************************************************* */ -// Create test graph according to Forst10book_pg171Ex5 +// Create Matlab's test graph as in http://www.mathworks.com/help/optim/ug/quadprog.html GaussianFactorGraph createTestMatlabQPEx() { GaussianFactorGraph graph; @@ -202,8 +202,8 @@ TEST(QPSolver, optimizeMatlabEx) { GaussianFactorGraph graph = createTestMatlabQPEx(); QPSolver solver(graph); VectorValues initials; - initials.insert(X(1), zeros(1,1)); - initials.insert(X(2), zeros(1,1)); + initials.insert(X(1), zero(1)); + initials.insert(X(2), zero(1)); VectorValues solution = solver.optimize(initials); VectorValues expectedSolution; expectedSolution.insert(X(1), (Vector(1)<< 2.0/3.0)); @@ -211,6 +211,40 @@ TEST(QPSolver, optimizeMatlabEx) { CHECK(assert_equal(expectedSolution, solution, 1e-7)); } +/* ************************************************************************* */ +// Create test graph as in Nocedal06book, Ex 16.4, pg. 475 +GaussianFactorGraph createTestNocedal06bookEx16_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) { + GaussianFactorGraph graph = createTestNocedal06bookEx16_4(); + QPSolver solver(graph); + VectorValues initials; + initials.insert(X(1), (Vector(1)<<2.0)); + initials.insert(X(2), zero(1)); + + VectorValues solution = solver.optimize(initials); + VectorValues expectedSolution; + expectedSolution.insert(X(1), (Vector(1)<< 1.4)); + expectedSolution.insert(X(2), (Vector(1)<< 1.7)); + CHECK(assert_equal(expectedSolution, solution, 1e-7)); +} + /* ************************************************************************* */ int main() { TestResult tr;