FactorGraph.h

Go to the documentation of this file.

/* ----------------------------------------------------------------------------

 * 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

 * -------------------------------------------------------------------------- */

// \callgraph

#pragma once

#include <gtsam/base/Testable.h>
#include <gtsam/base/FastVector.h>
#include <gtsam/inference/Key.h>

#include <Eigen/Core>  // for Eigen::aligned_allocator

#include <boost/serialization/nvp.hpp>
#include <boost/serialization/vector.hpp>
#include <boost/assign/list_inserter.hpp>
#include <boost/bind.hpp>
#include <boost/make_shared.hpp>

#include <type_traits>
#include <utility>

namespace gtsam {

  // Forward declarations
  template<class CLIQUE> class BayesTree;

  template<class C>
  class CRefCallPushBack
  {
    C& obj;
  public:
    CRefCallPushBack(C& obj) : obj(obj) {}
    template<typename A>
    void operator()(const A& a) { obj.push_back(a); }
  };

  template<class C>
  class RefCallPushBack
  {
    C& obj;
  public:
    RefCallPushBack(C& obj) : obj(obj) {}
    template<typename A>
    void operator()(A& a) { obj.push_back(a); }
  };

  template<class C>
  class CRefCallAddCopy
  {
    C& obj;
  public:
    CRefCallAddCopy(C& obj) : obj(obj) {}
    template<typename A>
    void operator()(const A& a) { obj.addCopy(a); }
  };

  template<class FACTOR>
  class FactorGraph {

  public:
    typedef FACTOR FactorType;  
    typedef boost::shared_ptr<FACTOR> sharedFactor;  
    typedef sharedFactor value_type;
    typedef typename FastVector<sharedFactor>::iterator iterator;
    typedef typename FastVector<sharedFactor>::const_iterator const_iterator;

  private:
    typedef FactorGraph<FACTOR> This;  
    typedef boost::shared_ptr<This> shared_ptr;  

  protected:
    GTSAM_CONCEPT_TESTABLE_TYPE(FACTOR)

    
    FastVector<sharedFactor> factors_;


    FactorGraph() {}

    template<typename ITERATOR>
    FactorGraph(ITERATOR firstFactor, ITERATOR lastFactor) { push_back(firstFactor, lastFactor); }

    template<class CONTAINER>
    explicit FactorGraph(const CONTAINER& factors) { push_back(factors); }


    // TODO: are these needed?

    // * @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<class CONDITIONAL>
    //FactorGraph(const BayesNet<CONDITIONAL>& bayesNet);

    //template<class CONDITIONAL, class CLIQUE>
    //FactorGraph(const BayesTree<CONDITIONAL, CLIQUE>& bayesTree);

    //template<class DERIVEDFACTOR>
    //FactorGraph(const FactorGraph<DERIVEDFACTOR>& factors) {
    //  factors_.assign(factors.begin(), factors.end());
    //}


  public:

    void reserve(size_t size) { factors_.reserve(size); }

    // TODO: are these needed?

    template<class DERIVEDFACTOR>
    typename std::enable_if<std::is_base_of<FactorType, DERIVEDFACTOR>::value>::type
    push_back(boost::shared_ptr<DERIVEDFACTOR> factor) {
      factors_.push_back(boost::shared_ptr<FACTOR>(factor)); }

    void push_back(const sharedFactor& factor) {
      factors_.push_back(factor); }

        template<class DERIVEDFACTOR, class... Args>
        typename std::enable_if<std::is_base_of<FactorType, DERIVEDFACTOR>::value>::type
                emplace_shared(Args&&... args) {
                factors_.push_back(boost::allocate_shared<DERIVEDFACTOR>(Eigen::aligned_allocator<DERIVEDFACTOR>(), std::forward<Args>(args)...));
        }

    template<typename ITERATOR>
    typename std::enable_if<std::is_base_of<FactorType, typename ITERATOR::value_type::element_type>::value>::type
      push_back(ITERATOR firstFactor, ITERATOR lastFactor) {
        factors_.insert(end(), firstFactor, lastFactor); }

    template<typename CONTAINER>
    typename std::enable_if<std::is_base_of<FactorType, typename CONTAINER::value_type::element_type>::value>::type
      push_back(const CONTAINER& container) {
        push_back(container.begin(), container.end());
    }

    template<class CLIQUE>
    typename std::enable_if<std::is_base_of<This, typename CLIQUE::FactorGraphType>::value>::type
    push_back(const BayesTree<CLIQUE>& bayesTree) {
      bayesTree.addFactorsToGraph(*this);
    }

//#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4
    template<class DERIVEDFACTOR>
    typename std::enable_if<std::is_base_of<FactorType, DERIVEDFACTOR>::value>::type
      push_back(const DERIVEDFACTOR& factor) {
        factors_.push_back(boost::allocate_shared<DERIVEDFACTOR>(Eigen::aligned_allocator<DERIVEDFACTOR>(), factor));
    }
//#endif

    template<typename ITERATOR>
    typename std::enable_if<std::is_base_of<FactorType, typename ITERATOR::value_type>::value>::type
      push_back(ITERATOR firstFactor, ITERATOR lastFactor) {
        for (ITERATOR f = firstFactor; f != lastFactor; ++f)
          push_back(*f);
    }

    template<typename CONTAINER>
    typename std::enable_if<std::is_base_of<FactorType, typename CONTAINER::value_type>::value>::type
      push_back(const CONTAINER& container) {
        push_back(container.begin(), container.end());
    }

    template<class DERIVEDFACTOR>
    typename std::enable_if<std::is_base_of<FactorType, DERIVEDFACTOR>::value,
      boost::assign::list_inserter<RefCallPushBack<This> > >::type
      operator+=(boost::shared_ptr<DERIVEDFACTOR> factor) {
        return boost::assign::make_list_inserter(RefCallPushBack<This>(*this))(factor);
    }

    boost::assign::list_inserter<CRefCallPushBack<This> >
      operator+=(const sharedFactor& factor) {
        return boost::assign::make_list_inserter(CRefCallPushBack<This>(*this))(factor);
    }

    template<class FACTOR_OR_CONTAINER>
    boost::assign::list_inserter<CRefCallPushBack<This> >
      operator+=(const FACTOR_OR_CONTAINER& factorOrContainer) {
        return boost::assign::make_list_inserter(CRefCallPushBack<This>(*this))(factorOrContainer);
    }

    template<class DERIVEDFACTOR>
    typename std::enable_if<std::is_base_of<FactorType, DERIVEDFACTOR>::value>::type
      add(boost::shared_ptr<DERIVEDFACTOR> factor) {
        push_back(factor);
    }

    void add(const sharedFactor& factor) {
      push_back(factor);
    }

    template<class FACTOR_OR_CONTAINER>
    void add(const FACTOR_OR_CONTAINER& factorOrContainer) {
      push_back(factorOrContainer);
    }


    void print(const std::string& s = "FactorGraph",
      const KeyFormatter& formatter = DefaultKeyFormatter) const;

    bool equals(const This& fg, double tol = 1e-9) const;

  public:

    size_t size() const { return factors_.size(); }

    bool empty() const { return factors_.empty(); }

    const sharedFactor at(size_t i) const { return factors_.at(i); }

    sharedFactor& at(size_t i) { return factors_.at(i); }

    const sharedFactor operator[](size_t i) const { return at(i); }

    sharedFactor& operator[](size_t i) { return at(i); }

    const_iterator begin() const { return factors_.begin();}

    const_iterator end()   const { return factors_.end();  }

    sharedFactor front() const { return factors_.front(); }

    sharedFactor back() const { return factors_.back(); }


    iterator begin()       { return factors_.begin();}

    iterator end()         { return factors_.end();  }

    void resize(size_t size) { factors_.resize(size); }

    void remove(size_t i) { factors_[i].reset();}

    void replace(size_t index, sharedFactor factor) { at(index) = factor; }

    iterator erase(iterator item) { return factors_.erase(item); }

    iterator erase(iterator first, iterator last) { return factors_.erase(first, last); }


    size_t nrFactors() const;

    KeySet keys() const;

    KeyVector keyVector() const;

    inline bool exists(size_t idx) const { return idx < size() && at(idx); }

  private:

    friend class boost::serialization::access;
    template<class ARCHIVE>
    void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
      ar & BOOST_SERIALIZATION_NVP(factors_);
    }


  }; // FactorGraph

} // namespace gtsam

#include <gtsam/inference/FactorGraph-inst.h>