From 8e62a1405e5cc7b4a059b691ffda6e96d2b328f7 Mon Sep 17 00:00:00 2001 From: Gerry Chen Date: Mon, 28 Oct 2019 17:41:16 -0400 Subject: [PATCH] deprecated functions for backwards compatibility also removed some edits that were tangential to the PR. --- gtsam/base/TestableAssertions.h | 8 +- gtsam/discrete/DiscreteConditional.cpp | 7 +- .../inference/EliminateableFactorGraph-inst.h | 16 ++-- gtsam/inference/EliminateableFactorGraph.h | 73 ++++++++++++++++--- gtsam/linear/GaussianFactorGraph.cpp | 7 ++ gtsam/linear/GaussianFactorGraph.h | 6 ++ gtsam/linear/Scatter.cpp | 35 ++++----- gtsam/linear/Scatter.h | 5 -- gtsam/nonlinear/Marginals.h | 21 +++++- gtsam/nonlinear/NonlinearFactorGraph.cpp | 32 ++++---- gtsam/nonlinear/NonlinearFactorGraph.h | 20 +++++ gtsam/nonlinear/NonlinearOptimizer.cpp | 16 ++-- tests/testGaussianFactorGraphB.cpp | 2 + tests/testNonlinearFactorGraph.cpp | 2 +- 14 files changed, 172 insertions(+), 78 deletions(-) diff --git a/gtsam/base/TestableAssertions.h b/gtsam/base/TestableAssertions.h index a698f8f98..1a31aa284 100644 --- a/gtsam/base/TestableAssertions.h +++ b/gtsam/base/TestableAssertions.h @@ -71,8 +71,12 @@ bool assert_equal(const V& expected, const boost::optional& actual, double to } template -bool assert_equal(const V& expected, double tol = 1e-9) { - return false; +bool assert_equal(const V& expected, const boost::optional& actual, double tol = 1e-9) { + if (!actual) { + std::cout << "actual is boost::none" << std::endl; + return false; + } + return assert_equal(expected, *actual, tol); } /** diff --git a/gtsam/discrete/DiscreteConditional.cpp b/gtsam/discrete/DiscreteConditional.cpp index 71d04f246..cf2997ce0 100644 --- a/gtsam/discrete/DiscreteConditional.cpp +++ b/gtsam/discrete/DiscreteConditional.cpp @@ -57,12 +57,7 @@ DiscreteConditional::DiscreteConditional(const DecisionTreeFactor& joint, /* ******************************************************************************** */ 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 - + DiscreteConditional(joint, marginal) { keys_.clear(); keys_.insert(keys_.end(), orderedKeys.begin(), orderedKeys.end()); } diff --git a/gtsam/inference/EliminateableFactorGraph-inst.h b/gtsam/inference/EliminateableFactorGraph-inst.h index a77d96537..81f4047a1 100644 --- a/gtsam/inference/EliminateableFactorGraph-inst.h +++ b/gtsam/inference/EliminateableFactorGraph-inst.h @@ -28,8 +28,8 @@ namespace gtsam { template boost::shared_ptr::BayesNetType> EliminateableFactorGraph::eliminateSequential( - const Eliminate& function, OptionalVariableIndex variableIndex, - OptionalOrderingType orderingType) const { + OptionalOrderingType orderingType, const Eliminate& function, + OptionalVariableIndex variableIndex) const { if(!variableIndex) { // If no VariableIndex provided, compute one and call this function again IMPORTANT: we check // for no variable index first so that it's always computed if we need to call COLAMD because @@ -56,12 +56,12 @@ namespace gtsam { boost::shared_ptr::BayesNetType> EliminateableFactorGraph::eliminateSequential( const Ordering& ordering, const Eliminate& function, - OptionalVariableIndex variableIndex, OptionalOrderingType orderingType) const + OptionalVariableIndex variableIndex) const { if(!variableIndex) { // If no VariableIndex provided, compute one and call this function again VariableIndex computedVariableIndex(asDerived()); - return eliminateSequential(ordering, function, computedVariableIndex, orderingType); + return eliminateSequential(ordering, function, computedVariableIndex); } else { gttic(eliminateSequential); // Do elimination @@ -81,8 +81,8 @@ namespace gtsam { template boost::shared_ptr::BayesTreeType> EliminateableFactorGraph::eliminateMultifrontal( - const Eliminate& function, OptionalVariableIndex variableIndex, - OptionalOrderingType orderingType) const + OptionalOrderingType orderingType, const Eliminate& function, + OptionalVariableIndex variableIndex) const { if(!variableIndex) { // If no VariableIndex provided, compute one and call this function again IMPORTANT: we check @@ -110,12 +110,12 @@ namespace gtsam { boost::shared_ptr::BayesTreeType> EliminateableFactorGraph::eliminateMultifrontal( const Ordering& ordering, const Eliminate& function, - OptionalVariableIndex variableIndex, OptionalOrderingType orderingType) const + OptionalVariableIndex variableIndex) const { if(!variableIndex) { // If no VariableIndex provided, compute one and call this function again VariableIndex computedVariableIndex(asDerived()); - return eliminateMultifrontal(ordering, function, computedVariableIndex, orderingType); + return eliminateMultifrontal(ordering, function, computedVariableIndex); } else { gttic(eliminateMultifrontal); // Do elimination with given ordering diff --git a/gtsam/inference/EliminateableFactorGraph.h b/gtsam/inference/EliminateableFactorGraph.h index 8a216918c..316ca8ee4 100644 --- a/gtsam/inference/EliminateableFactorGraph.h +++ b/gtsam/inference/EliminateableFactorGraph.h @@ -89,7 +89,7 @@ namespace gtsam { typedef boost::function Eliminate; /// Typedef for an optional variable index as an argument to elimination functions - typedef const boost::optional& OptionalVariableIndex; + typedef boost::optional OptionalVariableIndex; /// Typedef for an optional ordering type typedef boost::optional OptionalOrderingType; @@ -115,9 +115,9 @@ namespace gtsam { * \endcode * */ boost::shared_ptr eliminateSequential( + OptionalOrderingType orderingType = boost::none, const Eliminate& function = EliminationTraitsType::DefaultEliminate, - OptionalVariableIndex variableIndex = boost::none, - OptionalOrderingType orderingType = boost::none) const; + OptionalVariableIndex variableIndex = boost::none) const; /** Do sequential elimination of all variables to produce a Bayes net. * @@ -136,8 +136,7 @@ namespace gtsam { boost::shared_ptr eliminateSequential( const Ordering& ordering, const Eliminate& function = EliminationTraitsType::DefaultEliminate, - OptionalVariableIndex variableIndex = boost::none, - OptionalOrderingType orderingType = boost::none) const; // orderingType is not necessary anymore, kept for backwards compatibility + OptionalVariableIndex variableIndex = boost::none) const; /** Do multifrontal elimination of all variables to produce a Bayes tree. If an ordering is not * provided, the ordering will be computed using either COLAMD or METIS, dependeing on @@ -156,9 +155,9 @@ namespace gtsam { * \endcode * */ boost::shared_ptr eliminateMultifrontal( + OptionalOrderingType orderingType = boost::none, const Eliminate& function = EliminationTraitsType::DefaultEliminate, - OptionalVariableIndex variableIndex = boost::none, - OptionalOrderingType orderingType = boost::none) const; + OptionalVariableIndex variableIndex = boost::none) const; /** Do multifrontal elimination of all variables to produce a Bayes tree. If an ordering is not * provided, the ordering will be computed using either COLAMD or METIS, dependeing on @@ -172,8 +171,7 @@ namespace gtsam { boost::shared_ptr eliminateMultifrontal( const Ordering& ordering, const Eliminate& function = EliminationTraitsType::DefaultEliminate, - OptionalVariableIndex variableIndex = boost::none, - OptionalOrderingType orderingType = boost::none) const; // orderingType no longer needed + OptionalVariableIndex variableIndex = boost::none) const; /** Do sequential elimination of some variables, in \c ordering provided, to produce a Bayes net * and a remaining factor graph. This computes the factorization \f$ p(X) = p(A|B) p(B) \f$, @@ -241,7 +239,7 @@ namespace gtsam { * provided one will be computed. */ boost::shared_ptr marginalMultifrontalBayesNet( boost::variant variables, - const Ordering& marginalizedVariableOrdering, // this no longer takes boost::none - potentially code breaking + const Ordering& marginalizedVariableOrdering, const Eliminate& function = EliminationTraitsType::DefaultEliminate, OptionalVariableIndex variableIndex = boost::none) const; @@ -271,7 +269,7 @@ namespace gtsam { * provided one will be computed. */ boost::shared_ptr marginalMultifrontalBayesTree( boost::variant variables, - const Ordering& marginalizedVariableOrdering, // this no longer takes boost::none - potentially code breaking + const Ordering& marginalizedVariableOrdering, const Eliminate& function = EliminationTraitsType::DefaultEliminate, OptionalVariableIndex variableIndex = boost::none) const; @@ -288,6 +286,59 @@ namespace gtsam { // Access the derived factor graph class FactorGraphType& asDerived() { return static_cast(*this); } + + public: + /** \deprecated ordering and orderingType shouldn't both be specified */ + boost::shared_ptr eliminateSequential( + const Ordering& ordering, + const Eliminate& function, + OptionalVariableIndex variableIndex, + OptionalOrderingType orderingType) const { + return eliminateSequential(ordering, function, variableIndex); + } + + /** \deprecated orderingType specified first for consistency */ + boost::shared_ptr eliminateSequential( + const Eliminate& function, + OptionalVariableIndex variableIndex = boost::none, + OptionalOrderingType orderingType = boost::none) const { + return eliminateSequential(orderingType, function, variableIndex); + } + + /** \deprecated ordering and orderingType shouldn't both be specified */ + boost::shared_ptr eliminateMultifrontal( + const Ordering& ordering, + const Eliminate& function, + OptionalVariableIndex variableIndex, + OptionalOrderingType orderingType) const { + return eliminateMultifrontal(ordering, function, variableIndex); + } + + /** \deprecated orderingType specified first for consistency */ + boost::shared_ptr eliminateMultifrontal( + const Eliminate& function, + OptionalVariableIndex variableIndex = boost::none, + OptionalOrderingType orderingType = boost::none) const { + return eliminateMultifrontal(orderingType, function, variableIndex); + } + + /** \deprecated */ + boost::shared_ptr marginalMultifrontalBayesNet( + boost::variant variables, + boost::none_t, + const Eliminate& function = EliminationTraitsType::DefaultEliminate, + OptionalVariableIndex variableIndex = boost::none) const { + return marginalMultifrontalBayesNet(variables, function, variableIndex); + } + + /** \deprecated */ + boost::shared_ptr marginalMultifrontalBayesTree( + boost::variant variables, + boost::none_t, + const Eliminate& function = EliminationTraitsType::DefaultEliminate, + OptionalVariableIndex variableIndex = boost::none) const { + return marginalMultifrontalBayesTree(variables, function, variableIndex); + } }; } diff --git a/gtsam/linear/GaussianFactorGraph.cpp b/gtsam/linear/GaussianFactorGraph.cpp index faa3f8bd6..8dc3600c6 100644 --- a/gtsam/linear/GaussianFactorGraph.cpp +++ b/gtsam/linear/GaussianFactorGraph.cpp @@ -494,4 +494,11 @@ namespace gtsam { return e; } + /* ************************************************************************* */ + /** \deprecated */ + VectorValues GaussianFactorGraph::optimize(boost::none_t, + const Eliminate& function) const { + return optimize(function); + } + } // namespace gtsam diff --git a/gtsam/linear/GaussianFactorGraph.h b/gtsam/linear/GaussianFactorGraph.h index f24fb602d..2b9e8e675 100644 --- a/gtsam/linear/GaussianFactorGraph.h +++ b/gtsam/linear/GaussianFactorGraph.h @@ -375,6 +375,12 @@ namespace gtsam { ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); } + public: + + /** \deprecated */ + VectorValues optimize(boost::none_t, + const Eliminate& function = EliminationTraitsType::DefaultEliminate) const; + }; /** diff --git a/gtsam/linear/Scatter.cpp b/gtsam/linear/Scatter.cpp index d201dfa78..07ecaf483 100644 --- a/gtsam/linear/Scatter.cpp +++ b/gtsam/linear/Scatter.cpp @@ -33,8 +33,19 @@ string SlotEntry::toString() const { return oss.str(); } +Scatter::Scatter(const GaussianFactorGraph& gfg) : Scatter(gfg, Ordering()) {} + /* ************************************************************************* */ -void Scatter::setDimensions(const GaussianFactorGraph& gfg, size_t sortStart) { +Scatter::Scatter(const GaussianFactorGraph& gfg, + const Ordering& ordering) { + gttic(Scatter_Constructor); + + // If we have an ordering, pre-fill the ordered variables first + for (Key key : ordering) { + add(key, 0); + } + + // Now, find dimensions of variables and/or extend for (const auto& factor : gfg) { if (!factor) continue; @@ -57,30 +68,10 @@ void Scatter::setDimensions(const GaussianFactorGraph& gfg, size_t sortStart) { // To keep the same behavior as before, sort the keys after the ordering iterator first = begin(); - first += sortStart; + first += ordering.size(); if (first != end()) std::sort(first, end()); } -/* ************************************************************************* */ -Scatter::Scatter(const GaussianFactorGraph& gfg) { - gttic(Scatter_Constructor); - - setDimensions(gfg, 0); -} - -/* ************************************************************************* */ -Scatter::Scatter(const GaussianFactorGraph& gfg, - const Ordering& ordering) { - gttic(Scatter_Constructor); - - // pre-fill the ordered variables first - for (Key key : ordering) { - add(key, 0); - } - - setDimensions(gfg, ordering.size()); -} - /* ************************************************************************* */ void Scatter::add(Key key, size_t dim) { emplace_back(SlotEntry(key, dim)); diff --git a/gtsam/linear/Scatter.h b/gtsam/linear/Scatter.h index 1583aa2f0..3b34d170c 100644 --- a/gtsam/linear/Scatter.h +++ b/gtsam/linear/Scatter.h @@ -61,11 +61,6 @@ class Scatter : public FastVector { void add(Key key, size_t dim); private: - - /// Helper function for constructors, adds/finds dimensions of variables and - // sorts starting from 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/Marginals.h b/gtsam/nonlinear/Marginals.h index abad71ea7..4e201cc38 100644 --- a/gtsam/nonlinear/Marginals.h +++ b/gtsam/nonlinear/Marginals.h @@ -64,7 +64,7 @@ public: * @param factorization The linear decomposition mode - either Marginals::CHOLESKY (faster and suitable for most problems) or Marginals::QR (slower but more numerically stable for poorly-conditioned problems). * @param ordering The ordering for elimination. */ - Marginals(const NonlinearFactorGraph& graph, const Values& solution, const Ordering& ordering, // argument order switch due to default value of factorization, potentially code breaking + Marginals(const NonlinearFactorGraph& graph, const Values& solution, const Ordering& ordering, Factorization factorization = CHOLESKY); /** Construct a marginals class from a linear factor graph. @@ -80,7 +80,7 @@ public: * @param factorization The linear decomposition mode - either Marginals::CHOLESKY (faster and suitable for most problems) or Marginals::QR (slower but more numerically stable for poorly-conditioned problems). * @param ordering The ordering for elimination. */ - Marginals(const GaussianFactorGraph& graph, const Values& solution, const Ordering& ordering, // argument order switch due to default value of factorization, potentially code breaking + Marginals(const GaussianFactorGraph& graph, const Values& solution, const Ordering& ordering, Factorization factorization = CHOLESKY); /** Construct a marginals class from a linear factor graph. @@ -97,7 +97,7 @@ public: * @param factorization The linear decomposition mode - either Marginals::CHOLESKY (faster and suitable for most problems) or Marginals::QR (slower but more numerically stable for poorly-conditioned problems). * @param ordering An optional variable ordering for elimination. */ - Marginals(const GaussianFactorGraph& graph, const VectorValues& solution, const Ordering& ordering, // argument order switch due to default value of factorization, potentially code breaking + Marginals(const GaussianFactorGraph& graph, const VectorValues& solution, const Ordering& ordering, Factorization factorization = CHOLESKY); /** print */ @@ -121,7 +121,7 @@ public: /** Optimize the bayes tree */ VectorValues optimize() const; - + protected: /** Compute the Bayes Tree as a helper function to the constructor */ @@ -130,6 +130,19 @@ protected: /** Compute the Bayes Tree as a helper function to the constructor */ void computeBayesTree(const Ordering& ordering); +public: + /** \deprecated argument order changed due to removing boost::optional */ + Marginals(const NonlinearFactorGraph& graph, const Values& solution, Factorization factorization, + const Ordering& ordering) : Marginals(graph, solution, ordering, factorization) {} + + /** \deprecated argument order changed due to removing boost::optional */ + Marginals(const GaussianFactorGraph& graph, const Values& solution, Factorization factorization, + const Ordering& ordering) : Marginals(graph, solution, ordering, factorization) {} + + /** \deprecated argument order changed due to removing boost::optional */ + Marginals(const GaussianFactorGraph& graph, const VectorValues& solution, Factorization factorization, + const Ordering& ordering) : Marginals(graph, solution, ordering, factorization) {} + }; /** diff --git a/gtsam/nonlinear/NonlinearFactorGraph.cpp b/gtsam/nonlinear/NonlinearFactorGraph.cpp index 3ad9cd9a6..d4b9fbb68 100644 --- a/gtsam/nonlinear/NonlinearFactorGraph.cpp +++ b/gtsam/nonlinear/NonlinearFactorGraph.cpp @@ -376,19 +376,7 @@ static Scatter scatterFromValues(const Values& values, const Ordering& ordering) /* ************************************************************************* */ HessianFactor::shared_ptr NonlinearFactorGraph::linearizeToHessianFactor( - 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); - + const Values& values, const Scatter& scatter, const Dampen& dampen) const { // NOTE(frank): we are heavily leaning on friendship below HessianFactor::shared_ptr hessianFactor(new HessianFactor(scatter)); @@ -409,6 +397,24 @@ HessianFactor::shared_ptr NonlinearFactorGraph::linearizeToHessianFactor( return hessianFactor; } +/* ************************************************************************* */ +HessianFactor::shared_ptr NonlinearFactorGraph::linearizeToHessianFactor( + const Values& values, const Ordering& order, const Dampen& dampen) const { + gttic(NonlinearFactorGraph_linearizeToHessianFactor); + + Scatter scatter = scatterFromValues(values, order); + return linearizeToHessianFactor(values, scatter, dampen); +} + +/* ************************************************************************* */ +HessianFactor::shared_ptr NonlinearFactorGraph::linearizeToHessianFactor( + const Values& values, const Dampen& dampen) const { + gttic(NonlinearFactorGraph_linearizeToHessianFactor); + + Scatter scatter = scatterFromValues(values); + return linearizeToHessianFactor(values, scatter, dampen); +} + /* ************************************************************************* */ Values NonlinearFactorGraph::updateCholesky(const Values& values, const Dampen& dampen) const { diff --git a/gtsam/nonlinear/NonlinearFactorGraph.h b/gtsam/nonlinear/NonlinearFactorGraph.h index 0b0db1b7b..902a47a0f 100644 --- a/gtsam/nonlinear/NonlinearFactorGraph.h +++ b/gtsam/nonlinear/NonlinearFactorGraph.h @@ -204,6 +204,13 @@ namespace gtsam { private: + /** + * Linearize from Scatter rather than from Ordering. Made private because + * it doesn't include gttic. + */ + boost::shared_ptr linearizeToHessianFactor( + const Values& values, const Scatter& scatter, const Dampen& dampen = nullptr) const; + /** Serialization function */ friend class boost::serialization::access; template @@ -211,6 +218,19 @@ namespace gtsam { ar & boost::serialization::make_nvp("NonlinearFactorGraph", boost::serialization::base_object(*this)); } + + public: + + /** \deprecated */ + boost::shared_ptr linearizeToHessianFactor( + const Values& values, boost::none_t, const Dampen& dampen = nullptr) const + {return linearizeToHessianFactor(values, dampen);} + + /** \deprecated */ + Values updateCholesky(const Values& values, boost::none_t, + const Dampen& dampen = nullptr) const + {return updateCholesky(values, dampen);} + }; /// traits diff --git a/gtsam/nonlinear/NonlinearOptimizer.cpp b/gtsam/nonlinear/NonlinearOptimizer.cpp index b12dcf70a..ad3b9c4cc 100644 --- a/gtsam/nonlinear/NonlinearOptimizer.cpp +++ b/gtsam/nonlinear/NonlinearOptimizer.cpp @@ -128,18 +128,22 @@ VectorValues NonlinearOptimizer::solve(const GaussianFactorGraph& gfg, const NonlinearOptimizerParams& params) const { // solution of linear solver is an update to the linearization point VectorValues delta; - Ordering ordering; - if (params.ordering) - ordering = *params.ordering; // Check which solver we are using if (params.isMultifrontal()) { // Multifrontal QR or Cholesky (decided by params.getEliminationFunction()) - delta = gfg.optimize(ordering, params.getEliminationFunction()); + if (params.ordering) + delta = gfg.optimize(*params.ordering, params.getEliminationFunction()); + else + delta = gfg.optimize(params.getEliminationFunction()); } else if (params.isSequential()) { // Sequential QR or Cholesky (decided by params.getEliminationFunction()) - delta = gfg.eliminateSequential(ordering, params.getEliminationFunction(), boost::none, - params.orderingType)->optimize(); + if (params.ordering) + delta = gfg.eliminateSequential(*params.ordering, params.getEliminationFunction(), + boost::none, params.orderingType)->optimize(); + else + delta = gfg.eliminateSequential(params.getEliminationFunction(), boost::none, + params.orderingType)->optimize(); } else if (params.isIterative()) { // Conjugate Gradient -> needs params.iterativeParams if (!params.iterativeParams) diff --git a/tests/testGaussianFactorGraphB.cpp b/tests/testGaussianFactorGraphB.cpp index bf968c8d7..9596f4af5 100644 --- a/tests/testGaussianFactorGraphB.cpp +++ b/tests/testGaussianFactorGraphB.cpp @@ -206,6 +206,7 @@ TEST(GaussianFactorGraph, optimize_Cholesky) { GaussianFactorGraph fg = createGaussianFactorGraph(); // optimize the graph + VectorValues actual1 = fg.optimize(boost::none, EliminateCholesky); VectorValues actual = fg.optimize(EliminateCholesky); // verify @@ -220,6 +221,7 @@ TEST( GaussianFactorGraph, optimize_QR ) GaussianFactorGraph fg = createGaussianFactorGraph(); // optimize the graph + VectorValues actual1 = fg.optimize(boost::none, EliminateQR); VectorValues actual = fg.optimize(EliminateQR); // verify diff --git a/tests/testNonlinearFactorGraph.cpp b/tests/testNonlinearFactorGraph.cpp index 6a7a963bc..290f0138e 100644 --- a/tests/testNonlinearFactorGraph.cpp +++ b/tests/testNonlinearFactorGraph.cpp @@ -198,7 +198,7 @@ TEST(NonlinearFactorGraph, UpdateCholesky) { } } }; - EXPECT(assert_equal(initial, fg.updateCholesky(initial, dampen), 1e-6)); + EXPECT(assert_equal(initial, fg.updateCholesky(initial, boost::none, dampen), 1e-6)); } /* ************************************************************************* */