From 1e3ae3b3d36e0248caa7ad80b8bc71d10e130c92 Mon Sep 17 00:00:00 2001 From: thduynguyen Date: Sat, 3 May 2014 18:04:37 -0400 Subject: [PATCH] Support non positive definite Hessian factors while doing EliminatePreferCholesky with some constrained factors. Currently, when eliminating a constrained variable, EliminatePreferCholesky converts every other factors to JacobianFactor before doing the special QR factorization for constrained variables. Unfortunately, after a constrained nonlinear graph is linearized, new hessian factors from constraints, multiplied with the dual variable (-lambda*\hessian{c} terms in the Lagrangian objective function), might become negative definite, thus cannot be converted to JacobianFactors. Following EliminateCholesky, this version of EliminatePreferCholesky for constrained var gathers all unconstrained factors into a big joint HessianFactor before converting it into a JacobianFactor to be eliminiated by QR together with the other constrained factors. Of course, this might not solve the non-positive-definite problem entirely, because (1) the original hessian factors might be non-positive definite and (2) large strange value of lambdas might cause the joint factor non-positive definite [is this true?]. But at least, this will help in typical cases. --- .gitignore | 4 ++- gtsam/linear/GaussianFactorGraph.cpp | 34 ++++++++++++++++++------- gtsam/linear/GaussianFactorGraph.h | 7 +++++ gtsam/linear/HessianFactor.cpp | 17 +++++++++++-- gtsam/linear/JacobianFactor.cpp | 3 +++ gtsam/linear/QPSolver.cpp | 14 +++++----- gtsam/linear/QPSolver.h | 7 ++--- gtsam/linear/tests/testQPSolver.cpp | 38 +++++++++++++++++++++++----- 8 files changed, 95 insertions(+), 29 deletions(-) diff --git a/.gitignore b/.gitignore index 030d51b09..73fd48c67 100644 --- a/.gitignore +++ b/.gitignore @@ -1,3 +1,5 @@ /build* *.pyc -*.DS_Store \ No newline at end of file +*.DS_Store +/debug/ +*.txt.user diff --git a/gtsam/linear/GaussianFactorGraph.cpp b/gtsam/linear/GaussianFactorGraph.cpp index b56270a55..e53abc213 100644 --- a/gtsam/linear/GaussianFactorGraph.cpp +++ b/gtsam/linear/GaussianFactorGraph.cpp @@ -371,16 +371,32 @@ namespace gtsam { } /* ************************************************************************* */ - // x += alpha*A'*e -void GaussianFactorGraph::transposeMultiplyAdd(double alpha, const Errors& e, - VectorValues& x) const { - // For each factor add the gradient contribution - Errors::const_iterator ei = e.begin(); - BOOST_FOREACH(const sharedFactor& Ai_G, *this) { - JacobianFactor::shared_ptr Ai = convertToJacobianFactorPtr(Ai_G); - Ai->transposeMultiplyAdd(alpha, *(ei++), x); + std::pair GaussianFactorGraph::splitConstraints() const { + typedef JacobianFactor J; + GaussianFactorGraph unconstraints, constraints; + BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, *this) { + J::shared_ptr jacobian(boost::dynamic_pointer_cast(factor)); + if (jacobian && jacobian->get_model() && jacobian->get_model()->isConstrained()) { + constraints.push_back(jacobian); + } + else { + unconstraints.push_back(factor); + } + } + return make_pair(unconstraints, constraints); + } + + /* ************************************************************************* */ + // x += alpha*A'*e + void GaussianFactorGraph::transposeMultiplyAdd(double alpha, const Errors& e, + VectorValues& x) const { + // For each factor add the gradient contribution + Errors::const_iterator ei = e.begin(); + BOOST_FOREACH(const sharedFactor& Ai_G, *this) { + JacobianFactor::shared_ptr Ai = convertToJacobianFactorPtr(Ai_G); + Ai->transposeMultiplyAdd(alpha, *(ei++), x); + } } -} ///* ************************************************************************* */ //void residual(const GaussianFactorGraph& fg, const VectorValues &x, VectorValues &r) { diff --git a/gtsam/linear/GaussianFactorGraph.h b/gtsam/linear/GaussianFactorGraph.h index 50442ac6b..67c86b621 100644 --- a/gtsam/linear/GaussianFactorGraph.h +++ b/gtsam/linear/GaussianFactorGraph.h @@ -313,6 +313,13 @@ namespace gtsam { /// @} + /** + * Split constraints and unconstrained factors into two different graphs + * @return a pair of graphs + */ + std::pair splitConstraints() const; + + private: /** Serialization function */ friend class boost::serialization::access; diff --git a/gtsam/linear/HessianFactor.cpp b/gtsam/linear/HessianFactor.cpp index 18fa44e8b..988761cb1 100644 --- a/gtsam/linear/HessianFactor.cpp +++ b/gtsam/linear/HessianFactor.cpp @@ -641,8 +641,21 @@ EliminatePreferCholesky(const GaussianFactorGraph& factors, const Ordering& keys // all factors to JacobianFactors. Otherwise, we can convert all factors // to HessianFactors. This is because QR can handle constrained noise // models but Cholesky cannot. - if (hasConstraints(factors)) - return EliminateQR(factors, keys); + GaussianFactorGraph unconstraints, constraints; + boost::tie(unconstraints, constraints) = factors.splitConstraints(); + if (constraints.size()>0) { + // Build joint factor + HessianFactor::shared_ptr jointFactor; + try { + jointFactor = boost::make_shared(unconstraints, Scatter(factors, keys)); + } catch(std::invalid_argument&) { + throw InvalidDenseElimination( + "EliminateCholesky was called with a request to eliminate variables that are not\n" + "involved in the provided factors."); + } + constraints.push_back(jointFactor); + return EliminateQR(constraints, keys); + } else return EliminateCholesky(factors, keys); } diff --git a/gtsam/linear/JacobianFactor.cpp b/gtsam/linear/JacobianFactor.cpp index 0a9365181..2c6a5ddee 100644 --- a/gtsam/linear/JacobianFactor.cpp +++ b/gtsam/linear/JacobianFactor.cpp @@ -115,6 +115,9 @@ JacobianFactor::JacobianFactor(const HessianFactor& factor) : bool success; boost::tie(maxrank, success) = choleskyCareful(Ab_.matrix()); + factor.print("HessianFactor to convert: "); + cout << "Maxrank: " << maxrank << ", success: " << int(success) << endl; + // Check for indefinite system if (!success) throw IndeterminantLinearSystemException(factor.keys().front()); diff --git a/gtsam/linear/QPSolver.cpp b/gtsam/linear/QPSolver.cpp index 3cc840f7a..3ac56f929 100644 --- a/gtsam/linear/QPSolver.cpp +++ b/gtsam/linear/QPSolver.cpp @@ -360,17 +360,15 @@ bool QPSolver::iterateInPlace(GaussianFactorGraph& workingGraph, VectorValues& c } /* ************************************************************************* */ -VectorValues QPSolver::optimize(const VectorValues& initials, - boost::optional lambdas) const { +std::pair QPSolver::optimize(const VectorValues& initials) const { GaussianFactorGraph workingGraph = graph_.clone(); VectorValues currentSolution = initials; - VectorValues workingLambdas; + VectorValues lambdas; bool converged = false; while (!converged) { - converged = iterateInPlace(workingGraph, currentSolution, workingLambdas); + converged = iterateInPlace(workingGraph, currentSolution, lambdas); } - if (lambdas) *lambdas = workingLambdas; - return currentSolution; + return make_pair(currentSolution, lambdas); } /* ************************************************************************* */ @@ -500,14 +498,14 @@ std::pair QPSolver::findFeasibleInitialValues() const { } /* ************************************************************************* */ -VectorValues QPSolver::optimize(boost::optional lambdas) const { +std::pair QPSolver::optimize() const { bool isFeasible; VectorValues initials; boost::tie(isFeasible, initials) = findFeasibleInitialValues(); if (!isFeasible) { throw std::runtime_error("LP subproblem is infeasible!"); } - return optimize(initials, lambdas); + return optimize(initials); } } /* namespace gtsam */ diff --git a/gtsam/linear/QPSolver.h b/gtsam/linear/QPSolver.h index 21a7e5c7c..00ec8df72 100644 --- a/gtsam/linear/QPSolver.h +++ b/gtsam/linear/QPSolver.h @@ -125,16 +125,17 @@ public: * 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(). + * @return a pair of solutions */ - VectorValues optimize(const VectorValues& initials, - boost::optional lambdas = boost::none) const; + std::pair optimize(const VectorValues& initials) 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! + * @return a pair of solutions */ - VectorValues optimize(boost::optional lambdas = boost::none) const; + std::pair optimize() const; /** diff --git a/gtsam/linear/tests/testQPSolver.cpp b/gtsam/linear/tests/testQPSolver.cpp index ee3f50cf2..67877ff16 100644 --- a/gtsam/linear/tests/testQPSolver.cpp +++ b/gtsam/linear/tests/testQPSolver.cpp @@ -165,7 +165,8 @@ TEST(QPSolver, optimizeForst10book_pg171Ex5) { VectorValues initials; initials.insert(X(1), zero(1)); initials.insert(X(2), zero(1)); - VectorValues solution = solver.optimize(initials); + VectorValues solution; + boost::tie(solution, boost::tuples::ignore) = solver.optimize(initials); VectorValues expectedSolution; expectedSolution.insert(X(1), (Vector(1)<< 1.5)); expectedSolution.insert(X(2), (Vector(1)<< 0.5)); @@ -205,7 +206,8 @@ TEST(QPSolver, optimizeMatlabEx) { VectorValues initials; initials.insert(X(1), zero(1)); initials.insert(X(2), zero(1)); - VectorValues solution = solver.optimize(initials); + VectorValues solution; + boost::tie(solution, boost::tuples::ignore) = solver.optimize(initials); VectorValues expectedSolution; expectedSolution.insert(X(1), (Vector(1)<< 2.0/3.0)); expectedSolution.insert(X(2), (Vector(1)<< 4.0/3.0)); @@ -239,7 +241,8 @@ TEST(QPSolver, optimizeNocedal06bookEx16_4) { initials.insert(X(1), (Vector(1)<<2.0)); initials.insert(X(2), zero(1)); - VectorValues solution = solver.optimize(initials); + VectorValues solution; + boost::tie(solution, boost::tuples::ignore) = solver.optimize(initials); VectorValues expectedSolution; expectedSolution.insert(X(1), (Vector(1)<< 1.4)); expectedSolution.insert(X(2), (Vector(1)<< 1.7)); @@ -310,7 +313,8 @@ TEST(QPSolver, optimizeNocedal06bookEx16_4_findInitialPoint) { 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(); + VectorValues solution; + boost::tie(solution, boost::tuples::ignore) = solver.optimize(); EXPECT(assert_equal(2.0*ones(1), solution.at(X(1)))); EXPECT(assert_equal(0.5*ones(1), solution.at(X(2)))); } @@ -326,14 +330,36 @@ TEST(QPSolver, optimizeNocedal06bookEx16_4_2) { expectedSolution.insert(X(1), (Vector(1)<< 1.4)); expectedSolution.insert(X(2), (Vector(1)<< 1.7)); - VectorValues solution = solver.optimize(initials); + VectorValues solution; + boost::tie(solution, boost::tuples::ignore) = solver.optimize(initials); // THIS should fail because of the bad infeasible initial point!! // CHECK(assert_equal(expectedSolution, solution, 1e-7)); - VectorValues solution2 = solver.optimize(); + VectorValues solution2; + boost::tie(solution2, boost::tuples::ignore) = solver.optimize(); CHECK(assert_equal(expectedSolution, solution2, 1e-7)); } +/* ************************************************************************* */ + +TEST(QPSolver, failedSubproblem) { + GaussianFactorGraph graph; + graph.push_back(JacobianFactor(X(1), eye(2), zero(2))); + graph.push_back(HessianFactor(X(1), zeros(2,2), zero(2), 100.0)); + graph.push_back(JacobianFactor(X(1), (Matrix(1,2)<<-1.0, 0.0), -ones(1), + noiseModel::Constrained::MixedSigmas(-ones(1)))); + + VectorValues expected; + expected.insert(X(1), (Vector(2)<< 1.0, 0.0)); + + QPSolver solver(graph); + VectorValues solution; + boost::tie(solution, boost::tuples::ignore) = solver.optimize(); + graph.print("Graph: "); + solution.print("Solution: "); + CHECK(assert_equal(expected, solution, 1e-7)); +} + /* ************************************************************************* */ int main() { TestResult tr;