From 013705232cce425887a47da1bec4ae61205dd166 Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Tue, 8 Jan 2013 23:31:06 +0000 Subject: [PATCH] Maintain reverse-lookup inside of Ordering, to allow fast partial permutations, and modified iSAM2 to use partial permutations on Ordering. Removed InvertedOrdering and updated other GTSAM code and unit tests to use the Ordering reverse-lookup function "key" instead of calculating and inverse ordering. --- gtsam/nonlinear/ISAM2-impl.cpp | 21 ++-- gtsam/nonlinear/ISAM2.cpp | 27 +++-- gtsam/nonlinear/ISAM2.h | 3 - gtsam/nonlinear/LinearContainerFactor.cpp | 48 +------- gtsam/nonlinear/LinearContainerFactor.h | 23 +--- gtsam/nonlinear/NonlinearFactorGraph.cpp | 4 +- gtsam/nonlinear/NonlinearISAM.cpp | 6 +- gtsam/nonlinear/Ordering.cpp | 95 ++++++++------- gtsam/nonlinear/Ordering.h | 136 ++++++++++------------ gtsam/nonlinear/tests/testOrdering.cpp | 63 +++++++--- tests/testGaussianFactorGraphB.cpp | 2 +- 11 files changed, 189 insertions(+), 239 deletions(-) diff --git a/gtsam/nonlinear/ISAM2-impl.cpp b/gtsam/nonlinear/ISAM2-impl.cpp index 2e840fe01..da0a997c2 100644 --- a/gtsam/nonlinear/ISAM2-impl.cpp +++ b/gtsam/nonlinear/ISAM2-impl.cpp @@ -55,10 +55,9 @@ void ISAM2::Impl::AddVariables( if(debug) cout << "Adding variable " << keyFormatter(key_value.key) << " with order " << nextVar << endl; ++ nextVar; } - assert(ordering.nVars() == delta.size()); assert(ordering.size() == delta.size()); } - replacedKeys.resize(ordering.nVars(), false); + replacedKeys.resize(ordering.size(), false); } /* ************************************************************************* */ @@ -115,9 +114,10 @@ void ISAM2::Impl::RemoveVariables(const FastSet& unusedKeys, const ISAM2Cli } // Reorder and remove from ordering and solution - ordering.permuteWithInverse(unusedToEndInverse); + ordering.permuteInPlace(unusedToEnd); BOOST_REVERSE_FOREACH(Key key, unusedKeys) { - ordering.pop_back(key); + Ordering::value_type removed = ordering.pop_back(); + assert(removed.first == key); theta.erase(key); } @@ -188,14 +188,14 @@ void CheckRelinearizationRecursiveDouble(FastSet& relinKeys, double thres } /* ************************************************************************* */ -void CheckRelinearizationRecursiveMap(FastSet& relinKeys, const FastMap& thresholds, const VectorValues& delta, const Ordering::InvertedMap& decoder, const ISAM2Clique::shared_ptr& clique) { +void CheckRelinearizationRecursiveMap(FastSet& relinKeys, const FastMap& thresholds, const VectorValues& delta, const Ordering& ordering, const ISAM2Clique::shared_ptr& clique) { // Check the current clique for relinearization bool relinearize = false; BOOST_FOREACH(Index var, clique->conditional()->keys()) { // Lookup the key associated with this index - Key key = decoder.at(var); + Key key = ordering.key(var); // Find the threshold for this variable type const Vector& threshold = thresholds.find(Symbol(key).chr())->second; @@ -214,7 +214,7 @@ void CheckRelinearizationRecursiveMap(FastSet& relinKeys, const FastMapchildren()) { - CheckRelinearizationRecursiveMap(relinKeys, thresholds, delta, decoder, child); + CheckRelinearizationRecursiveMap(relinKeys, thresholds, delta, ordering, child); } } } @@ -229,8 +229,7 @@ FastSet ISAM2::Impl::CheckRelinearizationPartial(const ISAM2Clique::share if(relinearizeThreshold.type() == typeid(double)) { CheckRelinearizationRecursiveDouble(relinKeys, boost::get(relinearizeThreshold), delta, root); } else if(relinearizeThreshold.type() == typeid(FastMap)) { - Ordering::InvertedMap decoder = ordering.invert(); - CheckRelinearizationRecursiveMap(relinKeys, boost::get >(relinearizeThreshold), delta, decoder, root); + CheckRelinearizationRecursiveMap(relinKeys, boost::get >(relinearizeThreshold), delta, ordering, root); } } @@ -267,8 +266,8 @@ void ISAM2::Impl::ExpmapMasked(Values& values, const VectorValues& delta, const invalidateIfDebug = boost::none; #endif - assert(values.size() == ordering.nVars()); - assert(delta.size() == ordering.nVars()); + assert(values.size() == ordering.size()); + assert(delta.size() == ordering.size()); Values::iterator key_value; Ordering::const_iterator key_index; for(key_value = values.begin(), key_index = ordering.begin(); diff --git a/gtsam/nonlinear/ISAM2.cpp b/gtsam/nonlinear/ISAM2.cpp index 7d579f206..256a3485a 100644 --- a/gtsam/nonlinear/ISAM2.cpp +++ b/gtsam/nonlinear/ISAM2.cpp @@ -353,7 +353,7 @@ boost::shared_ptr > ISAM2::recalculate(const FastSet& mark RgProd_.permuteInPlace(*colamd); gttoc(permute_delta); gttic(permute_ordering); - ordering_.permuteWithInverse(*colamdInverse); + ordering_.permuteInPlace(*colamd); gttoc(permute_ordering); gttoc(reorder); @@ -412,7 +412,7 @@ boost::shared_ptr > ISAM2::recalculate(const FastSet& mark // Reeliminated keys for detailed results if(params_.enableDetailedResults) { BOOST_FOREACH(Index index, affectedAndNewKeys) { - result.detail->variableStatus[inverseOrdering_->at(index)].isReeliminated = true; + result.detail->variableStatus[ordering_.key(index)].isReeliminated = true; } } @@ -450,7 +450,7 @@ boost::shared_ptr > ISAM2::recalculate(const FastSet& mark gttic(PartialSolve); Impl::ReorderingMode reorderingMode; - reorderingMode.nFullSystemVars = ordering_.nVars(); + reorderingMode.nFullSystemVars = ordering_.size(); reorderingMode.algorithm = Impl::ReorderingMode::COLAMD; reorderingMode.constrain = Impl::ReorderingMode::CONSTRAIN_LAST; if(constrainKeys) { @@ -489,7 +489,7 @@ boost::shared_ptr > ISAM2::recalculate(const FastSet& mark RgProd_.permuteInPlace(partialSolveResult.reorderingSelector, partialSolveResult.reorderingPermutation); gttoc(permute_delta); gttic(permute_ordering); - ordering_.reduceWithInverse(partialSolveResult.reorderingInverse); + ordering_.permuteInPlace(partialSolveResult.reorderingSelector, partialSolveResult.reorderingPermutation); gttoc(permute_ordering); if(params_.cacheLinearizedFactors) { gttic(permute_cached_linear); @@ -538,7 +538,7 @@ boost::shared_ptr > ISAM2::recalculate(const FastSet& mark // Root clique variables for detailed results if(params_.enableDetailedResults) { BOOST_FOREACH(Index index, this->root()->conditional()->frontals()) { - result.detail->variableStatus[inverseOrdering_->at(index)].inRootClique = true; + result.detail->variableStatus[ordering_.key(index)].inRootClique = true; } } @@ -628,7 +628,6 @@ ISAM2Result ISAM2::update( Impl::AddVariables(newTheta, theta_, delta_, deltaNewton_, RgProd_, deltaReplacedMask_, ordering_); // New keys for detailed results if(params_.enableDetailedResults) { - inverseOrdering_ = ordering_.invert(); BOOST_FOREACH(Key key, newTheta.keys()) { result.detail->variableStatus[key].isNew = true; } } gttoc(add_new_variables); @@ -649,7 +648,7 @@ ISAM2Result ISAM2::update( // Observed keys for detailed results if(params_.enableDetailedResults) { BOOST_FOREACH(Index index, markedKeys) { - result.detail->variableStatus[inverseOrdering_->at(index)].isObserved = true; + result.detail->variableStatus[ordering_.key(index)].isObserved = true; } } // NOTE: we use assign instead of the iterator constructor here because this @@ -666,7 +665,7 @@ ISAM2Result ISAM2::update( FastSet relinKeys; if (relinearizeThisStep) { gttic(gather_relinearize_keys); - vector markedRelinMask(ordering_.nVars(), false); + vector markedRelinMask(ordering_.size(), false); // 4. Mark keys in \Delta above threshold \beta: J=\{\Delta_{j}\in\Delta|\Delta_{j}\geq\beta\}. if(params_.enablePartialRelinearizationCheck) relinKeys = Impl::CheckRelinearizationPartial(root_, delta_, ordering_, params_.relinearizeThreshold); @@ -677,8 +676,8 @@ ISAM2Result ISAM2::update( // Above relin threshold keys for detailed results if(params_.enableDetailedResults) { BOOST_FOREACH(Index index, relinKeys) { - result.detail->variableStatus[inverseOrdering_->at(index)].isAboveRelinThreshold = true; - result.detail->variableStatus[inverseOrdering_->at(index)].isRelinearized = true; } } + result.detail->variableStatus[ordering_.key(index)].isAboveRelinThreshold = true; + result.detail->variableStatus[ordering_.key(index)].isRelinearized = true; } } // Add the variables being relinearized to the marked keys BOOST_FOREACH(const Index j, relinKeys) { markedRelinMask[j] = true; } @@ -696,9 +695,9 @@ ISAM2Result ISAM2::update( FastSet involvedRelinKeys; Impl::FindAll(this->root(), involvedRelinKeys, markedRelinMask); BOOST_FOREACH(Index index, involvedRelinKeys) { - if(!result.detail->variableStatus[inverseOrdering_->at(index)].isAboveRelinThreshold) { - result.detail->variableStatus[inverseOrdering_->at(index)].isRelinearizeInvolved = true; - result.detail->variableStatus[inverseOrdering_->at(index)].isRelinearized = true; } } + if(!result.detail->variableStatus[ordering_.key(index)].isAboveRelinThreshold) { + result.detail->variableStatus[ordering_.key(index)].isRelinearizeInvolved = true; + result.detail->variableStatus[ordering_.key(index)].isRelinearized = true; } } } } gttoc(fluid_find_all); @@ -818,7 +817,7 @@ Values ISAM2::calculateEstimate() const { const VectorValues& delta(getDelta()); gttoc(getDelta); gttic(Expmap); - vector mask(ordering_.nVars(), true); + vector mask(ordering_.size(), true); Impl::ExpmapMasked(ret, delta, ordering_, mask); gttoc(Expmap); return ret; diff --git a/gtsam/nonlinear/ISAM2.h b/gtsam/nonlinear/ISAM2.h index 51a876031..5243efd27 100644 --- a/gtsam/nonlinear/ISAM2.h +++ b/gtsam/nonlinear/ISAM2.h @@ -502,9 +502,6 @@ protected: /** The current Dogleg Delta (trust region radius) */ mutable boost::optional doglegDelta_; - /** The inverse ordering, only used for creating ISAM2Result::DetailedResults */ - boost::optional inverseOrdering_; - public: typedef ISAM2 This; ///< This class diff --git a/gtsam/nonlinear/LinearContainerFactor.cpp b/gtsam/nonlinear/LinearContainerFactor.cpp index 2fe39f5f1..c0c072571 100644 --- a/gtsam/nonlinear/LinearContainerFactor.cpp +++ b/gtsam/nonlinear/LinearContainerFactor.cpp @@ -13,14 +13,8 @@ namespace gtsam { /* ************************************************************************* */ void LinearContainerFactor::rekeyFactor(const Ordering& ordering) { - Ordering::InvertedMap invOrdering = ordering.invert(); // TODO: inefficient - make more selective ordering invert - rekeyFactor(invOrdering); -} - -/* ************************************************************************* */ -void LinearContainerFactor::rekeyFactor(const Ordering::InvertedMap& invOrdering) { BOOST_FOREACH(Index& idx, factor_->keys()) { - Key fullKey = invOrdering.find(idx)->second; + Key fullKey = ordering.key(idx); idx = fullKey; keys_.push_back(fullKey); } @@ -86,34 +80,6 @@ LinearContainerFactor::LinearContainerFactor( initializeLinearizationPoint(linearizationPoint); } -/* ************************************************************************* */ -LinearContainerFactor::LinearContainerFactor(const JacobianFactor& factor, - const Ordering::InvertedMap& inverted_ordering, - const Values& linearizationPoint) -: factor_(factor.clone()) { - rekeyFactor(inverted_ordering); - initializeLinearizationPoint(linearizationPoint); -} - -/* ************************************************************************* */ -LinearContainerFactor::LinearContainerFactor(const HessianFactor& factor, - const Ordering::InvertedMap& inverted_ordering, - const Values& linearizationPoint) -: factor_(factor.clone()) { - rekeyFactor(inverted_ordering); - initializeLinearizationPoint(linearizationPoint); -} - -/* ************************************************************************* */ -LinearContainerFactor::LinearContainerFactor( - const GaussianFactor::shared_ptr& factor, - const Ordering::InvertedMap& ordering, - const Values& linearizationPoint) -: factor_(factor->clone()) { - rekeyFactor(ordering); - initializeLinearizationPoint(linearizationPoint); -} - /* ************************************************************************* */ void LinearContainerFactor::print(const std::string& s, const KeyFormatter& keyFormatter) const { Base::print(s+"LinearContainerFactor", keyFormatter); @@ -215,9 +181,8 @@ GaussianFactor::shared_ptr LinearContainerFactor::linearize( } // reset ordering - Ordering::InvertedMap invLocalOrdering = localOrdering.invert(); BOOST_FOREACH(Index& idx, linFactor->keys()) - idx = ordering[invLocalOrdering[idx] ]; + idx = ordering[localOrdering.key(idx)]; return linFactor; } @@ -259,18 +224,11 @@ NonlinearFactor::shared_ptr LinearContainerFactor::negate() const { NonlinearFactorGraph LinearContainerFactor::convertLinearGraph( const GaussianFactorGraph& linear_graph, const Ordering& ordering, const Values& linearizationPoint) { - return convertLinearGraph(linear_graph, ordering.invert()); -} - -/* ************************************************************************* */ -NonlinearFactorGraph LinearContainerFactor::convertLinearGraph( - const GaussianFactorGraph& linear_graph, const InvertedOrdering& invOrdering, - const Values& linearizationPoint) { NonlinearFactorGraph result; BOOST_FOREACH(const GaussianFactor::shared_ptr& f, linear_graph) if (f) result.push_back(NonlinearFactorGraph::sharedFactor( - new LinearContainerFactor(f, invOrdering, linearizationPoint))); + new LinearContainerFactor(f, ordering, linearizationPoint))); return result; } diff --git a/gtsam/nonlinear/LinearContainerFactor.h b/gtsam/nonlinear/LinearContainerFactor.h index b2b3f9645..30a0c33d7 100644 --- a/gtsam/nonlinear/LinearContainerFactor.h +++ b/gtsam/nonlinear/LinearContainerFactor.h @@ -45,21 +45,6 @@ public: LinearContainerFactor(const GaussianFactor::shared_ptr& factor, const Values& linearizationPoint = Values()); - /** Alternate constructor: store a linear factor and decode keys with inverted ordering*/ - LinearContainerFactor(const JacobianFactor& factor, - const Ordering::InvertedMap& inverted_ordering, - const Values& linearizationPoint = Values()); - - /** Alternate constructor: store a linear factor and decode keys with inverted ordering*/ - LinearContainerFactor(const HessianFactor& factor, - const Ordering::InvertedMap& inverted_ordering, - const Values& linearizationPoint = Values()); - - /** Constructor from shared_ptr with inverted ordering*/ - LinearContainerFactor(const GaussianFactor::shared_ptr& factor, - const Ordering::InvertedMap& ordering, - const Values& linearizationPoint = Values()); - // Access const GaussianFactor::shared_ptr& factor() const { return factor_; } @@ -130,19 +115,13 @@ public: /** * Utility function for converting linear graphs to nonlinear graphs - * consisting of LinearContainerFactors. Two versions are available, using - * either the ordering the linear graph was linearized around, or the inverse ordering. - * If the inverse ordering is present, it will be faster. + * consisting of LinearContainerFactors. */ static NonlinearFactorGraph convertLinearGraph(const GaussianFactorGraph& linear_graph, const Ordering& ordering, const Values& linearizationPoint = Values()); - static NonlinearFactorGraph convertLinearGraph(const GaussianFactorGraph& linear_graph, - const InvertedOrdering& invOrdering, const Values& linearizationPoint = Values()); - protected: void rekeyFactor(const Ordering& ordering); - void rekeyFactor(const Ordering::InvertedMap& invOrdering); void initializeLinearizationPoint(const Values& linearizationPoint); }; // \class LinearContainerFactor diff --git a/gtsam/nonlinear/NonlinearFactorGraph.cpp b/gtsam/nonlinear/NonlinearFactorGraph.cpp index 0479b06a5..b84d39a09 100644 --- a/gtsam/nonlinear/NonlinearFactorGraph.cpp +++ b/gtsam/nonlinear/NonlinearFactorGraph.cpp @@ -223,7 +223,7 @@ Ordering::shared_ptr NonlinearFactorGraph::orderingCOLAMD( variableIndex)); // Permute the Ordering with the COLAMD ordering - ordering->permuteWithInverse(*colamdPerm->inverse()); + ordering->permuteInPlace(*colamdPerm); return ordering; } @@ -254,7 +254,7 @@ Ordering::shared_ptr NonlinearFactorGraph::orderingCOLAMDConstrained(const Value variableIndex, index_constraints)); // Permute the Ordering with the COLAMD ordering - ordering->permuteWithInverse(*colamdPerm->inverse()); + ordering->permuteInPlace(*colamdPerm); return ordering; } diff --git a/gtsam/nonlinear/NonlinearISAM.cpp b/gtsam/nonlinear/NonlinearISAM.cpp index 6cd917212..214c1c887 100644 --- a/gtsam/nonlinear/NonlinearISAM.cpp +++ b/gtsam/nonlinear/NonlinearISAM.cpp @@ -55,10 +55,8 @@ void NonlinearISAM::update(const NonlinearFactorGraph& newFactors, // Augment ordering // TODO: allow for ordering constraints within the new variables - // FIXME: should just loop over new values - BOOST_FOREACH(const NonlinearFactorGraph::sharedFactor& factor, newFactors) - BOOST_FOREACH(Key key, factor->keys()) - ordering_.tryInsert(key, ordering_.nVars()); // will do nothing if already present + BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, initialValues) + ordering_.insert(key_value.key, ordering_.size()); boost::shared_ptr linearizedNewFactors = newFactors.linearize(linPoint_, ordering_); diff --git a/gtsam/nonlinear/Ordering.cpp b/gtsam/nonlinear/Ordering.cpp index 576e7fbb6..4ab3a2b75 100644 --- a/gtsam/nonlinear/Ordering.cpp +++ b/gtsam/nonlinear/Ordering.cpp @@ -26,24 +26,53 @@ using namespace std; namespace gtsam { /* ************************************************************************* */ -Ordering::Ordering(const std::list & L):nVars_(0) { +Ordering::Ordering(const std::list & L) { int i = 0; BOOST_FOREACH( Key s, L ) insert(s, i++) ; } /* ************************************************************************* */ -void Ordering::permuteWithInverse(const Permutation& inversePermutation) { - gttic(Ordering_permuteWithInverse); - BOOST_FOREACH(Ordering::value_type& key_order, *this) { - key_order.second = inversePermutation[key_order.second]; - } +Ordering::Ordering(const Ordering& other) : order_(other.order_), orderingIndex_(other.size()) { + for(iterator item = order_.begin(); item != order_.end(); ++item) + orderingIndex_[item->second] = item; } /* ************************************************************************* */ -void Ordering::reduceWithInverse(const internal::Reduction& inverseReduction) { - BOOST_FOREACH(Ordering::value_type& key_order, *this) { - key_order.second = inverseReduction[key_order.second]; +Ordering& Ordering::operator=(const Ordering& rhs) { + order_ = rhs.order_; + orderingIndex_.resize(rhs.size()); + for(iterator item = order_.begin(); item != order_.end(); ++item) + orderingIndex_[item->second] = item; + return *this; +} + +/* ************************************************************************* */ +void Ordering::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 Ordering::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]; } } @@ -51,16 +80,15 @@ void Ordering::reduceWithInverse(const internal::Reduction& inverseReduction) { void Ordering::print(const string& str, const KeyFormatter& keyFormatter) const { cout << str; // Print ordering in index order - Ordering::InvertedMap inverted = this->invert(); // 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 Ordering::InvertedMap::value_type& index_key, inverted) { - if(index_key.first % varsPerLine != 0) + BOOST_FOREACH(const Map::iterator& index_key, orderingIndex_) { + if(index_key->second % varsPerLine != 0) cout << ", "; - cout << index_key.first << ":" << keyFormatter(index_key.second); - if(index_key.first % varsPerLine == varsPerLine - 1) { + cout << index_key->second<< ":" << keyFormatter(index_key->first); + if(index_key->second % varsPerLine == varsPerLine - 1) { cout << "\n"; endedOnNewline = true; } else { @@ -79,40 +107,11 @@ bool Ordering::equals(const Ordering& rhs, double tol) const { /* ************************************************************************* */ Ordering::value_type Ordering::pop_back() { - // FIXME: is there a way of doing this without searching over the entire structure? - for (iterator it=begin(); it!=end(); ++it) { - if (it->second == nVars_ - 1) { - value_type result = *it; - order_.erase(it); - --nVars_; - return result; - } - } - return value_type(); -} - -/* ************************************************************************* */ -Index Ordering::pop_back(Key key) { - Map::iterator item = order_.find(key); - if(item == order_.end()) { - throw invalid_argument("Attempting to remove a key from an ordering that does not contain that key"); - } else { - if(item->second != nVars_ - 1) { - throw invalid_argument("Attempting to remove a key from an ordering in which that key is not last"); - } else { - order_.erase(item); - -- nVars_; - return nVars_; - } - } -} - -/* ************************************************************************* */ -Ordering::InvertedMap Ordering::invert() const { - InvertedMap result; - BOOST_FOREACH(const value_type& p, *this) - result.insert(make_pair(p.second, p.first)); - return result; + 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 } /* ************************************************************************* */ diff --git a/gtsam/nonlinear/Ordering.h b/gtsam/nonlinear/Ordering.h index 60a21e5a1..3ee2da832 100644 --- a/gtsam/nonlinear/Ordering.h +++ b/gtsam/nonlinear/Ordering.h @@ -24,7 +24,7 @@ #include #include #include -#include +#include namespace gtsam { @@ -35,8 +35,9 @@ namespace gtsam { class Ordering { protected: typedef FastMap Map; + typedef std::vector OrderingIndex; Map order_; - Index nVars_; + OrderingIndex orderingIndex_; public: @@ -51,25 +52,33 @@ public: /// @{ /// Default constructor for empty ordering - Ordering() : nVars_(0) {} + Ordering() {} + + /// Copy constructor + Ordering(const Ordering& other); /// Construct from list, assigns order indices sequentially to list items. - Ordering(const std::list & L) ; + Ordering(const std::list & L); + + /// Assignment operator + Ordering& operator=(const Ordering& rhs); /// @} /// @name Standard Interface /// @{ - /** One greater than the maximum ordering index, i.e. including missing indices in the count. See also size(). */ - Index nVars() const { return nVars_; } - /** The actual number of variables in this ordering, i.e. not including missing indices in the count. See also nVars(). */ - Index size() const { return order_.size(); } + 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 @@ -85,8 +94,7 @@ public: index = i->second; return true; } else - return false; - } + 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 @@ -122,23 +130,25 @@ public: */ const_iterator find(Key key) const { return order_.find(key); } - /** - * Attempts to insert a symbol/order pair with same semantics as stl::Map::insert(), - * i.e., returns a pair of iterator and success (false if already present) - */ - std::pair tryInsert(const value_type& key_order) { - std::pair it_ok(order_.insert(key_order)); - if(it_ok.second == true && key_order.second+1 > nVars_) - nVars_ = key_order.second+1; - return it_ok; - } - - /** Try insert, but will fail if the key is already present */ + /** 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(tryInsert(key_order)); - if(!it_ok.second) throw std::invalid_argument(std::string("Attempting to insert a key into an ordering that already contains that key")); - else return it_ok.first; - } + 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 @@ -154,18 +164,6 @@ public: */ iterator end() { return order_.end(); } - /// Test if the key exists in the ordering. - bool exists(Key key) const { return order_.count(key) > 0; } - - ///TODO: comment - std::pair tryInsert(Key key, Index order) { return tryInsert(std::make_pair(key,order)); } - - ///TODO: comment - 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, nVars_))->second; } - /** 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). @@ -177,16 +175,6 @@ public: */ value_type pop_back(); - /** Remove the last-ordered symbol from the ordering (this version is - * \f$ O(\log n) \f$, use it if you already know the last-ordered key). - * - * Throws std::invalid_argument if the requested key is not actually the - * last-ordered. - * - * @return The index of the symbol that was removed. - */ - Index pop_back(Key key); - /** * += operator allows statements like 'ordering += x0,x1,x2,x3;', which are * very useful for unit tests. This functionality is courtesy of @@ -201,24 +189,13 @@ public: * internally, permuting an initial key-sorted ordering into a fill-reducing * ordering. */ - void permuteWithInverse(const Permutation& inversePermutation); + void permuteInPlace(const Permutation& permutation); - /** - * Reorder the variables with a permutation. This is typically used - * internally, permuting an initial key-sorted ordering into a fill-reducing - * ordering. - */ - void reduceWithInverse(const internal::Reduction& inverseReduction); + void permuteInPlace(const Permutation& selector, const Permutation& permutation); /// Synonym for operator[](Key) Index& at(Key key) { return operator[](key); } - /** - * Create an inverse mapping from Index->Key, useful for decoding linear systems - * @return inverse mapping structure - */ - InvertedMap invert() const; - /// @} /// @name Testable /// @{ @@ -235,16 +212,26 @@ private: /** Serialization function */ friend class boost::serialization::access; - template - void serialize(ARCHIVE & ar, const unsigned int version) { + template + void save(ARCHIVE & ar, const unsigned int version) const + { ar & BOOST_SERIALIZATION_NVP(order_); - ar & BOOST_SERIALIZATION_NVP(nVars_); - } + 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 -// typedef for use with matlab -typedef Ordering::InvertedMap InvertedOrdering; - /** * @class Unordered * @brief a set of unordered indices @@ -270,14 +257,15 @@ public: // 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. -struct OrderingIndexFormatter { - Ordering::InvertedMap inverseOrdering; - const KeyFormatter& keyFormatter; +class OrderingIndexFormatter { +private: + Ordering ordering_; + KeyFormatter keyFormatter_; +public: OrderingIndexFormatter(const Ordering& ordering, const KeyFormatter& keyFormatter) : - inverseOrdering(ordering.invert()), keyFormatter(keyFormatter) {} + ordering_(ordering), keyFormatter_(keyFormatter) {} std::string operator()(Index index) { - return keyFormatter(inverseOrdering.at(index)); - } + return keyFormatter_(ordering_.key(index)); } }; } // \namespace gtsam diff --git a/gtsam/nonlinear/tests/testOrdering.cpp b/gtsam/nonlinear/tests/testOrdering.cpp index 42a176761..a62278d1e 100644 --- a/gtsam/nonlinear/tests/testOrdering.cpp +++ b/gtsam/nonlinear/tests/testOrdering.cpp @@ -23,26 +23,31 @@ using namespace std; using namespace gtsam; /* ************************************************************************* */ -TEST( testOrdering, simple_modifications ) { +TEST( Ordering, simple_modifications ) { Ordering ordering; // create an ordering Symbol x1('x', 1), x2('x', 2), x3('x', 3), x4('x', 4); ordering += x1, x2, x3, x4; + EXPECT_LONGS_EQUAL(0, ordering[x1]); + EXPECT_LONGS_EQUAL(1, ordering[x2]); + EXPECT_LONGS_EQUAL(2, ordering[x3]); + EXPECT_LONGS_EQUAL(3, ordering[x4]); + EXPECT_LONGS_EQUAL(Key(x1), ordering.key(0)); + EXPECT_LONGS_EQUAL(Key(x2), ordering.key(1)); + EXPECT_LONGS_EQUAL(Key(x3), ordering.key(2)); + EXPECT_LONGS_EQUAL(Key(x4), ordering.key(3)); + // pop the last two elements Ordering::value_type x4p = ordering.pop_back(); EXPECT_LONGS_EQUAL(3, ordering.size()); EXPECT(assert_equal(x4, x4p.first)); - Index x3p = ordering.pop_back(x3); + Index x3p = ordering.pop_back().second; EXPECT_LONGS_EQUAL(2, ordering.size()); EXPECT_LONGS_EQUAL(2, (int)x3p); - // try to pop an element that doesn't exist and isn't last - CHECK_EXCEPTION(ordering.pop_back(x4), std::invalid_argument); - CHECK_EXCEPTION(ordering.pop_back(x1), std::invalid_argument); - // reassemble back make the ordering 1, 2, 4, 3 EXPECT_LONGS_EQUAL(2, ordering.push_back(x4)); EXPECT_LONGS_EQUAL(3, ordering.push_back(x3)); @@ -50,6 +55,9 @@ TEST( testOrdering, simple_modifications ) { EXPECT_LONGS_EQUAL(2, ordering[x4]); EXPECT_LONGS_EQUAL(3, ordering[x3]); + EXPECT_LONGS_EQUAL(Key(x4), ordering.key(2)); + EXPECT_LONGS_EQUAL(Key(x3), ordering.key(3)); + // verify Ordering expectedFinal; expectedFinal += x1, x2, x4, x3; @@ -57,7 +65,36 @@ TEST( testOrdering, simple_modifications ) { } /* ************************************************************************* */ -TEST( testOrdering, invert ) { +TEST(Ordering, permute) { + Ordering ordering; + ordering += 2, 4, 6, 8; + + Ordering expected; + expected += 2, 8, 6, 4; + + Permutation permutation(4); + permutation[0] = 0; + permutation[1] = 3; + permutation[2] = 2; + permutation[3] = 1; + + Ordering actual = ordering; + actual.permuteInPlace(permutation); + + EXPECT(assert_equal(expected, actual)); + + EXPECT_LONGS_EQUAL(0, actual[2]); + EXPECT_LONGS_EQUAL(1, actual[8]); + EXPECT_LONGS_EQUAL(2, actual[6]); + EXPECT_LONGS_EQUAL(3, actual[4]); + EXPECT_LONGS_EQUAL(2, actual.key(0)); + EXPECT_LONGS_EQUAL(8, actual.key(1)); + EXPECT_LONGS_EQUAL(6, actual.key(2)); + EXPECT_LONGS_EQUAL(4, actual.key(3)); +} + +/* ************************************************************************* */ +TEST( Ordering, invert ) { // creates a map with the opposite mapping: Index->Key Ordering ordering; @@ -65,14 +102,10 @@ TEST( testOrdering, invert ) { Symbol x1('x', 1), x2('x', 2), x3('x', 3), x4('x', 4); ordering += x1, x2, x3, x4; - Ordering::InvertedMap actual = ordering.invert(); - Ordering::InvertedMap expected; - expected.insert(make_pair(0, x1)); - expected.insert(make_pair(1, x2)); - expected.insert(make_pair(2, x3)); - expected.insert(make_pair(3, x4)); - - EXPECT(assert_container_equality(expected, actual)); + EXPECT_LONGS_EQUAL(Key(x1), ordering.key(0)); + EXPECT_LONGS_EQUAL(Key(x2), ordering.key(1)); + EXPECT_LONGS_EQUAL(Key(x3), ordering.key(2)); + EXPECT_LONGS_EQUAL(Key(x4), ordering.key(3)); } /* ************************************************************************* */ diff --git a/tests/testGaussianFactorGraphB.cpp b/tests/testGaussianFactorGraphB.cpp index 94476d87a..b8bea8c6e 100644 --- a/tests/testGaussianFactorGraphB.cpp +++ b/tests/testGaussianFactorGraphB.cpp @@ -252,7 +252,7 @@ TEST( GaussianFactorGraph, getOrdering) Ordering original; original += L(1),X(1),X(2); FactorGraph symbolic(createGaussianFactorGraph(original)); Permutation perm(*inference::PermutationCOLAMD(VariableIndex(symbolic))); - Ordering actual = original; actual.permuteWithInverse((*perm.inverse())); + Ordering actual = original; actual.permuteInPlace(perm); Ordering expected; expected += L(1),X(2),X(1); EXPECT(assert_equal(expected,actual)); }