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/inference/DotWriter.h>
#include <gtsam/inference/Key.h>
#include <gtsam/base/FastVector.h>
#include <gtsam/base/Testable.h>
 
#include <Eigen/Core>  // for Eigen::aligned_allocator
 
#if GTSAM_ENABLE_BOOST_SERIALIZATION
#include <boost/serialization/nvp.hpp>
#include <boost/serialization/vector.hpp>
#endif
 
#include <string>
#include <type_traits>
#include <utility>
#include <iosfwd>
 
namespace gtsam {
typedef FastVector<FactorIndex> FactorIndices;
 
// Forward declarations
template <class CLIQUE>
class BayesTree;
 
class HybridValues;

template <class FACTOR>
class FactorGraph {
 public:
  typedef FACTOR FactorType;  
  typedef std::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 std::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_;

  bool isEqual(const FactorGraph& other) const {
    return factors_ == other.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:

  virtual ~FactorGraph() = default;

  template <class DERIVEDFACTOR, typename = IsDerived<DERIVEDFACTOR>>
  FactorGraph(std::initializer_list<std::shared_ptr<DERIVEDFACTOR>> sharedFactors)
      : factors_(sharedFactors) {}


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

  template <class DERIVEDFACTOR>
  IsDerived<DERIVEDFACTOR> push_back(std::shared_ptr<DERIVEDFACTOR> factor) {
    factors_.push_back(std::shared_ptr<FACTOR>(factor));
  }

  template <class DERIVEDFACTOR, class... Args>
  IsDerived<DERIVEDFACTOR> emplace_shared(Args&&... args) {
    factors_.push_back(std::allocate_shared<DERIVEDFACTOR>(
        Eigen::aligned_allocator<DERIVEDFACTOR>(),
        std::forward<Args>(args)...));
  }

  template <class DERIVEDFACTOR>
  IsDerived<DERIVEDFACTOR> push_back(const DERIVEDFACTOR& factor) {
    factors_.push_back(std::allocate_shared<DERIVEDFACTOR>(
        Eigen::aligned_allocator<DERIVEDFACTOR>(), factor));
  }

  template <class DERIVEDFACTOR>
  IsDerived<DERIVEDFACTOR> add(std::shared_ptr<DERIVEDFACTOR> factor) {
    push_back(factor);
  }

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

  template <class DERIVEDFACTOR>
  typename std::enable_if<std::is_base_of<FactorType, DERIVEDFACTOR>::value, This>::type& operator,(
      std::shared_ptr<DERIVEDFACTOR> factor) {
    push_back(factor);
    return *this;
  }


  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>
  This& operator+=(const FACTOR_OR_CONTAINER& factorOrContainer) {
    push_back(factorOrContainer);
    return *this;
  }


  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); }

  template <typename F>
  std::shared_ptr<F> at(size_t i) {
    return std::dynamic_pointer_cast<F>(factors_.at(i));
  }

  template <typename F>
  const std::shared_ptr<F> at(size_t i) const {
    return std::dynamic_pointer_cast<F>(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(); }

  double error(const HybridValues &values) const;


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

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

  virtual 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);
  }


  void dot(std::ostream& os,
           const KeyFormatter& keyFormatter = DefaultKeyFormatter,
           const DotWriter& writer = DotWriter()) const;

  std::string dot(const KeyFormatter& keyFormatter = DefaultKeyFormatter,
                  const DotWriter& writer = DotWriter()) const;

  void saveGraph(const std::string& filename,
                 const KeyFormatter& keyFormatter = DefaultKeyFormatter,
                 const DotWriter& writer = DotWriter()) const;


  size_t nrFactors() const;

  KeySet keys() const;

  KeyVector keyVector() const;

  inline bool exists(size_t idx) const { return idx < size() && at(idx); }
 
 private:
#if GTSAM_ENABLE_BOOST_SERIALIZATION
  friend class boost::serialization::access;
  template <class ARCHIVE>
  void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
    ar& BOOST_SERIALIZATION_NVP(factors_);
  }
#endif

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