From 80e2179a8db6e4582fb142d67f7bfeab784f55b7 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 9 Jun 2012 21:33:10 +0000 Subject: [PATCH] Cleaned up typedefs in FactorGraph.h (and removed FactorizationResult), and also made sure ::shared_ptr was never assumed to exist for a FACTOR template argument. Should it exist, ever? --- gtsam/inference/EliminationTree-inl.h | 2 +- gtsam/inference/EliminationTree.h | 2 +- gtsam/inference/FactorGraph-inl.h | 22 +++-- gtsam/inference/FactorGraph.h | 81 +++++++---------- .../inference/GenericMultifrontalSolver-inl.h | 2 +- gtsam/inference/GenericMultifrontalSolver.h | 2 +- gtsam/inference/GenericSequentialSolver-inl.h | 8 +- gtsam/inference/GenericSequentialSolver.h | 2 +- gtsam/inference/inference-inl.h | 25 +++--- gtsam/inference/inference.h | 88 ++++++++++--------- gtsam/linear/GaussianFactorGraph.h | 2 +- tests/testGaussianFactorGraphB.cpp | 12 +-- 12 files changed, 126 insertions(+), 122 deletions(-) diff --git a/gtsam/inference/EliminationTree-inl.h b/gtsam/inference/EliminationTree-inl.h index 2f62f79af..c4cb9d4cf 100644 --- a/gtsam/inference/EliminationTree-inl.h +++ b/gtsam/inference/EliminationTree-inl.h @@ -132,7 +132,7 @@ typename EliminationTree::shared_ptr EliminationTree::Create( // Hang factors in right places tic(3, "hang factors"); - BOOST_FOREACH(const typename DERIVEDFACTOR::shared_ptr& derivedFactor, factorGraph) { + BOOST_FOREACH(const typename boost::shared_ptr& derivedFactor, factorGraph) { // Here we upwards-cast to the factor type of this EliminationTree. This // allows performing symbolic elimination on, for example, GaussianFactors. if(derivedFactor) { diff --git a/gtsam/inference/EliminationTree.h b/gtsam/inference/EliminationTree.h index eb45deaf0..6ab7dcf49 100644 --- a/gtsam/inference/EliminationTree.h +++ b/gtsam/inference/EliminationTree.h @@ -52,7 +52,7 @@ public: typedef EliminationTree This; ///< This class typedef boost::shared_ptr shared_ptr; ///< Shared pointer to this class - typedef typename FACTOR::shared_ptr sharedFactor; ///< Shared pointer to a factor + typedef typename boost::shared_ptr sharedFactor; ///< Shared pointer to a factor typedef gtsam::BayesNet BayesNet; ///< The BayesNet corresponding to FACTOR /** Typedef for an eliminate subroutine */ diff --git a/gtsam/inference/FactorGraph-inl.h b/gtsam/inference/FactorGraph-inl.h index 8da4465d5..90212f39f 100644 --- a/gtsam/inference/FactorGraph-inl.h +++ b/gtsam/inference/FactorGraph-inl.h @@ -36,6 +36,16 @@ namespace gtsam { + /* ************************************************************************* */ + template + template + FactorGraph::FactorGraph(const BayesNet& bayesNet) { + factors_.reserve(bayesNet.size()); + BOOST_FOREACH(const typename CONDITIONAL::shared_ptr& cond, bayesNet) { + this->push_back(cond->toFactor()); + } + } + /* ************************************************************************* */ template void FactorGraph::print(const std::string& s) const { @@ -50,7 +60,7 @@ namespace gtsam { /* ************************************************************************* */ template - bool FactorGraph::equals(const FactorGraph& fg, double tol) const { + bool FactorGraph::equals(const This& fg, double tol) const { /** check whether the two factor graphs have the same number of factors_ */ if (factors_.size() != fg.size()) return false; @@ -111,20 +121,20 @@ namespace gtsam { } /* ************************************************************************* */ + // Recursive function to add factors in cliques to vector of factors_io template void _FactorGraph_BayesTree_adder( - std::vector::sharedFactor>& factors, + std::vector >& factors_io, const typename BayesTree::sharedClique& clique) { if(clique) { // Add factor from this clique - factors.push_back((*clique)->toFactor()); + factors_io.push_back((*clique)->toFactor()); // Traverse children typedef typename BayesTree::sharedClique sharedClique; - BOOST_FOREACH(const sharedClique& child, clique->children()) { - _FactorGraph_BayesTree_adder(factors, child); - } + BOOST_FOREACH(const sharedClique& child, clique->children()) + _FactorGraph_BayesTree_adder(factors_io, child); } } diff --git a/gtsam/inference/FactorGraph.h b/gtsam/inference/FactorGraph.h index 90e9e8a1f..621c18b9f 100644 --- a/gtsam/inference/FactorGraph.h +++ b/gtsam/inference/FactorGraph.h @@ -21,14 +21,13 @@ #pragma once -#include -#include -#include - #include #include #include +#include +#include + namespace gtsam { // Forward declarations @@ -41,30 +40,28 @@ template class BayesTree; */ template class FactorGraph { + public: - typedef FACTOR FactorType; - typedef typename FACTOR::KeyType KeyType; - typedef boost::shared_ptr > shared_ptr; - typedef typename boost::shared_ptr sharedFactor; + + typedef FACTOR FactorType; ///< factor type + typedef typename FACTOR::KeyType KeyType; ///< type of Keys we use to index factors with + typedef boost::shared_ptr sharedFactor; ///< Shared pointer to a factor + typedef boost::shared_ptr sharedConditional; ///< Shared pointer to a conditional + + typedef FactorGraph This; ///< Typedef for this class + typedef boost::shared_ptr shared_ptr; ///< Shared pointer for this class typedef typename std::vector::iterator iterator; typedef typename std::vector::const_iterator const_iterator; - /** typedef for elimination result */ - typedef std::pair< - boost::shared_ptr, - typename FACTOR::shared_ptr> EliminationResult; + /** typedef for elimination result */ + typedef std::pair EliminationResult; - /** typedef for an eliminate subroutine */ - typedef boost::function&, size_t)> Eliminate; - - /** Typedef for the result of factorization */ - typedef std::pair< - boost::shared_ptr, - FactorGraph > FactorizationResult; + /** typedef for an eliminate subroutine */ + typedef boost::function Eliminate; protected: - /** concept check */ + /** concept check, makes sure FACTOR defines print and equals */ GTSAM_CONCEPT_TESTABLE_TYPE(FACTOR) /** Collection of factors */ @@ -82,7 +79,11 @@ template class BayesTree; /// @name Advanced Constructors /// @{ - /** convert from Bayes net */ + /** + * @brief Constructor from a Bayes net + * @param bayesNet the Bayes net to convert, type CONDITIONAL must yield compatible factor + * @return a factor graph with all the conditionals, as factors + */ template FactorGraph(const BayesNet& bayesNet); @@ -113,7 +114,7 @@ template class BayesTree; } /** push back many factors */ - void push_back(const FactorGraph& factors) { + void push_back(const This& factors) { factors_.insert(end(), factors.begin(), factors.end()); } @@ -123,9 +124,14 @@ template class BayesTree; factors_.insert(end(), firstFactor, lastFactor); } - /** push back many factors stored in a vector*/ + /** + * @brief Add a vector of derived factors + * @param factors to add + */ template - void push_back(const std::vector >& factors); + void push_back(const std::vector >& factors) { + factors_.insert(end(), factors.begin(), factors.end()); + } /// @} /// @name Testable @@ -135,7 +141,7 @@ template class BayesTree; void print(const std::string& s = "FactorGraph") const; /** Check equality */ - bool equals(const FactorGraph& fg, double tol = 1e-9) const; + bool equals(const This& fg, double tol = 1e-9) const; /// @} /// @name Standard Interface @@ -252,31 +258,6 @@ template class BayesTree; template FACTORGRAPH combine(const FACTORGRAPH& fg1, const FACTORGRAPH& fg2); - /* - * These functions are defined here because they are templated on an - * additional parameter. Putting them in the -inl.h file would mean these - * would have to be explicitly instantiated for any possible derived factor - * type. - */ - - /* ************************************************************************* */ - template - template - FactorGraph::FactorGraph(const BayesNet& bayesNet) { - factors_.reserve(bayesNet.size()); - BOOST_FOREACH(const typename CONDITIONAL::shared_ptr& cond, bayesNet) { - this->push_back(cond->toFactor()); - } - } - - /* ************************************************************************* */ - template - template - void FactorGraph::push_back(const std::vector >& factors) { - BOOST_FOREACH(const boost::shared_ptr& factor, factors) - this->push_back(factor); - } - } // namespace gtsam #include diff --git a/gtsam/inference/GenericMultifrontalSolver-inl.h b/gtsam/inference/GenericMultifrontalSolver-inl.h index 40a70bfd7..192436959 100644 --- a/gtsam/inference/GenericMultifrontalSolver-inl.h +++ b/gtsam/inference/GenericMultifrontalSolver-inl.h @@ -78,7 +78,7 @@ namespace gtsam { /* ************************************************************************* */ template - typename F::shared_ptr GenericMultifrontalSolver::marginalFactor( + typename boost::shared_ptr GenericMultifrontalSolver::marginalFactor( Index j, Eliminate function) const { return eliminate(function)->marginalFactor(j, function); } diff --git a/gtsam/inference/GenericMultifrontalSolver.h b/gtsam/inference/GenericMultifrontalSolver.h index 5d6cc74ef..a64df38e9 100644 --- a/gtsam/inference/GenericMultifrontalSolver.h +++ b/gtsam/inference/GenericMultifrontalSolver.h @@ -102,7 +102,7 @@ namespace gtsam { * Compute the marginal density over a variable, by integrating out * all of the other variables. This function returns the result as a factor. */ - typename FACTOR::shared_ptr marginalFactor(Index j, + typename boost::shared_ptr marginalFactor(Index j, Eliminate function) const; /// @} diff --git a/gtsam/inference/GenericSequentialSolver-inl.h b/gtsam/inference/GenericSequentialSolver-inl.h index 5b3e0dd28..cc7e6146c 100644 --- a/gtsam/inference/GenericSequentialSolver-inl.h +++ b/gtsam/inference/GenericSequentialSolver-inl.h @@ -95,7 +95,7 @@ namespace gtsam { // Permute the factors - NOTE that this permutes the original factors, not // copies. Other parts of the code may hold shared_ptr's to these factors so // we must undo the permutation before returning. - BOOST_FOREACH(const typename FACTOR::shared_ptr& factor, *factors_) + BOOST_FOREACH(const typename boost::shared_ptr& factor, *factors_) if (factor) factor->permuteWithInverse(*permutationInverse); // Eliminate all variables @@ -103,7 +103,7 @@ namespace gtsam { bayesNet(EliminationTree::Create(*factors_)->eliminate(function)); // Undo the permuation on the original factors and on the structure. - BOOST_FOREACH(const typename FACTOR::shared_ptr& factor, *factors_) + BOOST_FOREACH(const typename boost::shared_ptr& factor, *factors_) if (factor) factor->permuteWithInverse(*permutation); // Take the joint marginal from the Bayes net. @@ -116,7 +116,7 @@ namespace gtsam { joint->push_back((*(conditional++))->toFactor()); // Undo the permutation on the eliminated joint marginal factors - BOOST_FOREACH(const typename FACTOR::shared_ptr& factor, *joint) + BOOST_FOREACH(const typename boost::shared_ptr& factor, *joint) factor->permuteWithInverse(*permutation); return joint; @@ -124,7 +124,7 @@ namespace gtsam { /* ************************************************************************* */ template - typename FACTOR::shared_ptr // + typename boost::shared_ptr // GenericSequentialSolver::marginalFactor(Index j, Eliminate function) const { // Create a container for the one variable index std::vector js(1); diff --git a/gtsam/inference/GenericSequentialSolver.h b/gtsam/inference/GenericSequentialSolver.h index 5486b3280..5b4d4b41e 100644 --- a/gtsam/inference/GenericSequentialSolver.h +++ b/gtsam/inference/GenericSequentialSolver.h @@ -130,7 +130,7 @@ namespace gtsam { * Compute the marginal Gaussian density over a variable, by integrating out * all of the other variables. This function returns the result as a factor. */ - typename FACTOR::shared_ptr marginalFactor(Index j, Eliminate function) const; + typename boost::shared_ptr marginalFactor(Index j, Eliminate function) const; /// @} diff --git a/gtsam/inference/inference-inl.h b/gtsam/inference/inference-inl.h index 02e78e8e9..b24c2d6d2 100644 --- a/gtsam/inference/inference-inl.h +++ b/gtsam/inference/inference-inl.h @@ -31,7 +31,8 @@ namespace inference { /* ************************************************************************* */ template -Permutation::shared_ptr PermutationCOLAMD(const VariableIndex& variableIndex, const CONSTRAINED& constrainLast) { +Permutation::shared_ptr PermutationCOLAMD( + const VariableIndex& variableIndex, const CONSTRAINED& constrainLast) { std::vector cmember(variableIndex.size(), 0); @@ -55,10 +56,14 @@ inline Permutation::shared_ptr PermutationCOLAMD(const VariableIndex& variableIn /* ************************************************************************* */ template -typename Graph::FactorizationResult eliminate(const Graph& factorGraph, const std::vector& variables, - const typename Graph::Eliminate& eliminateFcn, boost::optional variableIndex_) { +std::pair eliminate( + const Graph& factorGraph, + const std::vector& variables, + const typename Graph::Eliminate& eliminateFcn, + boost::optional variableIndex_) { - const VariableIndex& variableIndex = variableIndex_ ? *variableIndex_ : VariableIndex(factorGraph); + const VariableIndex& variableIndex = + variableIndex_ ? *variableIndex_ : VariableIndex(factorGraph); // First find the involved factors Graph involvedFactors; @@ -108,13 +113,11 @@ typename Graph::FactorizationResult eliminate(const Graph& factorGraph, const st if(remainingFactor->size() != 0) remainingGraph.push_back(remainingFactor); - return typename Graph::FactorizationResult(conditional, remainingGraph); + return std::make_pair(conditional, remainingGraph); -} - - -} - -} +} // eliminate + +} // namespace inference +} // namespace gtsam diff --git a/gtsam/inference/inference.h b/gtsam/inference/inference.h index 38f896a0f..15373e62a 100644 --- a/gtsam/inference/inference.h +++ b/gtsam/inference/inference.h @@ -29,52 +29,60 @@ namespace gtsam { -namespace inference { + namespace inference { -/** - * Compute a permutation (variable ordering) using colamd - */ -Permutation::shared_ptr PermutationCOLAMD(const VariableIndex& variableIndex); + /** + * Compute a permutation (variable ordering) using colamd + */ + Permutation::shared_ptr PermutationCOLAMD( + const VariableIndex& variableIndex); -/** - * Compute a permutation (variable ordering) using constrained colamd - */ -template -Permutation::shared_ptr PermutationCOLAMD(const VariableIndex& variableIndex, const CONSTRAINED& constrainLast); + /** + * Compute a permutation (variable ordering) using constrained colamd + */ + template + Permutation::shared_ptr PermutationCOLAMD( + const VariableIndex& variableIndex, const CONSTRAINED& constrainLast); -/** - * Compute a CCOLAMD permutation using the constraint groups in cmember. - */ -Permutation::shared_ptr PermutationCOLAMD_(const VariableIndex& variableIndex, std::vector& cmember); + /** + * Compute a CCOLAMD permutation using the constraint groups in cmember. + */ + Permutation::shared_ptr PermutationCOLAMD_( + const VariableIndex& variableIndex, std::vector& cmember); -/** Factor the factor graph into a conditional and a remaining factor graph. - * Given the factor graph \f$ f(X) \f$, and \c variables to factorize out - * \f$ V \f$, this function factorizes into \f$ f(X) = f(V;Y)f(Y) \f$, where - * \f$ Y := X\V \f$ are the remaining variables. If \f$ f(X) = p(X) \f$ is - * a probability density or likelihood, the factorization produces a - * conditional probability density and a marginal \f$ p(X) = p(V|Y)p(Y) \f$. - * - * For efficiency, this function treats the variables to eliminate - * \c variables as fully-connected, so produces a dense (fully-connected) - * conditional on all of the variables in \c variables, instead of a sparse - * BayesNet. If the variables are not fully-connected, it is more efficient - * to sequentially factorize multiple times. - */ -template -typename Graph::FactorizationResult eliminate(const Graph& factorGraph, const std::vector& variables, - const typename Graph::Eliminate& eliminateFcn, boost::optional variableIndex = boost::none); + /** Factor the factor graph into a conditional and a remaining factor graph. + * Given the factor graph \f$ f(X) \f$, and \c variables to factorize out + * \f$ V \f$, this function factorizes into \f$ f(X) = f(V;Y)f(Y) \f$, where + * \f$ Y := X\V \f$ are the remaining variables. If \f$ f(X) = p(X) \f$ is + * a probability density or likelihood, the factorization produces a + * conditional probability density and a marginal \f$ p(X) = p(V|Y)p(Y) \f$. + * + * For efficiency, this function treats the variables to eliminate + * \c variables as fully-connected, so produces a dense (fully-connected) + * conditional on all of the variables in \c variables, instead of a sparse + * BayesNet. If the variables are not fully-connected, it is more efficient + * to sequentially factorize multiple times. + */ + template + std::pair eliminate( + const Graph& factorGraph, + const std::vector& variables, + const typename Graph::Eliminate& eliminateFcn, + boost::optional variableIndex = boost::none); -/** Eliminate a single variable, by calling - * eliminate(const Graph&, const std::vector&, const typename Graph::Eliminate&, boost::optional) - */ -template -typename Graph::FactorizationResult eliminateOne(const Graph& factorGraph, typename Graph::KeyType variable, - const typename Graph::Eliminate& eliminateFcn, boost::optional variableIndex = boost::none) { - std::vector variables(1, variable); - return eliminate(factorGraph, variables, eliminateFcn, variableIndex); -} + /** Eliminate a single variable, by calling + * eliminate(const Graph&, const std::vector&, const typename Graph::Eliminate&, boost::optional) + */ + template + std::pair eliminateOne( + const Graph& factorGraph, typename Graph::KeyType variable, + const typename Graph::Eliminate& eliminateFcn, + boost::optional variableIndex = boost::none) { + std::vector variables(1, variable); + return eliminate(factorGraph, variables, eliminateFcn, variableIndex); + } -} + } // namespace inference } // namespace gtsam diff --git a/gtsam/linear/GaussianFactorGraph.h b/gtsam/linear/GaussianFactorGraph.h index 07c2e94ad..07d0c8774 100644 --- a/gtsam/linear/GaussianFactorGraph.h +++ b/gtsam/linear/GaussianFactorGraph.h @@ -45,7 +45,7 @@ namespace gtsam { template boost::shared_ptr gaussianErrors_(const FactorGraph& fg, const VectorValues& x) { boost::shared_ptr e(new Errors); - BOOST_FOREACH(const typename FACTOR::shared_ptr& factor, fg) { + BOOST_FOREACH(const typename boost::shared_ptr& factor, fg) { e->push_back(factor->error_vector(x)); } return e; diff --git a/tests/testGaussianFactorGraphB.cpp b/tests/testGaussianFactorGraphB.cpp index 718dbc302..9b3f9eb0d 100644 --- a/tests/testGaussianFactorGraphB.cpp +++ b/tests/testGaussianFactorGraphB.cpp @@ -200,14 +200,16 @@ TEST( GaussianFactorGraph, eliminateOne_x1 ) Ordering ordering; ordering += X(1),L(1),X(2); GaussianFactorGraph fg = createGaussianFactorGraph(ordering); - GaussianFactorGraph::FactorizationResult result = inference::eliminateOne(fg, 0, EliminateQR); + GaussianConditional::shared_ptr conditional; + GaussianFactorGraph remaining; + boost::tie(conditional,remaining) = inference::eliminateOne(fg, 0, EliminateQR); // create expected Conditional Gaussian Matrix I = 15*eye(2), R11 = I, S12 = -0.111111*I, S13 = -0.444444*I; Vector d = Vector_(2, -0.133333, -0.0222222), sigma = ones(2); GaussianConditional expected(ordering[X(1)],15*d,R11,ordering[L(1)],S12,ordering[X(2)],S13,sigma); - EXPECT(assert_equal(expected,*result.first,tol)); + EXPECT(assert_equal(expected,*conditional,tol)); } /* ************************************************************************* */ @@ -247,9 +249,9 @@ TEST( GaussianFactorGraph, eliminateOne_x1_fast ) { Ordering ordering; ordering += X(1),L(1),X(2); GaussianFactorGraph fg = createGaussianFactorGraph(ordering); - GaussianFactorGraph::FactorizationResult result = inference::eliminateOne(fg, ordering[X(1)], EliminateQR); - GaussianConditional::shared_ptr conditional = result.first; - GaussianFactorGraph remaining = result.second; + GaussianConditional::shared_ptr conditional; + GaussianFactorGraph remaining; + boost::tie(conditional,remaining) = inference::eliminateOne(fg, ordering[X(1)], EliminateQR); // create expected Conditional Gaussian Matrix I = 15*eye(2), R11 = I, S12 = -0.111111*I, S13 = -0.444444*I;