134 lines
4.2 KiB
C++
134 lines
4.2 KiB
C++
/* ----------------------------------------------------------------------------
|
|
|
|
* 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
|
|
|
|
* -------------------------------------------------------------------------- */
|
|
|
|
/**
|
|
* @file Ordering.h
|
|
* @brief
|
|
* @author Richard Roberts
|
|
* @created Sep 2, 2010
|
|
*/
|
|
|
|
#pragma once
|
|
|
|
#include <map>
|
|
|
|
|
|
|
|
#include <gtsam/base/Testable.h>
|
|
#include <gtsam/nonlinear/Key.h>
|
|
#include <gtsam/inference/inference.h>
|
|
|
|
#include <boost/foreach.hpp>
|
|
#include <boost/shared_ptr.hpp>
|
|
#include <boost/assign/list_inserter.hpp>
|
|
#include <boost/pool/pool_alloc.hpp>
|
|
|
|
namespace gtsam {
|
|
|
|
class Ordering : Testable<Ordering> {
|
|
protected:
|
|
typedef boost::fast_pool_allocator<std::pair<const Symbol, Index> > Allocator;
|
|
typedef std::map<Symbol, Index, std::less<Symbol>, Allocator> Map;
|
|
Map order_;
|
|
Index nVars_;
|
|
|
|
public:
|
|
|
|
typedef boost::shared_ptr<Ordering> shared_ptr;
|
|
|
|
typedef std::pair<const Symbol, Index> value_type;
|
|
typedef Map::iterator iterator;
|
|
typedef Map::const_iterator const_iterator;
|
|
|
|
Ordering() : nVars_(0) {}
|
|
Ordering(const std::list<Symbol> & L) ;
|
|
|
|
/** One greater than the maximum ordering index. */
|
|
Index nVars() const { return nVars_; }
|
|
|
|
/** The number of variables in this ordering. */
|
|
Index size() const { return order_.size(); }
|
|
|
|
iterator begin() { return order_.begin(); }
|
|
const_iterator begin() const { return order_.begin(); }
|
|
iterator end() { return order_.end(); }
|
|
const_iterator end() const { return order_.end(); }
|
|
|
|
Index& at(const Symbol& key) { return operator[](key); }
|
|
Index at(const Symbol& key) const { return operator[](key); }
|
|
Index& operator[](const Symbol& key) {
|
|
iterator i=order_.find(key); assert(i != order_.end()); return i->second; }
|
|
Index operator[](const Symbol& key) const {
|
|
const_iterator i=order_.find(key); assert(i != order_.end()); return i->second; }
|
|
|
|
iterator insert(const value_type& key_order) {
|
|
std::pair<iterator,bool> it_ok(tryInsert(key_order));
|
|
assert(it_ok.second);
|
|
return it_ok.first; }
|
|
iterator insert(const Symbol& key, Index order) { return insert(std::make_pair(key,order)); }
|
|
std::pair<iterator,bool> tryInsert(const value_type& key_order) {
|
|
std::pair<iterator,bool> it_ok(order_.insert(key_order));
|
|
if(key_order.second+1 > nVars_) nVars_ = key_order.second+1;
|
|
return it_ok; }
|
|
std::pair<iterator,bool> tryInsert(const Symbol& key, Index order) { return tryInsert(std::make_pair(key,order)); }
|
|
|
|
Index push_back(const Symbol& key) { return insert(std::make_pair(key, nVars_))->second; }
|
|
|
|
/**
|
|
* += operator allows statements like 'ordering += x0,x1,x2,x3;', which are
|
|
* very useful for unit tests. This functionality is courtesy of
|
|
* boost::assign.
|
|
*/
|
|
inline boost::assign::list_inserter<boost::assign_detail::call_push_back<Ordering>, Symbol>
|
|
operator+=(const Symbol& key) {
|
|
return boost::assign::make_list_inserter(boost::assign_detail::call_push_back<Ordering>(*this))(key); }
|
|
|
|
/**
|
|
* Reorder the variables with a permutation. This is typically used
|
|
* internally, permuting an initial key-sorted ordering into a fill-reducing
|
|
* ordering.
|
|
*/
|
|
void permuteWithInverse(const Permutation& inversePermutation);
|
|
|
|
/** print (from Testable) for testing and debugging */
|
|
void print(const std::string& str = "Ordering:") const;
|
|
|
|
/** equals (from Testable) for testing and debugging */
|
|
bool equals(const Ordering& rhs, double tol = 0.0) const;
|
|
|
|
};
|
|
|
|
/**
|
|
* @class Unordered
|
|
* @brief a set of unordered indice
|
|
*/
|
|
class Unordered: public std::set<Index>, public Testable<Unordered> {
|
|
public:
|
|
/** Default constructor creates empty ordering */
|
|
Unordered() { }
|
|
|
|
/** Create from a single symbol */
|
|
Unordered(Index key) { insert(key); }
|
|
|
|
/** Copy constructor */
|
|
Unordered(const std::set<Index>& keys_in) : std::set<Index>(keys_in) {}
|
|
|
|
/** whether a key exists */
|
|
bool exists(const Index& key) { return find(key) != end(); }
|
|
|
|
// Testable
|
|
void print(const std::string& s = "Unordered") const;
|
|
bool equals(const Unordered &t, double tol=0) const;
|
|
};
|
|
|
|
}
|
|
|