Ordering.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 <algorithm>
#include <vector>
#include <boost/assign/list_inserter.hpp>

#include <gtsam/base/FastSet.h>
#include <gtsam/inference/Key.h>
#include <gtsam/inference/VariableIndex.h>
#include <gtsam/inference/FactorGraph.h>

namespace gtsam {
  class Ordering : public std::vector<Key> {
  protected:
    typedef std::vector<Key> Base;

  public:
    typedef Ordering This; 
    typedef boost::shared_ptr<This> shared_ptr; 

    GTSAM_EXPORT Ordering() {}

    template<typename KEYS>
    explicit Ordering(const KEYS& keys) : Base(keys.begin(), keys.end()) {}

    template<typename ITERATOR>
    Ordering(ITERATOR firstKey, ITERATOR lastKey) : Base(firstKey, lastKey) {}

    boost::assign::list_inserter<boost::assign_detail::call_push_back<This> >
      operator+=(Key key) {
        return boost::assign::make_list_inserter(boost::assign_detail::call_push_back<This>(*this))(key);
    }

    FastMap<Key, size_t> invert() const;


    template<class FACTOR>
    static Ordering COLAMD(const FactorGraph<FACTOR>& graph) {
      return COLAMD(VariableIndex(graph)); }

    static GTSAM_EXPORT Ordering COLAMD(const VariableIndex& variableIndex);

    template<class FACTOR>
    static Ordering COLAMDConstrainedLast(const FactorGraph<FACTOR>& graph,
      const std::vector<Key>& constrainLast, bool forceOrder = false) {
        return COLAMDConstrainedLast(VariableIndex(graph), constrainLast, forceOrder); }

    static GTSAM_EXPORT Ordering COLAMDConstrainedLast(const VariableIndex& variableIndex,
      const std::vector<Key>& constrainLast, bool forceOrder = false);

    template<class FACTOR>
    static Ordering COLAMDConstrainedFirst(const FactorGraph<FACTOR>& graph,
      const std::vector<Key>& constrainFirst, bool forceOrder = false) {
        return COLAMDConstrainedFirst(VariableIndex(graph), constrainFirst, forceOrder); }

    static GTSAM_EXPORT Ordering COLAMDConstrainedFirst(const VariableIndex& variableIndex,
      const std::vector<Key>& constrainFirst, bool forceOrder = false);

    template<class FACTOR>
    static Ordering COLAMDConstrained(const FactorGraph<FACTOR>& graph,
      const FastMap<Key, int>& groups) {
        return COLAMDConstrained(VariableIndex(graph), groups); }

    static GTSAM_EXPORT Ordering COLAMDConstrained(const VariableIndex& variableIndex,
      const FastMap<Key, int>& groups);

    template <class FACTOR>
    static Ordering Natural(const FactorGraph<FACTOR> &fg) {
      FastSet<Key> src = fg.keys();
      std::vector<Key> keys(src.begin(), src.end());
      std::stable_sort(keys.begin(), keys.end());
      return Ordering(keys);
    }



    GTSAM_EXPORT void print(const std::string& str = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const;

    GTSAM_EXPORT bool equals(const Ordering& other, double tol = 1e-9) const;


  private:
    static GTSAM_EXPORT Ordering COLAMDConstrained(
      const VariableIndex& variableIndex, std::vector<int>& cmember);

    friend class boost::serialization::access;
    template<class ARCHIVE>
    void serialize(ARCHIVE & ar, const unsigned int version) {
      ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
    }
  };
}