From d07486bf090aa15709b5ecdbb36131614b733b23 Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Tue, 27 Sep 2011 20:24:39 +0000 Subject: [PATCH] ISAM2 refactoring --- gtsam/nonlinear/ISAM2-impl-inl.h | 233 +++++++++++++++++++++++ gtsam/nonlinear/ISAM2-inl.h | 315 +++++-------------------------- gtsam/nonlinear/ISAM2.h | 6 +- 3 files changed, 278 insertions(+), 276 deletions(-) diff --git a/gtsam/nonlinear/ISAM2-impl-inl.h b/gtsam/nonlinear/ISAM2-impl-inl.h index dfd9f633f..2ea4f3725 100644 --- a/gtsam/nonlinear/ISAM2-impl-inl.h +++ b/gtsam/nonlinear/ISAM2-impl-inl.h @@ -14,6 +14,22 @@ namespace gtsam { template struct ISAM2::Impl { + + typedef ISAM2 ISAM2Type; + + struct PartialSolveResult { + ISAM2Type::sharedClique bayesTree; + Permutation fullReordering; + Permutation fullReorderingInverse; + }; + + struct ReorderingMode { + size_t nFullSystemVars; + enum { AS_ADDED, COLAMD } algorithm; + enum { NO_CONSTRAINT, LATEST_LAST } constrain; + boost::optional&> latestKeys; + }; + /** * Add new variables to the ISAM2 system. * @param newTheta Initial values for new variables @@ -42,6 +58,56 @@ struct ISAM2::Impl { * equal to relinearizeThreshold */ static FastSet CheckRelinearization(Permuted& delta, double relinearizeThreshold); + + /** + * Recursively search this clique and its children for marked keys appearing + * in the separator, and add the *frontal* keys of any cliques whose + * separator contains any marked keys to the set \c keys. The purpose of + * this is to discover the cliques that need to be redone due to information + * propagating to them from cliques that directly contain factors being + * relinearized. + * + * The original comment says this finds all variables directly connected to + * the marked ones by measurements. Is this true, because it seems like this + * would also pull in variables indirectly connected through other frontal or + * separator variables? + * + * Alternatively could we trace up towards the root for each variable here? + */ + static void FindAll(ISAM2Type::sharedClique clique, FastSet& keys, const vector& markedMask); + + /** + * Apply expmap to the given values, but only for indices appearing in + * \c markedRelinMask. Values are expmapped in-place. + * \param [in][out] values The value to expmap in-place + * \param delta The linear delta with which to expmap + * \param ordering The ordering + * \param mask Mask on linear indices, only \c true entries are expmapped + * \param invalidateIfDebug If this is true, *and* NDEBUG is not defined, + * expmapped deltas will be set to an invalid value (infinity) to catch bugs + * where we might expmap something twice, or expmap it but then not + * recalculate its delta. + */ + static void ExpmapMasked(VALUES& values, const Permuted& delta, + const Ordering& ordering, const vector& mask, + boost::optional&> invalidateIfDebug = boost::optional&>()); + + /** + * Reorder and eliminate factors. These factors form a subset of the full + * problem, so along with the BayesTree we get a partial reordering of the + * problem that needs to be applied to the other data in ISAM2, which is the + * VariableIndex, the delta, the ordering, and the orphans (including cached + * factors). + * \param factors The factors to eliminate, representing part of the full + * problem. This is permuted during use and so is cleared when this function + * returns in order to invalidate it. + * \param keys The set of indices used in \c factors. + * \return The eliminated BayesTree and the permutation to be applied to the + * rest of the ISAM2 data. + */ + static PartialSolveResult PartialSolve(GaussianFactorGraph& factors, const FastSet& keys, + const ReorderingMode& reorderingMode); + }; /* ************************************************************************* */ @@ -109,4 +175,171 @@ FastSet ISAM2::Impl::CheckRelinearization(Permuted +void ISAM2::Impl::FindAll(ISAM2Type::sharedClique clique, FastSet& keys, const vector& markedMask) { + static const bool debug = false; + // does the separator contain any of the variables? + bool found = false; + BOOST_FOREACH(const Index& key, (*clique)->parents()) { + if (markedMask[key]) + found = true; + } + if (found) { + // then add this clique + keys.insert((*clique)->beginFrontals(), (*clique)->endFrontals()); + if(debug) clique->print("Key(s) marked in clique "); + if(debug) cout << "so marking key " << (*clique)->keys().front() << endl; + } + BOOST_FOREACH(const sharedClique& child, clique->children_) { + FindAll(child, keys, markedMask); + } +} + +/* ************************************************************************* */ +// Internal class used in ExpmapMasked() +// This debug version sets delta entries that are applied to "Inf". The +// idea is that if a delta is applied, the variable is being relinearized, +// so the same delta should not be re-applied because it will be recalc- +// ulated. This is a debug check to prevent against a mix-up of indices +// or not keeping track of recalculated variables. +struct _SelectiveExpmapAndClear { + const Permuted& delta; + const Ordering& ordering; + const vector& mask; + const boost::optional&> invalidate; + _SelectiveExpmapAndClear(const Permuted& _delta, const Ordering& _ordering, const vector& _mask, boost::optional&> _invalidate) : + delta(_delta), ordering(_ordering), mask(_mask), invalidate(_invalidate) {} + template + void operator()(I it_x) { + Index var = ordering[it_x->first]; + if(ISDEBUG("ISAM2 update verbose")) { + if(mask[var]) + cout << "expmap " << (string)it_x->first << " (j = " << var << "), delta = " << delta[var].transpose() << endl; + else + cout << " " << (string)it_x->first << " (j = " << var << "), delta = " << delta[var].transpose() << endl; + } + assert(delta[var].size() == (int)it_x->second.dim()); + assert(delta[var].unaryExpr(&isfinite).all()); + if(mask[var]) { + it_x->second = it_x->second.expmap(delta[var]); + if(invalidate) + (*invalidate)[var].operator=(Vector::Constant(delta[var].rows(), numeric_limits::infinity())); // Strange syntax to work with clang++ (bug in clang?) + } + } +}; + +/* ************************************************************************* */ +template +void ISAM2::Impl::ExpmapMasked(VALUES& values, const Permuted& delta, + const Ordering& ordering, const vector& mask, boost::optional&> invalidateIfDebug) { + // If debugging, invalidate if requested, otherwise do not invalidate. + // Invalidating means setting expmapped entries to Inf, to trigger assertions + // if we try to re-use them. +#ifdef NDEBUG + invalidateIfDebug = boost::optional&>(); +#endif + + _SelectiveExpmapAndClear selectiveExpmapper(delta, ordering, mask, invalidateIfDebug); + values.apply(selectiveExpmapper); +} + +/* ************************************************************************* */ +template +typename ISAM2::Impl::PartialSolveResult +ISAM2::Impl::PartialSolve(GaussianFactorGraph& factors, + const FastSet& keys, const ReorderingMode& reorderingMode) { + + static const bool debug = ISDEBUG("ISAM2 recalculate"); + + PartialSolveResult result; + + tic(1,"select affected variables"); +#ifndef NDEBUG + // Debug check that all variables involved in the factors to be re-eliminated + // are in affectedKeys, since we will use it to select a subset of variables. + BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, factors) { + BOOST_FOREACH(Index key, factor->keys()) { + assert(find(keys.begin(), keys.end(), key) != keys.end()); + } + } +#endif + Permutation affectedKeysSelector(keys.size()); // Create a permutation that pulls the affected keys to the front + Permutation affectedKeysSelectorInverse(keys.size() > 0 ? *keys.rbegin()+1 : 0 /*ordering_.nVars()*/); // And its inverse +#ifndef NDEBUG + // If debugging, fill with invalid values that will trip asserts if dereferenced + std::fill(affectedKeysSelectorInverse.begin(), affectedKeysSelectorInverse.end(), numeric_limits::max()); +#endif + { Index position=0; BOOST_FOREACH(Index var, keys) { + affectedKeysSelector[position] = var; + affectedKeysSelectorInverse[var] = position; + ++ position; } } + if(debug) affectedKeysSelector.print("affectedKeysSelector: "); + if(debug) affectedKeysSelectorInverse.print("affectedKeysSelectorInverse: "); + factors.permuteWithInverse(affectedKeysSelectorInverse); + if(debug) factors.print("Factors to reorder/re-eliminate: "); + toc(1,"select affected variables"); + tic(2,"variable index"); + VariableIndex affectedFactorsIndex(factors); // Create a variable index for the factors to be re-eliminated + if(debug) affectedFactorsIndex.print("affectedFactorsIndex: "); + toc(2,"variable index"); + tic(3,"ccolamd"); + vector cmember(affectedKeysSelector.size(), 0); + if(reorderingMode.constrain == ReorderingMode::LATEST_LAST) { + assert(reorderingMode.latestKeys); + if(keys.size() > reorderingMode.latestKeys->size()) { + BOOST_FOREACH(Index var, *reorderingMode.latestKeys) { + cmember[affectedKeysSelectorInverse[var]] = 1; + } + } + } + Permutation::shared_ptr affectedColamd(Inference::PermutationCOLAMD_(affectedFactorsIndex, cmember)); + toc(3,"ccolamd"); + tic(4,"ccolamd permutations"); + 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: "); + toc(4,"ccolamd permutations"); + + tic(5,"permute affected variable index"); + affectedFactorsIndex.permute(*affectedColamd); + toc(5,"permute affected variable index"); + + tic(6,"permute affected factors"); + factors.permuteWithInverse(*affectedColamdInverse); + toc(6,"permute affected factors"); + + if(debug) factors.print("Colamd-ordered affected factors: "); + +#ifndef NDEBUG + VariableIndex fromScratchIndex(factors); + assert(assert_equal(fromScratchIndex, affectedFactorsIndex)); +#endif + + // eliminate into a Bayes net + tic(7,"eliminate"); + GaussianJunctionTree jt(factors, affectedFactorsIndex); + result.bayesTree = jt.eliminate(EliminatePreferLDL, true); + if(debug && result.bayesTree) { + cout << "Re-eliminated BT:\n"; + result.bayesTree->printTree(""); + } + toc(7,"eliminate"); + + tic(8,"permute eliminated"); + if(result.bayesTree) result.bayesTree->permuteWithInverse(affectedKeysSelector); + if(debug && result.bayesTree) { + cout << "Full var-ordered eliminated BT:\n"; + result.bayesTree->printTree(""); + } + toc(8,"permute eliminated"); + + return result; +} + } diff --git a/gtsam/nonlinear/ISAM2-inl.h b/gtsam/nonlinear/ISAM2-inl.h index b07f49f1c..f8b32c9a8 100644 --- a/gtsam/nonlinear/ISAM2-inl.h +++ b/gtsam/nonlinear/ISAM2-inl.h @@ -153,7 +153,10 @@ ISAM2::getCachedBoundaryFactors(Cliques& orphans) { } template -boost::shared_ptr > ISAM2::recalculate(const FastSet& markedKeys, const FastSet& structuralKeys, const vector& newKeys, const FactorGraph::shared_ptr newFactors) { +boost::shared_ptr > ISAM2::recalculate( + const FastSet& markedKeys, const FastSet& structuralKeys, const FastVector& newKeys, const FactorGraph::shared_ptr newFactors) { + + // TODO: new factors are linearized twice, the newFactors passed in are not used. static const bool debug = ISDEBUG("ISAM2 recalculate"); @@ -193,7 +196,6 @@ boost::shared_ptr > ISAM2::recalculate(const toc(0, "affectedStructuralKeys"); } -// if(debug) newFactors->print("Recalculating factors: "); if(debug) { cout << "markedKeys: "; BOOST_FOREACH(const Index key, markedKeys) { cout << key << " "; } @@ -283,7 +285,7 @@ boost::shared_ptr > ISAM2::recalculate(const tic(5,"eliminate"); GaussianJunctionTree jt(factors, variableIndex_); - sharedClique newRoot = jt.eliminate(EliminateQR, true); + sharedClique newRoot = jt.eliminate(EliminatePreferLDL, true); if(debug) newRoot->print("Eliminated: "); toc(5,"eliminate"); @@ -336,15 +338,6 @@ boost::shared_ptr > ISAM2::recalculate(const << " avgCliqueSize: " << avgClique << " #Cliques: " << numCliques << " nnzR: " << nnzR << endl; #endif - //#ifndef NDEBUG - // for(Index var=0; varbegin(), cached_[var]->end(), var) == cached_[var]->end()); - // } - // } - //#endif - tic(2,"cached"); // add the cached intermediate results from the boundary of the orphans ... FactorGraph cachedBoundary = getCachedBoundaryFactors(orphans); @@ -354,12 +347,11 @@ boost::shared_ptr > ISAM2::recalculate(const BOOST_FOREACH(const CacheFactor::shared_ptr& cached, cachedBoundary) { #ifndef NDEBUG BOOST_FOREACH(const Index key, *cached) { - assert(lastRelinVariables_[key] == false); + assert(lastRelinVariables_[key] == false); // No variables in the cached factors should be being relinearized } #endif factors.push_back(GaussianFactor::shared_ptr(new CacheFactor(*cached))); } - // factors.push_back(cachedBoundary); toc(2,"cached"); // END OF COPIED CODE @@ -384,119 +376,24 @@ boost::shared_ptr > ISAM2::recalculate(const // 3. Re-order and eliminate the factor graph into a Bayes net (Algorithm [alg:eliminate]), and re-assemble into a new Bayes tree (Algorithm [alg:BayesTree]) - tic(4,"reorder"); + tic(4,"reorder and eliminate"); - //#define PRESORT_ALPHA - - tic(1,"select affected variables"); + tic(1,"list to set"); // create a partial reordering for the new and contaminated factors // markedKeys are passed in: those variables will be forced to the end in the ordering boost::shared_ptr > affectedKeysSet(new FastSet(markedKeys)); affectedKeysSet->insert(affectedKeys.begin(), affectedKeys.end()); - //#ifndef NDEBUG - // // All affected keys should be contiguous and at the end of the elimination order - // for(set::const_iterator key=affectedKeysSet->begin(); key!=affectedKeysSet->end(); ++key) { - // if(key != affectedKeysSet->begin()) { - // set::const_iterator prev = key; --prev; - // assert(*prev == *key - 1); - // } - // } - // assert(*(affectedKeysSet->end()) == variableIndex_.size() - 1); - //#endif + toc(1,"list to set"); -#ifndef NDEBUG - // Debug check that all variables involved in the factors to be re-eliminated - // are in affectedKeys, since we will use it to select a subset of variables. - BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, factors) { - BOOST_FOREACH(Index key, factor->keys()) { - assert(find(affectedKeysSet->begin(), affectedKeysSet->end(), key) != affectedKeysSet->end()); - } - } -#endif - Permutation affectedKeysSelector(affectedKeysSet->size()); // Create a permutation that pulls the affected keys to the front - Permutation affectedKeysSelectorInverse(affectedKeysSet->size() > 0 ? *(--affectedKeysSet->end())+1 : 0 /*ordering_.nVars()*/); // And its inverse -#ifndef NDEBUG - // If debugging, fill with invalid values that will trip asserts if dereferenced - std::fill(affectedKeysSelectorInverse.begin(), affectedKeysSelectorInverse.end(), numeric_limits::max()); -#endif - { Index position=0; BOOST_FOREACH(Index var, *affectedKeysSet) { - affectedKeysSelector[position] = var; - affectedKeysSelectorInverse[var] = position; - ++ position; } } - // if(disableReordering) { assert(affectedKeysSelector.equals(Permutation::Identity(ordering_.nVars()))); assert(affectedKeysSelectorInverse.equals(Permutation::Identity(ordering_.nVars()))); } - if(debug) affectedKeysSelector.print("affectedKeysSelector: "); - if(debug) affectedKeysSelectorInverse.print("affectedKeysSelectorInverse: "); -#ifndef NDEBUG - VariableIndex beforePermutationIndex(factors); -#endif - factors.permuteWithInverse(affectedKeysSelectorInverse); - if(debug) factors.print("Factors to reorder/re-eliminate: "); - toc(1,"select affected variables"); - tic(2,"variable index"); - VariableIndex affectedFactorsIndex(factors); // Create a variable index for the factors to be re-eliminated -#ifndef NDEBUG - // beforePermutationIndex.permute(affectedKeysSelector); - // assert(assert_equal(affectedFactorsIndex, beforePermutationIndex)); -#endif - if(debug) affectedFactorsIndex.print("affectedFactorsIndex: "); - toc(2,"variable index"); - tic(3,"ccolamd"); -#ifdef PRESORT_ALPHA - Permutation alphaOrder(affectedKeysSet->size()); - vector orderedKeys; orderedKeys.reserve(ordering_.size()); - Index alphaVar = 0; - BOOST_FOREACH(const Ordering::value_type& key_order, ordering_) { - Permutation::const_iterator selected = find(affectedKeysSelector.begin(), affectedKeysSelector.end(), key_order.second); - if(selected != affectedKeysSelector.end()) { - Index selectedVar = selected - affectedKeysSelector.begin(); - alphaOrder[alphaVar] = selectedVar; - ++ alphaVar; - } - } - assert(alphaVar == affectedKeysSet->size()); - vector markedKeysSelected; markedKeysSelected.reserve(markedKeys.size()); - BOOST_FOREACH(Index var, markedKeys) { markedKeysSelected.push_back(alphaOrder[affectedKeysSelectorInverse[var]]); } - GaussianVariableIndex<> origAffectedFactorsIndex(affectedFactorsIndex); - affectedFactorsIndex.permute(alphaOrder); - Permutation::shared_ptr affectedColamd(Inference::PermutationCOLAMD(affectedFactorsIndex, markedKeysSelected)); - affectedFactorsIndex.permute(*alphaOrder.inverse()); - affectedColamd = alphaOrder.permute(*affectedColamd); -#else - // vector markedKeysSelected; markedKeysSelected.reserve(markedKeys.size()); - // BOOST_FOREACH(Index var, markedKeys) { markedKeysSelected.push_back(affectedKeysSelectorInverse[var]); } - // vector newKeysSelected; newKeysSelected.reserve(newKeys.size()); - // BOOST_FOREACH(Index var, newKeys) { newKeysSelected.push_back(affectedKeysSelectorInverse[var]); } - vector cmember(affectedKeysSelector.size(), 0); - if(structuralLast) { - if(affectedKeysSelector.size() > affectedStructuralKeys.size()) { - BOOST_FOREACH(Index var, affectedStructuralKeys) { cmember[affectedKeysSelectorInverse[var]] = 1; } - if(latestLast) { BOOST_FOREACH(Index var, newKeys) { cmember[affectedKeysSelectorInverse[var]] = 2; } } - } - } else if(latestLast) { - FastSet newKeysSet(newKeys.begin(), newKeys.end()); - if(theta_.size() > newKeysSet.size()) { - BOOST_FOREACH(Index var, newKeys) { cmember[affectedKeysSelectorInverse[var]] = 1; } - } - } - Permutation::shared_ptr affectedColamd(Inference::PermutationCOLAMD_(affectedFactorsIndex, cmember)); - if(disableReordering) { - affectedColamd.reset(new Permutation(Permutation::Identity(affectedKeysSelector.size()))); - // assert(affectedColamd->equals(Permutation::Identity(ordering_.nVars()))); - } -#endif - toc(3,"ccolamd"); - tic(4,"ccolamd permutations"); - Permutation::shared_ptr affectedColamdInverse(affectedColamd->inverse()); - // if(disableReordering) assert(affectedColamdInverse->equals(Permutation::Identity(ordering_.nVars()))); - if(debug) affectedColamd->print("affectedColamd: "); - if(debug) affectedColamdInverse->print("affectedColamdInverse: "); - Permutation::shared_ptr partialReordering( - Permutation::Identity(ordering_.nVars()).partialPermutation(affectedKeysSelector, *affectedColamd)); - Permutation::shared_ptr partialReorderingInverse( - Permutation::Identity(ordering_.nVars()).partialPermutation(affectedKeysSelector, *affectedColamdInverse)); - // if(disableReordering) { assert(partialReordering->equals(Permutation::Identity(ordering_.nVars()))); assert(partialReorderingInverse->equals(Permutation::Identity(ordering_.nVars()))); } - if(debug) partialReordering->print("partialReordering: "); - toc(4,"ccolamd permutations"); + tic(2,"PartialSolve"); + typename Impl::ReorderingMode reorderingMode; + reorderingMode.nFullSystemVars = ordering_.nVars(); + reorderingMode.algorithm = Impl::ReorderingMode::COLAMD; + reorderingMode.constrain = Impl::ReorderingMode::LATEST_LAST; + reorderingMode.latestKeys = FastSet(newKeys.begin(), newKeys.end()); + typename Impl::PartialSolveResult partialSolveResult = + Impl::PartialSolve(factors, *affectedKeysSet, reorderingMode); + toc(2,"PartialSolve"); // We now need to permute everything according this partial reordering: the // delta vector, the global ordering, and the factors we're about to @@ -504,62 +401,30 @@ boost::shared_ptr > ISAM2::recalculate(const // orphans and the leftover cached factors. // NOTE: We have shared_ptr's to cached factors that we permute here, thus we // undo this permutation after elimination. - tic(5,"permute global variable index"); - variableIndex_.permute(*partialReordering); - toc(5,"permute global variable index"); - tic(6,"permute affected variable index"); - affectedFactorsIndex.permute(*affectedColamd); - toc(6,"permute affected variable index"); - tic(7,"permute delta"); - delta_.permute(*partialReordering); - toc(7,"permute delta"); - tic(8,"permute ordering"); - ordering_.permuteWithInverse(*partialReorderingInverse); - toc(8,"permute ordering"); - tic(9,"permute affected factors"); - factors.permuteWithInverse(*affectedColamdInverse); - toc(9,"permute affected factors"); + tic(3,"permute global variable index"); + variableIndex_.permute(partialSolveResult.fullReordering); + toc(3,"permute global variable index"); + tic(4,"permute delta"); + delta_.permute(partialSolveResult.fullReordering); + toc(4,"permute delta"); + tic(5,"permute ordering"); + ordering_.permuteWithInverse(partialSolveResult.fullReorderingInverse); + toc(5,"permute ordering"); - if(debug) factors.print("Colamd-ordered affected factors: "); - -#ifndef NDEBUG - VariableIndex fromScratchIndex(factors); - assert(assert_equal(fromScratchIndex, affectedFactorsIndex)); - // beforePermutationIndex.permute(*affectedColamd); - // assert(assert_equal(fromScratchIndex, beforePermutationIndex)); -#endif - - // Permutation::shared_ptr reorderedSelectorInverse(affectedKeysSelector.permute(*affectedColamd)); - // reorderedSelectorInverse->print("reorderedSelectorInverse: "); - toc(4,"reorder"); - - // eliminate into a Bayes net - tic(5,"eliminate"); - GaussianJunctionTree jt(factors); - sharedClique newRoot = jt.eliminate(EliminateQR, true); - if(debug && newRoot) cout << "Re-eliminated BT:\n"; - if(debug && newRoot) newRoot->printTree(""); - toc(5,"eliminate"); + toc(4,"reorder and eliminate"); tic(6,"re-assemble"); - tic(1,"permute eliminated"); - if(newRoot) newRoot->permuteWithInverse(affectedKeysSelector); - if(debug && newRoot) cout << "Full var-ordered eliminated BT:\n"; - if(debug && newRoot) newRoot->printTree(""); - toc(1,"permute eliminated"); - tic(2,"insert"); - if(newRoot) { + if(partialSolveResult.bayesTree) { assert(!this->root_); - this->insert(newRoot); + this->insert(partialSolveResult.bayesTree); } - toc(2,"insert"); toc(6,"re-assemble"); // 4. Insert the orphans back into the new Bayes tree. tic(7,"orphans"); tic(1,"permute"); BOOST_FOREACH(sharedClique orphan, orphans) { - (void)orphan->permuteSeparatorWithInverse(*partialReorderingInverse); + (void)orphan->permuteSeparatorWithInverse(partialSolveResult.fullReorderingInverse); } toc(1,"permute"); tic(2,"insert"); @@ -588,91 +453,6 @@ boost::shared_ptr > ISAM2::recalculate(const // affectedKeysSet->insert(affectedKeys.begin(), affectedKeys.end()); } -///* ************************************************************************* */ -//template -//void ISAM2::linear_update(const GaussianFactorGraph& newFactors) { -// const list markedKeys = newFactors.keys(); -// recalculate(markedKeys, &newFactors); -//} - -/* ************************************************************************* */ -// find all variables that are directly connected by a measurement to one of the marked variables -template -void ISAM2::find_all(sharedClique clique, FastSet& keys, const vector& markedMask) { - static const bool debug = false; - // does the separator contain any of the variables? - bool found = false; - BOOST_FOREACH(const Index& key, (*clique)->parents()) { - if (markedMask[key]) - found = true; - } - if (found) { - // then add this clique - keys.insert((*clique)->beginFrontals(), (*clique)->endFrontals()); - if(debug) clique->print("Key(s) marked in clique "); - if(debug) cout << "so marking key " << (*clique)->keys().front() << endl; - } - BOOST_FOREACH(const sharedClique& child, clique->children_) { - find_all(child, keys, markedMask); - } -} - -/* ************************************************************************* */ -struct _SelectiveExpmap { - const Permuted& delta; - const Ordering& ordering; - const vector& mask; - _SelectiveExpmap(const Permuted& _delta, const Ordering& _ordering, const vector& _mask) : - delta(_delta), ordering(_ordering), mask(_mask) {} - template - void operator()(I it_x) { - Index var = ordering[it_x->first]; - if(ISDEBUG("ISAM2 update verbose")) { - if(mask[var]) - cout << "expmap " << (string)it_x->first << " (j = " << var << "), delta = " << delta[var].transpose() << endl; - else - cout << " " << (string)it_x->first << " (j = " << var << "), delta = " << delta[var].transpose() << endl; - } - if(mask[var]) it_x->second = it_x->second.expmap(delta[var]); - } -}; -#ifndef NDEBUG -// This debug version sets delta entries that are applied to "Inf". The -// idea is that if a delta is applied, the variable is being relinearized, -// so the same delta should not be re-applied because it will be recalc- -// ulated. This is a debug check to prevent against a mix-up of indices -// or not keeping track of recalculated variables. -struct _SelectiveExpmapAndClear { - Permuted& delta; - const Ordering& ordering; - const vector& mask; - _SelectiveExpmapAndClear(Permuted& _delta, const Ordering& _ordering, const vector& _mask) : - delta(_delta), ordering(_ordering), mask(_mask) {} - template - void operator()(I it_x) { - Index var = ordering[it_x->first]; - if(ISDEBUG("ISAM2 update verbose")) { - if(mask[var]) - cout << "expmap " << (string)it_x->first << " (j = " << var << "), delta = " << delta[var].transpose() << endl; - else - cout << " " << (string)it_x->first << " (j = " << var << "), delta = " << delta[var].transpose() << endl; - } - assert(delta[var].size() == (int)it_x->second.dim()); - assert(delta[var].unaryExpr(&isfinite).all()); - if(disableReordering) { - assert(mask[var]); - //assert(it_x->first.index() == var); - //assert(equal(delta[var], delta.container()[var])); - assert(delta.permutation()[var] == var); - } - if(mask[var]) { - it_x->second = it_x->second.expmap(delta[var]); - delta[var].operator=(Vector::Constant(delta[var].rows(), numeric_limits::infinity())); // Strange syntax to work with clang++ (bug in clang?) - } - } -}; -#endif - /* ************************************************************************* */ template void ISAM2::update( @@ -710,46 +490,34 @@ void ISAM2::update( tic(3,"gather involved keys"); // 3. Mark linear update FastSet markedKeys = Impl::IndicesFromFactors(ordering_, newFactors); // Get keys from new factors - vector newKeys; newKeys.reserve(markedKeys.size()); - newKeys.assign(markedKeys.begin(), markedKeys.end()); // Make a copy of these, as we'll soon add to them + FastVector newKeys(markedKeys.begin(), markedKeys.end()); // Make a copy of these, as we'll soon add to them FastSet structuralKeys; if(structuralLast) structuralKeys = markedKeys; // If we're using structural-last ordering, make another copy toc(3,"gather involved keys"); - // Check relinearization if we're at a 10th step, or we are using a looser loop relin threshold + // Check relinearization if we're at the nth step, or we are using a looser loop relin threshold if (force_relinearize || (params_.enableRelinearization && count % params_.relinearizeSkip == 0)) { // todo: every n steps tic(4,"gather relinearize keys"); vector markedRelinMask(ordering_.nVars(), false); - bool relinAny = false; // 4. Mark keys in \Delta above threshold \beta: J=\{\Delta_{j}\in\Delta|\Delta_{j}\geq\beta\}. FastSet relinKeys = Impl::CheckRelinearization(delta_, params_.relinearizeThreshold); if(disableReordering) relinKeys = Impl::CheckRelinearization(delta_, 0.0); // This is used for debugging + // Add the variables being relinearized to the marked keys BOOST_FOREACH(const Index j, relinKeys) { markedRelinMask[j] = true; } markedKeys.insert(relinKeys.begin(), relinKeys.end()); - if(!relinKeys.empty()) - relinAny = true; toc(4,"gather relinearize keys"); tic(5,"fluid find_all"); // 5. Mark all cliques that involve marked variables \Theta_{J} and all their ancestors. - if (relinAny) { - // mark all cliques that involve marked variables - if(this->root()) - find_all(this->root(), markedKeys, markedRelinMask); // add other cliques that have the marked ones in the separator - } + if (!relinKeys.empty() && this->root()) + Impl::FindAll(this->root(), markedKeys, markedRelinMask); // add other cliques that have the marked ones in the separator toc(5,"fluid find_all"); tic(6,"expmap"); // 6. Update linearization point for marked variables: \Theta_{J}:=\Theta_{J}+\Delta_{J}. - if (relinAny) { -#ifndef NDEBUG - _SelectiveExpmapAndClear selectiveExpmap(delta_, ordering_, markedRelinMask); -#else - _SelectiveExpmap selectiveExpmap(delta_, ordering_, markedRelinMask); -#endif - theta_.apply(selectiveExpmap); - } + if (!relinKeys.empty()) + Impl::ExpmapMasked(theta_, delta_, ordering_, markedRelinMask, delta_); toc(6,"expmap"); #ifndef NDEBUG @@ -776,7 +544,7 @@ void ISAM2::update( tic(8,"recalculate"); // 8. Redo top of Bayes tree boost::shared_ptr > replacedKeys; - if(markedKeys.size() > 0 || newKeys.size() > 0) + if(!markedKeys.empty() || !newKeys.empty()) replacedKeys = recalculate(markedKeys, structuralKeys, newKeys, linearFactors); toc(8,"recalculate"); @@ -808,10 +576,11 @@ void ISAM2::update( /* ************************************************************************* */ template Values ISAM2::calculateEstimate() const { + // We use ExpmapMasked here instead of regular expmap because the former + // handles Permuted Values ret(theta_); vector mask(ordering_.nVars(), true); - _SelectiveExpmap selectiveExpmap(delta_, ordering_, mask); - ret.apply(selectiveExpmap); + Impl::ExpmapMasked(ret, delta_, ordering_, mask); return ret; } diff --git a/gtsam/nonlinear/ISAM2.h b/gtsam/nonlinear/ISAM2.h index 33db9c7f1..d76099017 100644 --- a/gtsam/nonlinear/ISAM2.h +++ b/gtsam/nonlinear/ISAM2.h @@ -102,7 +102,7 @@ private: std::vector lastRelinVariables_; #endif - typedef JacobianFactor CacheFactor; + typedef HessianFactor CacheFactor; public: @@ -180,9 +180,9 @@ private: FactorGraph::shared_ptr relinearizeAffectedFactors(const FastList& affectedKeys) const; FactorGraph getCachedBoundaryFactors(Cliques& orphans); - boost::shared_ptr > recalculate(const FastSet& markedKeys, const FastSet& structuralKeys, const std::vector& newKeys, const FactorGraph::shared_ptr newFactors = FactorGraph::shared_ptr()); + boost::shared_ptr > recalculate(const FastSet& markedKeys, const FastSet& structuralKeys, + const FastVector& newKeys, const FactorGraph::shared_ptr newFactors = FactorGraph::shared_ptr()); // void linear_update(const GaussianFactorGraph& newFactors); - void find_all(sharedClique clique, FastSet& keys, const std::vector& marked); // helper function }; // ISAM2