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/FastVector.h>
#include <gtsam/base/Testable.h>
#include <gtsam/inference/Key.h>
 
#include <Eigen/Core>  // for Eigen::aligned_allocator
 
#include <boost/assign/list_inserter.hpp>
#include <boost/make_shared.hpp>
#include <boost/serialization/nvp.hpp>
#include <boost/serialization/vector.hpp>
 
#include <string>
#include <type_traits>
#include <utility>
 
namespace gtsam {
typedef FastVector<FactorIndex> FactorIndices;
 
// Forward declarations
template <class CLIQUE>
class BayesTree;
 
template <class C>
class CRefCallPushBack {
  C& obj;
 
 public:
  explicit 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:
  explicit RefCallPushBack(C& obj) : obj(obj) {}
  template <typename A>
  void operator()(A& a) {
    obj.push_back(a);
  }
};
 
template <class C>
class CRefCallAddCopy {
  C& obj;
 
 public:
  explicit 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;  
 
  template <typename DERIVEDFACTOR>
  using IsDerived = typename std::enable_if<
      std::is_base_of<FactorType, DERIVEDFACTOR>::value>::type;
 
  template <typename T>
  using HasDerivedValueType = typename std::enable_if<
      std::is_base_of<FactorType, typename T::value_type>::value>::type;
 
  template <typename T>
  using HasDerivedElementType = typename std::enable_if<std::is_base_of<
      FactorType, typename T::value_type::element_type>::value>::type;
 
 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);
  }
 
 
 public:
  // Public and virtual so boost serialization can call it.
  virtual ~FactorGraph() = default;
 
 
  void reserve(size_t size) { factors_.reserve(size); }
 
  template <class DERIVEDFACTOR>
  IsDerived<DERIVEDFACTOR> push_back(boost::shared_ptr<DERIVEDFACTOR> factor) {
    factors_.push_back(boost::shared_ptr<FACTOR>(factor));
  }
 
  template <class DERIVEDFACTOR, class... Args>
  IsDerived<DERIVEDFACTOR> emplace_shared(Args&&... args) {
    factors_.push_back(boost::allocate_shared<DERIVEDFACTOR>(
        Eigen::aligned_allocator<DERIVEDFACTOR>(),
        std::forward<Args>(args)...));
  }
 
  template <class DERIVEDFACTOR>
  IsDerived<DERIVEDFACTOR> push_back(const DERIVEDFACTOR& factor) {
    factors_.push_back(boost::allocate_shared<DERIVEDFACTOR>(
        Eigen::aligned_allocator<DERIVEDFACTOR>(), factor));
  }
 
  template <class DERIVEDFACTOR>
  IsDerived<DERIVEDFACTOR> add(boost::shared_ptr<DERIVEDFACTOR> factor) {
    push_back(factor);
  }
 
  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);
  }
 
 
  template <typename ITERATOR>
  HasDerivedElementType<ITERATOR> push_back(ITERATOR firstFactor,
                                            ITERATOR lastFactor) {
    factors_.insert(end(), firstFactor, lastFactor);
  }
 
  template <typename ITERATOR>
  HasDerivedValueType<ITERATOR> push_back(ITERATOR firstFactor,
                                          ITERATOR lastFactor) {
    for (ITERATOR f = firstFactor; f != lastFactor; ++f) push_back(*f);
  }
 
 
  template <typename CONTAINER>
  HasDerivedElementType<CONTAINER> push_back(const CONTAINER& container) {
    push_back(container.begin(), container.end());
  }
 
  template <typename CONTAINER>
  HasDerivedValueType<CONTAINER> push_back(const CONTAINER& container) {
    push_back(container.begin(), container.end());
  }
 
  template <class FACTOR_OR_CONTAINER>
  void add(const FACTOR_OR_CONTAINER& factorOrContainer) {
    push_back(factorOrContainer);
  }
 
  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 CLIQUE>
  typename std::enable_if<
      std::is_base_of<This, typename CLIQUE::FactorGraphType>::value>::type
  push_back(const BayesTree<CLIQUE>& bayesTree) {
    bayesTree.addFactorsToGraph(this);
  }
 
  template <typename CONTAINER, typename = HasDerivedElementType<CONTAINER>>
  FactorIndices add_factors(const CONTAINER& factors,
                            bool useEmptySlots = false);
 
 
  virtual 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_.at(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>