From aa25f7629d14fc7b1e5fa3947375caacf3999bf9 Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Mon, 5 Aug 2013 22:30:53 +0000 Subject: [PATCH] Removed OrderingOrdered --- gtsam/nonlinear/OrderingOrdered.cpp | 130 ------------- gtsam/nonlinear/OrderingOrdered.h | 277 ---------------------------- 2 files changed, 407 deletions(-) delete mode 100644 gtsam/nonlinear/OrderingOrdered.cpp delete mode 100644 gtsam/nonlinear/OrderingOrdered.h diff --git a/gtsam/nonlinear/OrderingOrdered.cpp b/gtsam/nonlinear/OrderingOrdered.cpp deleted file mode 100644 index 8bece926f..000000000 --- a/gtsam/nonlinear/OrderingOrdered.cpp +++ /dev/null @@ -1,130 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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.cpp - * @author Richard Roberts - * @date Sep 2, 2010 - */ - -#include - -#include -#include -#include - -using namespace std; - -namespace gtsam { - -/* ************************************************************************* */ -OrderingOrdered::OrderingOrdered(const std::list & L) { - int i = 0; - BOOST_FOREACH( Key s, L ) - insert(s, i++) ; -} - -/* ************************************************************************* */ -OrderingOrdered::OrderingOrdered(const OrderingOrdered& other) : order_(other.order_), orderingIndex_(other.size()) { - for(iterator item = order_.begin(); item != order_.end(); ++item) - orderingIndex_[item->second] = item; -} - -/* ************************************************************************* */ -OrderingOrdered& OrderingOrdered::operator=(const OrderingOrdered& rhs) { - order_ = rhs.order_; - orderingIndex_.resize(rhs.size()); - for(iterator item = order_.begin(); item != order_.end(); ++item) - orderingIndex_[item->second] = item; - return *this; -} - -/* ************************************************************************* */ -void OrderingOrdered::permuteInPlace(const Permutation& permutation) { - gttic(Ordering_permuteInPlace); - // Allocate new index and permute in map iterators - OrderingIndex newIndex(permutation.size()); - for(size_t j = 0; j < newIndex.size(); ++j) { - newIndex[j] = orderingIndex_[permutation[j]]; // Assign the iterator - newIndex[j]->second = j; // Change the stored index to its permuted value - } - // Swap the new index into this Ordering class - orderingIndex_.swap(newIndex); -} - -/* ************************************************************************* */ -void OrderingOrdered::permuteInPlace(const Permutation& selector, const Permutation& permutation) { - if(selector.size() != permutation.size()) - throw invalid_argument("Ordering::permuteInPlace (partial permutation version) called with selector and permutation of different sizes."); - // Create new index the size of the permuted entries - OrderingIndex newIndex(selector.size()); - // Permute the affected entries into the new index - for(size_t dstSlot = 0; dstSlot < selector.size(); ++dstSlot) - newIndex[dstSlot] = orderingIndex_[selector[permutation[dstSlot]]]; - // Put the affected entries back in the new order and fix the indices - for(size_t slot = 0; slot < selector.size(); ++slot) { - orderingIndex_[selector[slot]] = newIndex[slot]; - orderingIndex_[selector[slot]]->second = selector[slot]; - } -} - -/* ************************************************************************* */ -void OrderingOrdered::print(const string& str, const KeyFormatter& keyFormatter) const { - cout << str; - // Print ordering in index order - // Print the ordering with varsPerLine ordering entries printed on each line, - // for compactness. - static const size_t varsPerLine = 10; - bool endedOnNewline = false; - BOOST_FOREACH(const Map::iterator& index_key, orderingIndex_) { - if(index_key->second % varsPerLine != 0) - cout << ", "; - cout << index_key->second<< ":" << keyFormatter(index_key->first); - if(index_key->second % varsPerLine == varsPerLine - 1) { - cout << "\n"; - endedOnNewline = true; - } else { - endedOnNewline = false; - } - } - if(!endedOnNewline) - cout << "\n"; - cout.flush(); -} - -/* ************************************************************************* */ -bool OrderingOrdered::equals(const OrderingOrdered& rhs, double tol) const { - return order_ == rhs.order_; -} - -/* ************************************************************************* */ -OrderingOrdered::value_type OrderingOrdered::pop_back() { - iterator lastItem = orderingIndex_.back(); // Get the map iterator to the highest-index entry - value_type removed = *lastItem; // Save the key-index pair to return - order_.erase(lastItem); // Erase the entry from the map - orderingIndex_.pop_back(); // Erase the entry from the index - return removed; // Return the removed item -} - -/* ************************************************************************* */ -void Unordered::print(const string& s) const { - cout << s << " (" << size() << "):"; - BOOST_FOREACH(Index key, *this) - cout << " " << key; - cout << endl; -} - -/* ************************************************************************* */ -bool Unordered::equals(const Unordered &other, double tol) const { - return *this == other; -} - -} diff --git a/gtsam/nonlinear/OrderingOrdered.h b/gtsam/nonlinear/OrderingOrdered.h deleted file mode 100644 index f47077975..000000000 --- a/gtsam/nonlinear/OrderingOrdered.h +++ /dev/null @@ -1,277 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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 - * @author Richard Roberts - * @date Sep 2, 2010 - */ - -#pragma once - -#include -#include -#include - -#include -#include -#include -#include - -namespace gtsam { - -/** - * An ordering is a map from symbols (non-typed keys) to integer indices - * \nosubgrouping - */ -class GTSAM_EXPORT OrderingOrdered { -protected: - typedef FastMap Map; - typedef std::vector OrderingIndex; - Map order_; - OrderingIndex orderingIndex_; - -public: - - typedef boost::shared_ptr shared_ptr; - - typedef std::pair value_type; - typedef Map::iterator iterator; - typedef Map::const_iterator const_iterator; - - /// @name Standard Constructors - /// @{ - - /// Default constructor for empty ordering - OrderingOrdered() {} - - /// Copy constructor - OrderingOrdered(const OrderingOrdered& other); - - /// Construct from list, assigns order indices sequentially to list items. - OrderingOrdered(const std::list & L); - - /// Assignment operator - OrderingOrdered& operator=(const OrderingOrdered& rhs); - - /// @} - /// @name Standard Interface - /// @{ - - /** The actual number of variables in this ordering, i.e. not including missing indices in the count. See also nVars(). */ - Index size() const { return orderingIndex_.size(); } - - const_iterator begin() const { return order_.begin(); } /**< Iterator in order of sorted symbols, not in elimination/index order! */ - const_iterator end() const { return order_.end(); } /**< Iterator in order of sorted symbols, not in elimination/index order! */ - - Index at(Key key) const { return operator[](key); } ///< Synonym for operator[](Key) const - Key key(Index index) const { - if(index >= orderingIndex_.size()) - throw std::out_of_range("Attempting to access out-of-range index in Ordering"); - else - return orderingIndex_[index]->first; } - - /** Assigns the ordering index of the requested \c key into \c index if the symbol - * is present in the ordering, otherwise does not modify \c index. The - * return value indicates whether the symbol is in fact present in the - * ordering. - * @param key The key whose index you request - * @param [out] index Reference into which to write the index of the requested key, if the key is present. - * @return true if the key is present and \c index was modified, false otherwise. - */ - bool tryAt(Key key, Index& index) const { - const_iterator i = order_.find(key); - if(i != order_.end()) { - index = i->second; - return true; - } else - return false; } - - /// Access the index for the requested key, throws std::out_of_range if the - /// key is not present in the ordering (note that this differs from the - /// behavior of std::map) - Index& operator[](Key key) { - iterator i=order_.find(key); - if(i == order_.end()) throw std::out_of_range( - std::string("Attempting to access a key from an ordering that does not contain that key:") + DefaultKeyFormatter(key)); - else return i->second; } - - /// Access the index for the requested key, throws std::out_of_range if the - /// key is not present in the ordering (note that this differs from the - /// behavior of std::map) - Index operator[](Key key) const { - const_iterator i=order_.find(key); - if(i == order_.end()) throw std::out_of_range( - std::string("Attempting to access a key from an ordering that does not contain that key:") + DefaultKeyFormatter(key)); - else return i->second; } - - /** Returns an iterator pointing to the symbol/index pair with the requested, - * or the end iterator if it does not exist. - * - * @return An iterator pointing to the symbol/index pair with the requested, - * or the end iterator if it does not exist. - */ - iterator find(Key key) { return order_.find(key); } - - /** Returns an iterator pointing to the symbol/index pair with the requested, - * or the end iterator if it does not exist. - * - * @return An iterator pointing to the symbol/index pair with the requested, - * or the end iterator if it does not exist. - */ - const_iterator find(Key key) const { return order_.find(key); } - - /** Insert a key-index pair, but will fail if the key is already present */ - iterator insert(const value_type& key_order) { - std::pair it_ok(order_.insert(key_order)); - if(it_ok.second) { - if(key_order.second >= orderingIndex_.size()) - orderingIndex_.resize(key_order.second + 1); - orderingIndex_[key_order.second] = it_ok.first; - return it_ok.first; - } else - throw std::invalid_argument(std::string("Attempting to insert a key into an ordering that already contains that key")); } - - /// Test if the key exists in the ordering. - bool exists(Key key) const { return order_.count(key) > 0; } - - /** Insert a key-index pair, but will fail if the key is already present */ - iterator insert(Key key, Index order) { return insert(std::make_pair(key,order)); } - - /// Adds a new key to the ordering with an index of one greater than the current highest index. - Index push_back(Key key) { return insert(std::make_pair(key, orderingIndex_.size()))->second; } - - /// @} - /// @name Advanced Interface - /// @{ - - /** - * Iterator in order of sorted symbols, not in elimination/index order! - */ - iterator begin() { return order_.begin(); } - - /** - * Iterator in order of sorted symbols, not in elimination/index order! - */ - iterator end() { return order_.end(); } - - /** Remove the last (last-ordered, not highest-sorting key) symbol/index pair - * from the ordering (this version is \f$ O(n) \f$, use it when you do not - * know the last-ordered key). - * - * If you already know the last-ordered symbol, call popback(Key) - * that accepts this symbol as an argument. - * - * @return The symbol and index that were removed. - */ - value_type pop_back(); - - /** - * += 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, Key> - operator+=(Key key) { - return boost::assign::make_list_inserter(boost::assign_detail::call_push_back(*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 permuteInPlace(const Permutation& permutation); - - void permuteInPlace(const Permutation& selector, const Permutation& permutation); - - /// Synonym for operator[](Key) - Index& at(Key key) { return operator[](key); } - - /// @} - /// @name Testable - /// @{ - - /** print (from Testable) for testing and debugging */ - void print(const std::string& str = "Ordering:\n", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const; - - /** equals (from Testable) for testing and debugging */ - bool equals(const OrderingOrdered& rhs, double tol = 0.0) const; - - /// @} - -private: - - /** Serialization function */ - friend class boost::serialization::access; - template - void save(ARCHIVE & ar, const unsigned int version) const - { - ar & BOOST_SERIALIZATION_NVP(order_); - size_t size_ = orderingIndex_.size(); // Save only the size but not the iterators - ar & BOOST_SERIALIZATION_NVP(size_); - } - template - void load(ARCHIVE & ar, const unsigned int version) - { - ar & BOOST_SERIALIZATION_NVP(order_); - size_t size_; - ar & BOOST_SERIALIZATION_NVP(size_); - orderingIndex_.resize(size_); - for(iterator item = order_.begin(); item != order_.end(); ++item) - orderingIndex_[item->second] = item; // Assign the iterators - } - BOOST_SERIALIZATION_SPLIT_MEMBER() -}; // \class Ordering - -/** - * @class Unordered - * @brief a set of unordered indices - */ -class Unordered: public std::set { -public: - /** Default constructor creates empty ordering */ - Unordered() { } - - /** Create from a single symbol */ - Unordered(Index key) { insert(key); } - - /** Copy constructor */ - Unordered(const std::set& keys_in) : std::set(keys_in) {} - - /** whether a key exists */ - bool exists(const Index& key) { return find(key) != end(); } - - // Testable - GTSAM_EXPORT void print(const std::string& s = "Unordered") const; - GTSAM_EXPORT bool equals(const Unordered &t, double tol=0) const; -}; - -// Create an index formatter that looks up the Key in an inverse ordering, then -// formats the key using the provided key formatter, used in saveGraph. -class GTSAM_EXPORT OrderingIndexFormatter { -private: - OrderingOrdered ordering_; - KeyFormatter keyFormatter_; -public: - OrderingIndexFormatter(const OrderingOrdered& ordering, const KeyFormatter& keyFormatter) : - ordering_(ordering), keyFormatter_(keyFormatter) {} - std::string operator()(Index index) { - return keyFormatter_(ordering_.key(index)); } -}; - -/// Version of orderingIndexFormatter using multi-robot formatter -struct GTSAM_EXPORT MultiRobotLinearFormatter : gtsam::OrderingIndexFormatter { - MultiRobotLinearFormatter(const gtsam::OrderingOrdered& ordering) - : gtsam::OrderingIndexFormatter(ordering, MultiRobotKeyFormatter) {} -}; - -} // \namespace gtsam -