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

#ifdef GTSAM_USE_BOOST_FEATURES
#include <boost/assign/list_inserter.hpp>
#endif

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

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


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

#ifdef GTSAM_USE_BOOST_FEATURES

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


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

  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:
#ifdef 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>