diff --git a/gtsam/base/tests/testTreeTraversal.cpp b/gtsam/base/tests/testTreeTraversal.cpp new file mode 100644 index 000000000..ae27a65e6 --- /dev/null +++ b/gtsam/base/tests/testTreeTraversal.cpp @@ -0,0 +1,109 @@ +/* ---------------------------------------------------------------------------- + + * 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 testTreeTraversal + * @brief Unit tests for tree traversal, cloning, and printing + * @author Richard Roberts + */ + +#include + +#include +#include +#include +#include +#include + +using boost::assign::operator+=; +using namespace std; + +struct TestNode { + typedef boost::shared_ptr shared_ptr; + int data; + vector children; + TestNode(int data) : data(data) {} +}; + +struct TestForest { + typedef TestNode::shared_ptr sharedNode; + vector roots_; + const vector& roots() const { return roots_; } +}; + +TestForest makeTestForest() { + // 0 1 + // / \ + // 2 3 + // | + // 4 + TestForest forest; + forest.roots_.push_back(boost::make_shared(0)); + forest.roots_.push_back(boost::make_shared(1)); + forest.roots_[0]->children.push_back(boost::make_shared(2)); + forest.roots_[0]->children.push_back(boost::make_shared(3)); + forest.roots_[0]->children[1]->children.push_back(boost::make_shared(4)); + return forest; +} + +/* ************************************************************************* */ +struct PreOrderVisitor { + // This visitor stores the nodes visited so the visit order can be checked later. It also returns + // the current node index as the data and checks that the parent index properly matches the index + // referenced in the node. + std::list visited; + bool parentsMatched; + PreOrderVisitor() : parentsMatched(true) {} + int operator()(const TestNode::shared_ptr& node, int parentData) { + visited.push_back(node->data); + // Check parent index + const int expectedParentIndex = + node->data == 0 ? -1 : + node->data == 1 ? -1 : + node->data == 2 ? 0 : + node->data == 3 ? 0 : + node->data == 4 ? 0 : + (throw std::runtime_error("Unexpected node index"), -1); + if(expectedParentIndex != parentData) + parentsMatched = false; + return node->data; + } +}; + +/* ************************************************************************* */ +struct PostOrderVisitor { + // This visitor stores the nodes visited so the visit order can be checked later. + std::list visited; + void operator()(const TestNode::shared_ptr& node, int myData) { + visited.push_back(node->data); + } +}; + +/* ************************************************************************* */ +TEST(treeTraversal, DepthFirst) +{ + // Get test forest + TestForest testForest = makeTestForest(); + + // Expected pre-order + std::list preOrderExpected; + preOrderExpected += 0, 2, 3, 4, 1; + std::list postOrderExpected; + postOrderExpected += 2, 4, 3, 0, 1; +} + +/* ************************************************************************* */ +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +/* ************************************************************************* */ + diff --git a/gtsam/base/treeTraversal-inst.h b/gtsam/base/treeTraversal-inst.h index afd12cc51..e4c806cdf 100644 --- a/gtsam/base/treeTraversal-inst.h +++ b/gtsam/base/treeTraversal-inst.h @@ -18,6 +18,10 @@ #include +#include +#include +#include + namespace gtsam { /** Internal functions used for traversing trees */ @@ -29,30 +33,15 @@ namespace gtsam { template struct TraversalNode { bool expanded; - NODE& const treeNode; + const boost::shared_ptr& treeNode; DATA data; - TraversalNode(NODE* _treeNode, const DATA& _data) : + TraversalNode(const boost::shared_ptr& _treeNode, const DATA& _data) : expanded(false), treeNode(_treeNode), data(_data) {} }; - // Functional object to visit a node and then add it to the traversal stack - template - struct PreOrderExpand { - VISITOR& visitor_; - DATA& parentData_; - STACK& stack_; - PreOrderExpand(VISITOR& visitor, DATA& parentData, STACK& stack) : - visitor_(visitor), parentData_(parentData), stack_(stack) {} - template - void operator()(const P& nodePointer) { - // Add node - stack_.push(TraversalNode(*nodePointer, visitor_(nodePointer, parentData_))); - } - }; - /// Do nothing - default argument for post-visitor for tree traversal template - void no_op(const NODE& node, const DATA& data) {} + void no_op(const boost::shared_ptr& node, const DATA& data) {} } /** Traverse a forest depth-first with pre-order and post-order visits. @@ -72,16 +61,19 @@ namespace gtsam { template void DepthFirstForest(FOREST& forest, DATA& rootData, VISITOR_PRE& visitorPre, VISITOR_POST& visitorPost) { + // Typedefs + typedef typename FOREST::Node Node; + typedef boost::shared_ptr sharedNode; + // Depth first traversal stack typedef TraversalNode TraversalNode; typedef std::stack > Stack; - typedef PreOrderExpand Expander; Stack stack; // Add roots to stack (use reverse iterators so children are processed in the order they // appear) - (void) std::for_each(forest.roots().rbegin(), forest.roots().rend(), - Expander(visitorPre, rootData, stack)); + BOOST_REVERSE_FOREACH(const sharedNode& root, forest.roots()) + stack.push(TraversalNode(root, visitorPre(root, rootData))); // Traverse while(!stack.empty()) @@ -97,8 +89,8 @@ namespace gtsam { } else { // If not already visited, visit the node and add its children (use reverse iterators so // children are processed in the order they appear) - (void) std::for_each(node.treeNode.children.rbegin(), node.treeNode.children.rend(), - Expander(visitorPre, node.data, stack)); + BOOST_REVERSE_FOREACH(const sharedNode& child, node.treeNode->children) + stack.push(TraversalNode(child, visitorPre(child, node.data))); node.expanded = true; } } @@ -118,8 +110,7 @@ namespace gtsam { template void DepthFirstForest(FOREST& forest, DATA& rootData, VISITOR_PRE& visitorPre) { - DepthFirstForest( - forest, rootData, visitorPre, no_op); + DepthFirstForest(forest, rootData, visitorPre, no_op); } @@ -127,10 +118,11 @@ namespace gtsam { /** Traversal function for CloneForest */ namespace { template - boost::shared_ptr CloneForestVisitorPre(const NODE& node, const boost::shared_ptr& parentPointer) + boost::shared_ptr + CloneForestVisitorPre(const boost::shared_ptr& node, const boost::shared_ptr& parentPointer) { // Clone the current node and add it to its cloned parent - boost::shared_ptr clone = boost::make_shared(node); + boost::shared_ptr clone = boost::make_shared(*node); parentPointer->children.push_back(clone); return clone; } @@ -155,10 +147,11 @@ namespace gtsam { /** Traversal function for PrintForest */ namespace { template - std::string PrintForestVisitorPre(const NODE& node, const std::string& parentString, const KeyFormatter& formatter) + std::string + PrintForestVisitorPre(const boost::shared_ptr& node, const std::string& parentString, const KeyFormatter& formatter) { // Print the current node - node.print(parentString + "-", formatter); + node->print(parentString + "-", formatter); // Increment the indentation return parentString + "| "; } @@ -169,7 +162,7 @@ namespace gtsam { template void PrintForest(const FOREST& forest, const std::string& str, const KeyFormatter& keyFormatter) { typedef typename FOREST::Node Node; - DepthFirstForest(forest, str, boost::bind(PrintForestVisitorPre, _1, _2, formatter)); + DepthFirstForest(forest, str, boost::bind(PrintForestVisitorPre, _1, _2, keyFormatter)); } } diff --git a/gtsam/inference/BayesNetUnordered-inst.h b/gtsam/inference/BayesNetUnordered-inst.h index 2d336bea6..312b01bb1 100644 --- a/gtsam/inference/BayesNetUnordered-inst.h +++ b/gtsam/inference/BayesNetUnordered-inst.h @@ -19,7 +19,9 @@ #pragma once #include -#include + +#include +#include namespace gtsam { @@ -32,17 +34,16 @@ namespace gtsam { /* ************************************************************************* */ template - void BayesNetUnordered::saveGraph( - const std::string &s, const KeyFormatter& keyFormatter = DefaultKeyFormatter) + void BayesNetUnordered::saveGraph(const std::string &s, const KeyFormatter& keyFormatter) const { std::ofstream of(s.c_str()); of << "digraph G{\n"; BOOST_REVERSE_FOREACH(const sharedConditional& conditional, *this) { typename CONDITIONAL::Frontals frontals = conditional->frontals(); - Index me = frontals.front(); + Key me = frontals.front(); typename CONDITIONAL::Parents parents = conditional->parents(); - BOOST_FOREACH(Index p, parents) + BOOST_FOREACH(Key p, parents) of << p << "->" << me << std::endl; } diff --git a/gtsam/inference/BayesTreeCliqueBaseUnordered-inst.h b/gtsam/inference/BayesTreeCliqueBaseUnordered-inst.h index 1e09509ab..56af348ad 100644 --- a/gtsam/inference/BayesTreeCliqueBaseUnordered-inst.h +++ b/gtsam/inference/BayesTreeCliqueBaseUnordered-inst.h @@ -22,22 +22,22 @@ namespace gtsam { /* ************************************************************************* */ - template + template std::vector - BayesTreeCliqueBaseUnordered::separator_setminus_B(derived_ptr B) const + BayesTreeCliqueBaseUnordered::separator_setminus_B(derived_ptr B) const { FastSet p_F_S_parents(this->conditional()->beginParents(), this->conditional()->endParents()); FastSet indicesB(B->conditional()->begin(), B->conditional()->end()); std::vector S_setminus_B; - std::set_difference(p_F_S->beginParents(), p_F_S->endParents(), + std::set_difference(p_F_S_parents.begin(), p_F_S_parents.end(), indicesB.begin(), indicesB.end(), back_inserter(S_setminus_B)); return S_setminus_B; } /* ************************************************************************* */ - template - std::vector BayesTreeCliqueBaseUnordered::shortcut_indices( - derived_ptr B, const FactorGraphUnordered& p_Cp_B) const + template + std::vector BayesTreeCliqueBaseUnordered::shortcut_indices( + derived_ptr B, const FactorGraphType& p_Cp_B) const { gttic(shortcut_indices); FastSet allKeys = p_Cp_B.keys(); @@ -54,31 +54,31 @@ namespace gtsam { } /* ************************************************************************* */ - template - void BayesTreeCliqueBaseUnordered::print( + template + void BayesTreeCliqueBaseUnordered::print( const std::string& s, const KeyFormatter& keyFormatter) const { conditional_->print(s, keyFormatter); } /* ************************************************************************* */ - template - size_t BayesTreeCliqueBaseUnordered::treeSize() const { + template + size_t BayesTreeCliqueBaseUnordered::treeSize() const { size_t size = 1; - BOOST_FOREACH(const derived_ptr& child, children_) + BOOST_FOREACH(const derived_ptr& child, children) size += child->treeSize(); return size; } /* ************************************************************************* */ - template - size_t BayesTreeCliqueBaseUnordered::numCachedSeparatorMarginals() const + template + size_t BayesTreeCliqueBaseUnordered::numCachedSeparatorMarginals() const { if (!cachedSeparatorMarginal_) return 0; size_t subtree_count = 1; - BOOST_FOREACH(const derived_ptr& child, children_) + BOOST_FOREACH(const derived_ptr& child, children) subtree_count += child->numCachedSeparatorMarginals(); return subtree_count; @@ -89,146 +89,146 @@ namespace gtsam { // clique on the root. We can compute it recursively from the parent shortcut // P(Sp|R) as \int P(Fp|Sp) P(Sp|R), where Fp are the frontal nodes in p /* ************************************************************************* */ - template - BayesNetUnordered BayesTreeCliqueBaseUnordered::shortcut( - derived_ptr B, Eliminate function) const - { - gttic(BayesTreeCliqueBaseUnordered_shortcut); + //template + //BayesNetUnordered BayesTreeCliqueBaseUnordered::shortcut( + // derived_ptr B, Eliminate function) const + //{ + // gttic(BayesTreeCliqueBaseUnordered_shortcut); - // We only calculate the shortcut when this clique is not B - // and when the S\B is not empty - std::vector S_setminus_B = separator_setminus_B(B); - if (B.get() != this && !S_setminus_B.empty()) { + // // We only calculate the shortcut when this clique is not B + // // and when the S\B is not empty + // std::vector S_setminus_B = separator_setminus_B(B); + // if (B.get() != this && !S_setminus_B.empty()) { - // Obtain P(Cp||B) = P(Fp|Sp) * P(Sp||B) as a factor graph - derived_ptr parent(parent_.lock()); - gttoc(BayesTreeCliqueBaseUnordered_shortcut); - FactorGraphType p_Cp_B(parent->shortcut(B, function)); // P(Sp||B) - gttic(BayesTreeCliqueBaseUnordered_shortcut); - p_Cp_B.push_back(parent->conditional()->toFactor()); // P(Fp|Sp) + // // Obtain P(Cp||B) = P(Fp|Sp) * P(Sp||B) as a factor graph + // derived_ptr parent(parent_.lock()); + // gttoc(BayesTreeCliqueBaseUnordered_shortcut); + // FactorGraphType p_Cp_B(parent->shortcut(B, function)); // P(Sp||B) + // gttic(BayesTreeCliqueBaseUnordered_shortcut); + // p_Cp_B.push_back(parent->conditional()->toFactor()); // P(Fp|Sp) - // Determine the variables we want to keepSet, S union B - std::vector keep = shortcut_indices(B, p_Cp_B); + // // Determine the variables we want to keepSet, S union B + // std::vector keep = shortcut_indices(B, p_Cp_B); - // Reduce the variable indices to start at zero - gttic(Reduce); - const Permutation reduction = internal::createReducingPermutation(p_Cp_B.keys()); - internal::Reduction inverseReduction = internal::Reduction::CreateAsInverse(reduction); - BOOST_FOREACH(const boost::shared_ptr& factor, p_Cp_B) { - if(factor) factor->reduceWithInverse(inverseReduction); } - inverseReduction.applyInverse(keep); - gttoc(Reduce); + // // Reduce the variable indices to start at zero + // gttic(Reduce); + // const Permutation reduction = internal::createReducingPermutation(p_Cp_B.keys()); + // internal::Reduction inverseReduction = internal::Reduction::CreateAsInverse(reduction); + // BOOST_FOREACH(const boost::shared_ptr& factor, p_Cp_B) { + // if(factor) factor->reduceWithInverse(inverseReduction); } + // inverseReduction.applyInverse(keep); + // gttoc(Reduce); - // Create solver that will marginalize for us - GenericSequentialSolver solver(p_Cp_B); + // // Create solver that will marginalize for us + // GenericSequentialSolver solver(p_Cp_B); - // Finally, we only want to have S\B variables in the Bayes net, so - size_t nrFrontals = S_setminus_B.size(); - BayesNet result = *solver.conditionalBayesNet(keep, nrFrontals, function); + // // Finally, we only want to have S\B variables in the Bayes net, so + // size_t nrFrontals = S_setminus_B.size(); + // BayesNet result = *solver.conditionalBayesNet(keep, nrFrontals, function); - // Undo the reduction - gttic(Undo_Reduce); - BOOST_FOREACH(const typename boost::shared_ptr& factor, p_Cp_B) { - if (factor) factor->permuteWithInverse(reduction); } - result.permuteWithInverse(reduction); - gttoc(Undo_Reduce); + // // Undo the reduction + // gttic(Undo_Reduce); + // BOOST_FOREACH(const typename boost::shared_ptr& factor, p_Cp_B) { + // if (factor) factor->permuteWithInverse(reduction); } + // result.permuteWithInverse(reduction); + // gttoc(Undo_Reduce); - assertInvariants(); + // assertInvariants(); - return result; - } else { - return BayesNet(); - } - } + // return result; + // } else { + // return BayesNet(); + // } + //} /* ************************************************************************* */ // separator marginal, uses separator marginal of parent recursively // P(C) = P(F|S) P(S) /* ************************************************************************* */ - template - FactorGraph::FactorType> BayesTreeCliqueBaseUnordered< - DERIVED, CONDITIONAL>::separatorMarginal(derived_ptr R, Eliminate function) const - { - gttic(BayesTreeCliqueBaseUnordered_separatorMarginal); - // Check if the Separator marginal was already calculated - if (!cachedSeparatorMarginal_) { - gttic(BayesTreeCliqueBaseUnordered_separatorMarginal_cachemiss); - // If this is the root, there is no separator - if (R.get() == this) { - // we are root, return empty - FactorGraph empty; - cachedSeparatorMarginal_ = empty; - } else { - // Obtain P(S) = \int P(Cp) = \int P(Fp|Sp) P(Sp) - // initialize P(Cp) with the parent separator marginal - derived_ptr parent(parent_.lock()); - gttoc(BayesTreeCliqueBaseUnordered_separatorMarginal_cachemiss); // Flatten recursion in timing outline - gttoc(BayesTreeCliqueBaseUnordered_separatorMarginal); - FactorGraph p_Cp(parent->separatorMarginal(R, function)); // P(Sp) - gttic(BayesTreeCliqueBaseUnordered_separatorMarginal); - gttic(BayesTreeCliqueBaseUnordered_separatorMarginal_cachemiss); - // now add the parent conditional - p_Cp.push_back(parent->conditional()->toFactor()); // P(Fp|Sp) + //template + //FactorGraph::FactorType> BayesTreeCliqueBaseUnordered< + // DERIVED, CONDITIONAL>::separatorMarginal(derived_ptr R, Eliminate function) const + //{ + // gttic(BayesTreeCliqueBaseUnordered_separatorMarginal); + // // Check if the Separator marginal was already calculated + // if (!cachedSeparatorMarginal_) { + // gttic(BayesTreeCliqueBaseUnordered_separatorMarginal_cachemiss); + // // If this is the root, there is no separator + // if (R.get() == this) { + // // we are root, return empty + // FactorGraph empty; + // cachedSeparatorMarginal_ = empty; + // } else { + // // Obtain P(S) = \int P(Cp) = \int P(Fp|Sp) P(Sp) + // // initialize P(Cp) with the parent separator marginal + // derived_ptr parent(parent_.lock()); + // gttoc(BayesTreeCliqueBaseUnordered_separatorMarginal_cachemiss); // Flatten recursion in timing outline + // gttoc(BayesTreeCliqueBaseUnordered_separatorMarginal); + // FactorGraph p_Cp(parent->separatorMarginal(R, function)); // P(Sp) + // gttic(BayesTreeCliqueBaseUnordered_separatorMarginal); + // gttic(BayesTreeCliqueBaseUnordered_separatorMarginal_cachemiss); + // // now add the parent conditional + // p_Cp.push_back(parent->conditional()->toFactor()); // P(Fp|Sp) - // Reduce the variable indices to start at zero - gttic(Reduce); - const Permutation reduction = internal::createReducingPermutation(p_Cp.keys()); - internal::Reduction inverseReduction = internal::Reduction::CreateAsInverse(reduction); - BOOST_FOREACH(const boost::shared_ptr& factor, p_Cp) { - if(factor) factor->reduceWithInverse(inverseReduction); } + // // Reduce the variable indices to start at zero + // gttic(Reduce); + // const Permutation reduction = internal::createReducingPermutation(p_Cp.keys()); + // internal::Reduction inverseReduction = internal::Reduction::CreateAsInverse(reduction); + // BOOST_FOREACH(const boost::shared_ptr& factor, p_Cp) { + // if(factor) factor->reduceWithInverse(inverseReduction); } - // The variables we want to keepSet are exactly the ones in S - sharedConditional p_F_S = this->conditional(); - std::vector indicesS(p_F_S->beginParents(), p_F_S->endParents()); - inverseReduction.applyInverse(indicesS); - gttoc(Reduce); + // // The variables we want to keepSet are exactly the ones in S + // sharedConditional p_F_S = this->conditional(); + // std::vector indicesS(p_F_S->beginParents(), p_F_S->endParents()); + // inverseReduction.applyInverse(indicesS); + // gttoc(Reduce); - // Create solver that will marginalize for us - GenericSequentialSolver solver(p_Cp); + // // Create solver that will marginalize for us + // GenericSequentialSolver solver(p_Cp); - cachedSeparatorMarginal_ = *(solver.jointBayesNet(indicesS, function)); + // cachedSeparatorMarginal_ = *(solver.jointBayesNet(indicesS, function)); - // Undo the reduction - gttic(Undo_Reduce); - BOOST_FOREACH(const typename boost::shared_ptr& factor, p_Cp) { - if (factor) factor->permuteWithInverse(reduction); } - BOOST_FOREACH(const typename boost::shared_ptr& factor, *cachedSeparatorMarginal_) { - if (factor) factor->permuteWithInverse(reduction); } - gttoc(Undo_Reduce); - } - } else { - gttic(BayesTreeCliqueBaseUnordered_separatorMarginal_cachehit); - } + // // Undo the reduction + // gttic(Undo_Reduce); + // BOOST_FOREACH(const typename boost::shared_ptr& factor, p_Cp) { + // if (factor) factor->permuteWithInverse(reduction); } + // BOOST_FOREACH(const typename boost::shared_ptr& factor, *cachedSeparatorMarginal_) { + // if (factor) factor->permuteWithInverse(reduction); } + // gttoc(Undo_Reduce); + // } + // } else { + // gttic(BayesTreeCliqueBaseUnordered_separatorMarginal_cachehit); + // } - // return the shortcut P(S||B) - return *cachedSeparatorMarginal_; // return the cached version - } + // // return the shortcut P(S||B) + // return *cachedSeparatorMarginal_; // return the cached version + //} /* ************************************************************************* */ // marginal2, uses separator marginal of parent recursively // P(C) = P(F|S) P(S) /* ************************************************************************* */ - template - FactorGraph::FactorType> BayesTreeCliqueBaseUnordered< - DERIVED, CONDITIONAL>::marginal2(derived_ptr R, Eliminate function) const - { - gttic(BayesTreeCliqueBaseUnordered_marginal2); - // initialize with separator marginal P(S) - FactorGraph p_C(this->separatorMarginal(R, function)); - // add the conditional P(F|S) - p_C.push_back(this->conditional()->toFactor()); - return p_C; - } + //template + //FactorGraph::FactorType> BayesTreeCliqueBaseUnordered< + // DERIVED, CONDITIONAL>::marginal2(derived_ptr R, Eliminate function) const + //{ + // gttic(BayesTreeCliqueBaseUnordered_marginal2); + // // initialize with separator marginal P(S) + // FactorGraph p_C(this->separatorMarginal(R, function)); + // // add the conditional P(F|S) + // p_C.push_back(this->conditional()->toFactor()); + // return p_C; + //} /* ************************************************************************* */ - template - void BayesTreeCliqueBaseUnordered::deleteCachedShortcuts() { + template + void BayesTreeCliqueBaseUnordered::deleteCachedShortcuts() { // When a shortcut is requested, all of the shortcuts between it and the // root are also generated. So, if this clique's cached shortcut is set, // recursively call over all child cliques. Otherwise, it is unnecessary. if (cachedSeparatorMarginal_) { - BOOST_FOREACH(derived_ptr& child, children_) { + BOOST_FOREACH(derived_ptr& child, children) { child->deleteCachedShortcuts(); } diff --git a/gtsam/inference/BayesTreeCliqueBaseUnordered.h b/gtsam/inference/BayesTreeCliqueBaseUnordered.h index e1aff08a6..a04fa0499 100644 --- a/gtsam/inference/BayesTreeCliqueBaseUnordered.h +++ b/gtsam/inference/BayesTreeCliqueBaseUnordered.h @@ -22,7 +22,7 @@ #include namespace gtsam { - template class BayesTreeUnordered; + template class BayesTreeUnordered; } namespace gtsam { @@ -43,18 +43,21 @@ namespace gtsam { template struct BayesTreeCliqueBaseUnordered { - public: + private: typedef BayesTreeCliqueBaseUnordered This; typedef DERIVED DerivedType; - typedef FACTORGRAPH FactorGraphType; - typedef BAYESNET BayesNetType; - typedef BayesNetType::ConditionalType ConditionalType; - typedef boost::shared_ptr sharedConditional; typedef boost::shared_ptr shared_ptr; typedef boost::weak_ptr weak_ptr; typedef boost::shared_ptr derived_ptr; typedef boost::weak_ptr derived_weak_ptr; - typedef FactorGraphType::FactorType FactorType; + + public: + typedef FACTORGRAPH FactorGraphType; + typedef BAYESNET BayesNetType; + typedef typename BayesNetType::ConditionalType ConditionalType; + typedef boost::shared_ptr sharedConditional; + typedef typename FactorGraphType::FactorType FactorType; + typedef typename FactorGraphType::Eliminate Eliminate; protected: @@ -87,8 +90,7 @@ namespace gtsam { } /** print this node */ - void print(const std::string& s = "", const KeyFormatter& keyFormatter = - DefaultKeyFormatter) const; + void print(const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const; /// @} /// @name Standard Interface @@ -113,14 +115,14 @@ namespace gtsam { /// @name Advanced Interface /// @{ - /** return the conditional P(S|Root) on the separator given the root */ - BayesNetType shortcut(derived_ptr root, Eliminate function) const; + ///** return the conditional P(S|Root) on the separator given the root */ + //BayesNetType shortcut(derived_ptr root, Eliminate function) const; - /** return the marginal P(S) on the separator */ - FactorGraphType separatorMarginal(derived_ptr root, Eliminate function) const; + ///** return the marginal P(S) on the separator */ + //FactorGraphType separatorMarginal(derived_ptr root, Eliminate function) const; - /** return the marginal P(C) of the clique, using marginal caching */ - FactorGraphType marginal2(derived_ptr root, Eliminate function) const; + ///** return the marginal P(C) of the clique, using marginal caching */ + //FactorGraphType marginal2(derived_ptr root, Eliminate function) const; /** * This deletes the cached shortcuts of all cliques (subtree) below this clique. @@ -146,30 +148,13 @@ namespace gtsam { * The factor graph p_Cp_B has keys from the parent clique Cp and from B. * But we only keep the variables not in S union B. */ - std::vector shortcut_indices(derived_ptr B, - const FactorGraph& p_Cp_B) const; + std::vector shortcut_indices(derived_ptr B, const FactorGraphType& p_Cp_B) const; /** Non-recursive delete cached shortcuts and marginals - internal only. */ void deleteCachedShortcutsNonRecursive() { cachedSeparatorMarginal_ = boost::none; } private: - /** - * Cliques cannot be copied except by the clone() method, which does not - * copy the parent and child pointers. - */ - BayesTreeCliqueBaseUnordered(const This& other) { - assert(false); - } - - /** Cliques cannot be copied except by the clone() method, which does not - * copy the parent and child pointers. - */ - This& operator=(const This& other) { - assert(false); - return *this; - } - /** Serialization function */ friend class boost::serialization::access; template diff --git a/gtsam/inference/BayesTreeCliqueDefault.h b/gtsam/inference/BayesTreeCliqueDefault.h index d6f4c006f..f8c72f018 100644 --- a/gtsam/inference/BayesTreeCliqueDefault.h +++ b/gtsam/inference/BayesTreeCliqueDefault.h @@ -40,13 +40,13 @@ namespace gtsam { public BayesTreeCliqueBaseUnordered, FACTORGRAPH, BAYESNET> { public: typedef typename BAYESNET::ConditionalType ConditionalType; + typedef typename FACTORGRAPH::FactorType FactorType; typedef BayesTreeCliqueDefaultUnordered This; typedef BayesTreeCliqueBaseUnordered Base; typedef boost::shared_ptr shared_ptr; typedef boost::weak_ptr weak_ptr; BayesTreeCliqueDefaultUnordered() {} - BayesTreeCliqueDefaultUnordered(const typename ConditionalType::shared_ptr& conditional) : Base(conditional) {} - BayesTreeCliqueDefaultUnordered(const std::pair& result) : Base(result) {} + BayesTreeCliqueDefaultUnordered(const boost::shared_ptr& conditional) : Base(conditional) {} private: /** Serialization function */ diff --git a/gtsam/inference/BayesTreeUnordered-inst.h b/gtsam/inference/BayesTreeUnordered-inst.h index db0f118bc..5516cefd4 100644 --- a/gtsam/inference/BayesTreeUnordered-inst.h +++ b/gtsam/inference/BayesTreeUnordered-inst.h @@ -21,8 +21,10 @@ #pragma once #include +#include #include +#include namespace gtsam { @@ -31,16 +33,17 @@ namespace gtsam { typename BayesTreeUnordered::CliqueData BayesTreeUnordered::getCliqueData() const { CliqueData data; - getCliqueData(data, root_); + BOOST_FOREACH(const sharedClique& root, roots_) + getCliqueData(data, root); return data; } /* ************************************************************************* */ template void BayesTreeUnordered::getCliqueData(CliqueData& data, sharedClique clique) const { - data.conditionalSizes.push_back((*clique)->nrFrontals()); - data.separatorSizes.push_back((*clique)->nrParents()); - BOOST_FOREACH(sharedClique c, clique->children_) { + data.conditionalSizes.push_back(clique->conditional()->nrFrontals()); + data.separatorSizes.push_back(clique->conditional()->nrParents()); + BOOST_FOREACH(sharedClique c, clique->children) { getCliqueData(data, c); } } @@ -48,16 +51,20 @@ namespace gtsam { /* ************************************************************************* */ template size_t BayesTreeUnordered::numCachedSeparatorMarginals() const { - return (root_) ? root_->numCachedSeparatorMarginals() : 0; + size_t count = 0; + BOOST_FOREACH(const sharedClique& root, roots_) + count += root->numCachedSeparatorMarginals(); + return count; } /* ************************************************************************* */ template void BayesTreeUnordered::saveGraph(const std::string &s, const KeyFormatter& keyFormatter) const { - if (!root_.get()) throw std::invalid_argument("the root of Bayes tree has not been initialized!"); + if (roots_.empty()) throw std::invalid_argument("the root of Bayes tree has not been initialized!"); std::ofstream of(s.c_str()); of<< "digraph G{\n"; - saveGraph(of, root_, keyFormatter); + BOOST_FOREACH(const sharedClique& root, roots_) + saveGraph(of, root, keyFormatter); of<<"}"; of.close(); } @@ -77,7 +84,7 @@ namespace gtsam { parent += indexFormatter(index); } - if( clique != root_){ + if(clique->parent()){ parent += " : "; s << parentnum << "->" << num << "\n"; } @@ -91,7 +98,7 @@ namespace gtsam { s << parent; parentnum = num; - BOOST_FOREACH(sharedClique c, clique->children_) { + BOOST_FOREACH(sharedClique c, clique->children) { num++; saveGraph(s, c, indexFormatter, parentnum); } @@ -152,27 +159,24 @@ namespace gtsam { /* ************************************************************************* */ template - typename BayesTreeUnordered::sharedClique - BayesTreeUnordered::addClique(const sharedConditional& conditional, const sharedClique& parent_clique) { - sharedClique new_clique(new Clique(conditional)); - addClique(new_clique, parent_clique); - return new_clique; + size_t BayesTreeUnordered::size() const { + size_t size = 0; + BOOST_FOREACH(const sharedClique& clique, roots_) + size += clique->treeSize(); + return size; } /* ************************************************************************* */ template void BayesTreeUnordered::addClique(const sharedClique& clique, const sharedClique& parent_clique) { - nodes_.resize(std::max((*clique)->lastFrontalKey()+1, nodes_.size())); - BOOST_FOREACH(Index j, (*clique)->frontals()) + BOOST_FOREACH(Index j, clique->conditional()->frontals()) nodes_[j] = clique; if (parent_clique != NULL) { clique->parent_ = parent_clique; - parent_clique->children_.push_back(clique); + parent_clique->children.push_back(clique); } else { - assert(!root_); - root_ = clique; + roots_.push_back(clique); } - clique->assertInvariants(); } /* ************************************************************************* */ @@ -181,13 +185,11 @@ namespace gtsam { const sharedConditional& conditional, std::list& child_cliques) { sharedClique new_clique(new Clique(conditional)); - nodes_.resize(std::max(conditional->lastFrontalKey()+1, nodes_.size())); BOOST_FOREACH(Index j, conditional->frontals()) nodes_[j] = new_clique; - new_clique->children_ = child_cliques; + new_clique->children.assign(child_cliques.begin(), child_cliques.end()); BOOST_FOREACH(sharedClique& child, child_cliques) child->parent_ = new_clique; - new_clique->assertInvariants(); return new_clique; } @@ -196,16 +198,16 @@ namespace gtsam { void BayesTreeUnordered::removeClique(sharedClique clique) { if (clique->isRoot()) - root_.reset(); + roots_.erase(std::find(roots_.begin(), roots_.end(), clique)); else { // detach clique from parent sharedClique parent = clique->parent_.lock(); - typename FastList::iterator child = std::find(parent->children().begin(), parent->children().end(), clique); - assert(child != parent->children().end()); - parent->children().erase(child); + typename std::vector::iterator child = std::find(parent->children.begin(), parent->children.end(), clique); + assert(child != parent->children.end()); + parent->children.erase(child); } // orphan my children - BOOST_FOREACH(sharedClique child, clique->children_) + BOOST_FOREACH(sharedClique child, clique->children) child->parent_ = typename Clique::weak_ptr(); BOOST_FOREACH(Key j, clique->conditional()->frontals()) { @@ -304,7 +306,9 @@ namespace gtsam { template BayesTreeUnordered& BayesTreeUnordered::operator=(const This& other) { this->clear(); - other.cloneTo(*this); + std::vector clonedRoots = treeTraversal::CloneForest(other); + BOOST_FOREACH(const sharedClique& root, clonedRoots) + insertRoot(root); return *this; } @@ -312,24 +316,25 @@ namespace gtsam { template void BayesTreeUnordered::print(const std::string& s, const KeyFormatter& keyFormatter) const { std::cout << s << ": cliques: " << size() << ", variables: " << nodes_.size() << std::endl; - treeTraversal::PrintForest(*this, s, keyFormatter) + treeTraversal::PrintForest(*this, s, keyFormatter); } /* ************************************************************************* */ // binary predicate to test equality of a pair for use in equals template bool check_sharedCliques( - const typename BayesTreeUnordered::sharedClique& v1, - const typename BayesTreeUnordered::sharedClique& v2 + const std::pair::sharedClique>& v1, + const std::pair::sharedClique>& v2 ) { - return (!v1 && !v2) || (v1 && v2 && v1->equals(*v2)); + return v1.first == v2.first && + ((!v1.second && !v2.second) || (v1.second && v2.second && v1.second->equals(*v2.second))); } /* ************************************************************************* */ template bool BayesTreeUnordered::equals(const BayesTreeUnordered& other, double tol) const { return size()==other.size() && - std::equal(nodes_.begin(), nodes_.end(), other.nodes_.begin(), &check_sharedCliques); + std::equal(nodes_.begin(), nodes_.end(), other.nodes_.begin(), &check_sharedCliques); } /* ************************************************************************* */ @@ -348,31 +353,15 @@ namespace gtsam { BOOST_FOREACH(const Key& j, subtree->conditional()->frontals()) { nodes_[j] = subtree; } // Fill index for each child typedef typename BayesTreeUnordered::sharedClique sharedClique; - BOOST_FOREACH(const sharedClique& child, subtree->children_) { + BOOST_FOREACH(const sharedClique& child, subtree->children) { fillNodesIndex(child); } } /* ************************************************************************* */ template - void BayesTreeUnordered::insert(const sharedClique& subtree) { - if(subtree) { - // Find the parent clique of the new subtree. By the running intersection - // property, those separator variables in the subtree that are ordered - // lower than the highest frontal variable of the subtree root will all - // appear in the separator of the subtree root. - if(subtree->conditional()->parents().empty()) { - assert(!root_); - root_ = subtree; - } else { - Key parentRepresentative = findParentClique(subtree->conditional()->parents()); - sharedClique parent = (*this)[parentRepresentative]; - parent->children_ += subtree; - subtree->parent_ = parent; // set new parent! - } - - // Now fill in the nodes index - fillNodesIndex(subtree); - } + void BayesTreeUnordered::insertRoot(const sharedClique& subtree) { + roots_.push_back(subtree); // Add to roots + fillNodesIndex(subtree); // Populate nodes index } /* ************************************************************************* */ @@ -576,7 +565,15 @@ namespace gtsam { void BayesTreeUnordered::clear() { // Remove all nodes and clear the root pointer nodes_.clear(); - root_.reset(); + roots_.clear(); + } + + /* ************************************************************************* */ + template + void BayesTreeUnordered::deleteCachedShortcuts() { + BOOST_FOREACH(const sharedClique& root, roots_) { + root->deleteCachedShortcuts(); + } } /* ************************************************************************* */ @@ -596,7 +593,8 @@ namespace gtsam { this->removePath(typename Clique::shared_ptr(clique->parent_.lock()), bn, orphans); // add children to list of orphans (splice also removed them from clique->children_) - orphans.splice(orphans.begin(), clique->children_); + orphans.insert(orphans.begin(), clique->children.begin(), clique->children.end()); + clique->children.clear(); bn.push_back(clique->conditional()); @@ -638,15 +636,16 @@ namespace gtsam { // Remove the first clique from its parents if(!subtree->isRoot()) - subtree->parent()->children().remove(subtree); + subtree->parent()->children.erase(std::find( + subtree->parent()->children.begin(), subtree->parent()->children.end(), subtree)); else - root_.reset(); + roots_.erase(std::find(roots_.begin(), roots_.end(), subtree)); // Add all subtree cliques and erase the children and parent of each for(typename Cliques::iterator clique = cliques.begin(); clique != cliques.end(); ++clique) { // Add children - BOOST_FOREACH(const sharedClique& child, (*clique)->children()) { + BOOST_FOREACH(const sharedClique& child, (*clique)->children) { cliques.push_back(child); } // Delete cached shortcuts @@ -658,7 +657,7 @@ namespace gtsam { // Erase the parent and children pointers (*clique)->parent_.reset(); - (*clique)->children_.clear(); + (*clique)->children.clear(); } return cliques; diff --git a/gtsam/inference/BayesTreeUnordered.h b/gtsam/inference/BayesTreeUnordered.h index 7411bc4de..06978af0f 100644 --- a/gtsam/inference/BayesTreeUnordered.h +++ b/gtsam/inference/BayesTreeUnordered.h @@ -47,6 +47,8 @@ namespace gtsam { typedef boost::shared_ptr shared_ptr; typedef CLIQUE Clique; ///< The clique type, normally BayesTreeClique typedef boost::shared_ptr sharedClique; ///< Shared pointer to a clique + typedef Clique Node; ///< Synonym for Clique (TODO: remove) + typedef sharedClique sharedNode; ///< Synonym for sharedClique (TODO: remove) typedef typename CLIQUE::ConditionalType ConditionalType; typedef boost::shared_ptr sharedConditional; typedef typename CLIQUE::BayesNetType BayesNetType; @@ -128,12 +130,7 @@ namespace gtsam { /// @{ /** number of cliques */ - inline size_t size() const { - if(root_) - return root_->treeSize(); - else - return 0; - } + size_t size() const; /** Check if there are any cliques in the tree */ inline bool empty() const { @@ -152,7 +149,7 @@ namespace gtsam { if(c == nodes_.end()) throw std::out_of_range("Requested the BayesTree clique for a key that is not in the BayesTree"); else - return *c; + return c->second; } /** Gather data on all cliques */ @@ -205,9 +202,7 @@ namespace gtsam { void clear(); /** Clear all shortcut caches - use before timing on marginal calculation to avoid residual cache data */ - void deleteCachedShortcuts() { - root_->deleteCachedShortcuts(); - } + void deleteCachedShortcuts(); /** * Remove path from clique to root and return that path as factors @@ -226,14 +221,10 @@ namespace gtsam { * Remove the requested subtree. */ Cliques removeSubtree(const sharedClique& subtree); - /** - * Hang a new subtree off of the existing tree. This finds the appropriate - * parent clique for the subtree (which may be the root), and updates the - * nodes index with the new cliques in the subtree. None of the frontal - * variables in the subtree may appear in the separators of the existing - * BayesTree. - */ - void insert(const sharedClique& subtree); + /** Insert a new subtree with known parent clique. This function does not check that the + * specified parent is the correct parent. This function updates all of the internal data + * structures associated with adding a subtree, such as populating the nodes index. */ + void insertRoot(const sharedClique& subtree); /** * Create a clone of this object as a shared pointer @@ -255,9 +246,6 @@ namespace gtsam { /** remove a clique: warning, can result in a forest */ void removeClique(sharedClique clique); - /** add a clique (top down) */ - sharedClique addClique(const sharedConditional& conditional, const sharedClique& parent_clique = sharedClique()); - /** add a clique (top down) */ void addClique(const sharedClique& clique, const sharedClique& parent_clique = sharedClique()); diff --git a/gtsam/inference/ConditionalUnordered.h b/gtsam/inference/ConditionalUnordered.h index 4665820bf..9734c40f4 100644 --- a/gtsam/inference/ConditionalUnordered.h +++ b/gtsam/inference/ConditionalUnordered.h @@ -75,7 +75,8 @@ namespace gtsam { void print(const std::string& s = "Conditional", const KeyFormatter& formatter = DefaultKeyFormatter) const; /** check equality */ - bool equals(const This& c, double tol = 1e-9) const { return nrFrontals_ == c.nrFrontals_; } + bool equals(const This& c, double tol = 1e-9) const { + return asFactor().equals(c.asFactor()) && nrFrontals_ == c.nrFrontals_; } /// @} /// @name Standard Interface diff --git a/gtsam/inference/EliminateableFactorGraph-inst.h b/gtsam/inference/EliminateableFactorGraph-inst.h index b842a4644..56017a327 100644 --- a/gtsam/inference/EliminateableFactorGraph-inst.h +++ b/gtsam/inference/EliminateableFactorGraph-inst.h @@ -35,11 +35,11 @@ namespace gtsam { std::pair, boost::shared_ptr > result; if(ordering) { // Do elimination with given ordering - result = EliminationTreeType(*this, variableIndex, *ordering).eliminate(function); + result = ELIMINATIONTREE(asDerived(), variableIndex, *ordering).eliminate(function); } else { // Compute ordering - OrderingUnordered colamdOrdering = variableIndex.orderingCOLAMD(); - result = EliminationTreeType(*this, variableIndex, colamdOrdering).eliminate(function); + OrderingUnordered colamdOrdering = OrderingUnordered::COLAMD(variableIndex); + result = ELIMINATIONTREE(asDerived(), variableIndex, colamdOrdering).eliminate(function); } // If any factors are remaining, the ordering was incomplete @@ -62,11 +62,11 @@ namespace gtsam { std::pair, boost::shared_ptr > result; if(ordering) { // Do elimination with given ordering - result = JunctionTreeType(*this, variableIndex, *ordering).eliminate(function); + result = JUNCTIONTREE(ELIMINATIONTREE(asDerived(), variableIndex, *ordering)).eliminate(function); } else { // Compute ordering - OrderingUnordered colamdOrdering = variableIndex.orderingCOLAMD(); - result = JunctionTreeType(*this, variableIndex, colamdOrdering).eliminate(function); + OrderingUnordered colamdOrdering = OrderingUnordered::COLAMD(variableIndex); + result = JUNCTIONTREE(ELIMINATIONTREE(asDerived(), variableIndex, colamdOrdering)).eliminate(function); } // If any factors are remaining, the ordering was incomplete @@ -76,4 +76,5 @@ namespace gtsam { // Return the Bayes tree return result.first; } + } diff --git a/gtsam/inference/EliminateableFactorGraph.h b/gtsam/inference/EliminateableFactorGraph.h index 16042c7ec..81d426999 100644 --- a/gtsam/inference/EliminateableFactorGraph.h +++ b/gtsam/inference/EliminateableFactorGraph.h @@ -121,6 +121,13 @@ namespace gtsam { eliminatePartialMultifrontal(const Eliminate& function, const std::vector& variables, const VariableIndexUnordered& variableIndex = VariableIndexUnordered(*this)); + private: + + // Access the derived factor graph class + const FACTORGRAPH& asDerived() const { return static_cast(*this); } + + // Access the derived factor graph class + FACTORGRAPH& asDerived() { return static_cast(*this); } }; } diff --git a/gtsam/inference/EliminationTreeUnordered-inst.h b/gtsam/inference/EliminationTreeUnordered-inst.h index ed0d6d472..8977be030 100644 --- a/gtsam/inference/EliminationTreeUnordered-inst.h +++ b/gtsam/inference/EliminationTreeUnordered-inst.h @@ -26,6 +26,7 @@ #include #include #include +#include #include namespace gtsam { @@ -64,8 +65,8 @@ namespace gtsam { void EliminationTreeUnordered::Node::print( const std::string& str, const KeyFormatter& keyFormatter) const { - std::cout << str << "(" << formatter(node->key) << ")\n"; - BOOST_FOREACH(const typename ETREE::sharedFactor& factor, node->factors) { + std::cout << str << "(" << keyFormatter(key) << ")\n"; + BOOST_FOREACH(const sharedFactor& factor, factors) { if(factor) factor->print(str + "| "); else @@ -77,7 +78,7 @@ namespace gtsam { /* ************************************************************************* */ template EliminationTreeUnordered::EliminationTreeUnordered(const FactorGraphType& graph, - const VariableIndexUnordered& structure, const std::vector& order) + const VariableIndexUnordered& structure, const OrderingUnordered& order) { gttic(ET_Create1); @@ -156,7 +157,7 @@ namespace gtsam { /* ************************************************************************* */ template EliminationTreeUnordered::EliminationTreeUnordered( - const FactorGraphType& factorGraph, const std::vector& order) + const FactorGraphType& factorGraph, const OrderingUnordered& order) { gttic(ET_Create2); // Build variable index first diff --git a/gtsam/inference/EliminationTreeUnordered.h b/gtsam/inference/EliminationTreeUnordered.h index 9eb0454a1..761a10500 100644 --- a/gtsam/inference/EliminationTreeUnordered.h +++ b/gtsam/inference/EliminationTreeUnordered.h @@ -27,6 +27,7 @@ class EliminationTreeUnorderedTester; // for unit tests, see testEliminationTree namespace gtsam { class VariableIndexUnordered; + class OrderingUnordered; /** * An elimination tree is a data structure used intermediately during @@ -98,14 +99,14 @@ namespace gtsam { * @return The elimination tree */ EliminationTreeUnordered(const FactorGraphType& factorGraph, - const VariableIndexUnordered& structure, const std::vector& order); + const VariableIndexUnordered& structure, const OrderingUnordered& order); /** Build the elimination tree of a factor graph. Note that this has to compute the column * structure as a VariableIndex, so if you already have this precomputed, use the other * constructor instead. * @param factorGraph The factor graph for which to build the elimination tree */ - EliminationTreeUnordered(const FactorGraphType& factorGraph, const std::vector& order); + EliminationTreeUnordered(const FactorGraphType& factorGraph, const OrderingUnordered& order); /** Copy constructor - makes a deep copy of the tree structure, but only pointers to factors are * copied, factors are not cloned. */ diff --git a/gtsam/inference/FactorGraphUnordered.h b/gtsam/inference/FactorGraphUnordered.h index eddc43781..4d1562402 100644 --- a/gtsam/inference/FactorGraphUnordered.h +++ b/gtsam/inference/FactorGraphUnordered.h @@ -60,7 +60,7 @@ namespace gtsam { public: - /// @name Standard Constructor + /// @name Standard Constructors /// @{ /** Default constructor */ diff --git a/gtsam/inference/JunctionTreeUnordered-inst.h b/gtsam/inference/JunctionTreeUnordered-inst.h index 18b01efd9..ccce28a8a 100644 --- a/gtsam/inference/JunctionTreeUnordered-inst.h +++ b/gtsam/inference/JunctionTreeUnordered-inst.h @@ -26,47 +26,19 @@ #include #include +#include namespace gtsam { - - /* ************************************************************************* */ - template - typename JunctionTreeUnordered::sharedFactor - JunctionTreeUnordered::Node::eliminate( - const boost::shared_ptr& output, - const Eliminate& function, const std::vector& childrenResults) const - { - // This function eliminates one node (Node::eliminate) - see below eliminate for the whole tree. - - assert(childrenResults.size() == children.size()); - - // Gather factors - std::vector gatheredFactors; - gatheredFactors.reserve(factors.size() + children.size()); - gatheredFactors.insert(gatheredFactors.end(), factors.begin(), factors.end()); - gatheredFactors.insert(gatheredFactors.end(), childrenResults.begin(), childrenResults.end()); - - // Do dense elimination step - std::pair, boost::shared_ptr > eliminationResult = - function(gatheredFactors, keys); - - // Add conditional to BayesNet - output->push_back(eliminationResult.first); - - // Return result - return eliminationResult.second; - } - - + namespace { /* ************************************************************************* */ template struct ConstructorTraversalData { - const ConstructorTraversalData* parentData; + ConstructorTraversalData* const parentData; typename JunctionTreeUnordered::sharedNode myJTNode; std::vector childSymbolicConditionals; std::vector childSymbolicFactors; - ConstructorTraversalData(const ConstructorTraversalData* _parentData) : parentData(_parentData) {} + ConstructorTraversalData(ConstructorTraversalData* _parentData) : parentData(_parentData) {} }; /* ************************************************************************* */ @@ -74,12 +46,12 @@ namespace gtsam { template ConstructorTraversalData ConstructorTraversalVisitorPre( const boost::shared_ptr& node, - const ConstructorTraversalData& parentData) + ConstructorTraversalData& parentData) { // On the pre-order pass, before children have been visited, we just set up a traversal data // structure with its own JT node, and create a child pointer in its parent. ConstructorTraversalData myData = ConstructorTraversalData(&parentData); - myData.myJTNode = boost::make_shared::Node>(); + myData.myJTNode = boost::make_shared::Node>(); myData.myJTNode->keys.push_back(node->key); myData.myJTNode->factors.insert(myData.myJTNode->factors.begin(), node->factors.begin(), node->factors.end()); parentData.myJTNode->children.push_back(myData.myJTNode); @@ -93,17 +65,27 @@ namespace gtsam { const boost::shared_ptr& node, const ConstructorTraversalData& myData) { + // In this post-order visitor, we combine the symbolic elimination results from the + // elimination tree children and symbolically eliminate the current elimination tree node. We + // then check whether each of our elimination tree child nodes should be merged with us. The + // check for this is that our number of symbolic elimination parents is exactly 1 less than + // our child's symbolic elimination parents - this condition indicates that eliminating the + // current node did not introduce any parents beyond those already in the child. + // Do symbolic elimination for this node std::vector symbolicFactors; symbolicFactors.reserve(node->factors.size() + myData.childSymbolicFactors.size()); + // Add symbolic versions of the ETree node factors BOOST_FOREACH(const typename GRAPH::sharedFactor& factor, node->factors) { - symbolicFactors.push_back(boost::make_shared(factor)); } + symbolicFactors.push_back(boost::make_shared( + SymbolicFactorUnordered::FromKeys(*factor))); } + // Add symbolic factors passed up from children symbolicFactors.insert(symbolicFactors.end(), myData.childSymbolicFactors.begin(), myData.childSymbolicFactors.end()); std::vector keyAsVector(1); keyAsVector[0] = node->key; std::pair symbolicElimResult = EliminateSymbolicUnordered(symbolicFactors, keyAsVector); - // Store symbolic elimination results + // Store symbolic elimination results in the parent myData.parentData->childSymbolicConditionals.push_back(symbolicElimResult.first); myData.parentData->childSymbolicFactors.push_back(symbolicElimResult.second); @@ -118,7 +100,7 @@ namespace gtsam { if(myNrParents + 1 == myData.childSymbolicConditionals[child]->nrParents()) { // Get a reference to the child, adjusting the index to account for children previously // merged and removed from the child list. - const typename JunctionTreeUnordered::Node& childToMerge = + const typename JunctionTreeUnordered::Node& childToMerge = *myData.myJTNode->children[child - nrMergedChildren]; // Merge keys, factors, and children. myData.myJTNode->keys.insert(myData.myJTNode->keys.end(), childToMerge.keys.begin(), childToMerge.keys.end()); @@ -129,12 +111,77 @@ namespace gtsam { } } } + + /* ************************************************************************* */ + // Elimination traversal data - stores a pointer to the parent data and collects the factors + // resulting from elimination of the children. Also sets up BayesTree cliques with parent and + // child pointers. + template + struct EliminationData { + EliminationData* const parentData; + std::vector childFactors; + boost::shared_ptr bayesTreeNode; + EliminationData(EliminationData* _parentData, size_t nChildren) : + parentData(_parentData), + bayesTreeNode(boost::make_shared()) { + childFactors.reserve(nChildren); + // Set up BayesTree parent and child pointers + if(parentData) { + bayesTreeNode->parent_ = parentData->bayesTreeNode; + parentData->bayesTreeNode->children.push_back(bayesTreeNode); + } + } + }; + + /* ************************************************************************* */ + // Elimination pre-order visitor - just creates the EliminationData structure for the visited + // node. + template + EliminationData eliminationPreOrderVisitor( + const typename JUNCTIONTREE::sharedNode& node, EliminationData& parentData) + { + return EliminationData(&parentData, node->children.size()); + } + + /* ************************************************************************* */ + // Elimination post-order visitor - combine the child factors with our own factors, add the + // resulting conditional to the BayesTree, and add the remaining factor to the parent. The + // extra argument 'eliminationFunction' is passed from JunctionTree::eliminate using + // boost::bind. + template + void eliminationPostOrderVisitor(const typename JUNCTIONTREE::sharedNode& node, EliminationData& myData, + const typename JUNCTIONTREE::Eliminate& eliminationFunction) + { + // Typedefs + typedef typename JUNCTIONTREE::sharedFactor sharedFactor; + typedef typename JUNCTIONTREE::FactorType FactorType; + typedef typename JUNCTIONTREE::ConditionalType ConditionalType; + typedef typename JUNCTIONTREE::BayesTreeType::Node BTNode; + + // Gather factors + assert(node->children.size() == myData.childFactors.size()); + std::vector gatheredFactors; + gatheredFactors.reserve(node->factors.size() + node->children.size()); + gatheredFactors.insert(gatheredFactors.end(), node->factors.begin(), node->factors.end()); + gatheredFactors.insert(gatheredFactors.end(), myData.childFactors.begin(), myData.childFactors.end()); + + // Do dense elimination step + std::pair, boost::shared_ptr > eliminationResult = + eliminationFunction(gatheredFactors, node->keys); + + // Store conditional in BayesTree clique + myData.bayesTreeNode->conditional_ = eliminationResult.first; + + // Store remaining factor in parent's gathered factors + myData.parentData->childFactors.push_back(eliminationResult.second); + } } /* ************************************************************************* */ template template - JunctionTreeUnordered::JunctionTreeUnordered(const ETREE& eliminationTree) + JunctionTreeUnordered + JunctionTreeUnordered::FromEliminationTree(const ETREE& eliminationTree) { // Here we rely on the BayesNet having been produced by this elimination tree, such that the // conditionals are arranged in DFS post-order. We traverse the elimination tree, and inspect @@ -144,16 +191,20 @@ namespace gtsam { // Traverse the elimination tree, doing symbolic elimination and merging nodes as we go. Gather // the created junction tree roots in a dummy Node. + typedef typename ETREE::Node ETreeNode; ConstructorTraversalData rootData(0); rootData.myJTNode = boost::make_shared(); // Make a dummy node to gather the junction tree roots - treeTraversal.DepthFirstForest(eliminationTree, rootData, - ConstructorTraversalVisitorPre, ConstructorTraversalVisitorPost); + treeTraversal::DepthFirstForest(eliminationTree, rootData, + ConstructorTraversalVisitorPre, ConstructorTraversalVisitorPost); // Assign roots from the dummy node - roots_ = rootData.myJTNode->children; + This result; + result.roots_ = rootData.myJTNode->children; // Transfer remaining factors from elimination tree - remainingFactors_ = eliminationTree.remainingFactors(); + result.remainingFactors_ = eliminationTree.remainingFactors(); + + return result; } /* ************************************************************************* */ @@ -175,16 +226,22 @@ namespace gtsam { std::pair, boost::shared_ptr > JunctionTreeUnordered::eliminate(const Eliminate& function) const { - // Allocate result - boost::shared_ptr result = boost::make_shared(); + // Do elimination (depth-first traversal). The rootsContainer stores a 'dummy' BayesTree node + // that contains all of the roots as its children. rootsContainer also stores the remaining + // uneliminated factors passed up from the roots. + EliminationData rootsContainer(0, roots_.size()); + treeTraversal::DepthFirstForest(*this, rootsContainer, eliminationPreOrderVisitor, + boost::bind(eliminationPostOrderVisitor, _1, _2, function)); - // Run tree elimination algorithm - std::vector remainingFactors = inference::EliminateTree(result, *this, function); + // Create BayesTree from roots stored in the dummy BayesTree node. + boost::shared_ptr result = boost::make_shared(); + BOOST_FOREACH(const typename BayesTreeType::sharedNode& root, rootsContainer.bayesTreeNode->children) + result->insertRoot(root); // Add remaining factors that were not involved with eliminated variables boost::shared_ptr allRemainingFactors = boost::make_shared(); allRemainingFactors->push_back(remainingFactors_.begin(), remainingFactors_.end()); - allRemainingFactors->push_back(remainingFactors.begin(), remainingFactors.end()); + allRemainingFactors->push_back(rootsContainer.childFactors.begin(), rootsContainer.childFactors.end()); // Return result return std::make_pair(result, allRemainingFactors); diff --git a/gtsam/inference/JunctionTreeUnordered.h b/gtsam/inference/JunctionTreeUnordered.h index 6f8936292..39a8f9a61 100644 --- a/gtsam/inference/JunctionTreeUnordered.h +++ b/gtsam/inference/JunctionTreeUnordered.h @@ -71,9 +71,6 @@ namespace gtsam { Keys keys; ///< Frontal keys of this node Factors factors; ///< Factors associated with this node Children children; ///< sub-trees - - sharedFactor eliminate(const boost::shared_ptr& output, - const Eliminate& function, const std::vector& childrenFactors) const; }; typedef boost::shared_ptr sharedNode; ///< Shared pointer to Node @@ -83,17 +80,17 @@ namespace gtsam { /** concept check */ GTSAM_CONCEPT_TESTABLE_TYPE(FactorType); - std::vector& roots_; + std::vector roots_; std::vector remainingFactors_; - public: + protected: /// @name Standard Constructors /// @{ /** Build the junction tree from an elimination tree. */ template - JunctionTreeUnordered(const ETREE& eliminationTree); + static This FromEliminationTree(const ETREE& eliminationTree); /** Copy constructor - makes a deep copy of the tree structure, but only pointers to factors are * copied, factors are not cloned. */ @@ -105,6 +102,8 @@ namespace gtsam { /// @} + public: + /// @name Standard Interface /// @{ @@ -129,6 +128,11 @@ namespace gtsam { /// @} + private: + + // Private default constructor (used in static construction methods) + JunctionTreeUnordered() {} + }; } \ No newline at end of file diff --git a/gtsam/inference/OrderingUnordered.cpp b/gtsam/inference/OrderingUnordered.cpp new file mode 100644 index 000000000..6dacd66a3 --- /dev/null +++ b/gtsam/inference/OrderingUnordered.cpp @@ -0,0 +1,95 @@ +/* ---------------------------------------------------------------------------- + + * 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 Ordering.cpp + * @author Richard Roberts + * @date Sep 2, 2010 + */ + +#pragma once + +#include +#include + +#include +#include + +using namespace std; + +namespace gtsam { + + /* ************************************************************************* */ + OrderingUnordered OrderingUnordered::COLAMD(const VariableIndexUnordered& variableIndex) + { + gttic(OrderingUnordered_COLAMD); + + gttic(Prepare); + size_t nEntries = variableIndex.nEntries(), nFactors = variableIndex.nFactors(), nVars = variableIndex.size(); + // Convert to compressed column major format colamd wants it in (== MATLAB format!) + size_t Alen = ccolamd_recommended((int)nEntries, (int)nFactors, (int)nVars); /* colamd arg 3: size of the array A */ + vector A = vector(Alen); /* colamd arg 4: row indices of A, of size Alen */ + vector p = vector(nVars + 1); /* colamd arg 5: column pointers of A, of size n_col+1 */ + vector cmember(nVars, 0); // For now, no constraints + + // Fill in input data for COLAMD + p[0] = 0; + int count = 0; + vector keys(nVars); // Array to store the keys in the order we add them so we can retrieve them in permuted order + size_t index = 0; + BOOST_FOREACH(const VariableIndexUnordered::const_iterator::value_type key_factors, variableIndex) { + // Arrange factor indices into COLAMD format + const VariableIndexUnordered::Factors& column = key_factors.second; + size_t lastFactorId = numeric_limits::max(); + BOOST_FOREACH(size_t factorIndex, column) { + if(lastFactorId != numeric_limits::max()) + assert(factorIndex > lastFactorId); + A[count++] = (int)factorIndex; // copy sparse column + } + p[index+1] = count; // column j (base 1) goes from A[j-1] to A[j]-1 + // Store key in array and increment index + keys[index] = key_factors.first; + ++ index; + } + + assert((size_t)count == variableIndex.nEntries()); + + //double* knobs = NULL; /* colamd arg 6: parameters (uses defaults if NULL) */ + double knobs[CCOLAMD_KNOBS]; + ccolamd_set_defaults(knobs); + knobs[CCOLAMD_DENSE_ROW]=-1; + knobs[CCOLAMD_DENSE_COL]=-1; + + int stats[CCOLAMD_STATS]; /* colamd arg 7: colamd output statistics and error codes */ + + gttoc(Prepare); + + // call colamd, result will be in p + /* returns (1) if successful, (0) otherwise*/ + if(nVars > 0) { + gttic(ccolamd); + int rv = ccolamd((int)nFactors, (int)nVars, (int)Alen, &A[0], &p[0], knobs, stats, &cmember[0]); + if(rv != 1) + throw runtime_error((boost::format("ccolamd failed with return value %1%")%rv).str()); + } + + // ccolamd_report(stats); + + gttic(Fill_Ordering); + // Convert elimination ordering in p to an ordering + OrderingUnordered result; + result.assign(p.begin(), p.end() - 1); + gttoc(Fill_Ordering); + + return result; + } + +} diff --git a/gtsam/inference/OrderingUnordered.h b/gtsam/inference/OrderingUnordered.h index 1fe364be9..aa50c2f01 100644 --- a/gtsam/inference/OrderingUnordered.h +++ b/gtsam/inference/OrderingUnordered.h @@ -20,10 +20,23 @@ #include #include +#include +#include namespace gtsam { - class OrderingUnordered : std::vector { + class OrderingUnordered : public std::vector { public: - OrderingUnordered() {} + /// Create an empty ordering + GTSAM_EXPORT OrderingUnordered() {} + + /// Compute an ordering using COLAMD directly from a factor graph - this internally builds a + /// VariableIndex so if you already have a VariableIndex, it is faster to use COLAMD(const + /// VariableIndexUnordered&) + template + static OrderingUnordered COLAMD(const FactorGraphUnordered& graph) { + return COLAMD(VariableIndexUnordered(graph)); + } + + static GTSAM_EXPORT OrderingUnordered COLAMD(const VariableIndexUnordered& variableIndex); }; } diff --git a/gtsam/inference/inference-inst.h b/gtsam/inference/inference-inst.h index 668f6e6e3..51ec5366d 100644 --- a/gtsam/inference/inference-inst.h +++ b/gtsam/inference/inference-inst.h @@ -20,6 +20,7 @@ #pragma once #include +#include #include #include @@ -49,11 +50,11 @@ namespace gtsam { /* ************************************************************************* */ template - void eliminationPostOrderVisitor(const typename TREE::Node& const node, EliminationData& myData, + void eliminationPostOrderVisitor(const typename TREE::sharedNode& node, EliminationData& myData, RESULT& result, const typename TREE::Eliminate& eliminationFunction) { // Call eliminate on the node and add the result to the parent's gathered factors - myData.parentData->childFactors.push_back(node.eliminate(result, eliminationFunction, myData.childFactors)); + myData.parentData->childFactors.push_back(node->eliminate(result, eliminationFunction, myData.childFactors)); } } diff --git a/gtsam/symbolic/SymbolicBayesNetUnordered.cpp b/gtsam/symbolic/SymbolicBayesNetUnordered.cpp index 429f7bbe0..e340f4f71 100644 --- a/gtsam/symbolic/SymbolicBayesNetUnordered.cpp +++ b/gtsam/symbolic/SymbolicBayesNetUnordered.cpp @@ -16,6 +16,7 @@ * @author Richard Roberts */ +#include #include namespace gtsam { diff --git a/gtsam/symbolic/SymbolicBayesNetUnordered.h b/gtsam/symbolic/SymbolicBayesNetUnordered.h index d4002daee..57cf1f9f0 100644 --- a/gtsam/symbolic/SymbolicBayesNetUnordered.h +++ b/gtsam/symbolic/SymbolicBayesNetUnordered.h @@ -27,7 +27,7 @@ namespace gtsam { /** Symbolic Bayes Net * \nosubgrouping */ - class SymbolicBayesNetUnordered: public BayesNetUnordered { + class GTSAM_EXPORT SymbolicBayesNetUnordered: public BayesNetUnordered { public: diff --git a/gtsam/symbolic/SymbolicBayesTreeUnordered.cpp b/gtsam/symbolic/SymbolicBayesTreeUnordered.cpp index 0a50e44a2..5f935a5f5 100644 --- a/gtsam/symbolic/SymbolicBayesTreeUnordered.cpp +++ b/gtsam/symbolic/SymbolicBayesTreeUnordered.cpp @@ -19,80 +19,86 @@ #include #include +#include +#include namespace gtsam { - /* ************************************************************************* */ - void SymbolicBayesTreeUnordered::insert(const sharedConditional& conditional) - { - static const bool debug = false; + void SymbolicBayesTreeUnordered::noop() const { - // get indices and parents - const typename CONDITIONAL::Parents& parents = conditional->parents(); - - if(debug) conditional->print("Adding conditional "); - - // if no parents, start a new root clique - if (parents.empty()) { - if(debug) std::cout << "No parents so making root" << std::endl; - bayesTree.root_ = bayesTree.addClique(conditional); - return; - } - - // otherwise, find the parent clique by using the index data structure - // to find the lowest-ordered parent - Index parentRepresentative = bayesTree.findParentClique(parents); - if(debug) std::cout << "First-eliminated parent is " << parentRepresentative << ", have " << bayesTree.nodes_.size() << " nodes." << std::endl; - sharedClique parent_clique = bayesTree[parentRepresentative]; - if(debug) parent_clique->print("Parent clique is "); - - // if the parents and parent clique have the same size, add to parent clique - if ((*parent_clique)->size() == size_t(parents.size())) { - if(debug) std::cout << "Adding to parent clique" << std::endl; -#ifndef NDEBUG - // Debug check that the parent indices of the new conditional match the indices - // currently in the clique. - // list::const_iterator parent = parents.begin(); - // typename Clique::const_iterator cond = parent_clique->begin(); - // while(parent != parents.end()) { - // assert(cond != parent_clique->end()); - // assert(*parent == (*cond)->key()); - // ++ parent; - // ++ cond; - // } -#endif - addToCliqueFront(bayesTree, conditional, parent_clique); - } else { - if(debug) std::cout << "Starting new clique" << std::endl; - // otherwise, start a new clique and add it to the tree - bayesTree.addClique(conditional,parent_clique); - } } - /* ************************************************************************* */ - void SymbolicBayesTreeUnordered::addToCliqueFront(const sharedConditional& conditional, const sharedClique& clique) { - static const bool debug = false; -#ifndef NDEBUG - // Debug check to make sure the conditional variable is ordered lower than - // its parents and that all of its parents are present either in this - // clique or its separator. - BOOST_FOREACH(Key parent, conditional->parents()) { - assert(parent > conditional->lastFrontalKey()); - const Clique& cliquer(*clique); - assert(find(cliquer->begin(), cliquer->end(), parent) != cliquer->end()); - } -#endif - if(debug) conditional->print("Adding conditional "); - if(debug) clique->print("To clique "); - Index j = conditional->lastFrontalKey(); - this->nodes_.resize(std::max(j+1, this->nodes_.size())); - this->nodes_[j] = clique; - FastVector newIndices(clique->conditional()->size() + 1); - newIndices[0] = j; - std::copy(clique->conditional()->begin(), clique->conditional()->end(), newIndices.begin()+1); - clique->conditional_ = ConditionalType::FromKeys(newIndices, (*clique)->nrFrontals() + 1); - if(debug) clique->print("Expanded clique is "); - clique->assertInvariants(); - } +// /* ************************************************************************* */ +// void SymbolicBayesTreeUnordered::insert(const sharedConditional& conditional) +// { +// static const bool debug = false; +// +// // get indices and parents +// const SymbolicConditionalUnordered::Parents& parents = conditional->parents(); +// +// if(debug) conditional->print("Adding conditional "); +// +// // if no parents, start a new root clique +// if (parents.empty()) { +// if(debug) std::cout << "No parents so making root" << std::endl; +// addClique(boost::make_shared(conditional)); +// return; +// } +// +// // otherwise, find the parent clique by using the index data structure +// // to find the lowest-ordered parent +// Index parentRepresentative = findParentClique(parents); +// if(debug) std::cout << "First-eliminated parent is " << parentRepresentative << ", have " << bayesTree.nodes_.size() << " nodes." << std::endl; +// sharedClique parent_clique = bayesTree[parentRepresentative]; +// if(debug) parent_clique->print("Parent clique is "); +// +// // if the parents and parent clique have the same size, add to parent clique +// if ((*parent_clique)->size() == size_t(parents.size())) { +// if(debug) std::cout << "Adding to parent clique" << std::endl; +//#ifndef NDEBUG +// // Debug check that the parent indices of the new conditional match the indices +// // currently in the clique. +// // list::const_iterator parent = parents.begin(); +// // typename Clique::const_iterator cond = parent_clique->begin(); +// // while(parent != parents.end()) { +// // assert(cond != parent_clique->end()); +// // assert(*parent == (*cond)->key()); +// // ++ parent; +// // ++ cond; +// // } +//#endif +// addToCliqueFront(bayesTree, conditional, parent_clique); +// } else { +// if(debug) std::cout << "Starting new clique" << std::endl; +// // otherwise, start a new clique and add it to the tree +// bayesTree.addClique(conditional,parent_clique); +// } +// } +// +// /* ************************************************************************* */ +// void SymbolicBayesTreeUnordered::addToCliqueFront(const sharedConditional& conditional, const sharedClique& clique) { +// static const bool debug = false; +//#ifndef NDEBUG +// // Debug check to make sure the conditional variable is ordered lower than +// // its parents and that all of its parents are present either in this +// // clique or its separator. +// BOOST_FOREACH(Key parent, conditional->parents()) { +// assert(parent > conditional->lastFrontalKey()); +// const Clique& cliquer(*clique); +// assert(find(cliquer->begin(), cliquer->end(), parent) != cliquer->end()); +// } +//#endif +// if(debug) conditional->print("Adding conditional "); +// if(debug) clique->print("To clique "); +// Index j = conditional->lastFrontalKey(); +// this->nodes_.resize(std::max(j+1, this->nodes_.size())); +// this->nodes_[j] = clique; +// FastVector newIndices(clique->conditional()->size() + 1); +// newIndices[0] = j; +// std::copy(clique->conditional()->begin(), clique->conditional()->end(), newIndices.begin()+1); +// clique->conditional_ = ConditionalType::FromKeys(newIndices, (*clique)->nrFrontals() + 1); +// if(debug) clique->print("Expanded clique is "); +// clique->assertInvariants(); +// } } diff --git a/gtsam/symbolic/SymbolicBayesTreeUnordered.h b/gtsam/symbolic/SymbolicBayesTreeUnordered.h index a627ae403..597393ead 100644 --- a/gtsam/symbolic/SymbolicBayesTreeUnordered.h +++ b/gtsam/symbolic/SymbolicBayesTreeUnordered.h @@ -19,18 +19,33 @@ #pragma once #include -#include -#include +#include +#include +#include namespace gtsam { - class SymbolicBayesTreeUnordered : - public BayesTreeUnordered + /* ************************************************************************* */ + class GTSAM_EXPORT SymbolicBayesTreeCliqueUnordered : + public BayesTreeCliqueBaseUnordered + { + public: + typedef SymbolicBayesTreeCliqueUnordered This; + typedef BayesTreeCliqueBaseUnordered Base; + typedef boost::shared_ptr shared_ptr; + typedef boost::weak_ptr weak_ptr; + SymbolicBayesTreeCliqueUnordered() {} + SymbolicBayesTreeCliqueUnordered(const SymbolicConditionalUnordered::shared_ptr& conditional) : Base(conditional) {} + }; + + /* ************************************************************************* */ + class GTSAM_EXPORT SymbolicBayesTreeUnordered : + public BayesTreeUnordered { public: /** Insert a new conditional */ - void insert(const sharedConditional& conditional); + //void insert(const sharedConditional& conditional); protected: @@ -39,7 +54,12 @@ namespace gtsam { * parents are already in the clique or its separators. This function does * not check for this condition, it just updates the data structures. */ - void addToCliqueFront(const sharedConditional& conditional, const sharedClique& clique); + //void addToCliqueFront(const sharedConditional& conditional, const sharedClique& clique); + + private: + + // Dummy method to export class + void noop() const; }; diff --git a/gtsam/symbolic/SymbolicConditionalUnordered.h b/gtsam/symbolic/SymbolicConditionalUnordered.h index 1bc810bb4..c31ea8a37 100644 --- a/gtsam/symbolic/SymbolicConditionalUnordered.h +++ b/gtsam/symbolic/SymbolicConditionalUnordered.h @@ -50,16 +50,16 @@ namespace gtsam { SymbolicConditionalUnordered() {} /** No parents */ - SymbolicConditionalUnordered(Index j) : BaseFactor(j), BaseConditional(0) {} + SymbolicConditionalUnordered(Index j) : BaseFactor(j), BaseConditional(1) {} /** Single parent */ SymbolicConditionalUnordered(Index j, Index parent) : BaseFactor(j, parent), BaseConditional(1) {} /** Two parents */ - SymbolicConditionalUnordered(Index j, Index parent1, Index parent2) : BaseFactor(j, parent1, parent2), BaseConditional(2) {} + SymbolicConditionalUnordered(Index j, Index parent1, Index parent2) : BaseFactor(j, parent1, parent2), BaseConditional(1) {} /** Three parents */ - SymbolicConditionalUnordered(Index j, Index parent1, Index parent2, Index parent3) : BaseFactor(j, parent1, parent2, parent3), BaseConditional(3) {} + SymbolicConditionalUnordered(Index j, Index parent1, Index parent2, Index parent3) : BaseFactor(j, parent1, parent2, parent3), BaseConditional(1) {} /** Named constructor from an arbitrary number of keys and frontals */ template @@ -77,6 +77,17 @@ namespace gtsam { /// @} + /// @name Testable + + /** Print with optional formatter */ + void print(const std::string& str = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { + BaseConditional::print(str, keyFormatter); } + + /** Check equality */ + bool equals(const This& c, double tol = 1e-9) const { return BaseConditional::equals(c); } + + /// @} + private: /** Serialization function */ friend class boost::serialization::access; diff --git a/gtsam/symbolic/SymbolicEliminationTreeUnordered.h b/gtsam/symbolic/SymbolicEliminationTreeUnordered.h index 67f0095eb..cfc3aac44 100644 --- a/gtsam/symbolic/SymbolicEliminationTreeUnordered.h +++ b/gtsam/symbolic/SymbolicEliminationTreeUnordered.h @@ -16,6 +16,8 @@ * @author Richard Roberts */ +#pragma once + #include #include #include @@ -38,7 +40,7 @@ namespace gtsam { * @return The elimination tree */ SymbolicEliminationTreeUnordered(const SymbolicFactorGraphUnordered& factorGraph, - const VariableIndexUnordered& structure, const std::vector& order) : + const VariableIndexUnordered& structure, const OrderingUnordered& order) : Base(factorGraph, structure, order) {} /** Build the elimination tree of a factor graph. Note that this has to compute the column @@ -46,7 +48,7 @@ namespace gtsam { * constructor instead. * @param factorGraph The factor graph for which to build the elimination tree */ - SymbolicEliminationTreeUnordered(const SymbolicFactorGraphUnordered& factorGraph, const std::vector& order) : + SymbolicEliminationTreeUnordered(const SymbolicFactorGraphUnordered& factorGraph, const OrderingUnordered& order) : Base(factorGraph, order) {} /** Copy constructor - makes a deep copy of the tree structure, but only pointers to factors are diff --git a/gtsam/symbolic/SymbolicFactorGraphUnordered.h b/gtsam/symbolic/SymbolicFactorGraphUnordered.h index 628fe3156..202359115 100644 --- a/gtsam/symbolic/SymbolicFactorGraphUnordered.h +++ b/gtsam/symbolic/SymbolicFactorGraphUnordered.h @@ -24,9 +24,9 @@ #include namespace gtsam { class SymbolicConditionalUnordered; } -namespace gtsam { class SymbolicBayesNet; } +namespace gtsam { class SymbolicBayesNetUnordered; } namespace gtsam { class SymbolicEliminationTreeUnordered; } -namespace gtsam { class SymbolicBayesTree; } +namespace gtsam { class SymbolicBayesTreeUnordered; } namespace gtsam { class SymbolicJunctionTreeUnordered; } namespace gtsam { @@ -37,18 +37,16 @@ namespace gtsam { class GTSAM_EXPORT SymbolicFactorGraphUnordered: public FactorGraphUnordered, public EliminateableFactorGraph< - SymbolicFactorGraphUnordered, SymbolicFactorUnordered, SymbolicConditionalUnordered, - SymbolicBayesNet, SymbolicEliminationTreeUnordered, SymbolicBayesTree, SymbolicJunctionTreeUnordered> + SymbolicFactorUnordered, SymbolicFactorGraphUnordered, SymbolicConditionalUnordered, SymbolicBayesNetUnordered, + SymbolicEliminationTreeUnordered, SymbolicBayesTreeUnordered, SymbolicJunctionTreeUnordered> { - public: typedef SymbolicFactorGraphUnordered This; typedef FactorGraphUnordered Base; typedef EliminateableFactorGraph< - SymbolicFactorGraphUnordered, SymbolicFactorUnordered, SymbolicConditionalUnordered, - SymbolicBayesNet, SymbolicEliminationTreeUnordered, SymbolicBayesTree, SymbolicJunctionTreeUnordered> - BaseEliminateable; + SymbolicFactorUnordered, SymbolicFactorGraphUnordered, SymbolicConditionalUnordered, SymbolicBayesNetUnordered, + SymbolicEliminationTreeUnordered, SymbolicBayesTreeUnordered, SymbolicJunctionTreeUnordered> BaseEliminateable; typedef BaseEliminateable::Eliminate Eliminate; /// @name Standard Constructors diff --git a/gtsam/symbolic/SymbolicJunctionTreeUnordered.cpp b/gtsam/symbolic/SymbolicJunctionTreeUnordered.cpp index 17c5eca7e..c9a8e8f1e 100644 --- a/gtsam/symbolic/SymbolicJunctionTreeUnordered.cpp +++ b/gtsam/symbolic/SymbolicJunctionTreeUnordered.cpp @@ -21,7 +21,7 @@ namespace gtsam { - SymbolicJunctionTreeUnordered::SymbolicJunctionTreeUnordered() { + void SymbolicJunctionTreeUnordered::noop() const { } diff --git a/gtsam/symbolic/SymbolicJunctionTreeUnordered.h b/gtsam/symbolic/SymbolicJunctionTreeUnordered.h index cc201177b..1049ad723 100644 --- a/gtsam/symbolic/SymbolicJunctionTreeUnordered.h +++ b/gtsam/symbolic/SymbolicJunctionTreeUnordered.h @@ -39,7 +39,7 @@ namespace gtsam { * @return The elimination tree */ SymbolicJunctionTreeUnordered(const SymbolicEliminationTreeUnordered& eliminationTree) : - Base(eliminationTree) {} + Base(Base::FromEliminationTree(eliminationTree)) {} /** Copy constructor - makes a deep copy of the tree structure, but only pointers to factors are * copied, factors are not cloned. */ @@ -51,8 +51,8 @@ namespace gtsam { private: - /// Private default constructor - SymbolicJunctionTreeUnordered(); + // Dummy method to export class type + void noop() const; }; } diff --git a/gtsam/symbolic/tests/testSymbolicEliminationTree.cpp b/gtsam/symbolic/tests/testSymbolicEliminationTree.cpp index 1721e01e4..b06d69b69 100644 --- a/gtsam/symbolic/tests/testSymbolicEliminationTree.cpp +++ b/gtsam/symbolic/tests/testSymbolicEliminationTree.cpp @@ -77,7 +77,7 @@ TEST(EliminationTree, Create) SymbolicEliminationTreeUnordered expected = EliminationTreeUnorderedTester::buildHardcodedTree(fg); // Build from factor graph - vector order; + OrderingUnordered order; order += 0,1,2,3,4; SymbolicEliminationTreeUnordered actual(fg, order); @@ -108,7 +108,7 @@ TEST_UNSAFE(EliminationTree, eliminate ) fg.push_factor(3, 4); // eliminate - vector order; + OrderingUnordered order; order += 0,1,2,3,4; SymbolicBayesNetUnordered actual = *SymbolicEliminationTreeUnordered(fg,order).eliminate(EliminateSymbolicUnordered).first; @@ -131,7 +131,7 @@ TEST(EliminationTree, disconnected_graph) { expected.push_back(boost::make_shared(3,4)); expected.push_back(boost::make_shared(4)); - vector order; + OrderingUnordered order; order += 0,1,2,3,4; SymbolicBayesNetUnordered actual = *SymbolicEliminationTreeUnordered(fg, order).eliminate(EliminateSymbolicUnordered).first;