Removed OrderingOrdered
Richard Roberts
parent
628d4e0d77
commit
aa25f7629d
|
|
@ -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 <gtsam/nonlinear/OrderingOrdered.h>
|
||||
|
||||
#include <string>
|
||||
#include <iostream>
|
||||
#include <boost/foreach.hpp>
|
||||
|
||||
using namespace std;
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
/* ************************************************************************* */
|
||||
OrderingOrdered::OrderingOrdered(const std::list<Key> & 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;
|
||||
}
|
||||
|
||||
}
|
||||
|
|
@ -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 <gtsam/inference/Key.h>
|
||||
#include <gtsam/inference/inferenceOrdered.h>
|
||||
#include <gtsam/base/FastMap.h>
|
||||
|
||||
#include <boost/foreach.hpp>
|
||||
#include <boost/assign/list_inserter.hpp>
|
||||
#include <boost/pool/pool_alloc.hpp>
|
||||
#include <vector>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
/**
|
||||
* An ordering is a map from symbols (non-typed keys) to integer indices
|
||||
* \nosubgrouping
|
||||
*/
|
||||
class GTSAM_EXPORT OrderingOrdered {
|
||||
protected:
|
||||
typedef FastMap<Key, Index> Map;
|
||||
typedef std::vector<Map::iterator> OrderingIndex;
|
||||
Map order_;
|
||||
OrderingIndex orderingIndex_;
|
||||
|
||||
public:
|
||||
|
||||
typedef boost::shared_ptr<OrderingOrdered> shared_ptr;
|
||||
|
||||
typedef std::pair<const Key, Index> 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<Key> & 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<iterator,bool> 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<boost::assign_detail::call_push_back<OrderingOrdered>, Key>
|
||||
operator+=(Key key) {
|
||||
return boost::assign::make_list_inserter(boost::assign_detail::call_push_back<OrderingOrdered>(*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<class ARCHIVE>
|
||||
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<class ARCHIVE>
|
||||
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<Index> {
|
||||
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
|
||||
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
|
||||
|
||||
Loading…
Reference in New Issue