From 4f92492e34e4eb60eca3fabbc28e11226f816839 Mon Sep 17 00:00:00 2001 From: krunalchande Date: Mon, 22 Dec 2014 15:47:28 -0500 Subject: [PATCH] Reapply hacks in EliminatePreferCholesky to deal with negative definite hessians obtained from multiplying dual variables with the hessians of nonlinear constraints needed for SQP. --- gtsam/linear/GaussianFactorGraph.cpp | 23 ++++++ gtsam/linear/GaussianFactorGraph.h | 6 ++ gtsam/linear/HessianFactor.cpp | 106 ++++++++++++++++++++++++++- 3 files changed, 132 insertions(+), 3 deletions(-) diff --git a/gtsam/linear/GaussianFactorGraph.cpp b/gtsam/linear/GaussianFactorGraph.cpp index 6953d2969..1d6066c1c 100644 --- a/gtsam/linear/GaussianFactorGraph.cpp +++ b/gtsam/linear/GaussianFactorGraph.cpp @@ -384,6 +384,29 @@ namespace gtsam { return false; } + /* ************************************************************************* */ + boost::tuple GaussianFactorGraph::splitConstraints() const { + typedef HessianFactor H; + typedef JacobianFactor J; + + GaussianFactorGraph hessians, jacobians, constraints; + BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, *this) { + H::shared_ptr hessian(boost::dynamic_pointer_cast(factor)); + if (hessian) + hessians.push_back(factor); + else { + J::shared_ptr jacobian(boost::dynamic_pointer_cast(factor)); + if (jacobian && jacobian->get_model() && jacobian->get_model()->isConstrained()) { + constraints.push_back(jacobian); + } + else { + jacobians.push_back(factor); + } + } + } + return boost::make_tuple(hessians, jacobians, constraints); + } + /* ************************************************************************* */ // x += alpha*A'*e void GaussianFactorGraph::transposeMultiplyAdd(double alpha, const Errors& e, diff --git a/gtsam/linear/GaussianFactorGraph.h b/gtsam/linear/GaussianFactorGraph.h index 811c0f8b0..067a42d03 100644 --- a/gtsam/linear/GaussianFactorGraph.h +++ b/gtsam/linear/GaussianFactorGraph.h @@ -316,6 +316,12 @@ namespace gtsam { /** In-place version e <- A*x that takes an iterator. */ void multiplyInPlace(const VectorValues& x, const Errors::iterator& e) const; + /** + * Split constraints and unconstrained factors into two different graphs + * @return a pair of graphs + */ + boost::tuple splitConstraints() const; + /// @} private: diff --git a/gtsam/linear/HessianFactor.cpp b/gtsam/linear/HessianFactor.cpp index f2bebcab9..755f9c313 100644 --- a/gtsam/linear/HessianFactor.cpp +++ b/gtsam/linear/HessianFactor.cpp @@ -616,10 +616,110 @@ 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); - else + + /* 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. + */ + GaussianFactorGraph hessians, jacobians, constraints; +// factors.print("factors: "); + boost::tie(hessians, jacobians, constraints) = factors.splitConstraints(); +// keys.print("Frontal variables to eliminate: "); +// hessians.print("Hessians: "); +// jacobians.print("Jacobians: "); +// constraints.print("Constraints: "); + + bool hasHessians = hessians.size() > 0; + + // Add all jacobians to gather as much info as we can + hessians.push_back(jacobians); + + if (constraints.size()>0) { +// // Build joint factor +// HessianFactor::shared_ptr jointFactor; +// try { +// jointFactor = boost::make_shared(hessians, 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); + + // If there are hessian factors, turn them into conditional + // by doing partial elimination, then use those jacobians when eliminating the constraints + GaussianFactor::shared_ptr unconstrainedNewFactor; + if (hessians.size() > 0) { + bool hasSeparator = false; + GaussianFactorGraph::Keys unconstrainedKeys = hessians.keys(); + BOOST_FOREACH(Key key, unconstrainedKeys) { + if (find(keys.begin(), keys.end(), key) == keys.end()) { + hasSeparator = true; + break; + } + } + + if (hasSeparator) { + // find frontal keys in the unconstrained factor to eliminate + Ordering subkeys; + BOOST_FOREACH(Key key, keys) { + if (unconstrainedKeys.exists(key)) + subkeys.push_back(key); + } + GaussianConditional::shared_ptr unconstrainedConditional; + boost::tie(unconstrainedConditional, unconstrainedNewFactor) + = EliminateCholesky(hessians, subkeys); + constraints.push_back(unconstrainedConditional); + } + else { + if (hasHessians) { + HessianFactor::shared_ptr jointFactor = boost::make_shared< + HessianFactor>(hessians, Scatter(factors, keys)); + constraints.push_back(jointFactor); + } + else { + constraints.push_back(hessians); + } + } + } + + // Now eliminate the constraints + GaussianConditional::shared_ptr constrainedConditional; + GaussianFactor::shared_ptr constrainedNewFactor; + boost::tie(constrainedConditional, constrainedNewFactor) = EliminateQR( + constraints, keys); +// constraints.print("constraints: "); +// constrainedConditional->print("constrainedConditional: "); +// constrainedNewFactor->print("constrainedNewFactor: "); + + if (unconstrainedNewFactor) { + GaussianFactorGraph newFactors; + newFactors.push_back(unconstrainedNewFactor); + newFactors.push_back(constrainedNewFactor); +// newFactors.print("newFactors: "); + HessianFactor::shared_ptr newFactor(new HessianFactor(newFactors)); + return make_pair(constrainedConditional, newFactor); + } else { + return make_pair(constrainedConditional, constrainedNewFactor); + } + } + else { return EliminateCholesky(factors, keys); + } } } // gtsam