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);