diff --git a/gtsam.h b/gtsam.h index 62ca26269..3925dff73 100644 --- a/gtsam.h +++ b/gtsam.h @@ -765,6 +765,7 @@ class IndexConditional { gtsam::IndexFactor* toFactor() const; //Advanced interface + bool permuteSeparatorWithInverse(const gtsam::Permutation& inversePermutation); void permuteWithInverse(const gtsam::Permutation& inversePermutation); }; @@ -787,6 +788,7 @@ virtual class BayesNet { void push_front(This& conditional); void pop_front(); void permuteWithInverse(const gtsam::Permutation& inversePermutation); + bool permuteSeparatorWithInverse(const gtsam::Permutation& inversePermutation); }; #include @@ -830,6 +832,7 @@ virtual class BayesTreeClique { // derived_ptr parent() const { return parent_.lock(); } void permuteWithInverse(const gtsam::Permutation& inversePermutation); + bool permuteSeparatorWithInverse(const gtsam::Permutation& inversePermutation); // FIXME: need wrapped versions graphs, BayesNet // BayesNet shortcut(derived_ptr root, Eliminate function) const; @@ -856,6 +859,7 @@ virtual class SymbolicBayesNet : gtsam::SymbolicBayesNetBase { //Advanced Interface void pop_front(); void permuteWithInverse(const gtsam::Permutation& inversePermutation); + bool permuteSeparatorWithInverse(const gtsam::Permutation& inversePermutation); }; #include @@ -1764,6 +1768,7 @@ virtual class ISAM2Clique { void print(string s); void permuteWithInverse(const gtsam::Permutation& inversePermutation); + bool permuteSeparatorWithInverse(const gtsam::Permutation& inversePermutation); }; class ISAM2Result { diff --git a/gtsam/inference/BayesNet-inl.h b/gtsam/inference/BayesNet-inl.h index 5375b209f..0fe78fbed 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 f8aaa7d17..1077c1cad 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::reduceSeparatorWithInverse( - const internal::Reduction& inverseReduction) - { - bool changed = conditional_->reduceSeparatorWithInverse(inverseReduction); + 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->reduceSeparatorWithInverse(inverseReduction) == false); } + assert(child->permuteSeparatorWithInverse(inversePermutation) == false); + } } #endif - if(changed) { + if (changed) { BOOST_FOREACH(const derived_ptr& child, children_) { - (void) child->reduceSeparatorWithInverse(inverseReduction); } + (void) child->permuteSeparatorWithInverse(inversePermutation); + } } assertInvariants(); return changed; diff --git a/gtsam/inference/BayesTreeCliqueBase.h b/gtsam/inference/BayesTreeCliqueBase.h index a00980983..487d10a0d 100644 --- a/gtsam/inference/BayesTreeCliqueBase.h +++ b/gtsam/inference/BayesTreeCliqueBase.h @@ -183,14 +183,7 @@ namespace gtsam { * traversing the whole tree. Returns whether any separator variables in * this subtree were reordered. */ - //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); + bool permuteSeparatorWithInverse(const Permutation& inversePermutation); /** 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 1a5e6d516..e0993a9a1 100644 --- a/gtsam/inference/IndexConditional.cpp +++ b/gtsam/inference/IndexConditional.cpp @@ -44,33 +44,16 @@ namespace gtsam { } /* ************************************************************************* */ - //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 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()) { - internal::Reduction::const_iterator it = inverseReduction.find(parent); - if(it != inverseReduction.end()) { + KeyType newParent = inversePermutation[parent]; + if(parent != newParent) { parentChanged = true; - parent = it->second; + parent = newParent; } } assertInvariants(); diff --git a/gtsam/inference/IndexConditional.h b/gtsam/inference/IndexConditional.h index 9643b1a84..895592547 100644 --- a/gtsam/inference/IndexConditional.h +++ b/gtsam/inference/IndexConditional.h @@ -112,13 +112,7 @@ namespace gtsam { * Returns true if any reordered variables appeared in the separator and * false if not. */ - //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); + bool permuteSeparatorWithInverse(const Permutation& inversePermutation); /** * Permutes the Conditional, but for efficiency requires the permutation diff --git a/gtsam/inference/Permutation.cpp b/gtsam/inference/Permutation.cpp index bd6d50c22..883041b86 100644 --- a/gtsam/inference/Permutation.cpp +++ b/gtsam/inference/Permutation.cpp @@ -161,16 +161,6 @@ 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) { @@ -193,10 +183,10 @@ namespace internal { } /* ************************************************************************* */ - const Index& Reduction::operator[](const Index& j) { + Index& Reduction::operator[](const Index& j) { iterator it = this->find(j); if(it == this->end()) - return j; + throw std::out_of_range("Index to Reduction::operator[] not present"); else return it->second; } @@ -205,7 +195,7 @@ namespace internal { const Index& Reduction::operator[](const Index& j) const { const_iterator it = this->find(j); if(it == this->end()) - return j; + throw std::out_of_range("Index to Reduction::operator[] not present"); else return it->second; } @@ -217,11 +207,6 @@ 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 4fbd06199..6014d704e 100644 --- a/gtsam/inference/Permutation.h +++ b/gtsam/inference/Permutation.h @@ -192,14 +192,12 @@ 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; - const Index& operator[](const Index& j); + 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 4e2da7492..e6e84cf1c 100644 --- a/gtsam/inference/VariableIndex.cpp +++ b/gtsam/inference/VariableIndex.cpp @@ -76,20 +76,6 @@ 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 df0a8fec9..762a05cc8 100644 --- a/gtsam/inference/VariableIndex.h +++ b/gtsam/inference/VariableIndex.h @@ -135,9 +135,6 @@ 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 98e94b4f7..1ccafb1a2 100644 --- a/gtsam/inference/tests/testPermutation.cpp +++ b/gtsam/inference/tests/testPermutation.cpp @@ -25,7 +25,6 @@ using namespace gtsam; using namespace std; -/* ************************************************************************* */ TEST(Permutation, Identity) { Permutation expected(5); expected[0] = 0; @@ -39,7 +38,6 @@ TEST(Permutation, Identity) { EXPECT(assert_equal(expected, actual)); } -/* ************************************************************************* */ TEST(Permutation, PullToFront) { Permutation expected(5); expected[0] = 4; @@ -57,7 +55,6 @@ TEST(Permutation, PullToFront) { EXPECT(assert_equal(expected, actual)); } -/* ************************************************************************* */ TEST(Permutation, PushToBack) { Permutation expected(5); expected[0] = 1; @@ -75,7 +72,6 @@ TEST(Permutation, PushToBack) { EXPECT(assert_equal(expected, actual)); } -/* ************************************************************************* */ TEST(Permutation, compose) { Permutation p1(5); p1[0] = 1; @@ -108,25 +104,6 @@ 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 593666657..bfc486fbf 100644 --- a/gtsam/nonlinear/ISAM2-impl.cpp +++ b/gtsam/nonlinear/ISAM2-impl.cpp @@ -353,9 +353,11 @@ ISAM2::Impl::PartialSolve(GaussianFactorGraph& factors, Permutation::shared_ptr affectedColamdInverse(affectedColamd->inverse()); if(debug) affectedColamd->print("affectedColamd: "); if(debug) affectedColamdInverse->print("affectedColamdInverse: "); - result.reorderingSelector = affectedKeysSelector; - result.reorderingPermutation = *affectedColamd; - result.reorderingInverse = internal::Reduction::CreateFromPartialPermutation(affectedKeysSelector, *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: "); gttoc(ccolamd_permutations); gttic(permute_affected_variable_index); diff --git a/gtsam/nonlinear/ISAM2-impl.h b/gtsam/nonlinear/ISAM2-impl.h index a39b321d8..4ce16de19 100644 --- a/gtsam/nonlinear/ISAM2-impl.h +++ b/gtsam/nonlinear/ISAM2-impl.h @@ -26,9 +26,8 @@ struct ISAM2::Impl { struct PartialSolveResult { ISAM2::sharedClique bayesTree; - Permutation reorderingSelector; - Permutation reorderingPermutation; - internal::Reduction reorderingInverse; + Permutation fullReordering; + Permutation fullReorderingInverse; }; struct ReorderingMode { diff --git a/gtsam/nonlinear/ISAM2.cpp b/gtsam/nonlinear/ISAM2.cpp index 058e0d736..be82fadc3 100644 --- a/gtsam/nonlinear/ISAM2.cpp +++ b/gtsam/nonlinear/ISAM2.cpp @@ -481,24 +481,22 @@ 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.reorderingSelector, partialSolveResult.reorderingPermutation); + variableIndex_.permuteInPlace(partialSolveResult.fullReordering); gttoc(permute_global_variable_index); gttic(permute_delta); - const Permutation fullReordering = *Permutation::Identity(delta_.size()). - partialPermutation(partialSolveResult.reorderingSelector, partialSolveResult.reorderingPermutation); - delta_ = delta_.permute(fullReordering); - deltaNewton_ = deltaNewton_.permute(fullReordering); - RgProd_ = RgProd_.permute(fullReordering); + delta_ = delta_.permute(partialSolveResult.fullReordering); + deltaNewton_ = deltaNewton_.permute(partialSolveResult.fullReordering); + RgProd_ = RgProd_.permute(partialSolveResult.fullReordering); gttoc(permute_delta); gttic(permute_ordering); - ordering_.reduceWithInverse(partialSolveResult.reorderingInverse); + ordering_.permuteWithInverse(partialSolveResult.fullReorderingInverse); 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]->reduceWithInverse(partialSolveResult.reorderingInverse); + linearFactors_[idx]->permuteWithInverse(partialSolveResult.fullReorderingInverse); } gttoc(permute_cached_linear); } @@ -516,7 +514,7 @@ boost::shared_ptr > ISAM2::recalculate(const FastSet& mark gttic(orphans); gttic(permute); BOOST_FOREACH(sharedClique orphan, orphans) { - (void)orphan->reduceSeparatorWithInverse(partialSolveResult.reorderingInverse); + (void)orphan->permuteSeparatorWithInverse(partialSolveResult.fullReorderingInverse); } gttoc(permute); gttic(insert); diff --git a/gtsam/nonlinear/ISAM2.h b/gtsam/nonlinear/ISAM2.h index 51a876031..7cc2c0518 100644 --- a/gtsam/nonlinear/ISAM2.h +++ b/gtsam/nonlinear/ISAM2.h @@ -405,15 +405,9 @@ public: Base::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); + bool permuteSeparatorWithInverse(const Permutation& inversePermutation) { + bool changed = Base::permuteSeparatorWithInverse(inversePermutation); + if(changed) if(cachedFactor_) cachedFactor_->permuteWithInverse(inversePermutation); return changed; } diff --git a/gtsam/nonlinear/Ordering.cpp b/gtsam/nonlinear/Ordering.cpp index fef827e79..6e8f70a0d 100644 --- a/gtsam/nonlinear/Ordering.cpp +++ b/gtsam/nonlinear/Ordering.cpp @@ -39,13 +39,6 @@ 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 60a21e5a1..ddffe787e 100644 --- a/gtsam/nonlinear/Ordering.h +++ b/gtsam/nonlinear/Ordering.h @@ -203,13 +203,6 @@ 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); } diff --git a/tests/timeiSAM2Chain.cpp b/tests/timeiSAM2Chain.cpp deleted file mode 100644 index 9ed18a1ef..000000000 --- a/tests/timeiSAM2Chain.cpp +++ /dev/null @@ -1,100 +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 timeiSAM2Chain.cpp - * @brief Times each iteration of a long chain in iSAM2, to measure asymptotic performance - * @author Richard Roberts - */ - -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include -#include -#include -#include -#include - -using namespace std; -using namespace gtsam; -using namespace gtsam::symbol_shorthand; - -typedef Pose2 Pose; - -typedef NoiseModelFactor1 NM1; -typedef NoiseModelFactor2 NM2; -noiseModel::Unit::shared_ptr model = noiseModel::Unit::Create(Pose::Dim()); - -int main(int argc, char *argv[]) { - - const size_t steps = 50000; - - cout << "Playing forward " << steps << " time steps..." << endl; - - ISAM2 isam2; - - // Times - vector times(steps); - - for(size_t step=0; step < steps; ++step) { - - Values newVariables; - NonlinearFactorGraph newFactors; - - // Collect measurements and new variables for the current step - gttic_(Create_measurements); - if(step == 0) { - // Add prior - newFactors.add(PriorFactor(0, Pose(), noiseModel::Unit::Create(Pose::Dim()))); - newVariables.insert(0, Pose()); - } else { - Vector eta = Vector::Random(Pose::Dim()) * 0.1; - Pose2 between = Pose().retract(eta); - // Add between - newFactors.add(BetweenFactor(step-1, step, between, model)); - newVariables.insert(step, isam2.calculateEstimate(step-1) * between); - } - - gttoc_(Create_measurements); - - // Update iSAM2 - gttic_(Update_ISAM2); - isam2.update(newFactors, newVariables); - gttoc_(Update_ISAM2); - - tictoc_finishedIteration_(); - - tictoc_getNode(node, Update_ISAM2); - times[step] = node->time(); - - if(step % 1000 == 0) { - cout << "Step " << step << endl; - tictoc_print_(); - } - } - - tictoc_print_(); - - // Write times - ofstream timesFile("times.txt"); - BOOST_FOREACH(double t, times) { - timesFile << t << "\n"; } - - return 0; -}