From 4877bdb4f3d7ec7daeb2f605997418fb93a52efb Mon Sep 17 00:00:00 2001 From: Gerry Chen Date: Sun, 20 Oct 2019 03:20:14 -0400 Subject: [PATCH] Caught some stragglers using boost::optional --- gtsam/discrete/DiscreteConditional.cpp | 19 ++++-- gtsam/discrete/DiscreteConditional.h | 6 +- gtsam/linear/Scatter.cpp | 7 +-- gtsam/linear/Scatter.h | 2 +- gtsam/nonlinear/NonlinearFactorGraph.cpp | 53 +++++++++++----- gtsam/nonlinear/NonlinearFactorGraph.h | 22 +++++-- gtsam/nonlinear/NonlinearOptimizerParams.h | 2 +- .../nonlinear/NonlinearClusterTree.h | 60 +++++++++++++++---- tests/testNonlinearFactorGraph.cpp | 2 +- 9 files changed, 131 insertions(+), 42 deletions(-) diff --git a/gtsam/discrete/DiscreteConditional.cpp b/gtsam/discrete/DiscreteConditional.cpp index f12cd2567..71d04f246 100644 --- a/gtsam/discrete/DiscreteConditional.cpp +++ b/gtsam/discrete/DiscreteConditional.cpp @@ -46,16 +46,25 @@ DiscreteConditional::DiscreteConditional(const size_t nrFrontals, /* ******************************************************************************** */ DiscreteConditional::DiscreteConditional(const DecisionTreeFactor& joint, - const DecisionTreeFactor& marginal, const boost::optional& orderedKeys) : + const DecisionTreeFactor& marginal) : BaseFactor( ISDEBUG("DiscreteConditional::COUNT") ? joint : joint / marginal), BaseConditional( joint.size()-marginal.size()) { if (ISDEBUG("DiscreteConditional::DiscreteConditional")) cout << (firstFrontalKey()) << endl; //TODO Print all keys - if (orderedKeys) { - keys_.clear(); - keys_.insert(keys_.end(), orderedKeys->begin(), orderedKeys->end()); - } +} + +/* ******************************************************************************** */ +DiscreteConditional::DiscreteConditional(const DecisionTreeFactor& joint, + const DecisionTreeFactor& marginal, const Ordering& orderedKeys) : + BaseFactor( + ISDEBUG("DiscreteConditional::COUNT") ? joint : joint / marginal), BaseConditional( + joint.size()-marginal.size()) { + if (ISDEBUG("DiscreteConditional::DiscreteConditional")) + cout << (firstFrontalKey()) << endl; //TODO Print all keys + + keys_.clear(); + keys_.insert(keys_.end(), orderedKeys.begin(), orderedKeys.end()); } /* ******************************************************************************** */ diff --git a/gtsam/discrete/DiscreteConditional.h b/gtsam/discrete/DiscreteConditional.h index 88c3e5620..3da8d0a82 100644 --- a/gtsam/discrete/DiscreteConditional.h +++ b/gtsam/discrete/DiscreteConditional.h @@ -60,7 +60,11 @@ public: /** construct P(X|Y)=P(X,Y)/P(Y) from P(X,Y) and P(Y) */ DiscreteConditional(const DecisionTreeFactor& joint, - const DecisionTreeFactor& marginal, const boost::optional& orderedKeys = boost::none); + const DecisionTreeFactor& marginal); + + /** construct P(X|Y)=P(X,Y)/P(Y) from P(X,Y) and P(Y) */ + DiscreteConditional(const DecisionTreeFactor& joint, + const DecisionTreeFactor& marginal, const Ordering& orderedKeys); /** * Combine several conditional into a single one. diff --git a/gtsam/linear/Scatter.cpp b/gtsam/linear/Scatter.cpp index 448120cdd..d201dfa78 100644 --- a/gtsam/linear/Scatter.cpp +++ b/gtsam/linear/Scatter.cpp @@ -34,8 +34,7 @@ string SlotEntry::toString() const { } /* ************************************************************************* */ -void Scatter::ScatterHelper(const GaussianFactorGraph& gfg, size_t sortStart) { - // Now, find dimensions of variables and/or extend +void Scatter::setDimensions(const GaussianFactorGraph& gfg, size_t sortStart) { for (const auto& factor : gfg) { if (!factor) continue; @@ -66,7 +65,7 @@ void Scatter::ScatterHelper(const GaussianFactorGraph& gfg, size_t sortStart) { Scatter::Scatter(const GaussianFactorGraph& gfg) { gttic(Scatter_Constructor); - ScatterHelper(gfg, 0); + setDimensions(gfg, 0); } /* ************************************************************************* */ @@ -79,7 +78,7 @@ Scatter::Scatter(const GaussianFactorGraph& gfg, add(key, 0); } - ScatterHelper(gfg, ordering.size()); + setDimensions(gfg, ordering.size()); } /* ************************************************************************* */ diff --git a/gtsam/linear/Scatter.h b/gtsam/linear/Scatter.h index 8936f827c..1583aa2f0 100644 --- a/gtsam/linear/Scatter.h +++ b/gtsam/linear/Scatter.h @@ -64,7 +64,7 @@ class Scatter : public FastVector { /// Helper function for constructors, adds/finds dimensions of variables and // sorts starting from sortStart - void ScatterHelper(const GaussianFactorGraph& gfg, size_t sortStart); + void setDimensions(const GaussianFactorGraph& gfg, size_t sortStart); /// Find the SlotEntry with the right key (linear time worst case) iterator find(Key key); diff --git a/gtsam/nonlinear/NonlinearFactorGraph.cpp b/gtsam/nonlinear/NonlinearFactorGraph.cpp index a4bdd83e3..3ad9cd9a6 100644 --- a/gtsam/nonlinear/NonlinearFactorGraph.cpp +++ b/gtsam/nonlinear/NonlinearFactorGraph.cpp @@ -344,23 +344,31 @@ GaussianFactorGraph::shared_ptr NonlinearFactorGraph::linearize(const Values& li } /* ************************************************************************* */ -static Scatter scatterFromValues(const Values& values, boost::optional ordering) { +static Scatter scatterFromValues(const Values& values) { gttic(scatterFromValues); Scatter scatter; scatter.reserve(values.size()); - if (!ordering) { - // use "natural" ordering with keys taken from the initial values - for (const auto& key_value : values) { - scatter.add(key_value.key, key_value.value.dim()); - } - } else { - // copy ordering into keys and lookup dimension in values, is O(n*log n) - for (Key key : *ordering) { - const Value& value = values.at(key); - scatter.add(key, value.dim()); - } + // use "natural" ordering with keys taken from the initial values + for (const auto& key_value : values) { + scatter.add(key_value.key, key_value.value.dim()); + } + + return scatter; +} + +/* ************************************************************************* */ +static Scatter scatterFromValues(const Values& values, const Ordering& ordering) { + gttic(scatterFromValues); + + Scatter scatter; + scatter.reserve(values.size()); + + // copy ordering into keys and lookup dimension in values, is O(n*log n) + for (Key key : ordering) { + const Value& value = values.at(key); + scatter.add(key, value.dim()); } return scatter; @@ -368,7 +376,15 @@ static Scatter scatterFromValues(const Values& values, boost::optional ordering, const Dampen& dampen) const { + const Values& values, const Dampen& dampen) const { + KeyVector keys = values.keys(); + Ordering defaultOrdering(keys); + return linearizeToHessianFactor(values, defaultOrdering, dampen); +} + +/* ************************************************************************* */ +HessianFactor::shared_ptr NonlinearFactorGraph::linearizeToHessianFactor( + const Values& values, const Ordering& ordering, const Dampen& dampen) const { gttic(NonlinearFactorGraph_linearizeToHessianFactor); Scatter scatter = scatterFromValues(values, ordering); @@ -395,7 +411,16 @@ HessianFactor::shared_ptr NonlinearFactorGraph::linearizeToHessianFactor( /* ************************************************************************* */ Values NonlinearFactorGraph::updateCholesky(const Values& values, - boost::optional ordering, + const Dampen& dampen) const { + gttic(NonlinearFactorGraph_updateCholesky); + auto hessianFactor = linearizeToHessianFactor(values, dampen); + VectorValues delta = hessianFactor->solve(); + return values.retract(delta); +} + +/* ************************************************************************* */ +Values NonlinearFactorGraph::updateCholesky(const Values& values, + const Ordering& ordering, const Dampen& dampen) const { gttic(NonlinearFactorGraph_updateCholesky); auto hessianFactor = linearizeToHessianFactor(values, ordering, dampen); diff --git a/gtsam/nonlinear/NonlinearFactorGraph.h b/gtsam/nonlinear/NonlinearFactorGraph.h index 323461459..0b0db1b7b 100644 --- a/gtsam/nonlinear/NonlinearFactorGraph.h +++ b/gtsam/nonlinear/NonlinearFactorGraph.h @@ -149,17 +149,31 @@ namespace gtsam { * Instead of producing a GaussianFactorGraph, pre-allocate and linearize directly * into a HessianFactor. Avoids the many mallocs and pointer indirection in constructing * a new graph, and hence useful in case a dense solve is appropriate for your problem. - * An optional ordering can be given that still decides how the Hessian is laid out. * An optional lambda function can be used to apply damping on the filled Hessian. * No parallelism is exploited, because all the factors write in the same memory. */ boost::shared_ptr linearizeToHessianFactor( - const Values& values, boost::optional ordering = boost::none, - const Dampen& dampen = nullptr) const; + const Values& values, const Dampen& dampen = nullptr) const; + + /** + * Instead of producing a GaussianFactorGraph, pre-allocate and linearize directly + * into a HessianFactor. Avoids the many mallocs and pointer indirection in constructing + * a new graph, and hence useful in case a dense solve is appropriate for your problem. + * An ordering is given that still decides how the Hessian is laid out. + * An optional lambda function can be used to apply damping on the filled Hessian. + * No parallelism is exploited, because all the factors write in the same memory. + */ + boost::shared_ptr linearizeToHessianFactor( + const Values& values, const Ordering& ordering, const Dampen& dampen = nullptr) const; /// Linearize and solve in one pass. /// Calls linearizeToHessianFactor, densely solves the normal equations, and updates the values. - Values updateCholesky(const Values& values, boost::optional ordering = boost::none, + Values updateCholesky(const Values& values, + const Dampen& dampen = nullptr) const; + + /// Linearize and solve in one pass. + /// Calls linearizeToHessianFactor, densely solves the normal equations, and updates the values. + Values updateCholesky(const Values& values, const Ordering& ordering, const Dampen& dampen = nullptr) const; /// Clone() performs a deep-copy of the graph, including all of the factors diff --git a/gtsam/nonlinear/NonlinearOptimizerParams.h b/gtsam/nonlinear/NonlinearOptimizerParams.h index 180f4fb84..9757cca73 100644 --- a/gtsam/nonlinear/NonlinearOptimizerParams.h +++ b/gtsam/nonlinear/NonlinearOptimizerParams.h @@ -82,7 +82,7 @@ public: }; LinearSolverType linearSolverType; ///< The type of linear solver to use in the nonlinear optimizer - boost::optional ordering; ///< The variable elimination ordering, or empty to use COLAMD (default: empty) + boost::optional ordering; ///< The optional variable elimination ordering, or empty to use COLAMD (default: empty) IterativeOptimizationParameters::shared_ptr iterativeParams; ///< The container for iterativeOptimization parameters. used in CG Solvers. inline bool isMultifrontal() const { diff --git a/gtsam_unstable/nonlinear/NonlinearClusterTree.h b/gtsam_unstable/nonlinear/NonlinearClusterTree.h index a483c9521..5d089f123 100644 --- a/gtsam_unstable/nonlinear/NonlinearClusterTree.h +++ b/gtsam_unstable/nonlinear/NonlinearClusterTree.h @@ -46,21 +46,26 @@ class NonlinearClusterTree : public ClusterTree { // linearize local custer factors straight into hessianFactor, which is returned // If no ordering given, uses colamd HessianFactor::shared_ptr linearizeToHessianFactor( - const Values& values, boost::optional ordering = boost::none, + const Values& values, const NonlinearFactorGraph::Dampen& dampen = nullptr) const { - if (!ordering) - ordering.reset(Ordering::ColamdConstrainedFirst(factors, orderedFrontalKeys, true)); - return factors.linearizeToHessianFactor(values, *ordering, dampen); + Ordering ordering; + ordering = Ordering::ColamdConstrainedFirst(factors, orderedFrontalKeys, true); + return factors.linearizeToHessianFactor(values, ordering, dampen); } - // Recursively eliminate subtree rooted at this Cluster into a Bayes net and factor on parent + // linearize local custer factors straight into hessianFactor, which is returned + // If no ordering given, uses colamd + HessianFactor::shared_ptr linearizeToHessianFactor( + const Values& values, const Ordering& ordering, + const NonlinearFactorGraph::Dampen& dampen = nullptr) const { + return factors.linearizeToHessianFactor(values, ordering, dampen); + } + + // Helper function: recursively eliminate subtree rooted at this Cluster into a Bayes net and factor on parent // TODO(frank): Use TBB to support deep trees and parallelism std::pair linearizeAndEliminate( - const Values& values, boost::optional ordering = boost::none, - const NonlinearFactorGraph::Dampen& dampen = nullptr) const { - // Linearize and create HessianFactor f(front,separator) - HessianFactor::shared_ptr localFactor = linearizeToHessianFactor(values, ordering, dampen); - + const Values& values, + const HessianFactor::shared_ptr& localFactor) const { // Get contributions f(front) from children, as well as p(children|front) GaussianBayesNet bayesNet; for (const auto& child : children) { @@ -72,12 +77,45 @@ class NonlinearClusterTree : public ClusterTree { return {bayesNet, localFactor}; } + // Recursively eliminate subtree rooted at this Cluster into a Bayes net and factor on parent + // TODO(frank): Use TBB to support deep trees and parallelism + std::pair linearizeAndEliminate( + const Values& values, + const NonlinearFactorGraph::Dampen& dampen = nullptr) const { + // Linearize and create HessianFactor f(front,separator) + HessianFactor::shared_ptr localFactor = linearizeToHessianFactor(values, dampen); + return linearizeAndEliminate(values, localFactor); + } + + // Recursively eliminate subtree rooted at this Cluster into a Bayes net and factor on parent + // TODO(frank): Use TBB to support deep trees and parallelism + std::pair linearizeAndEliminate( + const Values& values, const Ordering& ordering, + const NonlinearFactorGraph::Dampen& dampen = nullptr) const { + // Linearize and create HessianFactor f(front,separator) + HessianFactor::shared_ptr localFactor = linearizeToHessianFactor(values, ordering, dampen); + return linearizeAndEliminate(values, localFactor); + } + // Recursively eliminate subtree rooted at this Cluster // Version that updates existing Bayes net and returns a new Hessian factor on parent clique // It is possible to pass in a nullptr for the bayesNet if only interested in the new factor HessianFactor::shared_ptr linearizeAndEliminate( const Values& values, GaussianBayesNet* bayesNet, - boost::optional ordering = boost::none, + const NonlinearFactorGraph::Dampen& dampen = nullptr) const { + auto bayesNet_newFactor_pair = linearizeAndEliminate(values, dampen); + if (bayesNet) { + bayesNet->push_back(bayesNet_newFactor_pair.first); + } + return bayesNet_newFactor_pair.second; + } + + // Recursively eliminate subtree rooted at this Cluster + // Version that updates existing Bayes net and returns a new Hessian factor on parent clique + // It is possible to pass in a nullptr for the bayesNet if only interested in the new factor + HessianFactor::shared_ptr linearizeAndEliminate( + const Values& values, GaussianBayesNet* bayesNet, + const Ordering& ordering, const NonlinearFactorGraph::Dampen& dampen = nullptr) const { auto bayesNet_newFactor_pair = linearizeAndEliminate(values, ordering, dampen); if (bayesNet) { diff --git a/tests/testNonlinearFactorGraph.cpp b/tests/testNonlinearFactorGraph.cpp index 290f0138e..6a7a963bc 100644 --- a/tests/testNonlinearFactorGraph.cpp +++ b/tests/testNonlinearFactorGraph.cpp @@ -198,7 +198,7 @@ TEST(NonlinearFactorGraph, UpdateCholesky) { } } }; - EXPECT(assert_equal(initial, fg.updateCholesky(initial, boost::none, dampen), 1e-6)); + EXPECT(assert_equal(initial, fg.updateCholesky(initial, dampen), 1e-6)); } /* ************************************************************************* */