diff --git a/gtsam/CMakeLists.txt b/gtsam/CMakeLists.txt index d5a2fee90..3d829c9ea 100644 --- a/gtsam/CMakeLists.txt +++ b/gtsam/CMakeLists.txt @@ -5,6 +5,7 @@ set (gtsam_subdirs base geometry inference + symbolic discrete linear nonlinear @@ -76,6 +77,7 @@ set(gtsam_srcs ${base_srcs} ${geometry_srcs} ${inference_srcs} + ${symbolic_srcs} ${discrete_srcs} ${linear_srcs} ${nonlinear_srcs} diff --git a/gtsam/inference/EliminationTreeUnordered-inl.h b/gtsam/inference/EliminationTreeUnordered-inl.h index e58476329..6051ec220 100644 --- a/gtsam/inference/EliminationTreeUnordered-inl.h +++ b/gtsam/inference/EliminationTreeUnordered-inl.h @@ -162,18 +162,21 @@ namespace gtsam { // Until the stack is empty while(!eliminationStack.empty()) { - // Process the next node. If it has children, add its children to the stack and skip it - - // we'll come back and eliminate it later after the children have been processed. If it has - // no children, we can eliminate it immediately and remove it from the stack. + // Process the next node. If it has children, add its children to the stack and mark it + // expanded - we'll come back and eliminate it later after the children have been processed. EliminationNode& node = nodeStack.top(); if(node.expanded) { // Remove from stack nodeStack.pop(); + // Do a dense elimination step + std::vector keyAsVector(1); keyAsVector[0] = node.key; std::pair, boost::shared_ptr > eliminationResult = - function(node.factors, node.key); + function(node.factors, keyAsVector); + // Add conditional to BayesNet and remaining factor to parent bayesNet->push_back(eliminationResult.first); + // TODO: Don't add null factor? if(node.parent) node.parent->factors.push_back(eliminationResult.second); diff --git a/gtsam/inference/EliminationTreeUnordered.h b/gtsam/inference/EliminationTreeUnordered.h index 90401f5ce..1b24b36c7 100644 --- a/gtsam/inference/EliminationTreeUnordered.h +++ b/gtsam/inference/EliminationTreeUnordered.h @@ -24,7 +24,7 @@ #include #include -class EliminationTreeTester; // for unit tests, see testEliminationTree +class EliminationTreeUnorderedTester; // for unit tests, see testEliminationTree namespace gtsam { @@ -61,6 +61,8 @@ namespace gtsam { typedef typename BayesNetType::ConditionalType ConditionalType; ///< The type of conditionals typedef typename GRAPH::Eliminate Eliminate; ///< Typedef for an eliminate subroutine + private: + class Node { public: typedef boost::shared_ptr shared_ptr; @@ -74,8 +76,6 @@ namespace gtsam { typedef Node::shared_ptr sharedNode; ///< Shared pointer to Node - private: - /** concept check */ GTSAM_CONCEPT_TESTABLE_TYPE(FactorType); @@ -105,13 +105,13 @@ namespace gtsam { */ EliminationTreeUnordered(const FactorGraphType& factorGraph, const std::vector& order); - /** Copy constructor - makes a deep copy of the tree structure, but only pointers to factors are + /** TODO: Copy constructor - makes a deep copy of the tree structure, but only pointers to factors are * copied, factors are not cloned. */ - EliminationTreeUnordered(const This& other); + EliminationTreeUnordered(const This& other) { throw std::runtime_error("Not implemented"); } - /** Assignment operator - makes a deep copy of the tree structure, but only pointers to factors are + /** TODO: Assignment operator - makes a deep copy of the tree structure, but only pointers to factors are * copied, factors are not cloned. */ - This& operator=(const This& other); + This& operator=(const This& other) { throw std::runtime_error("Not implemented"); } /// @} /// @name Standard Interface @@ -141,7 +141,7 @@ namespace gtsam { private: /// Allow access to constructor and add methods for testing purposes - friend class ::EliminationTreeTester; + friend class ::EliminationTreeUnorderedTester; }; diff --git a/gtsam/inference/FactorGraphUnordered.h b/gtsam/inference/FactorGraphUnordered.h index e7d2aa2fa..1bccc04b3 100644 --- a/gtsam/inference/FactorGraphUnordered.h +++ b/gtsam/inference/FactorGraphUnordered.h @@ -185,37 +185,6 @@ namespace gtsam { /** Get the last factor */ sharedFactor back() const { return factors_.back(); } - ///** Eliminate the first \c n frontal variables, returning the resulting - // * conditional and remaining factor graph - this is very inefficient for - // * eliminating all variables, to do that use EliminationTree or - // * JunctionTree. - // */ - //std::pair > eliminateFrontals(size_t nFrontals, const Eliminate& eliminate) const; - // - ///** 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 \backslash 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. - // */ - //std::pair > eliminate( - // const std::vector& variables, const Eliminate& eliminateFcn, - // boost::optional variableIndex = boost::none) const; - - ///** Eliminate a single variable, by calling FactorGraph::eliminate. */ - //std::pair > eliminateOne( - // KeyType variable, const Eliminate& eliminateFcn, - // boost::optional variableIndex = boost::none) const { - // std::vector variables(1, variable); - // return eliminate(variables, eliminateFcn, variableIndex); - //} - /// @} /// @name Modifying Factor Graphs (imperative, discouraged) /// @{ diff --git a/gtsam/inference/FactorUnordered.h b/gtsam/inference/FactorUnordered.h index f263e72d6..90f26dbef 100644 --- a/gtsam/inference/FactorUnordered.h +++ b/gtsam/inference/FactorUnordered.h @@ -77,13 +77,16 @@ public: FactorUnordered() {} /** Construct unary factor */ - FactorUnordered(Key key) : keys_(1) { keys_[0] = key; } + FactorUnordered(Key key) : keys_(1) { + keys_[0] = key; } /** Construct binary factor */ - FactorUnordered(Key key1, Key key2) : keys_(2) { keys_[0] = key1; keys_[1] = key2; } + FactorUnordered(Key key1, Key key2) : keys_(2) { + keys_[0] = key1; keys_[1] = key2; } /** Construct ternary factor */ - FactorUnordered(Key key1, Key key2, Key key3) : keys_(3) { keys_[0] = key1; keys_[1] = key2; keys_[2] = key3; } + FactorUnordered(Key key1, Key key2, Key key3) : keys_(3) { + keys_[0] = key1; keys_[1] = key2; keys_[2] = key3; } /** Construct 4-way factor */ FactorUnordered(Key key1, Key key2, Key key3, Key key4) : keys_(4) { @@ -102,15 +105,15 @@ public: /// @name Advanced Constructors /// @{ + + /** Construct n-way factor from iterator over keys. */ + template static FactorUnordered FromIterator(ITERATOR first, ITERATOR last) { + FactorUnordered result; result.keys_.assign(first, last); } /** Construct n-way factor from container of keys. */ template static FactorUnordered FromKeys(const CONTAINER& keys) { return FromIterator(keys.begin(), keys.end()); } - /** Construct n-way factor from iterator over keys. */ - template static FactorUnordered FromIterator(ITERATOR first, ITERATOR last) { - FactorUnordered result; result.keys_.assign(first, last); } - /// @} diff --git a/gtsam/inference/SymbolicFactorGraphUnordered.h b/gtsam/inference/SymbolicFactorGraphUnordered.h deleted file mode 100644 index fde935a81..000000000 --- a/gtsam/inference/SymbolicFactorGraphUnordered.h +++ /dev/null @@ -1,104 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * GTSAM Copyright 2010, Georgia Tech Research Corporation, - * Atlanta, Georgia 30332-0415 - * All Rights Reserved - * Authors: Frank Dellaert, et al. (see THANKS for the full author list) - - * See LICENSE for the license information - - * -------------------------------------------------------------------------- */ - -/** - * @file SymbolicFactorGraph.h - * @date Oct 29, 2009 - * @author Frank Dellaert - */ - -#pragma once - -#include -#include -#include - -namespace gtsam { - - /** Symbolic Factor Graph - * \nosubgrouping - */ - class SymbolicFactorGraphUnordered: public FactorGraphUnordered { - - public: - - /// @name Standard Constructors - /// @{ - - /** Construct empty factor graph */ - SymbolicFactorGraphUnordered() {} - - ///** Eliminate the first \c n frontal variables, returning the resulting - // * conditional and remaining factor graph - this is very inefficient for - // * eliminating all variables, to do that use EliminationTree or - // * JunctionTree. Note that this version simply calls - // * FactorGraph::eliminateFrontals with EliminateSymbolic - // * as the eliminate function argument. - // */ - //GTSAM_EXPORT std::pair eliminateFrontals(size_t nFrontals) const; - // - ///** 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. - // * Note that this version simply calls - // * FactorGraph::eliminate with EliminateSymbolic as the eliminate - // * function argument. - // */ - //GTSAM_EXPORT std::pair eliminate(const std::vector& variables) const; - - ///** Eliminate a single variable, by calling SymbolicFactorGraph::eliminate. */ - //GTSAM_EXPORT std::pair eliminateOne(Index variable) const; - - /// @} - /// @name Standard Interface - /// @{ - - /// @} - /// @name Advanced Interface - /// @{ - - /** Push back unary factor */ - GTSAM_EXPORT void push_factor(Key key); - - /** Push back binary factor */ - GTSAM_EXPORT void push_factor(Key key1, Key key2); - - /** Push back ternary factor */ - GTSAM_EXPORT void push_factor(Key key1, Key key2, Key key3); - - /** Push back 4-way factor */ - GTSAM_EXPORT void push_factor(Key key1, Key key2, Key key3, Key key4); - }; - - /** Create a combined joint factor (new style for EliminationTree). */ - GTSAM_EXPORT IndexFactor::shared_ptr CombineSymbolic(const FactorGraph& factors, - const FastMap >& variableSlots); - - /** - * CombineAndEliminate provides symbolic elimination. - * Combine and eliminate can also be called separately, but for this and - * derived classes calling them separately generally does extra work. - */ - GTSAM_EXPORT std::pair, boost::shared_ptr > - EliminateSymbolic(const FactorGraph&, size_t nrFrontals = 1); - - /// @} - -} // namespace gtsam diff --git a/gtsam/inference/SymbolicFactorUnordered.cpp b/gtsam/inference/SymbolicFactorUnordered.cpp deleted file mode 100644 index 5912b389c..000000000 --- a/gtsam/inference/SymbolicFactorUnordered.cpp +++ /dev/null @@ -1,23 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * GTSAM Copyright 2010, Georgia Tech Research Corporation, - * Atlanta, Georgia 30332-0415 - * All Rights Reserved - * Authors: Frank Dellaert, et al. (see THANKS for the full author list) - - * See LICENSE for the license information - - * -------------------------------------------------------------------------- */ - -/** - * @file SymbolicFactor.cpp - * @author Richard Roberts - * @date Oct 17, 2010 - */ - -#include - -using namespace std; - -namespace gtsam { -} // gtsam diff --git a/gtsam/symbolic/CMakeLists.txt b/gtsam/symbolic/CMakeLists.txt new file mode 100644 index 000000000..545a759b6 --- /dev/null +++ b/gtsam/symbolic/CMakeLists.txt @@ -0,0 +1,29 @@ +# Install headers +file(GLOB symbolic_headers "*.h") +install(FILES ${symbolic_headers} DESTINATION include/gtsam/symbolic) + +# Components to link tests in this subfolder against +set(symbolic_local_libs + symbolic + inference + geometry + base + ccolamd +) + +# Files to exclude from compilation of tests and timing scripts +set(symbolic_excluded_files +# "${CMAKE_CURRENT_SOURCE_DIR}/tests/testTypedDiscreteFactor.cpp" # Example of excluding a test + "" # Add to this list, with full path, to exclude +) + +# Build tests +if (GTSAM_BUILD_TESTS) + gtsam_add_subdir_tests(symbolic "${symbolic_local_libs}" "${gtsam-default}" "${symbolic_excluded_files}") +endif(GTSAM_BUILD_TESTS) + +# Build timing scripts +if (GTSAM_BUILD_TIMING) + gtsam_add_subdir_timing(symbolic "${symbolic_local_libs}" "${gtsam-default}" "${symbolic_excluded_files}") +endif(GTSAM_BUILD_TIMING) + diff --git a/gtsam/symbolic/SymbolicBayesNetUnordered.h b/gtsam/symbolic/SymbolicBayesNetUnordered.h new file mode 100644 index 000000000..025b0a7ff --- /dev/null +++ b/gtsam/symbolic/SymbolicBayesNetUnordered.h @@ -0,0 +1,44 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file SymbolicBayesNet.h + * @date Oct 29, 2009 + * @author Frank Dellaert + * @author Richard Roberts + */ + +#pragma once + +#include +#include + +namespace gtsam { + + /** Symbolic Bayes Net + * \nosubgrouping + */ + class SymbolicBayesNetUnordered: public SymbolicFactorGraphUnordered { + + public: + + /// @name Standard Constructors + /// @{ + + /** Construct empty factor graph */ + SymbolicBayesNetUnordered() {} + + /// @} + /// @name Standard Interface + /// @{ + }; + +} // namespace gtsam diff --git a/gtsam/inference/SymbolicConditionalUnordered.cpp b/gtsam/symbolic/SymbolicConditionalUnordered.cpp similarity index 90% rename from gtsam/inference/SymbolicConditionalUnordered.cpp rename to gtsam/symbolic/SymbolicConditionalUnordered.cpp index 8a9d6ba39..07492f312 100644 --- a/gtsam/inference/SymbolicConditionalUnordered.cpp +++ b/gtsam/symbolic/SymbolicConditionalUnordered.cpp @@ -15,7 +15,7 @@ * @date Oct 17, 2010 */ -#include +#include namespace gtsam { diff --git a/gtsam/inference/SymbolicConditionalUnordered.h b/gtsam/symbolic/SymbolicConditionalUnordered.h similarity index 83% rename from gtsam/inference/SymbolicConditionalUnordered.h rename to gtsam/symbolic/SymbolicConditionalUnordered.h index e2d4809e7..ca3694386 100644 --- a/gtsam/inference/SymbolicConditionalUnordered.h +++ b/gtsam/symbolic/SymbolicConditionalUnordered.h @@ -18,8 +18,8 @@ #pragma once #include -#include #include +#include namespace gtsam { @@ -62,14 +62,22 @@ namespace gtsam { SymbolicConditionalUnordered(Index j, Index parent1, Index parent2, Index parent3) : BaseFactor(j, parent1, parent2, parent3), BaseConditional(3) {} /** Named constructor from an arbitrary number of keys and frontals */ - template - static SymbolicConditionalUnordered FromIterator(ITERATOR firstKey, ITERATOR lastKey, size_t nrFrontals) : + template + static SymbolicConditionalUnordered FromIterator(ITERATOR firstKey, ITERATOR lastKey, size_t nrFrontals) { SymbolicConditionalUnordered result; - result.keys_.assign(firstKey, lastKey); + result = BaseFactor::FromIterator(firstKey, lastKey); result.nrFrontals_ = nrFrontals; - return result; - } + return result; } + + /** Named constructor from an arbitrary number of keys and frontals */ + template + static SymbolicConditionalUnordered FromKeys(const CONTAINER& keys, size_t nrFrontals) + { + SymbolicConditionalUnordered result; + result = BaseFactor::FromKeys(keys); + result.nrFrontals_ = nrFrontals; + return result; } /// @} diff --git a/gtsam/inference/SymbolicEliminationTreeUnordered.h b/gtsam/symbolic/SymbolicEliminationTreeUnordered.h similarity index 81% rename from gtsam/inference/SymbolicEliminationTreeUnordered.h rename to gtsam/symbolic/SymbolicEliminationTreeUnordered.h index 3ad76b426..0514e1492 100644 --- a/gtsam/inference/SymbolicEliminationTreeUnordered.h +++ b/gtsam/symbolic/SymbolicEliminationTreeUnordered.h @@ -16,15 +16,16 @@ * @author Richard Roberts */ -#include -#include #include +#include +#include namespace gtsam { - class SymbolicEliminationTreeUnordered : public EliminationTreeUnordered { + class SymbolicEliminationTreeUnordered : + public EliminationTreeUnordered { public: - typedef EliminationTreeUnordered Base; ///< Base class + typedef EliminationTreeUnordered Base; ///< Base class typedef SymbolicEliminationTreeUnordered This; ///< This class typedef boost::shared_ptr shared_ptr; ///< Shared pointer to this class @@ -45,7 +46,7 @@ namespace gtsam { * constructor instead. * @param factorGraph The factor graph for which to build the elimination tree */ - SymbolicEliminationTreeUnordered(const FactorGraphType& factorGraph, const std::vector& order) : + SymbolicEliminationTreeUnordered(const SymbolicFactorGraphUnordered& factorGraph, const std::vector& order) : Base(factorGraph, order) {} /** Copy constructor - makes a deep copy of the tree structure, but only pointers to factors are diff --git a/gtsam/inference/SymbolicFactorGraphUnordered.cpp b/gtsam/symbolic/SymbolicFactorGraphUnordered.cpp similarity index 98% rename from gtsam/inference/SymbolicFactorGraphUnordered.cpp rename to gtsam/symbolic/SymbolicFactorGraphUnordered.cpp index 827ee6196..5843a681b 100644 --- a/gtsam/inference/SymbolicFactorGraphUnordered.cpp +++ b/gtsam/symbolic/SymbolicFactorGraphUnordered.cpp @@ -17,7 +17,7 @@ #include -#include +#include namespace gtsam { diff --git a/gtsam/symbolic/SymbolicFactorGraphUnordered.h b/gtsam/symbolic/SymbolicFactorGraphUnordered.h new file mode 100644 index 000000000..c7f56dd08 --- /dev/null +++ b/gtsam/symbolic/SymbolicFactorGraphUnordered.h @@ -0,0 +1,59 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file SymbolicFactorGraph.h + * @date Oct 29, 2009 + * @author Frank Dellaert + * @author Richard Roberts + */ + +#pragma once + +#include +#include +#include + +namespace gtsam { + + /** Symbolic Factor Graph + * \nosubgrouping + */ + class SymbolicFactorGraphUnordered: public FactorGraphUnordered { + + public: + + /// @name Standard Constructors + /// @{ + + /** Construct empty factor graph */ + SymbolicFactorGraphUnordered() {} + + /// @} + /// @name Standard Interface + /// @{ + + /** Push back unary factor */ + GTSAM_EXPORT void push_factor(Key key); + + /** Push back binary factor */ + GTSAM_EXPORT void push_factor(Key key1, Key key2); + + /** Push back ternary factor */ + GTSAM_EXPORT void push_factor(Key key1, Key key2, Key key3); + + /** Push back 4-way factor */ + GTSAM_EXPORT void push_factor(Key key1, Key key2, Key key3, Key key4); + + /// @} + }; + +} // namespace gtsam diff --git a/gtsam/symbolic/SymbolicFactorUnordered.cpp b/gtsam/symbolic/SymbolicFactorUnordered.cpp new file mode 100644 index 000000000..6a455958b --- /dev/null +++ b/gtsam/symbolic/SymbolicFactorUnordered.cpp @@ -0,0 +1,54 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file SymbolicFactor.cpp + * @author Richard Roberts + * @date Oct 17, 2010 + */ + +#include +#include + +#include +#include + +using namespace std; + +namespace gtsam { + + /* ************************************************************************* */ + std::pair, boost::shared_ptr > + EliminateSymbolicUnordered(const vector& factors, const vector& keys) + { + // Gather all keys + FastSet allKeys; + BOOST_FOREACH(const SymbolicFactorUnordered::shared_ptr& factor, factors) { + allKeys.insert(factor->begin(), factor->end()); } + + // Sort frontal keys + FastSet frontals(keys); + const size_t nFrontals = keys.size(); + + // Build a key vector with the frontals followed by the separator + vector orderedKeys(allKeys.size()); + std::copy(keys.begin(), keys.end(), orderedKeys.begin()); + std::set_difference(allKeys.begin(), allKeys.end(), frontals.begin(), frontals.end(), orderedKeys.begin() + nFrontals); + + // Return resulting conditional and factor + return make_pair( + boost::make_shared( + SymbolicConditionalUnordered::FromKeys(orderedKeys, nFrontals)), + boost::make_shared( + SymbolicFactorUnordered::FromIterator(orderedKeys.begin() + nFrontals, orderedKeys.end()))); + } + +} // gtsam diff --git a/gtsam/inference/SymbolicFactorUnordered.h b/gtsam/symbolic/SymbolicFactorUnordered.h similarity index 78% rename from gtsam/inference/SymbolicFactorUnordered.h rename to gtsam/symbolic/SymbolicFactorUnordered.h index a347f75e2..46c319fc5 100644 --- a/gtsam/inference/SymbolicFactorUnordered.h +++ b/gtsam/symbolic/SymbolicFactorUnordered.h @@ -48,7 +48,7 @@ namespace gtsam { /// @{ /** Virtual destructor */ - virtual ~SymbolicFactorUnordered() {} + ~SymbolicFactorUnordered() {} /** Copy constructor */ SymbolicFactorUnordered(const This& f) : Base(f) {} @@ -80,7 +80,18 @@ namespace gtsam { /// @{ /** Constructor from a collection of keys */ - template SymbolicFactorUnordered(KeyIterator beginKey, KeyIterator endKey) : Base(beginKey, endKey) {} + template + static SymbolicFactorUnordered FromIterator(KEYITERATOR beginKey, KEYITERATOR endKey) { + SymbolicFactorUnordered result; + result = Base::FromIterator(beginKey, endKey); + return result; } + + /** Constructor from a collection of keys */ + template + static SymbolicFactorUnordered FromKeys(const CONTAINER& keys) { + SymbolicFactorUnordered result; + result = Base::FromKeys(keys); + return result; } /// @} @@ -91,7 +102,10 @@ namespace gtsam { void serialize(ARCHIVE & ar, const unsigned int version) { ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); } - }; // IndexFactor + + std::pair, boost::shared_ptr > + EliminateSymbolicUnordered(const vector& factors, const vector& keys); + } diff --git a/gtsam/symbolic/tests/testSymbolicEliminationTree.cpp b/gtsam/symbolic/tests/testSymbolicEliminationTree.cpp new file mode 100644 index 000000000..28f7cc72b --- /dev/null +++ b/gtsam/symbolic/tests/testSymbolicEliminationTree.cpp @@ -0,0 +1,117 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file testSymbolicEliminationTree.cpp + * @brief + * @author Richard Roberts + * @date Oct 14, 2010 + */ + +#include +#include +#include + +using namespace gtsam; +using namespace std; + +class EliminationTreeUnorderedTester { +public: + // build hardcoded tree + static SymbolicEliminationTreeUnordered buildHardcodedTree(const SymbolicFactorGraphUnordered& fg) { + + SymbolicEliminationTreeUnordered::sharedNode leaf0(new SymbolicEliminationTree); + leaf0->add(fg[0]); + leaf0->add(fg[1]); + + SymbolicEliminationTreeUnordered::sharedNode node1(new SymbolicEliminationTree(1)); + node1->add(fg[2]); + node1->add(leaf0); + + SymbolicEliminationTreeUnordered::sharedNode node2(new SymbolicEliminationTree(2)); + node2->add(fg[3]); + node2->add(node1); + + SymbolicEliminationTreeUnordered::sharedNode leaf3(new SymbolicEliminationTree(3)); + leaf3->add(fg[4]); + + SymbolicEliminationTreeUnordered::sharedNode etree(new SymbolicEliminationTree(4)); + etree->add(leaf3); + etree->add(node2); + + return etree; + } +}; + +TEST(EliminationTree, Create) +{ + // create example factor graph + SymbolicFactorGraph fg; + fg.push_factor(0, 1); + fg.push_factor(0, 2); + fg.push_factor(1, 4); + fg.push_factor(2, 4); + fg.push_factor(3, 4); + + SymbolicEliminationTree::shared_ptr expected = EliminationTreeTester::buildHardcodedTree(fg); + + // Build from factor graph + SymbolicEliminationTree::shared_ptr actual = SymbolicEliminationTree::Create(fg); + + CHECK(assert_equal(*expected,*actual)); +} + +/* ************************************************************************* */ +// Test to drive elimination tree development +// graph: f(0,1) f(0,2) f(1,4) f(2,4) f(3,4) +/* ************************************************************************* */ + +TEST(EliminationTree, eliminate ) +{ + // create expected Chordal bayes Net + SymbolicBayesNet expected; + expected.push_front(boost::make_shared(4)); + expected.push_front(boost::make_shared(3,4)); + expected.push_front(boost::make_shared(2,4)); + expected.push_front(boost::make_shared(1,2,4)); + expected.push_front(boost::make_shared(0,1,2)); + + // Create factor graph + SymbolicFactorGraph fg; + fg.push_factor(0, 1); + fg.push_factor(0, 2); + fg.push_factor(1, 4); + fg.push_factor(2, 4); + fg.push_factor(3, 4); + + // eliminate + SymbolicBayesNet actual = *SymbolicSequentialSolver(fg).eliminate(); + + CHECK(assert_equal(expected,actual)); +} + +/* ************************************************************************* */ +TEST(EliminationTree, disconnected_graph) { + SymbolicFactorGraph fg; + fg.push_factor(0, 1); + fg.push_factor(0, 2); + fg.push_factor(1, 2); + fg.push_factor(3, 4); + + CHECK_EXCEPTION(SymbolicEliminationTree::Create(fg), DisconnectedGraphException); +} + +/* ************************************************************************* */ +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +/* ************************************************************************* */