From da1b7f92f2d73dff90491bf15fadd3d6eac21f69 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 1 Jun 2019 16:46:17 -0400 Subject: [PATCH] Modernized enable_if, re-grouped methods to add factors --- gtsam/inference/FactorGraph.h | 586 ++++++++++++++++++---------------- 1 file changed, 305 insertions(+), 281 deletions(-) diff --git a/gtsam/inference/FactorGraph.h b/gtsam/inference/FactorGraph.h index 0ecfd87f1..ed14f1863 100644 --- a/gtsam/inference/FactorGraph.h +++ b/gtsam/inference/FactorGraph.h @@ -22,350 +22,374 @@ #pragma once -#include #include +#include #include #include // for Eigen::aligned_allocator -#include -#include #include #include #include +#include +#include +#include #include #include namespace gtsam { +/// Define collection type: +typedef FastVector FactorIndices; - // Forward declarations - template class BayesTree; +// Forward declarations +template +class BayesTree; - /** Helper */ - template - class CRefCallPushBack - { - C& obj; - public: - CRefCallPushBack(C& obj) : obj(obj) {} - template - void operator()(const A& a) { obj.push_back(a); } - }; +/** Helper */ +template +class CRefCallPushBack { + C& obj; - /** Helper */ - template - class RefCallPushBack - { - C& obj; - public: - RefCallPushBack(C& obj) : obj(obj) {} - template - void operator()(A& a) { obj.push_back(a); } - }; + public: + explicit CRefCallPushBack(C& obj) : obj(obj) {} + template + void operator()(const A& a) { + obj.push_back(a); + } +}; - /** Helper */ - template - class CRefCallAddCopy - { - C& obj; - public: - CRefCallAddCopy(C& obj) : obj(obj) {} - template - void operator()(const A& a) { obj.addCopy(a); } - }; +/** Helper */ +template +class RefCallPushBack { + C& obj; + + public: + explicit RefCallPushBack(C& obj) : obj(obj) {} + template + void operator()(A& a) { + obj.push_back(a); + } +}; + +/** Helper */ +template +class CRefCallAddCopy { + C& obj; + + public: + explicit CRefCallAddCopy(C& obj) : obj(obj) {} + template + void operator()(const A& a) { + obj.addCopy(a); + } +}; + +/** + * A factor graph is a bipartite graph with factor nodes connected to variable + * nodes. In this class, however, only factor nodes are kept around. + * \nosubgrouping + */ +template +class FactorGraph { + public: + typedef FACTOR FactorType; ///< factor type + typedef boost::shared_ptr + sharedFactor; ///< Shared pointer to a factor + typedef sharedFactor value_type; + typedef typename FastVector::iterator iterator; + typedef typename FastVector::const_iterator const_iterator; + + private: + typedef FactorGraph This; ///< Typedef for this class + typedef boost::shared_ptr + shared_ptr; ///< Shared pointer for this class + + /// Check if a DERIVEDFACTOR is in fact derived from FactorType. + template + using IsDerived = typename std::enable_if< + std::is_base_of::value>::type; + + /// Check if T has a value_type derived from FactorType. + template + using HasDerivedValueType = typename std::enable_if< + std::is_base_of::value>::type; + + /// Check if T has a value_type derived from FactorType. + template + using HasDerivedElementType = typename std::enable_if::value>::type; + + protected: + /** concept check, makes sure FACTOR defines print and equals */ + GTSAM_CONCEPT_TESTABLE_TYPE(FACTOR) + + /** Collection of factors */ + FastVector factors_; + + /// @name Standard Constructors + /// @{ + + /** Default constructor */ + FactorGraph() {} + + /** Constructor from iterator over factors (shared_ptr or plain objects) */ + template + FactorGraph(ITERATOR firstFactor, ITERATOR lastFactor) { + push_back(firstFactor, lastFactor); + } + + /** Construct from container of factors (shared_ptr or plain objects) */ + template + explicit FactorGraph(const CONTAINER& factors) { + push_back(factors); + } + + /// @} + + public: + /// @name Adding Single Factors + /// @{ /** - * A factor graph is a bipartite graph with factor nodes connected to variable nodes. - * In this class, however, only factor nodes are kept around. - * \nosubgrouping + * Reserve space for the specified number of factors if you know in + * advance how many there will be (works like FastVector::reserve). */ - template - class FactorGraph { + void reserve(size_t size) { factors_.reserve(size); } - public: - typedef FACTOR FactorType; ///< factor type - typedef boost::shared_ptr sharedFactor; ///< Shared pointer to a factor - typedef sharedFactor value_type; - typedef typename FastVector::iterator iterator; - typedef typename FastVector::const_iterator const_iterator; + /// Add a factor directly using a shared_ptr. + template + IsDerived push_back(boost::shared_ptr factor) { + factors_.push_back(boost::shared_ptr(factor)); + } - private: - typedef FactorGraph This; ///< Typedef for this class - typedef boost::shared_ptr shared_ptr; ///< Shared pointer for this class + /// Emplace a shared pointer to factor of given type. + template + IsDerived emplace_shared(Args&&... args) { + factors_.push_back(boost::allocate_shared( + Eigen::aligned_allocator(), + std::forward(args)...)); + } - protected: - /** concept check, makes sure FACTOR defines print and equals */ - GTSAM_CONCEPT_TESTABLE_TYPE(FACTOR) + /** + * Add a factor by value, will be copy-constructed (use push_back with a + * shared_ptr to avoid the copy). + */ + template + IsDerived push_back(const DERIVEDFACTOR& factor) { + factors_.push_back(boost::allocate_shared( + Eigen::aligned_allocator(), factor)); + } - /** Collection of factors */ - FastVector factors_; + /// `add` is a synonym for push_back. + template + IsDerived add(boost::shared_ptr factor) { + push_back(factor); + } - /// @name Standard Constructors - /// @{ + /// `+=` works well with boost::assign list inserter. + template + typename std::enable_if< + std::is_base_of::value, + boost::assign::list_inserter>>::type + operator+=(boost::shared_ptr factor) { + return boost::assign::make_list_inserter(RefCallPushBack(*this))( + factor); + } - /** Default constructor */ - FactorGraph() {} + /// @} + /// @name Adding via iterators + /// @{ - /** Constructor from iterator over factors (shared_ptr or plain objects) */ - template - FactorGraph(ITERATOR firstFactor, ITERATOR lastFactor) { push_back(firstFactor, lastFactor); } + /** + * Push back many factors with an iterator over shared_ptr (factors are not + * copied) + */ + template + HasDerivedElementType push_back(ITERATOR firstFactor, + ITERATOR lastFactor) { + factors_.insert(end(), firstFactor, lastFactor); + } - /** Construct from container of factors (shared_ptr or plain objects) */ - template - explicit FactorGraph(const CONTAINER& factors) { push_back(factors); } + /// Push back many factors with an iterator (factors are copied) + template + HasDerivedValueType push_back(ITERATOR firstFactor, + ITERATOR lastFactor) { + for (ITERATOR f = firstFactor; f != lastFactor; ++f) push_back(*f); + } - /// @} - /// @name Advanced Constructors - /// @{ + /// @} + /// @name Adding via container + /// @{ - // TODO: are these needed? + /** + * Push back many factors as shared_ptr's in a container (factors are not + * copied) + */ + template + HasDerivedElementType push_back(const CONTAINER& container) { + push_back(container.begin(), container.end()); + } - ///** - // * @brief Constructor from a Bayes net - // * @param bayesNet the Bayes net to convert, type CONDITIONAL must yield compatible factor - // * @return a factor graph with all the conditionals, as factors - // */ - //template - //FactorGraph(const BayesNet& bayesNet); + /// Push back non-pointer objects in a container (factors are copied). + template + HasDerivedValueType push_back(const CONTAINER& container) { + push_back(container.begin(), container.end()); + } - ///** convert from Bayes tree */ - //template - //FactorGraph(const BayesTree& bayesTree); + /** + * Add a factor or container of factors, including STL collections, + * BayesTrees, etc. + */ + template + void add(const FACTOR_OR_CONTAINER& factorOrContainer) { + push_back(factorOrContainer); + } - ///** convert from a derived type */ - //template - //FactorGraph(const FactorGraph& factors) { - // factors_.assign(factors.begin(), factors.end()); - //} + /** + * Add a factor or container of factors, including STL collections, + * BayesTrees, etc. + */ + template + boost::assign::list_inserter> operator+=( + const FACTOR_OR_CONTAINER& factorOrContainer) { + return boost::assign::make_list_inserter(CRefCallPushBack(*this))( + factorOrContainer); + } - /// @} + /// @} + /// @name Specialized versions + /// @{ - public: - /// @name Adding Factors - /// @{ + /** + * Push back a BayesTree as a collection of factors. + * NOTE: This should be hidden in derived classes in favor of a + * type-specialized version that calls this templated function. + */ + template + typename std::enable_if< + std::is_base_of::value>::type + push_back(const BayesTree& bayesTree) { + bayesTree.addFactorsToGraph(*this); + } - /** - * Reserve space for the specified number of factors if you know in - * advance how many there will be (works like FastVector::reserve). - */ - void reserve(size_t size) { factors_.reserve(size); } + /// @} + /// @name Testable + /// @{ - // TODO: are these needed? + /** print out graph */ + void print(const std::string& s = "FactorGraph", + const KeyFormatter& formatter = DefaultKeyFormatter) const; - /** Add a factor directly using a shared_ptr */ - template - typename std::enable_if::value>::type - push_back(boost::shared_ptr factor) { - factors_.push_back(boost::shared_ptr(factor)); } + /** Check equality */ + bool equals(const This& fg, double tol = 1e-9) const; + /// @} - /** Add a factor directly using a shared_ptr */ - void push_back(const sharedFactor& factor) { - factors_.push_back(factor); } + public: + /// @name Standard Interface + /// @{ - /** Emplace a factor */ - template - typename std::enable_if::value>::type - emplace_shared(Args&&... args) { - factors_.push_back(boost::allocate_shared(Eigen::aligned_allocator(), std::forward(args)...)); - } + /** return the number of factors (including any null factors set by remove() + * ). */ + size_t size() const { return factors_.size(); } - /** push back many factors with an iterator over shared_ptr (factors are not copied) */ - template - typename std::enable_if::value>::type - push_back(ITERATOR firstFactor, ITERATOR lastFactor) { - factors_.insert(end(), firstFactor, lastFactor); } + /** Check if the graph is empty (null factors set by remove() will cause + * this to return false). */ + bool empty() const { return factors_.empty(); } - /** push back many factors as shared_ptr's in a container (factors are not copied) */ - template - typename std::enable_if::value>::type - push_back(const CONTAINER& container) { - push_back(container.begin(), container.end()); - } + /** Get a specific factor by index (this checks array bounds and may throw + * an exception, as opposed to operator[] which does not). + */ + const sharedFactor at(size_t i) const { return factors_.at(i); } - /** push back a BayesTree as a collection of factors. NOTE: This should be hidden in derived - * classes in favor of a type-specialized version that calls this templated function. */ - template - typename std::enable_if::value>::type - push_back(const BayesTree& bayesTree) { - bayesTree.addFactorsToGraph(*this); - } + /** Get a specific factor by index (this checks array bounds and may throw + * an exception, as opposed to operator[] which does not). + */ + sharedFactor& at(size_t i) { return factors_.at(i); } -//#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4 - /** Add a factor by value, will be copy-constructed (use push_back with a shared_ptr to avoid - * the copy). */ - template - typename std::enable_if::value>::type - push_back(const DERIVEDFACTOR& factor) { - factors_.push_back(boost::allocate_shared(Eigen::aligned_allocator(), factor)); - } -//#endif + /** Get a specific factor by index (this does not check array bounds, as + * opposed to at() which does). + */ + const sharedFactor operator[](size_t i) const { return at(i); } - /** push back many factors with an iterator over plain factors (factors are copied) */ - template - typename std::enable_if::value>::type - push_back(ITERATOR firstFactor, ITERATOR lastFactor) { - for (ITERATOR f = firstFactor; f != lastFactor; ++f) - push_back(*f); - } + /** Get a specific factor by index (this does not check array bounds, as + * opposed to at() which does). + */ + sharedFactor& operator[](size_t i) { return at(i); } - /** push back many factors as non-pointer objects in a container (factors are copied) */ - template - typename std::enable_if::value>::type - push_back(const CONTAINER& container) { - push_back(container.begin(), container.end()); - } + /** Iterator to beginning of factors. */ + const_iterator begin() const { return factors_.begin(); } - /** Add a factor directly using a shared_ptr */ - template - typename std::enable_if::value, - boost::assign::list_inserter > >::type - operator+=(boost::shared_ptr factor) { - return boost::assign::make_list_inserter(RefCallPushBack(*this))(factor); - } + /** Iterator to end of factors. */ + const_iterator end() const { return factors_.end(); } - /** Add a factor directly using a shared_ptr */ - boost::assign::list_inserter > - operator+=(const sharedFactor& factor) { - return boost::assign::make_list_inserter(CRefCallPushBack(*this))(factor); - } + /** Get the first factor */ + sharedFactor front() const { return factors_.front(); } - /** Add a factor or container of factors, including STL collections, BayesTrees, etc. */ - template - boost::assign::list_inserter > - operator+=(const FACTOR_OR_CONTAINER& factorOrContainer) { - return boost::assign::make_list_inserter(CRefCallPushBack(*this))(factorOrContainer); - } + /** Get the last factor */ + sharedFactor back() const { return factors_.back(); } - /** Add a factor directly using a shared_ptr */ - template - typename std::enable_if::value>::type - add(boost::shared_ptr factor) { - push_back(factor); - } + /// @} + /// @name Modifying Factor Graphs (imperative, discouraged) + /// @{ - /** Add a factor directly using a shared_ptr */ - void add(const sharedFactor& factor) { - push_back(factor); - } + /** non-const STL-style begin() */ + iterator begin() { return factors_.begin(); } - /** Add a factor or container of factors, including STL collections, BayesTrees, etc. */ - template - void add(const FACTOR_OR_CONTAINER& factorOrContainer) { - push_back(factorOrContainer); - } + /** non-const STL-style end() */ + iterator end() { return factors_.end(); } - /// @} - /// @name Testable - /// @{ + /** Directly resize the number of factors in the graph. If the new size is + * less than the original, factors at the end will be removed. If the new + * size is larger than the original, null factors will be appended. + */ + void resize(size_t size) { factors_.resize(size); } - /** print out graph */ - void print(const std::string& s = "FactorGraph", - const KeyFormatter& formatter = DefaultKeyFormatter) const; + /** delete factor without re-arranging indexes by inserting a NULL pointer + */ + void remove(size_t i) { factors_[i].reset(); } - /** Check equality */ - bool equals(const This& fg, double tol = 1e-9) const; - /// @} + /** replace a factor by index */ + void replace(size_t index, sharedFactor factor) { at(index) = factor; } - public: - /// @name Standard Interface - /// @{ + /** Erase factor and rearrange other factors to take up the empty space */ + iterator erase(iterator item) { return factors_.erase(item); } - /** return the number of factors (including any null factors set by remove() ). */ - size_t size() const { return factors_.size(); } + /** Erase factors and rearrange other factors to take up the empty space */ + iterator erase(iterator first, iterator last) { + return factors_.erase(first, last); + } - /** Check if the graph is empty (null factors set by remove() will cause this to return false). */ - bool empty() const { return factors_.empty(); } + /// @} + /// @name Advanced Interface + /// @{ - /** Get a specific factor by index (this checks array bounds and may throw an exception, as - * opposed to operator[] which does not). - */ - const sharedFactor at(size_t i) const { return factors_.at(i); } + /** return the number of non-null factors */ + size_t nrFactors() const; - /** Get a specific factor by index (this checks array bounds and may throw an exception, as - * opposed to operator[] which does not). - */ - sharedFactor& at(size_t i) { return factors_.at(i); } + /** Potentially slow function to return all keys involved, sorted, as a set + */ + KeySet keys() const; - /** Get a specific factor by index (this does not check array bounds, as opposed to at() which - * does). - */ - const sharedFactor operator[](size_t i) const { return at(i); } + /** Potentially slow function to return all keys involved, sorted, as a + * vector + */ + KeyVector keyVector() const; - /** Get a specific factor by index (this does not check array bounds, as opposed to at() which - * does). - */ - sharedFactor& operator[](size_t i) { return at(i); } + /** MATLAB interface utility: Checks whether a factor index idx exists in + * the graph and is a live pointer */ + inline bool exists(size_t idx) const { return idx < size() && at(idx); } - /** Iterator to beginning of factors. */ - const_iterator begin() const { return factors_.begin();} + private: + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE& ar, const unsigned int /*version*/) { + ar& BOOST_SERIALIZATION_NVP(factors_); + } - /** Iterator to end of factors. */ - const_iterator end() const { return factors_.end(); } - - /** Get the first factor */ - sharedFactor front() const { return factors_.front(); } - - /** Get the last factor */ - sharedFactor back() const { return factors_.back(); } - - /// @} - /// @name Modifying Factor Graphs (imperative, discouraged) - /// @{ - - /** non-const STL-style begin() */ - iterator begin() { return factors_.begin();} - - /** non-const STL-style end() */ - iterator end() { return factors_.end(); } - - /** Directly resize the number of factors in the graph. If the new size is less than the - * original, factors at the end will be removed. If the new size is larger than the original, - * null factors will be appended. - */ - void resize(size_t size) { factors_.resize(size); } - - /** delete factor without re-arranging indexes by inserting a NULL pointer */ - void remove(size_t i) { factors_[i].reset();} - - /** replace a factor by index */ - void replace(size_t index, sharedFactor factor) { at(index) = factor; } - - /** Erase factor and rearrange other factors to take up the empty space */ - iterator erase(iterator item) { return factors_.erase(item); } - - /** Erase factors and rearrange other factors to take up the empty space */ - iterator erase(iterator first, iterator last) { return factors_.erase(first, last); } - - /// @} - /// @name Advanced Interface - /// @{ - - /** return the number of non-null factors */ - size_t nrFactors() const; - - /** Potentially slow function to return all keys involved, sorted, as a set */ - KeySet keys() const; - - /** Potentially slow function to return all keys involved, sorted, as a vector */ - KeyVector keyVector() const; - - /** MATLAB interface utility: Checks whether a factor index idx exists in the graph and is a live pointer */ - inline bool exists(size_t idx) const { return idx < size() && at(idx); } - - private: - - /** Serialization function */ - friend class boost::serialization::access; - template - void serialize(ARCHIVE & ar, const unsigned int /*version*/) { - ar & BOOST_SERIALIZATION_NVP(factors_); - } - - /// @} - - }; // FactorGraph - -} // namespace gtsam + /// @} +}; // FactorGraph +} // namespace gtsam #include