diff --git a/gtsam/inference/BayesNet-inl.h b/gtsam/inference/BayesNet-inl.h index 0fe78fbed..5375b209f 100644 --- a/gtsam/inference/BayesNet-inl.h +++ b/gtsam/inference/BayesNet-inl.h @@ -118,16 +118,16 @@ namespace gtsam { } /* ************************************************************************* */ - template - bool BayesNet::permuteSeparatorWithInverse( - const Permutation& inversePermutation) { - bool separatorChanged = false; - BOOST_FOREACH(sharedConditional conditional, conditionals_) { - if (conditional->permuteSeparatorWithInverse(inversePermutation)) - separatorChanged = true; - } - return separatorChanged; - } + //template + //bool BayesNet::permuteSeparatorWithInverse( + // const Permutation& inversePermutation) { + // bool separatorChanged = false; + // BOOST_FOREACH(sharedConditional conditional, conditionals_) { + // if (conditional->permuteSeparatorWithInverse(inversePermutation)) + // separatorChanged = true; + // } + // return separatorChanged; + //} /* ************************************************************************* */ template diff --git a/gtsam/inference/BayesNet.h b/gtsam/inference/BayesNet.h index 1077c1cad..f8aaa7d17 100644 --- a/gtsam/inference/BayesNet.h +++ b/gtsam/inference/BayesNet.h @@ -225,7 +225,7 @@ public: * Returns true if any reordered variables appeared in the separator and * false if not. */ - bool permuteSeparatorWithInverse(const Permutation& inversePermutation); + //bool permuteSeparatorWithInverse(const Permutation& inversePermutation); iterator begin() {return conditionals_.begin();} /// +// bool BayesTreeCliqueBase::permuteSeparatorWithInverse( +// const Permutation& inversePermutation) { +// bool changed = conditional_->permuteSeparatorWithInverse( +// inversePermutation); +//#ifndef NDEBUG +// if(!changed) { +// BOOST_FOREACH(Index& separatorKey, conditional_->parents()) {assert(separatorKey == inversePermutation[separatorKey]);} +// BOOST_FOREACH(const derived_ptr& child, children_) { +// assert(child->permuteSeparatorWithInverse(inversePermutation) == false); +// } +// } +//#endif +// if (changed) { +// BOOST_FOREACH(const derived_ptr& child, children_) { +// (void) child->permuteSeparatorWithInverse(inversePermutation); +// } +// } +// assertInvariants(); +// return changed; +// } + /* ************************************************************************* */ template - bool BayesTreeCliqueBase::permuteSeparatorWithInverse( - const Permutation& inversePermutation) { - bool changed = conditional_->permuteSeparatorWithInverse( - inversePermutation); + bool BayesTreeCliqueBase::reduceSeparatorWithInverse( + const internal::Reduction& inverseReduction) + { + bool changed = conditional_->reduceSeparatorWithInverse(inverseReduction); #ifndef NDEBUG if(!changed) { - BOOST_FOREACH(Index& separatorKey, conditional_->parents()) {assert(separatorKey == inversePermutation[separatorKey]);} BOOST_FOREACH(const derived_ptr& child, children_) { - assert(child->permuteSeparatorWithInverse(inversePermutation) == false); - } + assert(child->reduceSeparatorWithInverse(inverseReduction) == false); } } #endif - if (changed) { + if(changed) { BOOST_FOREACH(const derived_ptr& child, children_) { - (void) child->permuteSeparatorWithInverse(inversePermutation); - } + (void) child->reduceSeparatorWithInverse(inverseReduction); } } assertInvariants(); return changed; diff --git a/gtsam/inference/BayesTreeCliqueBase.h b/gtsam/inference/BayesTreeCliqueBase.h index 487d10a0d..a00980983 100644 --- a/gtsam/inference/BayesTreeCliqueBase.h +++ b/gtsam/inference/BayesTreeCliqueBase.h @@ -183,7 +183,14 @@ namespace gtsam { * traversing the whole tree. Returns whether any separator variables in * this subtree were reordered. */ - bool permuteSeparatorWithInverse(const Permutation& inversePermutation); + //bool permuteSeparatorWithInverse(const Permutation& inversePermutation); + + /** Permute variables when they only appear in the separators. In this + * case the running intersection property will be used to prevent always + * traversing the whole tree. Returns whether any separator variables in + * this subtree were reordered. + */ + bool reduceSeparatorWithInverse(const internal::Reduction& inverseReduction); /** return the conditional P(S|Root) on the separator given the root */ BayesNet shortcut(derived_ptr root, Eliminate function) const; diff --git a/gtsam/inference/IndexConditional.cpp b/gtsam/inference/IndexConditional.cpp index e0993a9a1..1a5e6d516 100644 --- a/gtsam/inference/IndexConditional.cpp +++ b/gtsam/inference/IndexConditional.cpp @@ -44,16 +44,33 @@ namespace gtsam { } /* ************************************************************************* */ - bool IndexConditional::permuteSeparatorWithInverse(const Permutation& inversePermutation) { - #ifndef NDEBUG - BOOST_FOREACH(KeyType key, frontals()) { assert(key == inversePermutation[key]); } - #endif + //bool IndexConditional::permuteSeparatorWithInverse(const Permutation& inversePermutation) { + //#ifndef NDEBUG + // BOOST_FOREACH(KeyType key, frontals()) { assert(key == inversePermutation[key]); } + //#endif + // bool parentChanged = false; + // BOOST_FOREACH(KeyType& parent, parents()) { + // KeyType newParent = inversePermutation[parent]; + // if(parent != newParent) { + // parentChanged = true; + // parent = newParent; + // } + // } + // assertInvariants(); + // return parentChanged; + //} + + /* ************************************************************************* */ + bool IndexConditional::reduceSeparatorWithInverse(const internal::Reduction& inverseReduction) { +#ifndef NDEBUG + BOOST_FOREACH(KeyType key, frontals()) { assert(inverseReduction.find(key) == inverseReduction.end()); } +#endif bool parentChanged = false; BOOST_FOREACH(KeyType& parent, parents()) { - KeyType newParent = inversePermutation[parent]; - if(parent != newParent) { + internal::Reduction::const_iterator it = inverseReduction.find(parent); + if(it != inverseReduction.end()) { parentChanged = true; - parent = newParent; + parent = it->second; } } assertInvariants(); diff --git a/gtsam/inference/IndexConditional.h b/gtsam/inference/IndexConditional.h index 895592547..9643b1a84 100644 --- a/gtsam/inference/IndexConditional.h +++ b/gtsam/inference/IndexConditional.h @@ -112,7 +112,13 @@ namespace gtsam { * Returns true if any reordered variables appeared in the separator and * false if not. */ - bool permuteSeparatorWithInverse(const Permutation& inversePermutation); + //bool permuteSeparatorWithInverse(const Permutation& inversePermutation); + + /** Permute the variables when only separator variables need to be permuted. + * Returns true if any reordered variables appeared in the separator and + * false if not. + */ + bool reduceSeparatorWithInverse(const internal::Reduction& inverseReduction); /** * Permutes the Conditional, but for efficiency requires the permutation diff --git a/gtsam/inference/Permutation.cpp b/gtsam/inference/Permutation.cpp index 883041b86..bd6d50c22 100644 --- a/gtsam/inference/Permutation.cpp +++ b/gtsam/inference/Permutation.cpp @@ -161,6 +161,16 @@ namespace internal { return result; } + /* ************************************************************************* */ + Reduction Reduction::CreateFromPartialPermutation(const Permutation& selector, const Permutation& p) { + if(selector.size() != p.size()) + throw invalid_argument("internal::Reduction::CreateFromPartialPermutation called with selector and permutation of different sizes"); + Reduction result; + for(size_t dstSlot = 0; dstSlot < p.size(); ++dstSlot) + result.insert(make_pair(selector[dstSlot], selector[p[dstSlot]])); + return result; + } + /* ************************************************************************* */ void Reduction::applyInverse(std::vector& js) const { BOOST_FOREACH(Index& j, js) { @@ -183,10 +193,10 @@ namespace internal { } /* ************************************************************************* */ - Index& Reduction::operator[](const Index& j) { + const Index& Reduction::operator[](const Index& j) { iterator it = this->find(j); if(it == this->end()) - throw std::out_of_range("Index to Reduction::operator[] not present"); + return j; else return it->second; } @@ -195,7 +205,7 @@ namespace internal { const Index& Reduction::operator[](const Index& j) const { const_iterator it = this->find(j); if(it == this->end()) - throw std::out_of_range("Index to Reduction::operator[] not present"); + return j; else return it->second; } @@ -207,6 +217,11 @@ namespace internal { cout << " " << p.first << " : " << p.second << endl; } + /* ************************************************************************* */ + bool Reduction::equals(const Reduction& other, double tol) const { + return (const Base&)(*this) == (const Base&)other; + } + /* ************************************************************************* */ Permutation createReducingPermutation(const std::set& indices) { Permutation p(indices.size()); diff --git a/gtsam/inference/Permutation.h b/gtsam/inference/Permutation.h index 6014d704e..4fbd06199 100644 --- a/gtsam/inference/Permutation.h +++ b/gtsam/inference/Permutation.h @@ -192,12 +192,14 @@ namespace internal { typedef gtsam::FastMap Base; static Reduction CreateAsInverse(const Permutation& p); + static Reduction CreateFromPartialPermutation(const Permutation& selector, const Permutation& p); void applyInverse(std::vector& js) const; Permutation inverse() const; - Index& operator[](const Index& j); + const Index& operator[](const Index& j); const Index& operator[](const Index& j) const; void print(const std::string& s="") const; + bool equals(const Reduction& other, double tol = 1e-9) const; }; // Reduce the variable indices so that those in the set are mapped to start at zero diff --git a/gtsam/inference/VariableIndex.cpp b/gtsam/inference/VariableIndex.cpp index 7e51c3d37..11ce68ec6 100644 --- a/gtsam/inference/VariableIndex.cpp +++ b/gtsam/inference/VariableIndex.cpp @@ -76,6 +76,20 @@ void VariableIndex::permuteInPlace(const Permutation& permutation) { index_.swap(newIndex); } +/* ************************************************************************* */ +void VariableIndex::permuteInPlace(const Permutation& selector, const Permutation& permutation) { + if(selector.size() != permutation.size()) + throw invalid_argument("VariableIndex::permuteInPlace (partial permutation version) called with selector and permutation of different sizes."); + // Create new index the size of the permuted entries + vector newIndex(selector.size()); + // Permute the affected entries into the new index + for(size_t dstSlot = 0; dstSlot < selector.size(); ++dstSlot) + newIndex[dstSlot].swap(this->index_[selector[permutation[dstSlot]]]); + // Put the affected entries back in the new order + for(size_t slot = 0; slot < selector.size(); ++slot) + this->index_[selector[slot]].swap(newIndex[slot]); +} + /* ************************************************************************* */ void VariableIndex::removeUnusedAtEnd(size_t nToRemove) { #ifndef NDEBUG diff --git a/gtsam/inference/VariableIndex.h b/gtsam/inference/VariableIndex.h index d465b57eb..149f8eaaf 100644 --- a/gtsam/inference/VariableIndex.h +++ b/gtsam/inference/VariableIndex.h @@ -135,6 +135,9 @@ public: /// Permute the variables in the VariableIndex according to the given permutation void permuteInPlace(const Permutation& permutation); + /// Permute the variables in the VariableIndex according to the given partial permutation + void permuteInPlace(const Permutation& selector, const Permutation& permutation); + /** Remove unused empty variables at the end of the ordering (in debug mode * verifies they are empty). * @param nToRemove The number of unused variables at the end to remove diff --git a/gtsam/inference/tests/testPermutation.cpp b/gtsam/inference/tests/testPermutation.cpp index 1ccafb1a2..98e94b4f7 100644 --- a/gtsam/inference/tests/testPermutation.cpp +++ b/gtsam/inference/tests/testPermutation.cpp @@ -25,6 +25,7 @@ using namespace gtsam; using namespace std; +/* ************************************************************************* */ TEST(Permutation, Identity) { Permutation expected(5); expected[0] = 0; @@ -38,6 +39,7 @@ TEST(Permutation, Identity) { EXPECT(assert_equal(expected, actual)); } +/* ************************************************************************* */ TEST(Permutation, PullToFront) { Permutation expected(5); expected[0] = 4; @@ -55,6 +57,7 @@ TEST(Permutation, PullToFront) { EXPECT(assert_equal(expected, actual)); } +/* ************************************************************************* */ TEST(Permutation, PushToBack) { Permutation expected(5); expected[0] = 1; @@ -72,6 +75,7 @@ TEST(Permutation, PushToBack) { EXPECT(assert_equal(expected, actual)); } +/* ************************************************************************* */ TEST(Permutation, compose) { Permutation p1(5); p1[0] = 1; @@ -104,6 +108,25 @@ TEST(Permutation, compose) { LONGS_EQUAL(p1[p2[4]], actual[4]); } +/* ************************************************************************* */ +TEST(Reduction, CreateFromPartialPermutation) { + Permutation selector(3); + selector[0] = 2; + selector[1] = 4; + selector[2] = 6; + Permutation p(3); + p[0] = 2; + p[1] = 0; + p[2] = 1; + + internal::Reduction expected; + expected.insert(make_pair(2,6)); + expected.insert(make_pair(4,2)); + expected.insert(make_pair(6,4)); + + internal::Reduction actual = internal::Reduction::CreateFromPartialPermutation(selector, p); +} + /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr); } diff --git a/gtsam/nonlinear/ISAM2-impl.cpp b/gtsam/nonlinear/ISAM2-impl.cpp index 40f32937d..2e840fe01 100644 --- a/gtsam/nonlinear/ISAM2-impl.cpp +++ b/gtsam/nonlinear/ISAM2-impl.cpp @@ -353,11 +353,9 @@ ISAM2::Impl::PartialSolve(GaussianFactorGraph& factors, Permutation::shared_ptr affectedColamdInverse(affectedColamd->inverse()); if(debug) affectedColamd->print("affectedColamd: "); if(debug) affectedColamdInverse->print("affectedColamdInverse: "); - result.fullReordering = - *Permutation::Identity(reorderingMode.nFullSystemVars).partialPermutation(affectedKeysSelector, *affectedColamd); - result.fullReorderingInverse = - *Permutation::Identity(reorderingMode.nFullSystemVars).partialPermutation(affectedKeysSelector, *affectedColamdInverse); - if(debug) result.fullReordering.print("partialReordering: "); + result.reorderingSelector = affectedKeysSelector; + result.reorderingPermutation = *affectedColamd; + result.reorderingInverse = internal::Reduction::CreateFromPartialPermutation(affectedKeysSelector, *affectedColamdInverse); gttoc(ccolamd_permutations); gttic(permute_affected_variable_index); diff --git a/gtsam/nonlinear/ISAM2-impl.h b/gtsam/nonlinear/ISAM2-impl.h index 4ce16de19..a39b321d8 100644 --- a/gtsam/nonlinear/ISAM2-impl.h +++ b/gtsam/nonlinear/ISAM2-impl.h @@ -26,8 +26,9 @@ struct ISAM2::Impl { struct PartialSolveResult { ISAM2::sharedClique bayesTree; - Permutation fullReordering; - Permutation fullReorderingInverse; + Permutation reorderingSelector; + Permutation reorderingPermutation; + internal::Reduction reorderingInverse; }; struct ReorderingMode { diff --git a/gtsam/nonlinear/ISAM2.cpp b/gtsam/nonlinear/ISAM2.cpp index 387de0b90..d19654ac9 100644 --- a/gtsam/nonlinear/ISAM2.cpp +++ b/gtsam/nonlinear/ISAM2.cpp @@ -481,22 +481,24 @@ boost::shared_ptr > ISAM2::recalculate(const FastSet& mark // re-eliminate. The reordered variables are also mentioned in the // orphans and the leftover cached factors. gttic(permute_global_variable_index); - variableIndex_.permuteInPlace(partialSolveResult.fullReordering); + variableIndex_.permuteInPlace(partialSolveResult.reorderingSelector, partialSolveResult.reorderingPermutation); gttoc(permute_global_variable_index); gttic(permute_delta); - delta_.permuteInPlace(partialSolveResult.fullReordering); - deltaNewton_.permuteInPlace(partialSolveResult.fullReordering); - RgProd_.permuteInPlace(partialSolveResult.fullReordering); + const Permutation fullReordering = *Permutation::Identity(delta_.size()). + partialPermutation(partialSolveResult.reorderingSelector, partialSolveResult.reorderingPermutation); + delta_.permuteInPlace(fullReordering); + deltaNewton_.permuteInPlace(fullReordering); + RgProd_.permuteInPlace(fullReordering); gttoc(permute_delta); gttic(permute_ordering); - ordering_.permuteWithInverse(partialSolveResult.fullReorderingInverse); + ordering_.reduceWithInverse(partialSolveResult.reorderingInverse); gttoc(permute_ordering); if(params_.cacheLinearizedFactors) { gttic(permute_cached_linear); //linearFactors_.permuteWithInverse(partialSolveResult.fullReorderingInverse); FastList permuteLinearIndices = getAffectedFactors(affectedAndNewKeys); BOOST_FOREACH(size_t idx, permuteLinearIndices) { - linearFactors_[idx]->permuteWithInverse(partialSolveResult.fullReorderingInverse); + linearFactors_[idx]->reduceWithInverse(partialSolveResult.reorderingInverse); } gttoc(permute_cached_linear); } @@ -514,7 +516,7 @@ boost::shared_ptr > ISAM2::recalculate(const FastSet& mark gttic(orphans); gttic(permute); BOOST_FOREACH(sharedClique orphan, orphans) { - (void)orphan->permuteSeparatorWithInverse(partialSolveResult.fullReorderingInverse); + (void)orphan->reduceSeparatorWithInverse(partialSolveResult.reorderingInverse); } gttoc(permute); gttic(insert); diff --git a/gtsam/nonlinear/ISAM2.h b/gtsam/nonlinear/ISAM2.h index 7cc2c0518..51a876031 100644 --- a/gtsam/nonlinear/ISAM2.h +++ b/gtsam/nonlinear/ISAM2.h @@ -405,9 +405,15 @@ public: Base::permuteWithInverse(inversePermutation); } - bool permuteSeparatorWithInverse(const Permutation& inversePermutation) { - bool changed = Base::permuteSeparatorWithInverse(inversePermutation); - if(changed) if(cachedFactor_) cachedFactor_->permuteWithInverse(inversePermutation); + //bool permuteSeparatorWithInverse(const Permutation& inversePermutation) { + // bool changed = Base::permuteSeparatorWithInverse(inversePermutation); + // if(changed) if(cachedFactor_) cachedFactor_->permuteWithInverse(inversePermutation); + // return changed; + //} + + bool reduceSeparatorWithInverse(const internal::Reduction& inverseReduction) { + bool changed = Base::reduceSeparatorWithInverse(inverseReduction); + if(changed) if(cachedFactor_) cachedFactor_->reduceWithInverse(inverseReduction); return changed; } diff --git a/gtsam/nonlinear/Ordering.cpp b/gtsam/nonlinear/Ordering.cpp index 1f7c5375b..576e7fbb6 100644 --- a/gtsam/nonlinear/Ordering.cpp +++ b/gtsam/nonlinear/Ordering.cpp @@ -40,6 +40,13 @@ void Ordering::permuteWithInverse(const Permutation& inversePermutation) { } } +/* ************************************************************************* */ +void Ordering::reduceWithInverse(const internal::Reduction& inverseReduction) { + BOOST_FOREACH(Ordering::value_type& key_order, *this) { + key_order.second = inverseReduction[key_order.second]; + } +} + /* ************************************************************************* */ void Ordering::print(const string& str, const KeyFormatter& keyFormatter) const { cout << str; diff --git a/gtsam/nonlinear/Ordering.h b/gtsam/nonlinear/Ordering.h index ddffe787e..60a21e5a1 100644 --- a/gtsam/nonlinear/Ordering.h +++ b/gtsam/nonlinear/Ordering.h @@ -203,6 +203,13 @@ public: */ void permuteWithInverse(const Permutation& inversePermutation); + /** + * 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); + /// Synonym for operator[](Key) Index& at(Key key) { return operator[](key); }