diff --git a/gtsam/inference/Conditional.h b/gtsam/inference/Conditional.h index 8363c1800..fa75df83f 100644 --- a/gtsam/inference/Conditional.h +++ b/gtsam/inference/Conditional.h @@ -16,196 +16,249 @@ */ // \callgraph - #pragma once -#include -#include // for noncopyable -#include -#include -#include -#include #include +#include +#include + namespace gtsam { -/** - * 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. - * - * 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. - * \nosubgrouping - */ -template -class Conditional: public gtsam::Factor { - -private: - - /** Create keys by adding key in front */ - template - static std::vector MakeKeys(KEY key, ITERATOR firstParent, ITERATOR lastParent) { - std::vector keys((lastParent - firstParent) + 1); - std::copy(firstParent, lastParent, keys.begin() + 1); - keys[0] = key; - return keys; - } - -protected: - - /** The first nFrontal variables are frontal and the rest are parents. */ - size_t nrFrontals_; - - // Calls the base class assertInvariants, which checks for unique keys - void assertInvariants() const { Factor::assertInvariants(); } - -public: - - typedef KEY KeyType; - typedef Conditional This; - typedef Factor Base; - /** - * 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. + * 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. + * + * 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. + * \nosubgrouping */ - typedef gtsam::Factor FactorType; + template + class Conditional: public gtsam::Factor { - /** A shared_ptr to this class. Derived classes must redefine this. */ - typedef boost::shared_ptr shared_ptr; + private: - /** Iterator over keys */ - typedef typename FactorType::iterator iterator; + /** Create keys by adding key in front */ + template + static std::vector MakeKeys(KEY key, ITERATOR firstParent, + ITERATOR lastParent) { + std::vector keys((lastParent - firstParent) + 1); + std::copy(firstParent, lastParent, keys.begin() + 1); + keys[0] = key; + return keys; + } - /** Const iterator over keys */ - typedef typename FactorType::const_iterator const_iterator; + protected: - /** View of the frontal keys (call frontals()) */ - typedef boost::iterator_range Frontals; + /** The first nFrontal variables are frontal and the rest are parents. */ + size_t nrFrontals_; - /** View of the separator keys (call parents()) */ - typedef boost::iterator_range Parents; + // Calls the base class assertInvariants, which checks for unique keys + void assertInvariants() const { + Factor::assertInvariants(); + } - /// @name Standard Constructors - /// @{ + public: - /** Empty Constructor to make serialization possible */ - Conditional() : nrFrontals_(0) { assertInvariants(); } + typedef KEY KeyType; + typedef Conditional This; + typedef Factor Base; - /** No parents */ - Conditional(KeyType key) : FactorType(key), nrFrontals_(1) { assertInvariants(); } + /** + * 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::Factor FactorType; - /** Single parent */ - Conditional(KeyType key, KeyType parent) - : FactorType(key, parent), nrFrontals_(1) { assertInvariants(); } + /** A shared_ptr to this class. Derived classes must redefine this. */ + typedef boost::shared_ptr shared_ptr; - /** Two parents */ - Conditional(KeyType key, KeyType parent1, KeyType parent2) - : FactorType(key, parent1, parent2), nrFrontals_(1) { assertInvariants(); } + /** Iterator over keys */ + typedef typename FactorType::iterator iterator; - /** Three parents */ - Conditional(KeyType key, KeyType parent1, KeyType parent2, KeyType parent3) - : FactorType(key, parent1, parent2, parent3), nrFrontals_(1) { assertInvariants(); } + /** Const iterator over keys */ + typedef typename FactorType::const_iterator const_iterator; - /// @} - /// @name Advanced Constructors - /// @{ + /** View of the frontal keys (call frontals()) */ + typedef boost::iterator_range Frontals; - /** Constructor from a frontal variable and a vector of parents */ - Conditional(KeyType key, const std::vector& parents) : - FactorType(MakeKeys(key, parents.begin(), parents.end())), nrFrontals_(1) { - assertInvariants(); - } + /** View of the separator keys (call parents()) */ + typedef boost::iterator_range Parents; - /** Constructor from keys and nr of frontal variables */ - Conditional(const std::vector& keys, size_t nrFrontals) : - FactorType(keys), nrFrontals_(nrFrontals) { - assertInvariants(); - } + /// @name Standard Constructors + /// @{ - /// @} - /// @name Testable - /// @{ + /** Empty Constructor to make serialization possible */ + Conditional() : + nrFrontals_(0) { + assertInvariants(); + } - /** print with optional formatter */ - void print(const std::string& s = "Conditional", const IndexFormatter& formatter = DefaultIndexFormatter) const; + /** No parents */ + Conditional(KeyType key) : + FactorType(key), nrFrontals_(1) { + assertInvariants(); + } - /** check equality */ - template - bool equals(const DERIVED& c, double tol = 1e-9) const { - return nrFrontals_ == c.nrFrontals_ && FactorType::equals(c, tol); } + /** Single parent */ + Conditional(KeyType key, KeyType parent) : + FactorType(key, parent), nrFrontals_(1) { + assertInvariants(); + } - /// @} - /// @name Standard Interface - /// @{ + /** Two parents */ + Conditional(KeyType key, KeyType parent1, KeyType parent2) : + FactorType(key, parent1, parent2), nrFrontals_(1) { + assertInvariants(); + } - /** return the number of frontals */ - size_t nrFrontals() const { return nrFrontals_; } + /** Three parents */ + Conditional(KeyType key, KeyType parent1, KeyType parent2, KeyType parent3) : + FactorType(key, parent1, parent2, parent3), nrFrontals_(1) { + assertInvariants(); + } - /** return the number of parents */ - size_t nrParents() const { return FactorType::size() - nrFrontals_; } + /// @} + /// @name Advanced Constructors + /// @{ - /** Special accessor when there is only one frontal variable. */ - KeyType firstFrontalKey() const { assert(nrFrontals_>0); return FactorType::front(); } - KeyType lastFrontalKey() const { assert(nrFrontals_>0); return *(endFrontals()-1); } + /** Constructor from a frontal variable and a vector of parents */ + Conditional(KeyType key, const std::vector& parents) : + FactorType(MakeKeys(key, parents.begin(), parents.end())), nrFrontals_( + 1) { + assertInvariants(); + } - /** return a view of the frontal keys */ - Frontals frontals() const { - return boost::make_iterator_range(beginFrontals(), endFrontals()); } + /** Constructor from keys and nr of frontal variables */ + Conditional(const std::vector& keys, size_t nrFrontals) : + FactorType(keys), nrFrontals_(nrFrontals) { + assertInvariants(); + } - /** return a view of the parent keys */ - Parents parents() const { - return boost::make_iterator_range(beginParents(), endParents()); } + /** Constructor from keys and nr of frontal variables */ + Conditional(const std::list& keys, size_t nrFrontals) : + FactorType(keys.begin(),keys.end()), nrFrontals_(nrFrontals) { + assertInvariants(); + } - /** Iterators over frontal and parent variables. */ - const_iterator beginFrontals() const { return FactorType::begin(); } /// + bool equals(const DERIVED& c, double tol = 1e-9) const { + return nrFrontals_ == c.nrFrontals_ && FactorType::equals(c, tol); + } - ///TODO: comment - boost::iterator_range frontals() { - return boost::make_iterator_range(beginFrontals(), endFrontals()); } + /// @} + /// @name Standard Interface + /// @{ - ///TODO: comment - boost::iterator_range parents() { - return boost::make_iterator_range(beginParents(), endParents()); } + /** return the number of frontals */ + size_t nrFrontals() const { + return nrFrontals_; + } -private: - /** Serialization function */ - friend class boost::serialization::access; - template - void serialize(ARCHIVE & ar, const unsigned int version) { - ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); - ar & BOOST_SERIALIZATION_NVP(nrFrontals_); + /** return the number of parents */ + size_t nrParents() const { + return FactorType::size() - nrFrontals_; + } + + /** Special accessor when there is only one frontal variable. */ + KeyType firstFrontalKey() const { + assert(nrFrontals_>0); + return FactorType::front(); + } + KeyType lastFrontalKey() const { + assert(nrFrontals_>0); + return *(endFrontals() - 1); + } + + /** return a view of the frontal keys */ + Frontals frontals() const { + return boost::make_iterator_range(beginFrontals(), endFrontals()); + } + + /** return a view of the parent keys */ + Parents parents() const { + return boost::make_iterator_range(beginParents(), endParents()); + } + + /** Iterators over frontal and parent variables. */ + const_iterator beginFrontals() const { + return FactorType::begin(); + } /// frontals() { + return boost::make_iterator_range(beginFrontals(), endFrontals()); + } + + ///TODO: comment + boost::iterator_range parents() { + return boost::make_iterator_range(beginParents(), endParents()); + } + + private: + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE & ar, const unsigned int version) { + ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); + ar & BOOST_SERIALIZATION_NVP(nrFrontals_); + } + + /// @} + + }; + + /* ************************************************************************* */ + template + void Conditional::print(const std::string& s, + const IndexFormatter& formatter) const { + std::cout << s << " P("; + BOOST_FOREACH(KeyType key, frontals()) + std::cout << " " << formatter(key); + if (nrParents() > 0) + std::cout << " |"; + BOOST_FOREACH(KeyType parent, parents()) + std::cout << " " << formatter(parent); + std::cout << ")" << std::endl; } - /// @} - -}; - - -/* ************************************************************************* */ -template -void Conditional::print(const std::string& s, const IndexFormatter& formatter) const { - std::cout << s << " P("; - BOOST_FOREACH(KeyType key, frontals()) std::cout << " " << formatter(key); - if (nrParents()>0) std::cout << " |"; - BOOST_FOREACH(KeyType parent, parents()) std::cout << " " << formatter(parent); - std::cout << ")" << std::endl; -} - } // gtsam