From f15fea202e4ae93ce45aab4c11bbf20652e9f3d9 Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Tue, 19 Oct 2010 21:31:13 +0000 Subject: [PATCH] Change that may be reverted again soon - templated factor base type on key and make IndexFactor and IndexConditional for all current code. Did this to experiment with using symbols on the linear side in an efficient way but am putting this on hold for now because of complications. Will revisit and either fix or revert to int-only later in the week. Also moved some files to experimental and templating EliminationTree on FACTOR instead of FACTORGRAPH. --- base/FastMap.h | 55 +++++++ base/FastSet.h | 52 ++++++ inference/BinaryConditional.h | 126 -------------- inference/Conditional.h | 79 ++++----- inference/EliminationTree-inl.h | 34 ++-- inference/EliminationTree.h | 19 ++- inference/Factor-inl.h | 91 ++++++++--- inference/Factor.cpp | 78 --------- inference/Factor.h | 81 +++++---- inference/IndexConditional.cpp | 25 +++ inference/IndexConditional.h | 67 ++++++++ inference/IndexFactor.cpp | 38 +++++ inference/IndexFactor.h | 85 ++++++++++ inference/IndexTable.h | 70 -------- inference/JunctionTree-inl.h | 6 +- inference/JunctionTree.h | 4 +- inference/Makefile.am | 5 +- inference/SymbolMap.h | 172 -------------------- inference/SymbolicFactorGraph.cpp | 20 +-- inference/SymbolicFactorGraph.h | 16 +- inference/VariableSlots.h | 10 +- inference/inference-inl.h | 42 ++--- inference/inference.cpp | 154 +++++++++--------- inference/inference.h | 11 +- inference/tests/testBayesTree.cpp | 84 +++++----- inference/tests/testConditional.cpp | 38 ++--- inference/tests/testEliminationTree.cpp | 14 +- inference/tests/testFactorGraph.cpp | 2 +- inference/tests/testISAM.cpp | 4 +- inference/tests/testJunctionTree.cpp | 6 +- inference/tests/testSymbolicBayesNet.cpp | 36 ++-- inference/tests/testSymbolicFactor.cpp | 20 +-- inference/tests/testSymbolicFactorGraph.cpp | 32 ++-- inference/tests/timeSymbolMaps.cpp | 2 +- linear/GaussianConditional.cpp | 10 +- linear/GaussianConditional.h | 6 +- linear/GaussianFactor.cpp | 20 +-- linear/GaussianFactor.h | 9 +- linear/VectorBTree.h | 2 +- nonlinear/Makefile.am | 8 +- nonlinear/NonlinearFactor.h | 30 ++-- nonlinear/NonlinearFactorGraph-inl.h | 4 +- slam/saveGraph.h | 2 +- tests/testGaussianFactorGraph.cpp | 8 +- tests/testGaussianJunctionTree.cpp | 18 +- tests/testSymbolicBayesNet.cpp | 17 +- tests/testSymbolicFactorGraph.cpp | 13 +- 47 files changed, 827 insertions(+), 898 deletions(-) create mode 100644 base/FastMap.h create mode 100644 base/FastSet.h delete mode 100644 inference/BinaryConditional.h delete mode 100644 inference/Factor.cpp create mode 100644 inference/IndexConditional.cpp create mode 100644 inference/IndexConditional.h create mode 100644 inference/IndexFactor.cpp create mode 100644 inference/IndexFactor.h delete mode 100644 inference/IndexTable.h delete mode 100644 inference/SymbolMap.h diff --git a/base/FastMap.h b/base/FastMap.h new file mode 100644 index 000000000..881273af6 --- /dev/null +++ b/base/FastMap.h @@ -0,0 +1,55 @@ +/* ---------------------------------------------------------------------------- + + * 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 FastMap.h + * @brief A thin wrapper around std::map that uses boost's fast_pool_allocator. + * @author Richard Roberts + * @created Oct 17, 2010 + */ + +#pragma once + +#include +#include + +namespace gtsam { + +/** + * FastMap is a thin wrapper around std::map that uses the boost + * fast_pool_allocator instead of the default STL allocator. This is just a + * convenience to avoid having lengthy types in the code. Through timing, + * we've seen that the fast_pool_allocator can lead to speedups of several + * percent. + */ +template +class FastMap : public std::map, boost::fast_pool_allocator > > { + +public: + + typedef std::map, boost::fast_pool_allocator > > Base; + + /** Default constructor */ + FastMap() {} + + /** Constructor from a range, passes through to base class */ + template + FastMap(InputIterator first, InputIterator last) : Base(first, last) {} + + /** Copy constructor from another FastMap */ + FastMap(const FastMap& x) : Base(x) {} + + /** Copy constructor from the base map class */ + FastMap(const Base& x) : Base(x) {} + +}; + +} diff --git a/base/FastSet.h b/base/FastSet.h new file mode 100644 index 000000000..133ac5636 --- /dev/null +++ b/base/FastSet.h @@ -0,0 +1,52 @@ +/* ---------------------------------------------------------------------------- + + * 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 FastSet.h + * @brief A thin wrapper around std::set that uses boost's fast_pool_allocator. + * @author Richard Roberts + * @created Oct 17, 2010 + */ + +#pragma once + +#include +#include + +namespace gtsam { + +/** + * FastSet is a thin wrapper around std::set that uses the boost + * fast_pool_allocator instead of the default STL allocator. This is just a + * convenience to avoid having lengthy types in the code. Through timing, + * we've seen that the fast_pool_allocator can lead to speedups of several + * percent. + */ +template +class FastSet: public std::set, boost::fast_pool_allocator > { + +public: + + typedef std::set, boost::fast_pool_allocator > Base; + + /** Constructor from a range, passes through to base class */ + template + FastSet(InputIterator first, InputIterator last) : Base(first, last) {} + + /** Copy constructor from another FastMap */ + FastSet(const FastSet& x) : Base(x) {} + + /** Copy constructor from the base map class */ + FastSet(const Base& x) : Base(x) {} + +}; + +} diff --git a/inference/BinaryConditional.h b/inference/BinaryConditional.h deleted file mode 100644 index d82e8a1b3..000000000 --- a/inference/BinaryConditional.h +++ /dev/null @@ -1,126 +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 DiscreteConditional.h - * @brief Discrete Conditional node for use in Bayes nets - * @author Manohar Paluri - */ - -// \callgraph - -#pragma once - -#include -#include -#include -#include -#include // TODO: make cpp file -#include -#include -#include -#include -#include - -namespace gtsam { - - /** - * Conditional node for use in a Bayes net - */ - class BinaryConditional: public Conditional { - - private: - - std::list parents_; - std::vector cpt_; - - public: - - /** convenience typename for a shared pointer to this class */ - typedef boost::shared_ptr shared_ptr; - - /** - * Empty Constructor to make serialization possible - */ - BinaryConditional(){} - - /** - * No parents - */ - BinaryConditional(const Symbol& key, double p) : - Conditional(key) { - cpt_.push_back(1-p); - cpt_.push_back(p); - } - - /** - * Single parent - */ - BinaryConditional(const Symbol& key, const Symbol& parent, const std::vector& cpt) : - Conditional(key) { - parents_.push_back(parent); - for( size_t i = 0 ; i < cpt.size() ; i++ ) - cpt_.push_back(1-cpt[i]); // p(!x|parents) - cpt_.insert(cpt_.end(),cpt.begin(),cpt.end()); // p(x|parents) - } - - double probability( SymbolMap config) { - int index = 0, count = 1; - BOOST_FOREACH(const Symbol& parent, parents_){ - index += count*(int)(config[parent]); - count = count << 1; - } - if( config.at(key_) ) - index += count; - return cpt_[index]; - } - - /** print */ - void print(const std::string& s = "BinaryConditional") const { - std::cout << s << " P(" << (std::string)key_; - if (parents_.size()>0) std::cout << " |"; - BOOST_FOREACH(std::string parent, parents_) std::cout << " " << parent; - std::cout << ")" << std::endl; - std::cout << "Conditional Probability Table::" << std::endl; - BOOST_FOREACH(double p, cpt_) std::cout << p << "\t"; - std::cout<< std::endl; - } - - /** check equality */ - bool equals(const Conditional& c, double tol = 1e-9) const { - if (!Conditional::equals(c)) return false; - const BinaryConditional* p = dynamic_cast (&c); - if (p == NULL) return false; - return (parents_ == p->parents_ && cpt_ == p->cpt_); - } - - /** return parents */ - std::list parents() const { return parents_;} - - /** return Conditional probability table*/ - std::vector cpt() const { return cpt_;} - - /** find the number of parents */ - size_t nrParents() const { - return parents_.size(); - } - - private: - /** Serialization function */ - friend class boost::serialization::access; - template - void serialize(Archive & ar, const unsigned int version) { - ar & boost::serialization::make_nvp("Conditional", boost::serialization::base_object(*this)); - ar & BOOST_SERIALIZATION_NVP(parents_); - ar & BOOST_SERIALIZATION_NVP(cpt_); - } - }; -} /// namespace gtsam diff --git a/inference/Conditional.h b/inference/Conditional.h index a79d5c9d7..9b9c184a0 100644 --- a/inference/Conditional.h +++ b/inference/Conditional.h @@ -38,7 +38,8 @@ namespace gtsam { * kept in pointer containers. To be safe, you should make them * immutable, i.e., practicing functional programming. */ -class Conditional: public Factor, boost::noncopyable, public Testable { +template +class ConditionalBase: public gtsam::FactorBase, boost::noncopyable, public Testable > { protected: @@ -47,40 +48,41 @@ protected: public: - /** convenience typename for a shared pointer to this class */ - typedef gtsam::Factor Factor; - typedef boost::shared_ptr shared_ptr; - typedef Factor::iterator iterator; - typedef Factor::const_iterator const_iterator; + typedef KEY Key; + typedef ConditionalBase This; + typedef gtsam::FactorBase Factor; + typedef boost::shared_ptr shared_ptr; + typedef typename Factor::iterator iterator; + typedef typename Factor::const_iterator const_iterator; typedef boost::iterator_range Frontals; typedef boost::iterator_range Parents; /** Empty Constructor to make serialization possible */ - Conditional() : nrFrontals_(0) {} + ConditionalBase() : nrFrontals_(0) {} /** No parents */ - Conditional(Index key) : Factor(key), nrFrontals_(1) {} + ConditionalBase(Key key) : Factor(key), nrFrontals_(1) {} /** Single parent */ - Conditional(Index key, Index parent) : Factor(key, parent), nrFrontals_(1) {} + ConditionalBase(Key key, Key parent) : Factor(key, parent), nrFrontals_(1) {} /** Two parents */ - Conditional(Index key, Index parent1, Index parent2) : Factor(key, parent1, parent2), nrFrontals_(1) {} + ConditionalBase(Key key, Key parent1, Key parent2) : Factor(key, parent1, parent2), nrFrontals_(1) {} /** Three parents */ - Conditional(Index key, Index parent1, Index parent2, Index parent3) : Factor(key, parent1, parent2, parent3), nrFrontals_(1) {} + ConditionalBase(Key key, Key parent1, Key parent2, Key parent3) : Factor(key, parent1, parent2, parent3), nrFrontals_(1) {} /** Constructor from a frontal variable and a vector of parents */ - Conditional(Index key, const std::vector& parents) : nrFrontals_(1) { - keys_.resize(1 + parents.size()); + ConditionalBase(Key key, const std::vector& parents) : nrFrontals_(1) { + Factor::keys_.resize(1 + parents.size()); *(beginFrontals()) = key; std::copy(parents.begin(), parents.end(), beginParents()); } /** Constructor from a frontal variable and an iterator range of parents */ - template - static Conditional::shared_ptr FromRange(Index key, Iterator firstParent, Iterator lastParent) { - Conditional::shared_ptr conditional(new Conditional); + template + static typename DERIVED::shared_ptr FromRange(Key key, ITERATOR firstParent, ITERATOR lastParent) { + typename DERIVED::shared_ptr conditional(new DERIVED); conditional->nrFrontals_ = 1; conditional->keys_.push_back(key); std::copy(firstParent, lastParent, back_inserter(conditional->keys_)); @@ -88,38 +90,39 @@ public: } /** Named constructor from any number of frontal variables and parents */ - template - static Conditional::shared_ptr FromRange(Iterator firstKey, Iterator lastKey, size_t nrFrontals) { - Conditional::shared_ptr conditional(new Conditional); + template + static typename DERIVED::shared_ptr FromRange(ITERATOR firstKey, ITERATOR lastKey, size_t nrFrontals) { + typename DERIVED::shared_ptr conditional(new DERIVED); conditional->nrFrontals_ = nrFrontals; std::copy(firstKey, lastKey, back_inserter(conditional->keys_)); return conditional; } /** check equality */ - bool equals(const Conditional& c, double tol = 1e-9) const { + template + bool equals(const DERIVED& c, double tol = 1e-9) const { return nrFrontals_ == c.nrFrontals_ && Factor::equals(c, tol); } /** return the number of frontals */ size_t nrFrontals() const { return nrFrontals_; } /** return the number of parents */ - size_t nrParents() const { return keys_.size() - nrFrontals_; } + size_t nrParents() const { return Factor::keys_.size() - nrFrontals_; } /** Special accessor when there is only one frontal variable. */ - Index key() const { assert(nrFrontals_==1); return keys_[0]; } + Key key() const { assert(nrFrontals_==1); return Factor::keys_[0]; } /** Iterators over frontal and parent variables. */ - const_iterator beginFrontals() const { return keys_.begin(); } - const_iterator endFrontals() const { return keys_.begin()+nrFrontals_; } - const_iterator beginParents() const { return keys_.begin()+nrFrontals_; } - const_iterator endParents() const { return keys_.end(); } + const_iterator beginFrontals() const { return Factor::keys_.begin(); } + const_iterator endFrontals() const { return Factor::keys_.begin()+nrFrontals_; } + const_iterator beginParents() const { return Factor::keys_.begin()+nrFrontals_; } + const_iterator endParents() const { return Factor::keys_.end(); } /** Mutable iterators and accessors */ - iterator beginFrontals() { return keys_.begin(); } - iterator endFrontals() { return keys_.begin()+nrFrontals_; } - iterator beginParents() { return keys_.begin()+nrFrontals_; } - iterator endParents() { return keys_.end(); } + iterator beginFrontals() { return Factor::keys_.begin(); } + iterator endFrontals() { return Factor::keys_.begin()+nrFrontals_; } + iterator beginParents() { return Factor::keys_.begin()+nrFrontals_; } + iterator endParents() { return Factor::keys_.end(); } boost::iterator_range frontals() { return boost::make_iterator_range(beginFrontals(), endFrontals()); } boost::iterator_range parents() { return boost::make_iterator_range(beginParents(), endParents()); } @@ -134,9 +137,9 @@ public: /** print */ void print(const std::string& s = "Conditional") const { std::cout << s << " P("; - BOOST_FOREACH(Index key, frontals()) std::cout << " " << key; + BOOST_FOREACH(Key key, frontals()) std::cout << " " << key; if (nrParents()>0) std::cout << " |"; - BOOST_FOREACH(Index parent, parents()) std::cout << " " << parent; + BOOST_FOREACH(Key parent, parents()) std::cout << " " << parent; std::cout << ")" << std::endl; } @@ -146,11 +149,11 @@ public: */ bool permuteSeparatorWithInverse(const Permutation& inversePermutation) { #ifndef NDEBUG - BOOST_FOREACH(Index key, frontals()) { assert(key == inversePermutation[key]); } + BOOST_FOREACH(Key key, frontals()) { assert(key == inversePermutation[key]); } #endif bool parentChanged = false; - BOOST_FOREACH(Index& parent, parents()) { - Index newParent = inversePermutation[parent]; + BOOST_FOREACH(Key& parent, parents()) { + Key newParent = inversePermutation[parent]; if(parent != newParent) { parentChanged = true; parent = newParent; @@ -166,8 +169,8 @@ public: void permuteWithInverse(const Permutation& inversePermutation) { // The permutation may not move the separators into the frontals #ifndef NDEBUG - BOOST_FOREACH(const Index frontal, this->frontals()) { - BOOST_FOREACH(const Index separator, this->parents()) { + BOOST_FOREACH(const Key frontal, this->frontals()) { + BOOST_FOREACH(const Key separator, this->parents()) { assert(inversePermutation[frontal] < inversePermutation[separator]); } } @@ -181,8 +184,6 @@ protected: */ void assertInvariants() const; - friend class Factor; - private: /** Serialization function */ friend class boost::serialization::access; diff --git a/inference/EliminationTree-inl.h b/inference/EliminationTree-inl.h index 26d9bd4d0..7e88845d0 100644 --- a/inference/EliminationTree-inl.h +++ b/inference/EliminationTree-inl.h @@ -7,6 +7,7 @@ #include #include +#include #include #include @@ -19,16 +20,16 @@ using namespace std; namespace gtsam { /* ************************************************************************* */ -template -typename EliminationTree::EliminationResult -EliminationTree::eliminate_() const { +template +typename EliminationTree::EliminationResult +EliminationTree::eliminate_() const { - typename FACTORGRAPH::bayesnet_type bayesNet; + BayesNet bayesNet; set, boost::fast_pool_allocator > separator; // Create the list of factors to be eliminated - FACTORGRAPH factors; + FactorGraph factors; factors.reserve(this->factors_.size() + this->subTrees_.size()); // add all factors associated with root @@ -42,15 +43,15 @@ EliminationTree::eliminate_() const { } // eliminate the joint factor and add the conditional to the bayes net - typename FACTORGRAPH::sharedFactor jointFactor(FACTORGRAPH::Factor::Combine(factors, VariableSlots(factors))); + sharedFactor jointFactor(FACTOR::Combine(factors, VariableSlots(factors))); bayesNet.push_back(jointFactor->eliminateFirst()); return EliminationResult(bayesNet, jointFactor); } /* ************************************************************************* */ -template -vector EliminationTree::ComputeParents(const VariableIndex<>& structure) { +template +vector EliminationTree::ComputeParents(const VariableIndex<>& structure) { // Number of factors and variables const size_t m = structure.nFactors(); @@ -83,9 +84,10 @@ vector EliminationTree::ComputeParents(const VariableIndex<> } /* ************************************************************************* */ +template template -typename EliminationTree::shared_ptr -EliminationTree::Create(const FACTORGRAPH& factorGraph) { +typename EliminationTree::shared_ptr +EliminationTree::Create(const FACTORGRAPH& factorGraph) { // Create column structure VariableIndex<> varIndex(factorGraph); @@ -108,7 +110,10 @@ EliminationTree::Create(const FACTORGRAPH& factorGraph) { } // Hang factors in right places - BOOST_FOREACH(const sharedFactor& factor, factorGraph) { + BOOST_FOREACH(const typename FACTORGRAPH::sharedFactor& derivedFactor, factorGraph) { + // Here we static_cast to the factor type of this EliminationTree. This + // allows performing symbolic elimination on, for example, GaussianFactors. + sharedFactor factor(boost::shared_static_cast(derivedFactor)); Index j = factor->front(); trees[j]->add(factor); } @@ -148,12 +153,13 @@ bool EliminationTree::equals(const EliminationTree& ex } /* ************************************************************************* */ -template -typename FACTORGRAPH::bayesnet_type::shared_ptr EliminationTree::eliminate() const { +template +typename EliminationTree::BayesNet::shared_ptr +EliminationTree::eliminate() const { // call recursive routine EliminationResult result = eliminate_(); - return typename FACTORGRAPH::bayesnet_type::shared_ptr(new typename FACTORGRAPH::bayesnet_type(result.first)); + return typename BayesNet::shared_ptr(new BayesNet(result.first)); } } diff --git a/inference/EliminationTree.h b/inference/EliminationTree.h index 953f9e9fb..b7098558d 100644 --- a/inference/EliminationTree.h +++ b/inference/EliminationTree.h @@ -12,22 +12,24 @@ #include #include +#include class EliminationTreeTester; // for unit tests, see testEliminationTree namespace gtsam { /** - * An elimination tree is a tree of factors + * An elimination tree is a data structure used intermediately during + * elimination, and it can be used to save work between multiple eliminations. */ -template -class EliminationTree: public Testable > { +template +class EliminationTree: public Testable > { public: - typedef boost::shared_ptr sharedFactor; - typedef boost::shared_ptr > shared_ptr; - typedef FACTORGRAPH FactorGraph; + typedef typename FACTOR::shared_ptr sharedFactor; + typedef boost::shared_ptr > shared_ptr; + typedef gtsam::BayesNet BayesNet; private: @@ -38,7 +40,7 @@ private: Factors factors_; /** factors associated with root */ SubTrees subTrees_; /** sub-trees */ - typedef std::pair EliminationResult; + typedef std::pair EliminationResult; /** default constructor, private, as you should use Create below */ EliminationTree(Index key = 0) : key_(key) {} @@ -66,6 +68,7 @@ private: public: /** Named constructor to build the elimination tree of a factor graph */ + template static shared_ptr Create(const FACTORGRAPH& factorGraph); /** Print the tree to cout */ @@ -75,7 +78,7 @@ public: bool equals(const EliminationTree& other, double tol = 1e-9) const; /** Eliminate the factors to a Bayes Net */ - typename FACTORGRAPH::bayesnet_type::shared_ptr eliminate() const; + typename BayesNet::shared_ptr eliminate() const; }; } diff --git a/inference/Factor-inl.h b/inference/Factor-inl.h index 5129ab7bf..53ec15115 100644 --- a/inference/Factor-inl.h +++ b/inference/Factor-inl.h @@ -25,46 +25,89 @@ #include #include #include +#include namespace gtsam { -///* ************************************************************************* */ -//template -//Factor::Factor(const boost::shared_ptr& c) { -// keys_.resize(c->parents().size()+1); -// keys_[0] = c->key(); -// size_t j = 1; -// BOOST_FOREACH(const Index parent, c->parents()) { -// keys_[j++] = parent; -// } -// checkSorted(); -//} +/* ************************************************************************* */ +template +FactorBase::FactorBase(const FactorBase& f) : keys_(f.keys_) {} /* ************************************************************************* */ -template Factor::Factor(KeyIterator beginKey, KeyIterator endKey) : - keys_(beginKey, endKey) { assertInvariants(); } +template +FactorBase::FactorBase(const Conditional& c) : keys_(c.keys()) {} /* ************************************************************************* */ -template -Factor::shared_ptr Factor::Combine(const FactorGraphType& factorGraph, - const VariableIndex& variableIndex, const std::vector& factors, - const std::vector& variables, const std::vector >& variablePositions) { - - return shared_ptr(new Factor(variables.begin(), variables.end())); +template +void FactorBase::assertInvariants() const { +#ifndef NDEBUG + std::set uniqueSorted(keys_.begin(), keys_.end()); + assert(uniqueSorted.size() == keys_.size()); + assert(std::equal(uniqueSorted.begin(), uniqueSorted.end(), keys_.begin())); +#endif } /* ************************************************************************* */ -template -Factor::shared_ptr Factor::Combine(const FactorGraph& factors, const std::map, std::less, MapAllocator>& variableSlots) { - typedef const std::map, std::less, MapAllocator> VariableSlots; +template +void FactorBase::print(const std::string& s) const { + std::cout << s << " "; + BOOST_FOREACH(KEY key, keys_) std::cout << " " << key; + std::cout << std::endl; +} + +/* ************************************************************************* */ +template +//template +bool FactorBase::equals(const This& other, double tol) const { + return keys_ == other.keys_; +} + +/* ************************************************************************* */ +template +template +typename DERIVED::shared_ptr FactorBase::Combine(const FactorGraph& factors, const FastMap >& variableSlots) { + typedef const FastMap > VariableSlots; typedef typeof(boost::lambda::bind(&VariableSlots::value_type::first, boost::lambda::_1)) FirstGetter; typedef boost::transform_iterator< FirstGetter, typename VariableSlots::const_iterator, - Index, Index> IndexIterator; + KEY, KEY> IndexIterator; FirstGetter firstGetter(boost::lambda::bind(&VariableSlots::value_type::first, boost::lambda::_1)); IndexIterator keysBegin(variableSlots.begin(), firstGetter); IndexIterator keysEnd(variableSlots.end(), firstGetter); - return shared_ptr(new Factor(keysBegin, keysEnd)); + return typename DERIVED::shared_ptr(new DERIVED(keysBegin, keysEnd)); +} + +/* ************************************************************************* */ +template +template +typename CONDITIONAL::shared_ptr FactorBase::eliminateFirst() { + assert(!keys_.empty()); + assertInvariants(); + KEY eliminated = keys_.front(); + keys_.erase(keys_.begin()); + return typename CONDITIONAL::shared_ptr(new CONDITIONAL(eliminated, keys_)); +} + +/* ************************************************************************* */ +template +template +typename BayesNet::shared_ptr FactorBase::eliminate(size_t nrFrontals) { + assert(keys_.size() >= nrFrontals); + assertInvariants(); + typename BayesNet::shared_ptr fragment(new BayesNet()); + const_iterator nextFrontal = this->begin(); + for(KEY n = 0; n < nrFrontals; ++n, ++nextFrontal) + fragment->push_back(CONDITIONAL::FromRange( + nextFrontal, const_iterator(this->end()), 1)); + if(nrFrontals > 0) + keys_.assign(fragment->back()->beginParents(), fragment->back()->endParents()); + return fragment; +} + +/* ************************************************************************* */ +template +void FactorBase::permuteWithInverse(const Permutation& inversePermutation) { + BOOST_FOREACH(KEY& key, keys_) { key = inversePermutation[key]; } } } diff --git a/inference/Factor.cpp b/inference/Factor.cpp deleted file mode 100644 index 05389217a..000000000 --- a/inference/Factor.cpp +++ /dev/null @@ -1,78 +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 Factor.cpp - * @brief - * @author Richard Roberts - * @created Sep 1, 2010 - */ - -#include -#include - -#include -#include -#include -#include -#include - -using namespace std; -using namespace boost::lambda; - -namespace gtsam { - -/* ************************************************************************* */ -Factor::Factor(const Factor& f) : keys_(f.keys_) {} - -/* ************************************************************************* */ -Factor::Factor(const Conditional& c) : keys_(c.keys()) {} - -/* ************************************************************************* */ -void Factor::print(const std::string& s) const { - cout << s << " "; - BOOST_FOREACH(Index key, keys_) cout << " " << key; - cout << endl; -} - -/* ************************************************************************* */ -bool Factor::equals(const Factor& other, double tol) const { - return keys_ == other.keys_; -} - -/* ************************************************************************* */ -Conditional::shared_ptr Factor::eliminateFirst() { - assert(!keys_.empty()); - assertInvariants(); - Index eliminated = keys_.front(); - keys_.erase(keys_.begin()); - return Conditional::shared_ptr(new Conditional(eliminated, keys_)); -} - -/* ************************************************************************* */ -boost::shared_ptr > Factor::eliminate(size_t nrFrontals) { - assert(keys_.size() >= nrFrontals); - assertInvariants(); - BayesNet::shared_ptr fragment(new BayesNet()); - const_iterator nextFrontal = this->begin(); - for(Index n = 0; n < nrFrontals; ++n, ++nextFrontal) - fragment->push_back(Conditional::FromRange(nextFrontal, const_iterator(this->end()), 1)); - if(nrFrontals > 0) - keys_.assign(fragment->back()->beginParents(), fragment->back()->endParents()); - return fragment; -} - -/* ************************************************************************* */ -void Factor::permuteWithInverse(const Permutation& inversePermutation) { - BOOST_FOREACH(Index& key, keys_) { key = inversePermutation[key]; } -} - -} diff --git a/inference/Factor.h b/inference/Factor.h index 15410632f..e62129ecb 100644 --- a/inference/Factor.h +++ b/inference/Factor.h @@ -27,11 +27,12 @@ #include #include #include +#include #include namespace gtsam { -class Conditional; +template class ConditionalBase; /** * A simple factor class to use in a factor graph. @@ -47,10 +48,21 @@ class Conditional; * variables, continuous ones, or a combination of both. It is up to the config to * provide the appropriate values at the appropriate time. */ -class Factor : public Testable { +template +class FactorBase : public Testable > { + +public: + + typedef KEY Key; + typedef FactorBase This; + typedef gtsam::ConditionalBase Conditional; + typedef boost::shared_ptr shared_ptr; + typedef std::vector::iterator iterator; + typedef std::vector::const_iterator const_iterator; + protected: - std::vector keys_; + std::vector keys_; /** Internal check to make sure keys are sorted. * If NDEBUG is defined, this is empty and optimized out. */ @@ -58,61 +70,60 @@ protected: public: - typedef gtsam::Conditional Conditional; - typedef boost::shared_ptr shared_ptr; - typedef std::vector::iterator iterator; - typedef std::vector::const_iterator const_iterator; - /** Copy constructor */ - Factor(const Factor& f); + FactorBase(const This& f); /** Construct from derived type */ - Factor(const Conditional& c); + FactorBase(const Conditional& c); /** Constructor from a collection of keys */ - template Factor(KeyIterator beginKey, KeyIterator endKey); + template FactorBase(KeyIterator beginKey, KeyIterator endKey) : + keys_(beginKey, endKey) { assertInvariants(); } /** Default constructor for I/O */ - Factor() {} + FactorBase() {} /** Construct unary factor */ - Factor(Index key) : keys_(1) { + FactorBase(Key key) : keys_(1) { keys_[0] = key; assertInvariants(); } /** Construct binary factor */ - Factor(Index key1, Index key2) : keys_(2) { + FactorBase(Key key1, Key key2) : keys_(2) { keys_[0] = key1; keys_[1] = key2; assertInvariants(); } /** Construct ternary factor */ - Factor(Index key1, Index key2, Index key3) : keys_(3) { + FactorBase(Key key1, Key key2, Key key3) : keys_(3) { keys_[0] = key1; keys_[1] = key2; keys_[2] = key3; assertInvariants(); } /** Construct 4-way factor */ - Factor(Index key1, Index key2, Index key3, Index key4) : keys_(4) { + FactorBase(Key key1, Key key2, Key key3, Key key4) : keys_(4) { keys_[0] = key1; keys_[1] = key2; keys_[2] = key3; keys_[3] = key4; assertInvariants(); } /** Named constructor for combining a set of factors with pre-computed set of * variables. (Old style - will be removed when scalar elimination is * removed in favor of the EliminationTree). */ - template - static shared_ptr Combine(const FactorGraphType& factorGraph, + template + static typename DERIVED::shared_ptr Combine(const FactorGraphType& factorGraph, const VariableIndex& variableIndex, const std::vector& factors, - const std::vector& variables, const std::vector >& variablePositions); + const std::vector& variables, const std::vector >& variablePositions) { + return typename DERIVED::shared_ptr(new DERIVED(variables.begin(), variables.end())); } /** Create a combined joint factor (new style for EliminationTree). */ - template - static shared_ptr Combine(const FactorGraph& factors, const std::map, std::less, MapAllocator>& variableSlots); + template + static typename DERIVED::shared_ptr Combine(const FactorGraph& factors, const FastMap >& variableSlots); /** * eliminate the first variable involved in this factor * @return a conditional on the eliminated variable */ - boost::shared_ptr eliminateFirst(); + template + typename CONDITIONAL::shared_ptr eliminateFirst(); /** * eliminate the first nrFrontals frontal variables. */ - boost::shared_ptr > eliminate(size_t nrFrontals = 1); + template + typename BayesNet::shared_ptr eliminate(size_t nrFrontals = 1); /** * Permutes the GaussianFactor, but for efficiency requires the permutation @@ -129,24 +140,25 @@ public: iterator end() { return keys_.end(); } /** First key*/ - Index front() const { return keys_.front(); } + Key front() const { return keys_.front(); } /** Last key */ - Index back() const { return keys_.back(); } + Key back() const { return keys_.back(); } /** find */ - const_iterator find(Index key) const { return std::find(begin(), end(), key); } + const_iterator find(Key key) const { return std::find(begin(), end(), key); } /** print */ void print(const std::string& s = "Factor") const; /** check equality */ - bool equals(const Factor& other, double tol = 1e-9) const; +// template + bool equals(const This& other, double tol = 1e-9) const; /** * return keys in order as created */ - const std::vector& keys() const { return keys_; } + const std::vector& keys() const { return keys_; } /** * @return the number of nodes the factor connects @@ -155,10 +167,6 @@ public: protected: - /** Conditional makes internal use of a Factor for storage */ - friend class gtsam::Conditional; - friend class GaussianConditional; - /** Serialization function */ friend class boost::serialization::access; template @@ -167,13 +175,4 @@ protected: } }; -/* ************************************************************************* */ -inline void Factor::assertInvariants() const { -#ifndef NDEBUG - std::set uniqueSorted(keys_.begin(), keys_.end()); - assert(uniqueSorted.size() == keys_.size()); - assert(std::equal(uniqueSorted.begin(), uniqueSorted.end(), keys_.begin())); -#endif -} - } diff --git a/inference/IndexConditional.cpp b/inference/IndexConditional.cpp new file mode 100644 index 000000000..bf176d6be --- /dev/null +++ b/inference/IndexConditional.cpp @@ -0,0 +1,25 @@ +/* ---------------------------------------------------------------------------- + + * 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 IndexConditional.cpp + * @brief + * @author Richard Roberts + * @created Oct 17, 2010 + */ + +#include + +namespace gtsam { + +template class ConditionalBase; + +} diff --git a/inference/IndexConditional.h b/inference/IndexConditional.h new file mode 100644 index 000000000..9501eb23c --- /dev/null +++ b/inference/IndexConditional.h @@ -0,0 +1,67 @@ +/* ---------------------------------------------------------------------------- + + * 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 IndexConditional.h + * @brief + * @author Richard Roberts + * @created Oct 17, 2010 + */ + +#pragma once + +#include +#include + +namespace gtsam { + +class IndexFactor; + +class IndexConditional : public ConditionalBase { + +public: + + typedef IndexConditional This; + typedef ConditionalBase Base; + typedef IndexFactor Factor; + typedef boost::shared_ptr shared_ptr; + + /** Empty Constructor to make serialization possible */ + IndexConditional() {} + + /** No parents */ + IndexConditional(Index j) : Base(j) {} + + /** Single parent */ + IndexConditional(Index j, Index parent) : Base(j, parent) {} + + /** Two parents */ + IndexConditional(Index j, Index parent1, Index parent2) : Base(j, parent1, parent2) {} + + /** Three parents */ + IndexConditional(Index j, Index parent1, Index parent2, Index parent3) : Base(j, parent1, parent2, parent3) {} + + /** Constructor from a frontal variable and a vector of parents */ + IndexConditional(Index j, const std::vector& parents) : Base(j, parents) {} + + /** Constructor from a frontal variable and an iterator range of parents */ + template + static shared_ptr FromRange(Index j, ITERATOR firstParent, ITERATOR lastParent) { + return Base::FromRange(j, firstParent, lastParent); } + + /** Named constructor from any number of frontal variables and parents */ + template + static shared_ptr FromRange(ITERATOR firstKey, ITERATOR lastKey, size_t nrFrontals) { + return Base::FromRange(firstKey, lastKey, nrFrontals); } + +}; + +} diff --git a/inference/IndexFactor.cpp b/inference/IndexFactor.cpp new file mode 100644 index 000000000..8ce8a2fbc --- /dev/null +++ b/inference/IndexFactor.cpp @@ -0,0 +1,38 @@ +/* ---------------------------------------------------------------------------- + + * 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 IndexFactor.cpp + * @brief + * @author Richard Roberts + * @created Oct 17, 2010 + */ + +#include +#include + +namespace gtsam { + +template class FactorBase; + +IndexFactor::IndexFactor(const IndexConditional& c) : Base(static_cast(c)) {} + +IndexFactor::shared_ptr IndexFactor::Combine( + const FactorGraph& factors, const FastMap >& variableSlots) { + return Base::Combine(factors, variableSlots); } + +boost::shared_ptr IndexFactor::eliminateFirst() { + return Base::eliminateFirst(); } + +boost::shared_ptr > IndexFactor::eliminate(size_t nrFrontals) { + return Base::eliminate(nrFrontals); } + +} diff --git a/inference/IndexFactor.h b/inference/IndexFactor.h new file mode 100644 index 000000000..e107b2fc2 --- /dev/null +++ b/inference/IndexFactor.h @@ -0,0 +1,85 @@ +/* ---------------------------------------------------------------------------- + + * 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 IndexFactor.h + * @brief + * @author Richard Roberts + * @created Oct 17, 2010 + */ + +#pragma once + +#include +#include + +namespace gtsam { + +class IndexConditional; + +class IndexFactor : public FactorBase { + +public: + + typedef IndexFactor This; + typedef FactorBase Base; + typedef IndexConditional Conditional; + typedef boost::shared_ptr shared_ptr; + + /** Copy constructor */ + IndexFactor(const This& f) : Base(static_cast(f)) {} + + /** Construct from derived type */ + IndexFactor(const IndexConditional& c); + + /** Constructor from a collection of keys */ + template IndexFactor(KeyIterator beginKey, KeyIterator endKey) : + Base(beginKey, endKey) {} + + /** Default constructor for I/O */ + IndexFactor() {} + + /** Construct unary factor */ + IndexFactor(Index j) : Base(j) {} + + /** Construct binary factor */ + IndexFactor(Index j1, Index j2) : Base(j1, j2) {} + + /** Construct ternary factor */ + IndexFactor(Index j1, Index j2, Index j3) : Base(j1, j2, j3) {} + + /** Construct 4-way factor */ + IndexFactor(Index j1, Index j2, Index j3, Index j4) : Base(j1, j2, j3, j4) {} + + /** Create a combined joint factor (new style for EliminationTree). */ + static shared_ptr + Combine(const FactorGraph& factors, const FastMap >& variableSlots); + + template + static shared_ptr Combine(const FactorGraphType& factorGraph, + const VariableIndex& variableIndex, const std::vector& factors, + const std::vector& variables, const std::vector >& variablePositions) { + return Base::Combine(factorGraph, variableIndex, factors, variables, variablePositions); } + + /** + * eliminate the first variable involved in this factor + * @return a conditional on the eliminated variable + */ + boost::shared_ptr eliminateFirst(); + + /** + * eliminate the first nrFrontals frontal variables. + */ + boost::shared_ptr > eliminate(size_t nrFrontals = 1); + +}; + +} diff --git a/inference/IndexTable.h b/inference/IndexTable.h deleted file mode 100644 index f88a7622f..000000000 --- a/inference/IndexTable.h +++ /dev/null @@ -1,70 +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 - - * -------------------------------------------------------------------------- */ - -/* - * IndexTable.h - * - * Created on: Jan 21, 2010 - * @Author: Frank Dellaert - */ - -#pragma once - -#include -#include // TODO should not be in header -#include - -namespace gtsam { - - /** - * An IndexTable maps from key to size_t index and back - * most commonly used templated on Symbol with orderings - */ - template - class IndexTable: public std::vector, public Testable > { - private: - - /* map back from key to size_t */ - typedef typename std::map Map; - Map key2index_; - - public: - - /* bake ordering into IndexTable */ - IndexTable(const std::list& ordering) { - size_t i = 0; - BOOST_FOREACH(const Key& key,ordering){ - this->push_back(key); - key2index_.insert(make_pair(key,i++)); - } - } - - // Testable - virtual void print(const std::string& s="") const { - std::cout << "IndexTable " << s << ":"; - BOOST_FOREACH(Key key,*this) std::cout << (std::string)key << " "; - } - virtual bool equals(const IndexTable& expected, double tol) const { - return key2index_==expected.key2index_; // TODO, sanity check - } - - /** Key to index by parentheses ! */ - size_t operator()(const Key& key) const { - typename Map::const_iterator it = key2index_.find(key); - if (it==key2index_.end()) - throw(std::invalid_argument("IndexTable::[] invalid key")); - return it->second; - } - - /* Index to Key is provided by base class operator[] */ - }; - -} diff --git a/inference/JunctionTree-inl.h b/inference/JunctionTree-inl.h index 6bc7646cd..9e771c768 100644 --- a/inference/JunctionTree-inl.h +++ b/inference/JunctionTree-inl.h @@ -24,6 +24,7 @@ #include #include #include +#include #include #include @@ -41,7 +42,10 @@ namespace gtsam { // Symbolic factorization: GaussianFactorGraph -> SymbolicFactorGraph // -> SymbolicBayesNet -> SymbolicBayesTree tic("JT 1.1 symbolic elimination"); - SymbolicBayesNet::shared_ptr sbn = Inference::EliminateSymbolic(fg); + SymbolicBayesNet::shared_ptr sbn = EliminationTree::Create(fg)->eliminate(); +// SymbolicFactorGraph sfg(fg); +// SymbolicBayesNet::shared_ptr sbn_orig = Inference::Eliminate(sfg); +// assert(assert_equal(*sbn, *sbn_orig)); toc("JT 1.1 symbolic elimination"); tic("JT 1.2 symbolic BayesTree"); SymbolicBayesTree sbt(*sbn); diff --git a/inference/JunctionTree.h b/inference/JunctionTree.h index 6ae7cee1a..b1de1e9af 100644 --- a/inference/JunctionTree.h +++ b/inference/JunctionTree.h @@ -26,7 +26,7 @@ #include #include #include -#include +#include namespace gtsam { @@ -49,7 +49,7 @@ namespace gtsam { typedef class BayesTree BayesTree; // And we will frequently refer to a symbolic Bayes tree - typedef gtsam::BayesTree SymbolicBayesTree; + typedef gtsam::BayesTree SymbolicBayesTree; private: // distribute the factors along the cluster tree diff --git a/inference/Makefile.am b/inference/Makefile.am index e20555ab4..c71122d33 100644 --- a/inference/Makefile.am +++ b/inference/Makefile.am @@ -15,16 +15,17 @@ check_PROGRAMS = #---------------------------------------------------------------------------------------------------- # GTSAM core -headers += SymbolMap.h Factor-inl.h Conditional.h IndexTable.h +headers += Factor.h Factor-inl.h Conditional.h # Symbolic Inference -sources += Factor.cpp SymbolicFactorGraph.cpp +sources += SymbolicFactorGraph.cpp check_PROGRAMS += tests/testSymbolicFactor tests/testSymbolicFactorGraph tests/testConditional check_PROGRAMS += tests/testSymbolicBayesNet tests/testVariableIndex tests/testVariableSlots # Inference headers += inference-inl.h VariableSlots-inl.h sources += inference.cpp VariableSlots.cpp Permutation.cpp +sources += IndexFactor.cpp IndexConditional.cpp headers += graph.h graph-inl.h headers += VariableIndex.h headers += FactorGraph.h FactorGraph-inl.h diff --git a/inference/SymbolMap.h b/inference/SymbolMap.h deleted file mode 100644 index 211c3eda6..000000000 --- a/inference/SymbolMap.h +++ /dev/null @@ -1,172 +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 - - * -------------------------------------------------------------------------- */ - -/* - * SymbolMap.h - * - * Created on: Jan 20, 2010 - * Author: richard - */ - -#pragma once - -//#define GTSAM_SYMBOL_HASH -#define GTSAM_SYMBOL_BINARY -#define GTSAM_SYMBOL_SPECIAL - -#include - -#include -#include - - -namespace gtsam { - -#ifdef GTSAM_SYMBOL_BINARY - template - class SymbolMap : public std::map { - private: - typedef std::map Base; - public: - SymbolMap() : std::map() {} - - const T& at(const Symbol& key) const { - typename Base::const_iterator it = Base::find(key); - if (it == Base::end()) - throw(std::invalid_argument("SymbolMap::[] invalid key: " + (std::string)key)); - return it->second; - } - - T& at(const Symbol& key) { - typename Base::iterator it = Base::find(key); - if (it == Base::end()) - throw(std::invalid_argument("SymbolMap::[] invalid key: " + (std::string)key)); - return it->second; - } - - //void find(void); - - //void clear() { throw std::runtime_error("Clear should not be used!"); } - - }; -#endif - - -#ifdef GTSAM_SYMBOL_HASH - struct SymbolHash : public std::unary_function { - std::size_t operator()(Symbol const& x) const { - std::size_t seed = 0; - boost::hash_combine(seed, x.chr()); - boost::hash_combine(seed, x.index()); - return ((size_t(x.chr()) << 24) & x.index()); - } - }; - - template - class SymbolMap : public boost::unordered_map { - public: - SymbolMap() : boost::unordered_map() {} - }; -#endif - - -#ifdef GTSAM_SYMBOL_SPECIAL - template - class FastSymbolMap { - private: - typedef std::vector > Map; - typedef std::vector Vec; - - Map values_; - - public: - typedef std::pair value_type; - - FastSymbolMap() { - values_.resize(256); - values_[size_t('x')].reserve(10000); - values_[size_t('l')].reserve(1000); - } - - const T& at(const Symbol& key) const { -// typename Map::const_iterator it = values_.find(key.chr()); -// if(it != values_.end()) -// return it->second.at(key.index()); -// else -// throw std::invalid_argument("Key " + (std::string)key + " not present"); - return values_.at(size_t(key.chr())).at(key.index()); - } - - void insert(const value_type& val) { - Vec& vec(values_[size_t(val.first.chr())]); - if(val.first.index() >= vec.size()) { - vec.reserve(val.first.index()+1); - vec.resize(val.first.index()); - vec.push_back(val.second); - } else - vec[val.first.index()] = val.second; - } - - bool empty() const { - return false; - } - - void erase(const Symbol& key) { - - } - - void clear() { - throw std::runtime_error("Can't clear a FastSymbolMap"); - } - -// typedef std::pair value_type; -// -// class iterator { -// typename Map::iterator map_it_; -// typename Map::iterator map_end_; -// typename Vec::iterator vec_it_; -// public: -// iterator() {} -// iterator(const iterator& it) : map_it_(it.map_it_), vec_it_(it.vec_it_) {} -// bool operator==(const iterator& it);// { return map_it_==it.map_it_ && vec_it_==it.vec_it_; } -// bool operator!=(const iterator& it);// { return map_it_!=it.map_it_ || vec_it_!=it.vec_it_; } -// bool operator*();// { return *it.vec_it_; } -// iterator& operator++(); /* { -// if(map_it_ != map_end_ && vec_it_ == map_it_->second.end()) -// do -// vec_it_ = (map_it_++)->second.begin(); -// while(map_it_ != map_end_ && vec_it_ == map_it_->second.end()); -// else -// vec_it_++; -// return *this; -// }*/ -// iterator operator++(int); /* { -// iterator tmp(*this); -// ++(*this); -// return tmp; -// }*/ -// }; -// class const_iterator {}; - - -// std::size_t size() const; -// T& at(const Symbol& key); -// const_iterator find(const Symbol& key); -// void insert(const std::pair& p); -// void clear() { throw std::runtime_error("Clear should not be used!"); } -// std::size_t count() const; -// -// const_iterator begin() const; -// const_iterator end() const; - }; - -#endif -} diff --git a/inference/SymbolicFactorGraph.cpp b/inference/SymbolicFactorGraph.cpp index 428b25779..31b44523d 100644 --- a/inference/SymbolicFactorGraph.cpp +++ b/inference/SymbolicFactorGraph.cpp @@ -22,7 +22,7 @@ #include #include #include -#include +#include #include #include @@ -31,35 +31,35 @@ using namespace std; namespace gtsam { // Explicitly instantiate so we don't have to include everywhere - template class FactorGraph; - template class BayesNet; - template class EliminationTree; + template class FactorGraph; + template class BayesNet; + template class EliminationTree; /* ************************************************************************* */ - SymbolicFactorGraph::SymbolicFactorGraph(const BayesNet& bayesNet) : - FactorGraph(bayesNet) {} + SymbolicFactorGraph::SymbolicFactorGraph(const BayesNet& bayesNet) : + FactorGraph(bayesNet) {} /* ************************************************************************* */ void SymbolicFactorGraph::push_factor(Index key) { - boost::shared_ptr factor(new Factor(key)); + boost::shared_ptr factor(new IndexFactor(key)); push_back(factor); } /** Push back binary factor */ void SymbolicFactorGraph::push_factor(Index key1, Index key2) { - boost::shared_ptr factor(new Factor(key1,key2)); + boost::shared_ptr factor(new IndexFactor(key1,key2)); push_back(factor); } /** Push back ternary factor */ void SymbolicFactorGraph::push_factor(Index key1, Index key2, Index key3) { - boost::shared_ptr factor(new Factor(key1,key2,key3)); + boost::shared_ptr factor(new IndexFactor(key1,key2,key3)); push_back(factor); } /** Push back 4-way factor */ void SymbolicFactorGraph::push_factor(Index key1, Index key2, Index key3, Index key4) { - boost::shared_ptr factor(new Factor(key1,key2,key3,key4)); + boost::shared_ptr factor(new IndexFactor(key1,key2,key3,key4)); push_back(factor); } diff --git a/inference/SymbolicFactorGraph.h b/inference/SymbolicFactorGraph.h index 4f3659673..7ae803a92 100644 --- a/inference/SymbolicFactorGraph.h +++ b/inference/SymbolicFactorGraph.h @@ -22,16 +22,16 @@ #include #include #include -#include +#include #include -#include +#include namespace gtsam { -typedef BayesNet SymbolicBayesNet; +typedef BayesNet SymbolicBayesNet; -/** Symbolic Factor Graph */ -class SymbolicFactorGraph: public FactorGraph { +/** Symbolic IndexFactor Graph */ +class SymbolicFactorGraph: public FactorGraph { public: typedef SymbolicBayesNet bayesnet_type; typedef VariableIndex<> variableindex_type; @@ -40,7 +40,7 @@ public: SymbolicFactorGraph() {} /** Construct from a BayesNet */ - SymbolicFactorGraph(const BayesNet& bayesNet); + SymbolicFactorGraph(const BayesNet& bayesNet); /** Push back unary factor */ void push_factor(Index key); @@ -77,10 +77,10 @@ template SymbolicFactorGraph::SymbolicFactorGraph(const FactorGraph& fg) { for (size_t i = 0; i < fg.size(); i++) { if(fg[i]) { - Factor::shared_ptr factor(new Factor(*fg[i])); + IndexFactor::shared_ptr factor(new IndexFactor(*fg[i])); push_back(factor); } else - push_back(Factor::shared_ptr()); + push_back(IndexFactor::shared_ptr()); } } diff --git a/inference/VariableSlots.h b/inference/VariableSlots.h index 91e81bc84..0dc2ae7fe 100644 --- a/inference/VariableSlots.h +++ b/inference/VariableSlots.h @@ -20,11 +20,11 @@ #include #include +#include #include #include #include -#include namespace gtsam { @@ -51,15 +51,11 @@ namespace gtsam { * is not performed by this class. */ -// Internal-use-only typedef for the VariableSlots map base class because this is such a long type name -typedef std::map, std::less, - boost::fast_pool_allocator > > > _VariableSlots_map; - -class VariableSlots : public _VariableSlots_map, public Testable { +class VariableSlots : public FastMap >, public Testable { public: - typedef _VariableSlots_map Base; + typedef FastMap > Base; /** * Constructor from a set of factors to be combined. Sorts the variables diff --git a/inference/inference-inl.h b/inference/inference-inl.h index d94c553cd..3b18d56d6 100644 --- a/inference/inference-inl.h +++ b/inference/inference-inl.h @@ -51,25 +51,25 @@ inline typename FactorGraph::bayesnet_type::shared_ptr Inference::Eliminate(cons } /* ************************************************************************* */ -template -BayesNet::shared_ptr Inference::EliminateSymbolic(const FactorGraph& factorGraph) { - - // Create a copy of the factor graph to eliminate in-place - FactorGraph eliminationGraph(factorGraph); - VariableIndex<> variableIndex(eliminationGraph); - - typename BayesNet::shared_ptr bayesnet(new BayesNet()); - - // Eliminate variables one-by-one, updating the eliminated factor graph and - // the variable index. - for(Index var = 0; var < variableIndex.size(); ++var) { - Conditional::shared_ptr conditional(EliminateOneSymbolic(eliminationGraph, variableIndex, var)); - if(conditional) // Will be NULL if the variable did not appear in the factor graph. - bayesnet->push_back(conditional); - } - - return bayesnet; -} +//template +//BayesNet::shared_ptr Inference::EliminateSymbolic(const FactorGraph& factorGraph) { +// +// // Create a copy of the factor graph to eliminate in-place +// FactorGraph eliminationGraph(factorGraph); +// VariableIndex<> variableIndex(eliminationGraph); +// +// typename BayesNet::shared_ptr bayesnet(new BayesNet()); +// +// // Eliminate variables one-by-one, updating the eliminated factor graph and +// // the variable index. +// for(Index var = 0; var < variableIndex.size(); ++var) { +// Conditional::shared_ptr conditional(EliminateOneSymbolic(eliminationGraph, variableIndex, var)); +// if(conditional) // Will be NULL if the variable did not appear in the factor graph. +// bayesnet->push_back(conditional); +// } +// +// return bayesnet; +//} /* ************************************************************************* */ template @@ -262,7 +262,9 @@ Inference::EliminateOne(FactorGraph& factorGraph, typename FactorGraph::variable // Join the factors and eliminate the variable from the joint factor tic("EliminateOne: Combine"); - typename FactorGraph::sharedFactor jointFactor(FactorGraph::Factor::Combine(factorGraph, variableIndex, removedFactorIdxs, sortedKeys, jointFactorPositions)); + typename FactorGraph::sharedFactor jointFactor( + FactorGraph::Factor::Combine( + factorGraph, variableIndex, removedFactorIdxs, sortedKeys, jointFactorPositions)); toc("EliminateOne: Combine"); // Remove the original factors diff --git a/inference/inference.cpp b/inference/inference.cpp index cf99e407a..616ddb1d2 100644 --- a/inference/inference.cpp +++ b/inference/inference.cpp @@ -23,82 +23,82 @@ namespace gtsam { /* ************************************************************************* */ -Conditional::shared_ptr -Inference::EliminateOneSymbolic(FactorGraph& factorGraph, VariableIndex<>& variableIndex, Index var) { - - tic("EliminateOne"); - - // Get the factors involving the eliminated variable - VariableIndex<>::mapped_type& varIndexEntry(variableIndex[var]); - typedef VariableIndex<>::mapped_factor_type mapped_factor_type; - - if(!varIndexEntry.empty()) { - - vector removedFactors(varIndexEntry.size()); - transform(varIndexEntry.begin(), varIndexEntry.end(), removedFactors.begin(), - boost::lambda::bind(&VariableIndex<>::mapped_factor_type::factorIndex, boost::lambda::_1)); - - // The new joint factor will be the last one in the factor graph - size_t jointFactorIndex = factorGraph.size(); - - static const bool debug = false; - - if(debug) { - cout << "Eliminating " << var; - factorGraph.print(" from graph: "); - cout << removedFactors.size() << " factors to remove" << endl; - } - - // Compute the involved keys, uses the variableIndex to mark whether each - // key has been added yet, but the positions stored in the variableIndex are - // from the unsorted positions and will be fixed later. - tic("EliminateOne: Find involved vars"); - typedef set, boost::fast_pool_allocator > InvolvedKeys; - InvolvedKeys involvedKeys; - BOOST_FOREACH(size_t removedFactorI, removedFactors) { - if(debug) cout << removedFactorI << " is involved" << endl; - // If the factor has not previously been removed - if(removedFactorI < factorGraph.size() && factorGraph[removedFactorI]) { - // Loop over the variables involved in the removed factor to update the - // variable index and joint factor positions of each variable. - BOOST_FOREACH(Index involvedVariable, factorGraph[removedFactorI]->keys()) { - if(debug) cout << " pulls in variable " << involvedVariable << endl; - // Mark the new joint factor as involving each variable in the removed factor. - assert(!variableIndex[involvedVariable].empty()); - involvedKeys.insert(involvedVariable); - } - } - - // Remove the original factor - factorGraph.remove(removedFactorI); - } - - // We need only mark the next variable to be eliminated as involved with the joint factor - if(involvedKeys.size() > 1) { - InvolvedKeys::const_iterator next = involvedKeys.begin(); ++ next; - variableIndex[*next].push_back(mapped_factor_type(jointFactorIndex,0)); - } - toc("EliminateOne: Find involved vars"); - if(debug) cout << removedFactors.size() << " factors to remove" << endl; - - // Join the factors and eliminate the variable from the joint factor - tic("EliminateOne: Combine"); - Conditional::shared_ptr conditional = Conditional::FromRange(involvedKeys.begin(), involvedKeys.end(), 1); - Factor::shared_ptr eliminated(new Factor(conditional->beginParents(), conditional->endParents())); - toc("EliminateOne: Combine"); - - tic("EliminateOne: store eliminated"); - factorGraph.push_back(eliminated); // Put the eliminated factor into the factor graph - toc("EliminateOne: store eliminated"); - - toc("EliminateOne"); - - return conditional; - - } else { // varIndexEntry.empty() - toc("EliminateOne"); - return Conditional::shared_ptr(); - } -} +//Conditional::shared_ptr +//Inference::EliminateOneSymbolic(FactorGraph& factorGraph, VariableIndex<>& variableIndex, Index var) { +// +// tic("EliminateOne"); +// +// // Get the factors involving the eliminated variable +// VariableIndex<>::mapped_type& varIndexEntry(variableIndex[var]); +// typedef VariableIndex<>::mapped_factor_type mapped_factor_type; +// +// if(!varIndexEntry.empty()) { +// +// vector removedFactors(varIndexEntry.size()); +// transform(varIndexEntry.begin(), varIndexEntry.end(), removedFactors.begin(), +// boost::lambda::bind(&VariableIndex<>::mapped_factor_type::factorIndex, boost::lambda::_1)); +// +// // The new joint factor will be the last one in the factor graph +// size_t jointFactorIndex = factorGraph.size(); +// +// static const bool debug = false; +// +// if(debug) { +// cout << "Eliminating " << var; +// factorGraph.print(" from graph: "); +// cout << removedFactors.size() << " factors to remove" << endl; +// } +// +// // Compute the involved keys, uses the variableIndex to mark whether each +// // key has been added yet, but the positions stored in the variableIndex are +// // from the unsorted positions and will be fixed later. +// tic("EliminateOne: Find involved vars"); +// typedef set, boost::fast_pool_allocator > InvolvedKeys; +// InvolvedKeys involvedKeys; +// BOOST_FOREACH(size_t removedFactorI, removedFactors) { +// if(debug) cout << removedFactorI << " is involved" << endl; +// // If the factor has not previously been removed +// if(removedFactorI < factorGraph.size() && factorGraph[removedFactorI]) { +// // Loop over the variables involved in the removed factor to update the +// // variable index and joint factor positions of each variable. +// BOOST_FOREACH(Index involvedVariable, factorGraph[removedFactorI]->keys()) { +// if(debug) cout << " pulls in variable " << involvedVariable << endl; +// // Mark the new joint factor as involving each variable in the removed factor. +// assert(!variableIndex[involvedVariable].empty()); +// involvedKeys.insert(involvedVariable); +// } +// } +// +// // Remove the original factor +// factorGraph.remove(removedFactorI); +// } +// +// // We need only mark the next variable to be eliminated as involved with the joint factor +// if(involvedKeys.size() > 1) { +// InvolvedKeys::const_iterator next = involvedKeys.begin(); ++ next; +// variableIndex[*next].push_back(mapped_factor_type(jointFactorIndex,0)); +// } +// toc("EliminateOne: Find involved vars"); +// if(debug) cout << removedFactors.size() << " factors to remove" << endl; +// +// // Join the factors and eliminate the variable from the joint factor +// tic("EliminateOne: Combine"); +// Conditional::shared_ptr conditional = Conditional::FromRange(involvedKeys.begin(), involvedKeys.end(), 1); +// Factor::shared_ptr eliminated(new Factor(conditional->beginParents(), conditional->endParents())); +// toc("EliminateOne: Combine"); +// +// tic("EliminateOne: store eliminated"); +// factorGraph.push_back(eliminated); // Put the eliminated factor into the factor graph +// toc("EliminateOne: store eliminated"); +// +// toc("EliminateOne"); +// +// return conditional; +// +// } else { // varIndexEntry.empty() +// toc("EliminateOne"); +// return Conditional::shared_ptr(); +// } +//} } diff --git a/inference/inference.h b/inference/inference.h index 9f18b376f..bc135f998 100644 --- a/inference/inference.h +++ b/inference/inference.h @@ -29,9 +29,6 @@ namespace gtsam { -class Factor; -class Conditional; - class Inference { private: /* Static members only, private constructor */ @@ -51,8 +48,8 @@ class Conditional; * variables in order starting from 0. Special fast version for symbolic * elimination. */ - template - static BayesNet::shared_ptr EliminateSymbolic(const FactorGraph& factorGraph); +// template +// static BayesNet::shared_ptr EliminateSymbolic(const FactorGraph& factorGraph); /** * Eliminate a factor graph in its natural ordering, i.e. eliminating @@ -93,8 +90,8 @@ class Conditional; * variable index. This is a specialized faster version for purely * symbolic factor graphs. */ - static boost::shared_ptr - EliminateOneSymbolic(FactorGraph& factorGraph, VariableIndex<>& variableIndex, Index var); +// static boost::shared_ptr +// EliminateOneSymbolic(FactorGraph& factorGraph, VariableIndex<>& variableIndex, Index var); /** * Eliminate all variables except the specified ones. Internally this diff --git a/inference/tests/testBayesTree.cpp b/inference/tests/testBayesTree.cpp index 22f62f202..7c8416d0c 100644 --- a/inference/tests/testBayesTree.cpp +++ b/inference/tests/testBayesTree.cpp @@ -25,22 +25,22 @@ using namespace boost::assign; #include #include -#include +#include #include using namespace gtsam; -typedef BayesTree SymbolicBayesTree; +typedef BayesTree SymbolicBayesTree; ///* ************************************************************************* */ //// SLAM example from RSS sqrtSAM paper static const Index _x3_=0, _x2_=1, _x1_=2, _l2_=3, _l1_=4; -//Conditional::shared_ptr -// x3(new Conditional(_x3_)), -// x2(new Conditional(_x2_,_x3_)), -// x1(new Conditional(_x1_,_x2_,_x3_)), -// l1(new Conditional(_l1_,_x1_,_x2_)), -// l2(new Conditional(_l2_,_x1_,_x3_)); +//IndexConditional::shared_ptr +// x3(new IndexConditional(_x3_)), +// x2(new IndexConditional(_x2_,_x3_)), +// x1(new IndexConditional(_x1_,_x2_,_x3_)), +// l1(new IndexConditional(_l1_,_x1_,_x2_)), +// l2(new IndexConditional(_l2_,_x1_,_x3_)); // //// Bayes Tree for sqrtSAM example //SymbolicBayesTree createSlamSymbolicBayesTree(){ @@ -58,13 +58,13 @@ static const Index _x3_=0, _x2_=1, _x1_=2, _l2_=3, _l1_=4; /* ************************************************************************* */ // Conditionals for ASIA example from the tutorial with A and D evidence static const Index _X_=0, _T_=1, _S_=2, _E_=3, _L_=4, _B_=5; -Conditional::shared_ptr - B(new Conditional(_B_)), - L(new Conditional(_L_, _B_)), - E(new Conditional(_E_, _L_, _B_)), - S(new Conditional(_S_, _L_, _B_)), - T(new Conditional(_T_, _E_, _L_)), - X(new Conditional(_X_, _E_)); +IndexConditional::shared_ptr + B(new IndexConditional(_B_)), + L(new IndexConditional(_L_, _B_)), + E(new IndexConditional(_E_, _L_, _B_)), + S(new IndexConditional(_S_, _L_, _B_)), + T(new IndexConditional(_T_, _E_, _L_)), + X(new IndexConditional(_X_, _E_)); // Bayes Tree for Asia example SymbolicBayesTree createAsiaSymbolicBayesTree() { @@ -89,15 +89,15 @@ TEST( BayesTree, constructor ) LONGS_EQUAL(4,bayesTree.size()); // Check root - BayesNet expected_root; + BayesNet expected_root; expected_root.push_back(E); expected_root.push_back(L); expected_root.push_back(B); - boost::shared_ptr > actual_root = bayesTree.root(); + boost::shared_ptr > actual_root = bayesTree.root(); CHECK(assert_equal(expected_root,*actual_root)); // Create from symbolic Bayes chain in which we want to discover cliques - BayesNet ASIA; + BayesNet ASIA; ASIA.push_back(X); ASIA.push_back(T); ASIA.push_back(S); @@ -145,13 +145,13 @@ Bayes Tree for testing conversion to a forest of orphans needed for incremental. TEST( BayesTree, removePath ) { const Index _A_=5, _B_=4, _C_=3, _D_=2, _E_=1, _F_=0; - Conditional::shared_ptr - A(new Conditional(_A_)), - B(new Conditional(_B_, _A_)), - C(new Conditional(_C_, _A_)), - D(new Conditional(_D_, _C_)), - E(new Conditional(_E_, _B_)), - F(new Conditional(_F_, _E_)); + IndexConditional::shared_ptr + A(new IndexConditional(_A_)), + B(new IndexConditional(_B_, _A_)), + C(new IndexConditional(_C_, _A_)), + D(new IndexConditional(_D_, _C_)), + E(new IndexConditional(_E_, _B_)), + F(new IndexConditional(_F_, _E_)); SymbolicBayesTree bayesTree; // Ordering ord; ord += _A_,_B_,_C_,_D_,_E_,_F_; bayesTree.insert(A); @@ -170,7 +170,7 @@ TEST( BayesTree, removePath ) SymbolicBayesTree::Cliques expectedOrphans; expectedOrphans += bayesTree[_D_], bayesTree[_E_]; - BayesNet bn; + BayesNet bn; SymbolicBayesTree::Cliques orphans; bayesTree.removePath(bayesTree[_C_], bn, orphans); SymbolicFactorGraph factors(bn); @@ -183,7 +183,7 @@ TEST( BayesTree, removePath ) SymbolicBayesTree::Cliques expectedOrphans2; expectedOrphans2 += bayesTree[_F_]; - BayesNet bn2; + BayesNet bn2; SymbolicBayesTree::Cliques orphans2; bayesTree.removePath(bayesTree[_E_], bn2, orphans2); SymbolicFactorGraph factors2(bn2); @@ -197,7 +197,7 @@ TEST( BayesTree, removePath2 ) SymbolicBayesTree bayesTree = createAsiaSymbolicBayesTree(); // Call remove-path with clique B - BayesNet bn; + BayesNet bn; SymbolicBayesTree::Cliques orphans; bayesTree.removePath(bayesTree[_B_], bn, orphans); SymbolicFactorGraph factors(bn); @@ -219,7 +219,7 @@ TEST( BayesTree, removePath3 ) SymbolicBayesTree bayesTree = createAsiaSymbolicBayesTree(); // Call remove-path with clique S - BayesNet bn; + BayesNet bn; SymbolicBayesTree::Cliques orphans; bayesTree.removePath(bayesTree[_S_], bn, orphans); SymbolicFactorGraph factors(bn); @@ -242,10 +242,10 @@ TEST( BayesTree, removeTop ) SymbolicBayesTree bayesTree = createAsiaSymbolicBayesTree(); // create a new factor to be inserted - boost::shared_ptr newFactor(new Factor(_S_,_B_)); + boost::shared_ptr newFactor(new IndexFactor(_S_,_B_)); // Remove the contaminated part of the Bayes tree - BayesNet bn; + BayesNet bn; SymbolicBayesTree::Cliques orphans; list keys; keys += _B_,_S_; bayesTree.removeTop(keys, bn, orphans); @@ -263,8 +263,8 @@ TEST( BayesTree, removeTop ) CHECK(assert_equal(expectedOrphans, orphans)); // Try removeTop again with a factor that should not change a thing - boost::shared_ptr newFactor2(new Factor(_B_)); - BayesNet bn2; + boost::shared_ptr newFactor2(new IndexFactor(_B_)); + BayesNet bn2; SymbolicBayesTree::Cliques orphans2; keys.clear(); keys += _B_; bayesTree.removeTop(keys, bn2, orphans2); @@ -286,7 +286,7 @@ TEST( BayesTree, removeTop2 ) newFactors.push_factor(_S_); // Remove the contaminated part of the Bayes tree - BayesNet bn; + BayesNet bn; SymbolicBayesTree::Cliques orphans; list keys; keys += _B_,_S_; bayesTree.removeTop(keys, bn, orphans); @@ -309,11 +309,11 @@ TEST( BayesTree, removeTop3 ) { const Index _x4_=5, _l5_=6; // simple test case that failed after COLAMD was fixed/activated - Conditional::shared_ptr - X(new Conditional(_l5_)), - A(new Conditional(_x4_, _l5_)), - B(new Conditional(_x2_, _x4_)), - C(new Conditional(_x3_, _x2_)); + IndexConditional::shared_ptr + X(new IndexConditional(_l5_)), + A(new IndexConditional(_x4_, _l5_)), + B(new IndexConditional(_x2_, _x4_)), + C(new IndexConditional(_x3_, _x2_)); // Ordering newOrdering; // newOrdering += _x3_, _x2_, _x1_, _l2_, _l1_, _x4_, _l5_; @@ -326,7 +326,7 @@ TEST( BayesTree, removeTop3 ) // remove all list keys; keys += _l5_, _x2_, _x3_, _x4_; - BayesNet bn; + BayesNet bn; SymbolicBayesTree::Cliques orphans; bayesTree.removeTop(keys, bn, orphans); SymbolicFactorGraph factors(bn); @@ -356,7 +356,7 @@ TEST( BayesTree, insert ) // Ordering ordering2; ordering2 += _x1_, _x2_; // Ordering ordering3; ordering3 += _x6_, _x5_; - BayesNet bn1, bn2, bn3; + BayesNet bn1, bn2, bn3; bn1 = *Inference::EliminateUntil(fg1, _x4_+1); bn2 = *Inference::EliminateUntil(fg2, _x2_+1); bn3 = *Inference::EliminateUntil(fg3, _x5_+1); @@ -383,7 +383,7 @@ TEST( BayesTree, insert ) fg.push_factor(_x6_, _x4_); // Ordering ordering; ordering += _x1_, _x2_, _x6_, _x5_, _x3_, _x4_; - BayesNet bn; + BayesNet bn; bn = *Inference::Eliminate(fg); SymbolicBayesTree expected(bn); CHECK(assert_equal(expected, actual)); diff --git a/inference/tests/testConditional.cpp b/inference/tests/testConditional.cpp index d1008c228..e0c232eec 100644 --- a/inference/tests/testConditional.cpp +++ b/inference/tests/testConditional.cpp @@ -11,7 +11,7 @@ /** * @file testConditional.cpp - * @brief Unit tests for Conditional class + * @brief Unit tests for IndexConditional class * @author Frank Dellaert */ @@ -20,76 +20,76 @@ using namespace boost::assign; #include -#include -#include +#include +#include using namespace std; using namespace gtsam; /* ************************************************************************* */ -TEST( Conditional, empty ) +TEST( IndexConditional, empty ) { - Conditional c0; + IndexConditional c0; LONGS_EQUAL(0,c0.nrFrontals()) LONGS_EQUAL(0,c0.nrParents()) } /* ************************************************************************* */ -TEST( Conditional, noParents ) +TEST( IndexConditional, noParents ) { - Conditional c0(0); + IndexConditional c0(0); LONGS_EQUAL(1,c0.nrFrontals()) LONGS_EQUAL(0,c0.nrParents()) } /* ************************************************************************* */ -TEST( Conditional, oneParents ) +TEST( IndexConditional, oneParents ) { - Conditional c0(0,1); + IndexConditional c0(0,1); LONGS_EQUAL(1,c0.nrFrontals()) LONGS_EQUAL(1,c0.nrParents()) } /* ************************************************************************* */ -TEST( Conditional, twoParents ) +TEST( IndexConditional, twoParents ) { - Conditional c0(0,1,2); + IndexConditional c0(0,1,2); LONGS_EQUAL(1,c0.nrFrontals()) LONGS_EQUAL(2,c0.nrParents()) } /* ************************************************************************* */ -TEST( Conditional, threeParents ) +TEST( IndexConditional, threeParents ) { - Conditional c0(0,1,2,3); + IndexConditional c0(0,1,2,3); LONGS_EQUAL(1,c0.nrFrontals()) LONGS_EQUAL(3,c0.nrParents()) } /* ************************************************************************* */ -TEST( Conditional, fourParents ) +TEST( IndexConditional, fourParents ) { vector parents; parents += 1,2,3,4; - Conditional c0(0,parents); + IndexConditional c0(0,parents); LONGS_EQUAL(1,c0.nrFrontals()) LONGS_EQUAL(4,c0.nrParents()) } /* ************************************************************************* */ -TEST( Conditional, FromRange ) +TEST( IndexConditional, FromRange ) { list keys; keys += 1,2,3,4,5; - Conditional::shared_ptr c0 = Conditional::FromRange(keys.begin(),keys.end(),2); + IndexConditional::shared_ptr c0 = IndexConditional::FromRange(keys.begin(),keys.end(),2); LONGS_EQUAL(2,c0->nrFrontals()) LONGS_EQUAL(3,c0->nrParents()) } /* ************************************************************************* */ -TEST( Conditional, equals ) +TEST( IndexConditional, equals ) { - Conditional c0(0, 1, 2), c1(0, 1, 2), c2(1, 2, 3), c3(3,4); + IndexConditional c0(0, 1, 2), c1(0, 1, 2), c2(1, 2, 3), c3(3,4); CHECK(c0.equals(c1)); CHECK(c1.equals(c0)); CHECK(!c0.equals(c2)); diff --git a/inference/tests/testEliminationTree.cpp b/inference/tests/testEliminationTree.cpp index 114827f60..ed2a0ac86 100644 --- a/inference/tests/testEliminationTree.cpp +++ b/inference/tests/testEliminationTree.cpp @@ -8,13 +8,13 @@ #include #include -#include +#include #include using namespace gtsam; using namespace std; -typedef EliminationTree SymbolicEliminationTree; +typedef EliminationTree SymbolicEliminationTree; struct EliminationTreeTester { // build hardcoded tree @@ -69,11 +69,11 @@ TEST(EliminationTree, Create) TEST(EliminationTree, eliminate ) { // create expected Chordal bayes Net - Conditional::shared_ptr c0(new Conditional(0, 1, 2)); - Conditional::shared_ptr c1(new Conditional(1, 2, 4)); - Conditional::shared_ptr c2(new Conditional(2, 4)); - Conditional::shared_ptr c3(new Conditional(3, 4)); - Conditional::shared_ptr c4(new Conditional(4)); + IndexConditional::shared_ptr c0(new IndexConditional(0, 1, 2)); + IndexConditional::shared_ptr c1(new IndexConditional(1, 2, 4)); + IndexConditional::shared_ptr c2(new IndexConditional(2, 4)); + IndexConditional::shared_ptr c3(new IndexConditional(3, 4)); + IndexConditional::shared_ptr c4(new IndexConditional(4)); SymbolicBayesNet expected; expected.push_back(c3); diff --git a/inference/tests/testFactorGraph.cpp b/inference/tests/testFactorGraph.cpp index 9be1edfa4..e9d88df96 100644 --- a/inference/tests/testFactorGraph.cpp +++ b/inference/tests/testFactorGraph.cpp @@ -11,7 +11,7 @@ /** * @file testFactorgraph.cpp - * @brief Unit tests for Factor Graphs + * @brief Unit tests for IndexFactor Graphs * @author Christian Potthast **/ diff --git a/inference/tests/testISAM.cpp b/inference/tests/testISAM.cpp index a5c18cdc9..38b0d9e25 100644 --- a/inference/tests/testISAM.cpp +++ b/inference/tests/testISAM.cpp @@ -24,14 +24,14 @@ using namespace boost::assign; #define GTSAM_MAGIC_KEY #include -#include +#include #include #include using namespace std; using namespace gtsam; -typedef ISAM SymbolicISAM; +typedef ISAM SymbolicISAM; /* ************************************************************************* */ // Some numbers that should be consistent among all smoother tests diff --git a/inference/tests/testJunctionTree.cpp b/inference/tests/testJunctionTree.cpp index 56823f950..62502904c 100644 --- a/inference/tests/testJunctionTree.cpp +++ b/inference/tests/testJunctionTree.cpp @@ -30,12 +30,12 @@ using namespace boost::assign; #include #include #include -#include +#include using namespace gtsam; typedef JunctionTree SymbolicJunctionTree; -typedef BayesTree SymbolicBayesTree; +typedef BayesTree SymbolicBayesTree; /* ************************************************************************* * * x1 - x2 - x3 - x4 @@ -83,7 +83,7 @@ TEST( JunctionTree, eliminate) SymbolicJunctionTree jt(fg); SymbolicBayesTree::sharedClique actual = jt.eliminate(); - BayesNet bn = *Inference::Eliminate(fg); + BayesNet bn = *Inference::Eliminate(fg); SymbolicBayesTree expected(bn); // cout << "BT from JT:\n"; diff --git a/inference/tests/testSymbolicBayesNet.cpp b/inference/tests/testSymbolicBayesNet.cpp index 5eb507cc2..3f1e42cba 100644 --- a/inference/tests/testSymbolicBayesNet.cpp +++ b/inference/tests/testSymbolicBayesNet.cpp @@ -33,17 +33,17 @@ static const Index _A_ = 1; static const Index _B_ = 2; static const Index _C_ = 3; -Conditional::shared_ptr - B(new Conditional(_B_)), - L(new Conditional(_L_, _B_)); +IndexConditional::shared_ptr + B(new IndexConditional(_B_)), + L(new IndexConditional(_L_, _B_)); /* ************************************************************************* */ TEST( SymbolicBayesNet, equals ) { - BayesNet f1; + BayesNet f1; f1.push_back(B); f1.push_back(L); - BayesNet f2; + BayesNet f2; f2.push_back(L); f2.push_back(B); CHECK(f1.equals(f1)); @@ -53,18 +53,18 @@ TEST( SymbolicBayesNet, equals ) /* ************************************************************************* */ TEST( SymbolicBayesNet, pop_front ) { - Conditional::shared_ptr - A(new Conditional(_A_,_B_,_C_)), - B(new Conditional(_B_,_C_)), - C(new Conditional(_C_)); + IndexConditional::shared_ptr + A(new IndexConditional(_A_,_B_,_C_)), + B(new IndexConditional(_B_,_C_)), + C(new IndexConditional(_C_)); // Expected after pop_front - BayesNet expected; + BayesNet expected; expected.push_back(B); expected.push_back(C); // Actual - BayesNet actual; + BayesNet actual; actual.push_back(A); actual.push_back(B); actual.push_back(C); @@ -76,24 +76,24 @@ TEST( SymbolicBayesNet, pop_front ) /* ************************************************************************* */ TEST( SymbolicBayesNet, combine ) { - Conditional::shared_ptr - A(new Conditional(_A_,_B_,_C_)), - B(new Conditional(_B_,_C_)), - C(new Conditional(_C_)); + IndexConditional::shared_ptr + A(new IndexConditional(_A_,_B_,_C_)), + B(new IndexConditional(_B_,_C_)), + C(new IndexConditional(_C_)); // p(A|BC) - BayesNet p_ABC; + BayesNet p_ABC; p_ABC.push_back(A); // P(BC)=P(B|C)P(C) - BayesNet p_BC; + BayesNet p_BC; p_BC.push_back(B); p_BC.push_back(C); // P(ABC) = P(A|BC) P(BC) p_ABC.push_back(p_BC); - BayesNet expected; + BayesNet expected; expected.push_back(A); expected.push_back(B); expected.push_back(C); diff --git a/inference/tests/testSymbolicFactor.cpp b/inference/tests/testSymbolicFactor.cpp index 950c32d54..01090c3da 100644 --- a/inference/tests/testSymbolicFactor.cpp +++ b/inference/tests/testSymbolicFactor.cpp @@ -11,14 +11,14 @@ /** * @file testSymbolicFactor.cpp - * @brief Unit tests for a symbolic Factor + * @brief Unit tests for a symbolic IndexFactor * @author Frank Dellaert */ #include #include -#include -#include +#include +#include #include @@ -29,17 +29,17 @@ using namespace boost::assign; /* ************************************************************************* */ TEST(SymbolicFactor, eliminate) { vector keys; keys += 2, 3, 4, 6, 7, 9, 10, 11; - Factor actual(keys.begin(), keys.end()); - BayesNet fragment = *actual.eliminate(3); + IndexFactor actual(keys.begin(), keys.end()); + BayesNet fragment = *actual.eliminate(3); - Factor expected(keys.begin()+3, keys.end()); - Conditional::shared_ptr expected0 = Conditional::FromRange(keys.begin(), keys.end(), 1); - Conditional::shared_ptr expected1 = Conditional::FromRange(keys.begin()+1, keys.end(), 1); - Conditional::shared_ptr expected2 = Conditional::FromRange(keys.begin()+2, keys.end(), 1); + IndexFactor expected(keys.begin()+3, keys.end()); + IndexConditional::shared_ptr expected0 = IndexConditional::FromRange(keys.begin(), keys.end(), 1); + IndexConditional::shared_ptr expected1 = IndexConditional::FromRange(keys.begin()+1, keys.end(), 1); + IndexConditional::shared_ptr expected2 = IndexConditional::FromRange(keys.begin()+2, keys.end(), 1); CHECK(assert_equal(fragment.size(), size_t(3))); CHECK(assert_equal(expected, actual)); - BayesNet::const_iterator fragmentCond = fragment.begin(); + BayesNet::const_iterator fragmentCond = fragment.begin(); CHECK(assert_equal(**fragmentCond++, *expected0)); CHECK(assert_equal(**fragmentCond++, *expected1)); CHECK(assert_equal(**fragmentCond++, *expected2)); diff --git a/inference/tests/testSymbolicFactorGraph.cpp b/inference/tests/testSymbolicFactorGraph.cpp index 3984940f9..41b215c37 100644 --- a/inference/tests/testSymbolicFactorGraph.cpp +++ b/inference/tests/testSymbolicFactorGraph.cpp @@ -11,7 +11,7 @@ /** * @file testSymbolicFactorGraph.cpp - * @brief Unit tests for a symbolic Factor Graph + * @brief Unit tests for a symbolic IndexFactor Graph * @author Frank Dellaert */ @@ -22,7 +22,7 @@ using namespace boost::assign; #include #include -#include +#include #include #include @@ -43,7 +43,7 @@ TEST( SymbolicFactorGraph, EliminateOne ) VariableIndex<> variableIndex(fg); Inference::EliminateOne(fg, variableIndex, vx2); SymbolicFactorGraph expected; - expected.push_back(boost::shared_ptr()); + expected.push_back(boost::shared_ptr()); expected.push_factor(vx1); CHECK(assert_equal(expected, fg)); @@ -59,11 +59,11 @@ TEST( SymbolicFactorGraph, constructFromBayesNet ) expected.push_factor(vx1); // create Bayes Net - Conditional::shared_ptr x2(new Conditional(vx2, vx1, vl1)); - Conditional::shared_ptr l1(new Conditional(vx1, vl1)); - Conditional::shared_ptr x1(new Conditional(vx1)); + IndexConditional::shared_ptr x2(new IndexConditional(vx2, vx1, vl1)); + IndexConditional::shared_ptr l1(new IndexConditional(vx1, vl1)); + IndexConditional::shared_ptr x1(new IndexConditional(vx1)); - BayesNet bayesNet; + BayesNet bayesNet; bayesNet.push_back(x2); bayesNet.push_back(l1); bayesNet.push_back(x1); @@ -109,7 +109,7 @@ TEST( SymbolicFactorGraph, push_back ) // //public: // -// typedef boost::shared_ptr sharedFactor; +// typedef boost::shared_ptr sharedFactor; // typedef boost::shared_ptr shared_ptr; // //private: @@ -118,7 +118,7 @@ TEST( SymbolicFactorGraph, push_back ) // list factors_; /** factors associated with root */ // list subTrees_; /** sub-trees */ // -// typedef pair Result; +// typedef pair Result; // // /** // * Recursive routine that eliminates the factors arranged in an elimination tree @@ -146,11 +146,11 @@ TEST( SymbolicFactorGraph, push_back ) // // Make the conditional from the key and separator, and insert it in Bayes net // vector parents; // std::copy(separator.begin(), separator.end(), back_inserter(parents)); -// Conditional::shared_ptr conditional(new Conditional(key_, parents)); +// IndexConditional::shared_ptr conditional(new IndexConditional(key_, parents)); // bn.push_back(conditional); // // // now create the new factor from separator to return to caller -// Factor newFactor(separator.begin(), separator.end()); +// IndexFactor newFactor(separator.begin(), separator.end()); // return Result(bn, newFactor); // } // @@ -366,11 +366,11 @@ TEST( SymbolicFactorGraph, push_back ) //TEST( SymbolicFactorGraph, eliminate ) //{ // // create expected Chordal bayes Net -// Conditional::shared_ptr c0(new Conditional(0, 1, 2)); -// Conditional::shared_ptr c1(new Conditional(1, 2, 4)); -// Conditional::shared_ptr c2(new Conditional(2, 4)); -// Conditional::shared_ptr c3(new Conditional(3, 4)); -// Conditional::shared_ptr c4(new Conditional(4)); +// IndexConditional::shared_ptr c0(new IndexConditional(0, 1, 2)); +// IndexConditional::shared_ptr c1(new IndexConditional(1, 2, 4)); +// IndexConditional::shared_ptr c2(new IndexConditional(2, 4)); +// IndexConditional::shared_ptr c3(new IndexConditional(3, 4)); +// IndexConditional::shared_ptr c4(new IndexConditional(4)); // // SymbolicBayesNet expected; // expected.push_back(c3); diff --git a/inference/tests/timeSymbolMaps.cpp b/inference/tests/timeSymbolMaps.cpp index 30f5cc974..d5efe5ca1 100644 --- a/inference/tests/timeSymbolMaps.cpp +++ b/inference/tests/timeSymbolMaps.cpp @@ -23,7 +23,7 @@ #include #include -#include +#include using namespace std; using namespace boost; diff --git a/linear/GaussianConditional.cpp b/linear/GaussianConditional.cpp index 66d2b7e50..2dc774678 100644 --- a/linear/GaussianConditional.cpp +++ b/linear/GaussianConditional.cpp @@ -33,11 +33,11 @@ namespace gtsam { GaussianConditional::GaussianConditional() : rsd_(matrix_) {} /* ************************************************************************* */ -GaussianConditional::GaussianConditional(Index key) : Conditional(key), rsd_(matrix_) {} +GaussianConditional::GaussianConditional(Index key) : IndexConditional(key), rsd_(matrix_) {} /* ************************************************************************* */ GaussianConditional::GaussianConditional(Index key,const Vector& d, const Matrix& R, const Vector& sigmas) : - Conditional(key), rsd_(matrix_), sigmas_(sigmas) { + IndexConditional(key), rsd_(matrix_), sigmas_(sigmas) { assert(R.size1() <= R.size2()); size_t dims[] = { R.size2(), 1 }; rsd_.copyStructureFrom(rsd_type(matrix_, dims, dims+2, d.size())); @@ -48,7 +48,7 @@ GaussianConditional::GaussianConditional(Index key,const Vector& d, const Matrix /* ************************************************************************* */ GaussianConditional::GaussianConditional(Index key, const Vector& d, const Matrix& R, Index name1, const Matrix& S, const Vector& sigmas) : - Conditional(key,name1), rsd_(matrix_), sigmas_(sigmas) { + IndexConditional(key,name1), rsd_(matrix_), sigmas_(sigmas) { assert(R.size1() <= R.size2()); size_t dims[] = { R.size2(), S.size2(), 1 }; rsd_.copyStructureFrom(rsd_type(matrix_, dims, dims+3, d.size())); @@ -60,7 +60,7 @@ GaussianConditional::GaussianConditional(Index key, const Vector& d, const Matri /* ************************************************************************* */ GaussianConditional::GaussianConditional(Index key, const Vector& d, const Matrix& R, Index name1, const Matrix& S, Index name2, const Matrix& T, const Vector& sigmas) : - Conditional(key,name1,name2), rsd_(matrix_), sigmas_(sigmas) { + IndexConditional(key,name1,name2), rsd_(matrix_), sigmas_(sigmas) { assert(R.size1() <= R.size2()); size_t dims[] = { R.size2(), S.size2(), T.size2(), 1 }; rsd_.copyStructureFrom(rsd_type(matrix_, dims, dims+4, d.size())); @@ -74,7 +74,7 @@ GaussianConditional::GaussianConditional(Index key, const Vector& d, const Matri GaussianConditional::GaussianConditional(Index key, const Vector& d, const Matrix& R, const list >& parents, const Vector& sigmas) : rsd_(matrix_), sigmas_(sigmas) { assert(R.size1() <= R.size2()); - Conditional::nrFrontals_ = 1; + IndexConditional::nrFrontals_ = 1; keys_.resize(1+parents.size()); size_t dims[1+parents.size()+1]; dims[0] = R.size2(); diff --git a/linear/GaussianConditional.h b/linear/GaussianConditional.h index 3365ffe01..12338e03a 100644 --- a/linear/GaussianConditional.h +++ b/linear/GaussianConditional.h @@ -25,7 +25,7 @@ #include #include -#include +#include #include #include #include @@ -39,10 +39,10 @@ class GaussianFactor; * It has a set of parents y,z, etc. and implements a probability density on x. * The negative log-probability is given by || Rx - (d - Sy - Tz - ...)||^2 */ -class GaussianConditional : public Conditional { +class GaussianConditional : public IndexConditional { public: - typedef GaussianFactor FactorType; + typedef GaussianFactor Factor; typedef boost::shared_ptr shared_ptr; /** Store the conditional matrix as upper-triangular column-major */ diff --git a/linear/GaussianFactor.cpp b/linear/GaussianFactor.cpp index ba4a91df8..c9bdcd4f0 100644 --- a/linear/GaussianFactor.cpp +++ b/linear/GaussianFactor.cpp @@ -45,14 +45,14 @@ namespace gtsam { /* ************************************************************************* */ inline void GaussianFactor::assertInvariants() const { #ifndef NDEBUG - Factor::assertInvariants(); + IndexFactor::assertInvariants(); assert((keys_.size() == 0 && Ab_.size1() == 0 && Ab_.nBlocks() == 0) || keys_.size()+1 == Ab_.nBlocks()); #endif } /* ************************************************************************* */ GaussianFactor::GaussianFactor(const GaussianFactor& gf) : - Factor(gf), model_(gf.model_), firstNonzeroBlocks_(gf.firstNonzeroBlocks_), Ab_(matrix_) { + IndexFactor(gf), model_(gf.model_), firstNonzeroBlocks_(gf.firstNonzeroBlocks_), Ab_(matrix_) { Ab_.assignNoalias(gf.Ab_); assertInvariants(); } @@ -71,7 +71,7 @@ GaussianFactor::GaussianFactor(const Vector& b_in) : firstNonzeroBlocks_(b_in.si /* ************************************************************************* */ GaussianFactor::GaussianFactor(Index i1, const Matrix& A1, const Vector& b, const SharedDiagonal& model) : - Factor(i1), model_(model), firstNonzeroBlocks_(b.size(), 0), Ab_(matrix_) { + IndexFactor(i1), model_(model), firstNonzeroBlocks_(b.size(), 0), Ab_(matrix_) { size_t dims[] = { A1.size2(), 1}; Ab_.copyStructureFrom(ab_type(matrix_, dims, dims+2, b.size())); Ab_(0) = A1; @@ -82,7 +82,7 @@ GaussianFactor::GaussianFactor(Index i1, const Matrix& A1, /* ************************************************************************* */ GaussianFactor::GaussianFactor(Index i1, const Matrix& A1, Index i2, const Matrix& A2, const Vector& b, const SharedDiagonal& model) : - Factor(i1,i2), model_(model), firstNonzeroBlocks_(b.size(), 0), Ab_(matrix_) { + IndexFactor(i1,i2), model_(model), firstNonzeroBlocks_(b.size(), 0), Ab_(matrix_) { size_t dims[] = { A1.size2(), A2.size2(), 1}; Ab_.copyStructureFrom(ab_type(matrix_, dims, dims+3, b.size())); Ab_(0) = A1; @@ -94,7 +94,7 @@ GaussianFactor::GaussianFactor(Index i1, const Matrix& A1, Index i2, const Matri /* ************************************************************************* */ GaussianFactor::GaussianFactor(Index i1, const Matrix& A1, Index i2, const Matrix& A2, Index i3, const Matrix& A3, const Vector& b, const SharedDiagonal& model) : - Factor(i1,i2,i3), model_(model), firstNonzeroBlocks_(b.size(), 0), Ab_(matrix_) { + IndexFactor(i1,i2,i3), model_(model), firstNonzeroBlocks_(b.size(), 0), Ab_(matrix_) { size_t dims[] = { A1.size2(), A2.size2(), A3.size2(), 1}; Ab_.copyStructureFrom(ab_type(matrix_, dims, dims+4, b.size())); Ab_(0) = A1; @@ -147,7 +147,7 @@ GaussianFactor::GaussianFactor(const std::list > &terms } /* ************************************************************************* */ -GaussianFactor::GaussianFactor(const GaussianConditional& cg) : Factor(cg), model_(noiseModel::Diagonal::Sigmas(cg.get_sigmas(), true)), Ab_(matrix_) { +GaussianFactor::GaussianFactor(const GaussianConditional& cg) : IndexFactor(cg), model_(noiseModel::Diagonal::Sigmas(cg.get_sigmas(), true)), Ab_(matrix_) { Ab_.assignNoalias(cg.rsd_); // todo SL: make firstNonzeroCols triangular? firstNonzeroBlocks_.resize(cg.get_d().size(), 0); // set sigmas from precisions @@ -590,11 +590,11 @@ struct _RowSource { }; /* Explicit instantiations for storage types */ -template GaussianFactor::shared_ptr GaussianFactor::Combine(const GaussianFactorGraph&, const GaussianVariableIndex&, const vector&, const vector&, const std::vector >&); -template GaussianFactor::shared_ptr GaussianFactor::Combine(const GaussianFactorGraph&, const GaussianVariableIndex&, const vector&, const vector&, const std::vector >&); +template GaussianFactor::shared_ptr GaussianFactor::Combine(const FactorGraph&, const GaussianVariableIndex&, const vector&, const vector&, const std::vector >&); +template GaussianFactor::shared_ptr GaussianFactor::Combine(const FactorGraph&, const GaussianVariableIndex&, const vector&, const vector&, const std::vector >&); template -GaussianFactor::shared_ptr GaussianFactor::Combine(const GaussianFactorGraph& factorGraph, +GaussianFactor::shared_ptr GaussianFactor::Combine(const FactorGraph& factorGraph, const GaussianVariableIndex& variableIndex, const vector& factors, const vector& variables, const std::vector >& variablePositions) { @@ -786,7 +786,7 @@ boost::tuple, size_t, size_t> countDims(const std::vector& factors, const VariableSlots& variableSlots) { static const bool verbose = false; static const bool debug = false; diff --git a/linear/GaussianFactor.h b/linear/GaussianFactor.h index 07a73e160..5997f8b9f 100644 --- a/linear/GaussianFactor.h +++ b/linear/GaussianFactor.h @@ -37,9 +37,10 @@ #include #include #include -#include +#include #include #include +#include #include #include #include @@ -58,7 +59,7 @@ typedef std::map Dimensions; * GaussianFactor is non-mutable (all methods const!). * The factor value is exp(-0.5*||Ax-b||^2) */ -class GaussianFactor: public Factor { +class GaussianFactor: public IndexFactor { public: @@ -149,7 +150,7 @@ public: /** Named constructor for combining a set of factors with pre-computed set of * variables. */ template - static shared_ptr Combine(const GaussianFactorGraph& factorGraph, + static shared_ptr Combine(const FactorGraph& factorGraph, const GaussianVariableIndex& variableIndex, const std::vector& factors, const std::vector& variables, const std::vector >& variablePositions); @@ -157,7 +158,7 @@ public: * Named constructor for combining a set of factors with pre-computed set of * variables. */ - static shared_ptr Combine(const GaussianFactorGraph& factors, const VariableSlots& variableSlots); + static shared_ptr Combine(const FactorGraph& factors, const VariableSlots& variableSlots); protected: diff --git a/linear/VectorBTree.h b/linear/VectorBTree.h index 8f993fefb..3f5ef486e 100644 --- a/linear/VectorBTree.h +++ b/linear/VectorBTree.h @@ -25,7 +25,7 @@ #include #include -#include +#include #include namespace gtsam { diff --git a/nonlinear/Makefile.am b/nonlinear/Makefile.am index d391aadac..6b871d53d 100644 --- a/nonlinear/Makefile.am +++ b/nonlinear/Makefile.am @@ -16,14 +16,16 @@ check_PROGRAMS = #---------------------------------------------------------------------------------------------------- # Lie Groups -headers += Key.h LieValues.h LieValues-inl.h TupleValues.h TupleValues-inl.h -check_PROGRAMS += tests/testLieValues tests/testKey +headers += LieValues.h LieValues-inl.h TupleValues.h TupleValues-inl.h +check_PROGRAMS += tests/testLieValues -# Nonlinear nonlinear +# Nonlinear nonlinear +headers += Key.h headers += NonlinearFactorGraph.h NonlinearFactorGraph-inl.h headers += NonlinearOptimizer-inl.h NonlinearOptimization.h NonlinearOptimization-inl.h NonlinearOptimizationParameters.h headers += NonlinearFactor.h sources += NonlinearOptimizer.cpp Ordering.cpp +check_PROGRAMS += tests/testKey # Nonlinear constraints headers += NonlinearEquality.h diff --git a/nonlinear/NonlinearFactor.h b/nonlinear/NonlinearFactor.h index 6087bdffb..962b6d748 100644 --- a/nonlinear/NonlinearFactor.h +++ b/nonlinear/NonlinearFactor.h @@ -26,7 +26,7 @@ #include #include -#include +#include #include #include #include @@ -138,7 +138,7 @@ namespace gtsam { * Create a symbolic factor using the given ordering to determine the * variable indices. */ - virtual Factor::shared_ptr symbolic(const Ordering& ordering) const = 0; + virtual IndexFactor::shared_ptr symbolic(const Ordering& ordering) const = 0; private: @@ -204,7 +204,7 @@ namespace gtsam { Base::print("parent"); } - /** Check if two factors are equal. Note type is Factor and needs cast. */ + /** Check if two factors are equal. Note type is IndexFactor and needs cast. */ bool equals(const NonlinearFactor1& f, double tol = 1e-9) const { return Base::noiseModel_->equals(*f.noiseModel_, tol) && (key_ == f.key_); } @@ -242,8 +242,8 @@ namespace gtsam { * Create a symbolic factor using the given ordering to determine the * variable indices. */ - virtual Factor::shared_ptr symbolic(const Ordering& ordering) const { - return Factor::shared_ptr(new Factor(ordering[key_])); + virtual IndexFactor::shared_ptr symbolic(const Ordering& ordering) const { + return IndexFactor::shared_ptr(new IndexFactor(ordering[key_])); } /* @@ -362,12 +362,12 @@ namespace gtsam { * Create a symbolic factor using the given ordering to determine the * variable indices. */ - virtual Factor::shared_ptr symbolic(const Ordering& ordering) const { + virtual IndexFactor::shared_ptr symbolic(const Ordering& ordering) const { const Index var1 = ordering[key1_], var2 = ordering[key2_]; if(var1 < var2) - return Factor::shared_ptr(new Factor(var1, var2)); + return IndexFactor::shared_ptr(new IndexFactor(var1, var2)); else - return Factor::shared_ptr(new Factor(var2, var1)); + return IndexFactor::shared_ptr(new IndexFactor(var2, var1)); } /** methods to retrieve both keys */ @@ -519,20 +519,20 @@ namespace gtsam { * Create a symbolic factor using the given ordering to determine the * variable indices. */ - virtual Factor::shared_ptr symbolic(const Ordering& ordering) const { + virtual IndexFactor::shared_ptr symbolic(const Ordering& ordering) const { const Index var1 = ordering[key1_], var2 = ordering[key2_], var3 = ordering[key3_]; if(var1 < var2 && var2 < var3) - return Factor::shared_ptr(new Factor(ordering[key1_], ordering[key2_], ordering[key3_])); + return IndexFactor::shared_ptr(new IndexFactor(ordering[key1_], ordering[key2_], ordering[key3_])); else if(var2 < var1 && var1 < var3) - return Factor::shared_ptr(new Factor(ordering[key2_], ordering[key2_], ordering[key3_])); + return IndexFactor::shared_ptr(new IndexFactor(ordering[key2_], ordering[key2_], ordering[key3_])); else if(var1 < var3 && var3 < var2) - return Factor::shared_ptr(new Factor(ordering[key1_], ordering[key3_], ordering[key2_])); + return IndexFactor::shared_ptr(new IndexFactor(ordering[key1_], ordering[key3_], ordering[key2_])); else if(var2 < var3 && var3 < var1) - return Factor::shared_ptr(new Factor(ordering[key2_], ordering[key3_], ordering[key1_])); + return IndexFactor::shared_ptr(new IndexFactor(ordering[key2_], ordering[key3_], ordering[key1_])); else if(var3 < var1 && var1 < var2) - return Factor::shared_ptr(new Factor(ordering[key3_], ordering[key1_], ordering[key2_])); + return IndexFactor::shared_ptr(new IndexFactor(ordering[key3_], ordering[key1_], ordering[key2_])); else if(var3 < var2 && var2 < var1) - return Factor::shared_ptr(new Factor(ordering[key3_], ordering[key2_], ordering[key1_])); + return IndexFactor::shared_ptr(new IndexFactor(ordering[key3_], ordering[key2_], ordering[key1_])); else assert(false); } diff --git a/nonlinear/NonlinearFactorGraph-inl.h b/nonlinear/NonlinearFactorGraph-inl.h index 6f9b82fe2..37ee92cb4 100644 --- a/nonlinear/NonlinearFactorGraph-inl.h +++ b/nonlinear/NonlinearFactorGraph-inl.h @@ -63,7 +63,7 @@ void NonlinearFactorGraph::print(const std::string& str) const { Ordering::shared_ptr NonlinearFactorGraph::orderingCOLAMD(const Values& config) const { // Create symbolic graph and initial (iterator) ordering - FactorGraph::shared_ptr symbolic; + SymbolicFactorGraph::shared_ptr symbolic; Ordering::shared_ptr ordering; boost::tie(symbolic,ordering) = this->symbolic(config); @@ -88,7 +88,7 @@ void NonlinearFactorGraph::print(const std::string& str) const { SymbolicFactorGraph::shared_ptr NonlinearFactorGraph::symbolic( const Values& config, const Ordering& ordering) const { // Generate the symbolic factor graph - SymbolicFactorGraph::shared_ptr symbolicfg(new FactorGraph); + SymbolicFactorGraph::shared_ptr symbolicfg(new SymbolicFactorGraph); symbolicfg->reserve(this->size()); BOOST_FOREACH(const sharedFactor& factor, this->factors_) { symbolicfg->push_back(factor->symbolic(ordering)); diff --git a/slam/saveGraph.h b/slam/saveGraph.h index 33674f607..f690edbf9 100644 --- a/slam/saveGraph.h +++ b/slam/saveGraph.h @@ -21,7 +21,7 @@ #include #include #include -#include +#include #include #include diff --git a/tests/testGaussianFactorGraph.cpp b/tests/testGaussianFactorGraph.cpp index cfac5fd46..efb5fbcc0 100644 --- a/tests/testGaussianFactorGraph.cpp +++ b/tests/testGaussianFactorGraph.cpp @@ -314,7 +314,7 @@ TEST( GaussianFactorGraph, eliminateAll ) GaussianBayesNet actual = *Inference::Eliminate(fg1); CHECK(assert_equal(expected,actual,tol)); - GaussianBayesNet actualET = *EliminationTree::Create(fg1)->eliminate(); + GaussianBayesNet actualET = *EliminationTree::Create(fg1)->eliminate(); CHECK(assert_equal(expected,actualET,tol)); } @@ -463,7 +463,7 @@ TEST( GaussianFactorGraph, CONSTRUCTOR_GaussianBayesNet ) TEST( GaussianFactorGraph, getOrdering) { Ordering original; original += "l1","x1","x2"; - FactorGraph symbolic(createGaussianFactorGraph(original)); + FactorGraph symbolic(createGaussianFactorGraph(original)); Permutation perm(*Inference::PermutationCOLAMD(VariableIndex<>(symbolic))); Ordering actual = original; actual.permuteWithInverse((*perm.inverse())); Ordering expected; expected += "l1","x2","x1"; @@ -491,7 +491,7 @@ TEST( GaussianFactorGraph, optimize ) // optimize the graph VectorValues actual = optimize(*Inference::Eliminate(fg)); - VectorValues actualET = optimize(*EliminationTree::Create(fg)->eliminate()); + VectorValues actualET = optimize(*EliminationTree::Create(fg)->eliminate()); // verify VectorValues expected = createCorrectDelta(ord); @@ -786,7 +786,7 @@ TEST( GaussianFactorGraph, constrained_simple ) // eliminate and solve VectorValues actual = optimize(*Inference::Eliminate(fg)); - VectorValues actualET = optimize(*EliminationTree::Create(fg)->eliminate()); + VectorValues actualET = optimize(*EliminationTree::Create(fg)->eliminate()); // verify VectorValues expected = createSimpleConstraintValues(); diff --git a/tests/testGaussianJunctionTree.cpp b/tests/testGaussianJunctionTree.cpp index db5ad1e16..494b5cd40 100644 --- a/tests/testGaussianJunctionTree.cpp +++ b/tests/testGaussianJunctionTree.cpp @@ -69,15 +69,15 @@ TEST( GaussianJunctionTree, constructor2 ) list::const_iterator child1it = child0it; ++child1it; GaussianJunctionTree::sharedClique child0 = *child0it; GaussianJunctionTree::sharedClique child1 = *child1it; - CHECK(assert_equal(frontal2, child0->frontal)); - CHECK(assert_equal(sep2, child0->separator)); - LONGS_EQUAL(4, child0->size()); - CHECK(assert_equal(frontal3, child0->children().front()->frontal)); - CHECK(assert_equal(sep3, child0->children().front()->separator)); - LONGS_EQUAL(2, child0->children().front()->size()); - CHECK(assert_equal(frontal4, child1->frontal)); - CHECK(assert_equal(sep4, child1->separator)); - LONGS_EQUAL(2, child1->size()); + CHECK(assert_equal(frontal2, child1->frontal)); + CHECK(assert_equal(sep2, child1->separator)); + LONGS_EQUAL(4, child1->size()); + CHECK(assert_equal(frontal3, child1->children().front()->frontal)); + CHECK(assert_equal(sep3, child1->children().front()->separator)); + LONGS_EQUAL(2, child1->children().front()->size()); + CHECK(assert_equal(frontal4, child0->frontal)); + CHECK(assert_equal(sep4, child0->separator)); + LONGS_EQUAL(2, child0->size()); } /* ************************************************************************* */ diff --git a/tests/testSymbolicBayesNet.cpp b/tests/testSymbolicBayesNet.cpp index b665982ad..f03a4adc8 100644 --- a/tests/testSymbolicBayesNet.cpp +++ b/tests/testSymbolicBayesNet.cpp @@ -26,7 +26,6 @@ using namespace boost::assign; #include #include #include -#include #include using namespace std; @@ -34,20 +33,20 @@ using namespace gtsam; using namespace example; //Symbol _B_('B', 0), _L_('L', 0); -//Conditional::shared_ptr -// B(new Conditional(_B_)), -// L(new Conditional(_L_, _B_)); +//IndexConditional::shared_ptr +// B(new IndexConditional(_B_)), +// L(new IndexConditional(_L_, _B_)); /* ************************************************************************* */ TEST( SymbolicBayesNet, constructor ) { Ordering o; o += "x2","l1","x1"; // Create manually - Conditional::shared_ptr - x2(new Conditional(o["x2"],o["l1"], o["x1"])), - l1(new Conditional(o["l1"],o["x1"])), - x1(new Conditional(o["x1"])); - BayesNet expected; + IndexConditional::shared_ptr + x2(new IndexConditional(o["x2"],o["l1"], o["x1"])), + l1(new IndexConditional(o["l1"],o["x1"])), + x1(new IndexConditional(o["x1"])); + BayesNet expected; expected.push_back(x2); expected.push_back(l1); expected.push_back(x1); diff --git a/tests/testSymbolicFactorGraph.cpp b/tests/testSymbolicFactorGraph.cpp index 13109caa8..bfee66673 100644 --- a/tests/testSymbolicFactorGraph.cpp +++ b/tests/testSymbolicFactorGraph.cpp @@ -24,7 +24,6 @@ using namespace boost::assign; #include #include -#include #include #include #include @@ -117,10 +116,10 @@ TEST( SymbolicFactorGraph, eliminateOne ) // eliminate VariableIndex<> varindex(fg); - Conditional::shared_ptr actual = Inference::EliminateOne(fg, varindex, o["x1"]); + IndexConditional::shared_ptr actual = Inference::EliminateOne(fg, varindex, o["x1"]); - // create expected symbolic Conditional - Conditional expected(o["x1"],o["l1"],o["x2"]); + // create expected symbolic IndexConditional + IndexConditional expected(o["x1"],o["l1"],o["x2"]); CHECK(assert_equal(expected,*actual)); } @@ -131,9 +130,9 @@ TEST( SymbolicFactorGraph, eliminate ) Ordering o; o += "x2","l1","x1"; // create expected Chordal bayes Net - Conditional::shared_ptr x2(new Conditional(o["x2"], o["l1"], o["x1"])); - Conditional::shared_ptr l1(new Conditional(o["l1"], o["x1"])); - Conditional::shared_ptr x1(new Conditional(o["x1"])); + IndexConditional::shared_ptr x2(new IndexConditional(o["x2"], o["l1"], o["x1"])); + IndexConditional::shared_ptr l1(new IndexConditional(o["l1"], o["x1"])); + IndexConditional::shared_ptr x1(new IndexConditional(o["x1"])); SymbolicBayesNet expected; expected.push_back(x2);