From 62b3db9535428475e93136bcabddd9b7888ad4ba Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Tue, 15 Feb 2011 16:22:35 +0000 Subject: [PATCH] Documentation for FactorBase, ConditionalBase, IndexFactor, IndexConditional --- gtsam/inference/ConditionalBase.h | 124 ++++++++++++++++++---------- gtsam/inference/FactorBase-inl.h | 3 +- gtsam/inference/FactorBase.h | 46 +++++++---- gtsam/inference/IndexConditional.h | 66 ++++++++------- gtsam/inference/IndexFactor.h | 127 +++++++++++++++++------------ gtsam/linear/JacobianFactor.cpp | 2 +- 6 files changed, 220 insertions(+), 148 deletions(-) diff --git a/gtsam/inference/ConditionalBase.h b/gtsam/inference/ConditionalBase.h index 2a2c462b8..4df461200 100644 --- a/gtsam/inference/ConditionalBase.h +++ b/gtsam/inference/ConditionalBase.h @@ -32,7 +32,15 @@ namespace gtsam { /** - * Base class for conditional densities + * Base class for conditional densities, templated on KEY type. This class + * provides storage for the keys involved in a conditional, and iterators and + * access to the frontal and separator keys. + * + * todo: Move permutation functions to IndexConditional. + * + * Derived classes *must* redefine the Factor and shared_ptr typedefs to refer + * to the associated factor type and shared_ptr type of the derived class. See + * IndexConditional and GaussianConditional for examples. * * We make it noncopyable so we enforce the fact that factors are * kept in pointer containers. To be safe, you should make them @@ -46,15 +54,36 @@ protected: /** The first nFrontal variables are frontal and the rest are parents. */ size_t nrFrontals_; + /** Debugging invariant that the keys should be in order, including that the + * conditioned variable is numbered lower than the parents. + */ + void assertInvariants() const; + public: typedef KEY Key; typedef ConditionalBase This; + + /** + * Typedef to the factor type that produces this conditional and that this + * conditional can be converted to using a factor constructor. Derived + * classes must redefine this. + */ typedef gtsam::FactorBase Factor; + + /** A shared_ptr to this class. Derived classes must redefine this. */ typedef boost::shared_ptr shared_ptr; + + /** Iterator over keys */ typedef typename Factor::iterator iterator; + + /** Const iterator over keys */ typedef typename Factor::const_iterator const_iterator; + + /** View of the frontal keys (call frontals()) */ typedef boost::iterator_range Frontals; + + /** View of the separator keys (call parents()) */ typedef boost::iterator_range Parents; /** Empty Constructor to make serialization possible */ @@ -135,62 +164,69 @@ public: return boost::make_iterator_range(beginParents(), endParents()); } /** print */ - void print(const std::string& s = "Conditional") const { - std::cout << s << " P("; - BOOST_FOREACH(Key key, frontals()) std::cout << " " << key; - if (nrParents()>0) std::cout << " |"; - BOOST_FOREACH(Key parent, parents()) std::cout << " " << parent; - std::cout << ")" << std::endl; - } + void print(const std::string& s = "Conditional") const; /** Permute the variables when only separator variables need to be permuted. * Returns true if any reordered variables appeared in the separator and * false if not. */ - bool permuteSeparatorWithInverse(const Permutation& inversePermutation) { -#ifndef NDEBUG - BOOST_FOREACH(Key key, frontals()) { assert(key == inversePermutation[key]); } -#endif - bool parentChanged = false; - BOOST_FOREACH(Key& parent, parents()) { - Key newParent = inversePermutation[parent]; - if(parent != newParent) { - parentChanged = true; - parent = newParent; - } - } - return parentChanged; - } + bool permuteSeparatorWithInverse(const Permutation& inversePermutation); /** * Permutes the Conditional, but for efficiency requires the permutation * to already be inverted. */ - void permuteWithInverse(const Permutation& inversePermutation) { - // The permutation may not move the separators into the frontals -#ifndef NDEBUG - BOOST_FOREACH(const Key frontal, this->frontals()) { - BOOST_FOREACH(const Key separator, this->parents()) { - assert(inversePermutation[frontal] < inversePermutation[separator]); - } - } -#endif - Factor::permuteWithInverse(inversePermutation); - } - -protected: - /** Debugging invariant that the keys should be in order, including that the - * conditioned variable is numbered lower than the parents. - */ - void assertInvariants() const; + void permuteWithInverse(const Permutation& inversePermutation); private: - /** Serialization function */ - friend class boost::serialization::access; - template - void serialize(ARCHIVE & ar, const unsigned int version) { + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE & ar, const unsigned int version) { ar & BOOST_SERIALIZATION_NVP(nrFrontals_); - } + } }; + +/* ************************************************************************* */ +template +void ConditionalBase::print(const std::string& s) const { + std::cout << s << " P("; + BOOST_FOREACH(Key key, frontals()) std::cout << " " << key; + if (nrParents()>0) std::cout << " |"; + BOOST_FOREACH(Key parent, parents()) std::cout << " " << parent; + std::cout << ")" << std::endl; +} + +/* ************************************************************************* */ +template +bool ConditionalBase::permuteSeparatorWithInverse(const Permutation& inversePermutation) { +#ifndef NDEBUG + BOOST_FOREACH(Key key, frontals()) { assert(key == inversePermutation[key]); } +#endif + bool parentChanged = false; + BOOST_FOREACH(Key& parent, parents()) { + Key newParent = inversePermutation[parent]; + if(parent != newParent) { + parentChanged = true; + parent = newParent; + } + } + return parentChanged; +} + +/* ************************************************************************* */ +template +void ConditionalBase::permuteWithInverse(const Permutation& inversePermutation) { + // The permutation may not move the separators into the frontals +#ifndef NDEBUG + BOOST_FOREACH(const Key frontal, this->frontals()) { + BOOST_FOREACH(const Key separator, this->parents()) { + assert(inversePermutation[frontal] < inversePermutation[separator]); + } + } +#endif + Factor::permuteWithInverse(inversePermutation); +} + } diff --git a/gtsam/inference/FactorBase-inl.h b/gtsam/inference/FactorBase-inl.h index 7dad0c180..5d61190ae 100644 --- a/gtsam/inference/FactorBase-inl.h +++ b/gtsam/inference/FactorBase-inl.h @@ -10,7 +10,7 @@ * -------------------------------------------------------------------------- */ /** - * @file Factor-inl.h + * @file FactorBase-inl.h * @brief * @author Richard Roberts * @created Sep 1, 2010 @@ -57,7 +57,6 @@ void FactorBase::print(const std::string& s) const { /* ************************************************************************* */ template -//template bool FactorBase::equals(const This& other, double tol) const { return keys_ == other.keys_; } diff --git a/gtsam/inference/FactorBase.h b/gtsam/inference/FactorBase.h index 9f25d0757..6d1ace326 100644 --- a/gtsam/inference/FactorBase.h +++ b/gtsam/inference/FactorBase.h @@ -10,11 +10,11 @@ * -------------------------------------------------------------------------- */ /** - * @file Factor.h - * @brief A simple factor class to use in a factor graph - * @brief factor + * @file FactorBase.h + * @brief The base class for all factors * @author Kai Ni * @author Frank Dellaert + * @author Richard Roberts */ // \callgraph @@ -36,18 +36,17 @@ namespace gtsam { template class ConditionalBase; /** - * A simple factor class to use in a factor graph. + * This is the base class for all factor types. It is templated on a KEY type, + * though currently only IndexFactor and IndexConditional derive from this + * class, using Index keys. This class does not store any data other than its + * keys. Derived classes store data such as matrices and probability tables. * - * We make it noncopyable so we enforce the fact that factors are - * kept in pointer containers. To be safe, you should make them - * immutable, i.e., practicing functional programming. However, this - * conflicts with efficiency as well, esp. in the case of incomplete - * QR factorization. A solution is still being sought. + * todo: Make NonlinearFactor derive from this too, which requires moving + * Combine, eliminate*, permute* and the sorted key invariant to IndexFactor. * - * A Factor is templated on a Values, for example VectorValues is a values structure of - * labeled vectors. This way, we can have factors that might be defined on discrete - * variables, continuous ones, or a combination of both. It is up to the config to - * provide the appropriate values at the appropriate time. + * Note that derived classes *must* redefine the Conditional and shared_ptr + * typedefs to refer to the associated conditional and shared_ptr types of the + * derived class. See IndexFactor, JacobianFactor, etc. for examples. */ template class FactorBase : public Testable > { @@ -56,17 +55,29 @@ public: typedef KEY Key; typedef FactorBase This; + + /** + * Typedef to the conditional type obtained by eliminating this factor. + * Derived classes must redefine this. + */ typedef gtsam::ConditionalBase Conditional; + + /** A shared_ptr to this class. Derived classes must redefine this. */ typedef boost::shared_ptr shared_ptr; + + /** Iterator over keys */ typedef std::vector::iterator iterator; + + /** Const iterator over keys */ typedef std::vector::const_iterator const_iterator; protected: + // The keys involved in this factor std::vector keys_; - /** Internal check to make sure keys are sorted. - * If NDEBUG is defined, this is empty and optimized out. */ + // Internal check to make sure keys are sorted. If NDEBUG is defined, this + // is empty and optimized out. void assertInvariants() const; public: @@ -150,16 +161,15 @@ public: void print(const std::string& s = "Factor") const; /** check equality */ -// template bool equals(const This& other, double tol = 1e-9) const; /** - * return keys in order as created + * @return keys involved in this factor */ const std::vector& keys() const { return keys_; } /** - * @return the number of nodes the factor connects + * @return the number of variables involved in this factor */ size_t size() const { return keys_.size(); } diff --git a/gtsam/inference/IndexConditional.h b/gtsam/inference/IndexConditional.h index f70013943..c6d19eb63 100644 --- a/gtsam/inference/IndexConditional.h +++ b/gtsam/inference/IndexConditional.h @@ -23,46 +23,54 @@ namespace gtsam { -class IndexConditional : public ConditionalBase { + /** + * IndexConditional serves two purposes. It is the base class for + * GaussianConditional, and also functions as a symbolic conditional with + * Index keys, produced by symbolic elimination of IndexFactor. + * + * It derives from ConditionalBase with a key type of Index, which is an + * unsigned integer. + */ + class IndexConditional : public ConditionalBase { -public: + public: - typedef IndexConditional This; - typedef ConditionalBase Base; - typedef IndexFactor Factor; - typedef boost::shared_ptr shared_ptr; + typedef IndexConditional This; + typedef ConditionalBase Base; + typedef IndexFactor Factor; + typedef boost::shared_ptr shared_ptr; - /** Empty Constructor to make serialization possible */ - IndexConditional() {} + /** Empty Constructor to make serialization possible */ + IndexConditional() {} - /** No parents */ - IndexConditional(Index j) : Base(j) {} + /** No parents */ + IndexConditional(Index j) : Base(j) {} - /** Single parent */ - IndexConditional(Index j, Index parent) : Base(j, parent) {} + /** Single parent */ + IndexConditional(Index j, Index parent) : Base(j, parent) {} - /** Two parents */ - IndexConditional(Index j, Index parent1, Index parent2) : Base(j, parent1, parent2) {} + /** 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) {} + /** 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 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); } + /** 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); } + /** 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); } - /** Convert to a factor */ - IndexFactor::shared_ptr toFactor() const { return IndexFactor::shared_ptr(new IndexFactor(*this)); } + /** Convert to a factor */ + IndexFactor::shared_ptr toFactor() const { return IndexFactor::shared_ptr(new IndexFactor(*this)); } -}; + }; } diff --git a/gtsam/inference/IndexFactor.h b/gtsam/inference/IndexFactor.h index 15d4b9a91..2f2c7edff 100644 --- a/gtsam/inference/IndexFactor.h +++ b/gtsam/inference/IndexFactor.h @@ -22,66 +22,85 @@ 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(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) {} - - /** Construct n-way factor */ - IndexFactor(const std::set& js) : Base(js) {} + // Forward declaration of IndexConditional + class IndexConditional; /** - * Combine and eliminate several factors. + * IndexFactor serves two purposes. It is the base class for all linear + * factors (GaussianFactor, JacobianFactor, HessianFactor), and also + * functions as a symbolic factor with Index keys, used to do symbolic + * elimination by JunctionTree. + * + * This class provides symbolic elimination, via the CombineAndEliminate + * function. Combine and eliminate can also be called separately, but for + * this and derived classes calling them separately generally does extra + * work. + * + * It derives from FactorBase with a key type of Index, which is an unsigned + * integer. */ - static std::pair::shared_ptr, shared_ptr> CombineAndEliminate( - const FactorGraph& factors, size_t nrFrontals=1); + class IndexFactor : public FactorBase { - /** Create a combined joint factor (new style for EliminationTree). */ - static shared_ptr - Combine(const FactorGraph& factors, const FastMap >& variableSlots); + public: - /** - * eliminate the first variable involved in this factor - * @return a conditional on the eliminated variable - */ - boost::shared_ptr eliminateFirst(); + typedef IndexFactor This; + typedef FactorBase Base; - /** - * eliminate the first nrFrontals frontal variables. - */ - boost::shared_ptr > eliminate(size_t nrFrontals = 1); + /** Elimination produces an IndexConditional */ + typedef IndexConditional Conditional; -}; + /** Overriding the shared_ptr typedef */ + typedef boost::shared_ptr shared_ptr; + + /** Copy constructor */ + IndexFactor(const This& f) : Base(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) {} + + /** Construct n-way factor */ + IndexFactor(const std::set& js) : Base(js) {} + + /** + * Combine and eliminate several factors. + */ + static std::pair::shared_ptr, shared_ptr> CombineAndEliminate( + const FactorGraph& factors, size_t nrFrontals=1); + + /** Create a combined joint factor (new style for EliminationTree). */ + static 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(); + + /** + * eliminate the first nrFrontals frontal variables. + */ + boost::shared_ptr > eliminate(size_t nrFrontals = 1); + + }; } diff --git a/gtsam/linear/JacobianFactor.cpp b/gtsam/linear/JacobianFactor.cpp index fa9f0713b..0c66ace99 100644 --- a/gtsam/linear/JacobianFactor.cpp +++ b/gtsam/linear/JacobianFactor.cpp @@ -52,7 +52,7 @@ namespace gtsam { /* ************************************************************************* */ inline void JacobianFactor::assertInvariants() const { #ifndef NDEBUG - IndexFactor::assertInvariants(); + IndexFactor::assertInvariants(); // The base class checks for sorted keys assert((keys_.size() == 0 && Ab_.size1() == 0 && Ab_.nBlocks() == 0) || keys_.size()+1 == Ab_.nBlocks()); assert(firstNonzeroBlocks_.size() == Ab_.size1()); for(size_t i=0; i