VariableIndex.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
 
 * -------------------------------------------------------------------------- */
 
#pragma once
 
#include <gtsam/inference/Factor.h>
#include <gtsam/inference/Key.h>
#include <gtsam/base/FastMap.h>
#include <gtsam/base/FastVector.h>
#include <gtsam/dllexport.h>
 
#include <cassert>
#include <stdexcept>
#include <optional>
 
namespace gtsam {
 
class GTSAM_EXPORT VariableIndex {
 public:
  typedef std::shared_ptr<VariableIndex> shared_ptr;
  typedef FactorIndices::iterator Factor_iterator;
  typedef FactorIndices::const_iterator Factor_const_iterator;
 
 protected:
  typedef FastMap<Key, FactorIndices> KeyMap;
  KeyMap index_;
  size_t nFactors_;  // Number of factors in the original factor graph.
  size_t nEntries_;  // Sum of involved variable counts of each factor.
 
 public:
  typedef KeyMap::const_iterator const_iterator;
  typedef KeyMap::const_iterator iterator;
  typedef KeyMap::value_type value_type;
 
 
  VariableIndex() : nFactors_(0), nEntries_(0) {}
 
  template <class FG>
  explicit VariableIndex(const FG& factorGraph) : nFactors_(0), nEntries_(0) {
    augment(factorGraph);
  }
 
 
  size_t size() const { return index_.size(); }
 
  size_t nFactors() const { return nFactors_; }
 
  size_t nEntries() const { return nEntries_; }
 
  const FactorIndices& operator[](Key variable) const {
    KeyMap::const_iterator item = index_.find(variable);
    if(item == index_.end())
      throw std::invalid_argument("Requested non-existent variable '" +
                                  DefaultKeyFormatter(variable) +
                                  "' from VariableIndex");
    else
      return item->second;
  }
 
  bool empty(Key variable) const {
    return (*this)[variable].empty();
  }
 
 
  bool equals(const VariableIndex& other, double tol=0.0) const;
 
  void print(const std::string& str = "VariableIndex: ",
      const KeyFormatter& keyFormatter = DefaultKeyFormatter) const;
 
  void outputMetisFormat(std::ostream& os) const;
 
 
 
  template<class FG>
  void augment(const FG& factors, const FactorIndices* newFactorIndices = nullptr);
 
  template<class FG>
  void augment(const FG& factor, const FactorIndices& newFactorIndices) {
    augment(factor, &newFactorIndices);
  }
 
  void augmentExistingFactor(const FactorIndex factorIndex, const KeySet & newKeys);
 
  template<typename ITERATOR, class FG>
  void remove(ITERATOR firstFactor, ITERATOR lastFactor, const FG& factors);
 
  template<typename ITERATOR>
  void removeUnusedVariables(ITERATOR firstKey, ITERATOR lastKey);
 
  const_iterator begin() const { return index_.begin(); }
 
  const_iterator end() const { return index_.end(); }
 
  const_iterator find(Key key) const { return index_.find(key); }
 
protected:
  Factor_iterator factorsBegin(Key variable) { return internalAt(variable).begin(); }
  Factor_iterator factorsEnd(Key variable) { return internalAt(variable).end(); }
 
  Factor_const_iterator factorsBegin(Key variable) const { return internalAt(variable).begin(); }
  Factor_const_iterator factorsEnd(Key variable) const { return internalAt(variable).end(); }
 
  const FactorIndices& internalAt(Key variable) const {
    const KeyMap::const_iterator item = index_.find(variable);
    assert(item != index_.end());
    return item->second; 
  }
 
  FactorIndices& internalAt(Key variable) {
    const KeyMap::iterator item = index_.find(variable);
    assert(item != index_.end());
    return item->second; 
  }
 
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(index_);
    ar & BOOST_SERIALIZATION_NVP(nFactors_);
    ar & BOOST_SERIALIZATION_NVP(nEntries_);
  }
#endif
 
};
 
template<>
struct traits<VariableIndex> : public Testable<VariableIndex> {
};
 
} //\ namespace gtsam
 
#include <gtsam/inference/VariableIndex-inl.h>