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 <vector>
#include <deque>
#include <stdexcept>
#include <boost/foreach.hpp>

#include <gtsam/base/FastList.h>
#include <gtsam/base/FastMap.h>
#include <gtsam/base/types.h>
#include <gtsam/base/timing.h>
#include <gtsam/inference/Key.h>

namespace gtsam {

class GTSAM_EXPORT VariableIndex {
public:

  typedef boost::shared_ptr<VariableIndex> shared_ptr;
  typedef FastList<size_t> Factors;
  typedef Factors::iterator Factor_iterator;
  typedef Factors::const_iterator Factor_const_iterator;  

protected:
  typedef FastMap<Key,Factors> 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;

public:


  VariableIndex() : nFactors_(0), nEntries_(0) {}

  template<class FG>
  VariableIndex(const FG& factorGraph) : nFactors_(0), nEntries_(0) { augment(factorGraph); }


  Key size() const { return index_.size(); }

  size_t nFactors() const { return nFactors_; }

  size_t nEntries() const { return nEntries_; }

  const Factors& operator[](Key variable) const {
    KeyMap::const_iterator item = index_.find(variable);
    if(item == index_.end())
      throw std::invalid_argument("Requested non-existent variable from VariableIndex");
    else
    return item->second;
  }


  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, boost::optional<const FastVector<size_t>&> newFactorIndices = boost::none);

  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 Factors& internalAt(Key variable) const {
    const KeyMap::const_iterator item = index_.find(variable);
    assert(item != index_.end());
    return item->second; }

  Factors& internalAt(Key variable) {
    const KeyMap::iterator item = index_.find(variable);
    assert(item != index_.end());
    return item->second; }

};

}

#include <gtsam/inference/VariableIndex-inl.h>