diff --git a/gtsam/inference/ISAM2-impl-inl.h b/gtsam/inference/ISAM2-impl-inl.h new file mode 100644 index 000000000..ea81426b7 --- /dev/null +++ b/gtsam/inference/ISAM2-impl-inl.h @@ -0,0 +1,49 @@ +/** + * @file ISAM2-impl-inl.h + * @brief Incremental update functionality (ISAM2) for BayesTree, with fluid relinearization. + * @author Michael Kaess, Richard Roberts + */ + +namespace gtsam { + +/* ************************************************************************* */ +struct _VariableAdder { + Ordering& ordering; + Permuted& vconfig; + _VariableAdder(Ordering& _ordering, Permuted& _vconfig) : ordering(_ordering), vconfig(_vconfig) {} + template + void operator()(I xIt) { + const bool debug = ISDEBUG("ISAM2 AddVariables"); + Index var = vconfig->push_back_preallocated(zero(xIt->second.dim())); + vconfig.permutation()[var] = var; + ordering.insert(xIt->first, var); + if(debug) cout << "Adding variable " << (string)xIt->first << " with order " << var << endl; + } +}; + +/* ************************************************************************* */ +template +void ISAM2::Impl::AddVariables( + const VALUES& newTheta, VALUES& theta, Permuted& delta, Ordering& ordering, typename Base::Nodes& nodes) { + const bool debug = ISDEBUG("ISAM2 AddVariables"); + + theta.insert(newTheta); + if(debug) newTheta.print("The new variables are: "); + // Add the new keys onto the ordering, add zeros to the delta for the new variables + vector dims(newTheta.dims(*newTheta.orderingArbitrary(ordering.nVars()))); + if(debug) cout << "New variables have total dimensionality " << accumulate(dims.begin(), dims.end(), 0) << endl; + delta.container().reserve(delta->size() + newTheta.size(), delta->dim() + accumulate(dims.begin(), dims.end(), 0)); + delta.permutation().resize(delta->size() + newTheta.size()); + { + _VariableAdder vadder(ordering, delta); + newTheta.apply(vadder); + assert(delta.permutation().size() == delta.container().size()); + assert(delta.container().dim() == delta.container().dimCapacity()); + assert(ordering.nVars() == delta.size()); + assert(ordering.size() == delta.size()); + } + assert(ordering.nVars() >= nodes.size()); + nodes.resize(ordering.nVars()); +} + +} diff --git a/gtsam/inference/ISAM2-inl.h b/gtsam/inference/ISAM2-inl.h new file mode 100644 index 000000000..860d45b2f --- /dev/null +++ b/gtsam/inference/ISAM2-inl.h @@ -0,0 +1,875 @@ +/** + * @file ISAM2-inl.h + * @brief Incremental update functionality (ISAM2) for BayesTree, with fluid relinearization. + * @author Michael Kaess, Richard Roberts + */ + +#include +#include // for operator += +using namespace boost::assign; + +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +#include +#include + +#include + +#include + +// for WAFR paper, separate update and relinearization steps if defined +//#define SEPARATE_STEPS + + +namespace gtsam { + +using namespace std; + +static const bool disableReordering = false; +static const double batchThreshold = 0.65; +static const bool latestLast = true; +static const bool structuralLast = false; + +/** Create an empty Bayes Tree */ +template +ISAM2::ISAM2() : BayesTree(), delta_(Permutation(), deltaUnpermuted_) {} + +/** Create a Bayes Tree from a nonlinear factor graph */ +//template +//ISAM2::ISAM2(const NonlinearFactorGraph& nlfg, const Ordering& ordering, const Values& config) : +//BayesTree(nlfg.linearize(config)->eliminate(ordering)), theta_(config), +//variableIndex_(nlfg.symbolic(config, ordering), config.dims(ordering)), deltaUnpermuted_(variableIndex_.dims()), +//delta_(Permutation::Identity(variableIndex_.size())), nonlinearFactors_(nlfg), ordering_(ordering) { +// // todo: repeats calculation above, just to set "cached" +// // De-referencing shared pointer can be quite expensive because creates temporary +// _eliminate_const(*nlfg.linearize(config, ordering), cached_, ordering); +//} + +/* ************************************************************************* */ +template +FastList ISAM2::getAffectedFactors(const FastList& keys) const { + static const bool debug = false; + if(debug) cout << "Getting affected factors for "; + if(debug) { BOOST_FOREACH(const Index key, keys) { cout << key << " "; } } + if(debug) cout << endl; + + FactorGraph > allAffected; + FastList indices; + BOOST_FOREACH(const Index key, keys) { +// const list l = nonlinearFactors_.factors(key); +// indices.insert(indices.begin(), l.begin(), l.end()); + const VariableIndex::Factors& factors(variableIndex_[key]); + BOOST_FOREACH(size_t factor, factors) { + if(debug) cout << "Variable " << key << " affects factor " << factor << endl; + indices.push_back(factor); + } + } + indices.sort(); + indices.unique(); + if(debug) cout << "Affected factors are: "; + if(debug) { BOOST_FOREACH(const size_t index, indices) { cout << index << " "; } } + if(debug) cout << endl; + return indices; +} + +/* ************************************************************************* */ +// retrieve all factors that ONLY contain the affected variables +// (note that the remaining stuff is summarized in the cached factors) +template +FactorGraph::shared_ptr +ISAM2::relinearizeAffectedFactors(const FastList& affectedKeys) const { + + tic(1,"getAffectedFactors"); + FastList candidates = getAffectedFactors(affectedKeys); + toc(1,"getAffectedFactors"); + + NonlinearFactorGraph nonlinearAffectedFactors; + + tic(2,"affectedKeysSet"); + // for fast lookup below + FastSet affectedKeysSet; + affectedKeysSet.insert(affectedKeys.begin(), affectedKeys.end()); + toc(2,"affectedKeysSet"); + + tic(3,"check candidates"); + BOOST_FOREACH(size_t idx, candidates) { + bool inside = true; + BOOST_FOREACH(const Symbol& key, nonlinearFactors_[idx]->keys()) { + Index var = ordering_[key]; + if (affectedKeysSet.find(var) == affectedKeysSet.end()) { + inside = false; + break; + } + } + if (inside) + nonlinearAffectedFactors.push_back(nonlinearFactors_[idx]); + } + toc(3,"check candidates"); + + tic(4,"linearize"); + FactorGraph::shared_ptr linearized(nonlinearAffectedFactors.linearize(theta_, ordering_)); + toc(4,"linearize"); + return linearized; +} + +/* ************************************************************************* */ +// find intermediate (linearized) factors from cache that are passed into the affected area +template +FactorGraph::CacheFactor> +ISAM2::getCachedBoundaryFactors(Cliques& orphans) { + + static const bool debug = false; + + FactorGraph cachedBoundary; + + BOOST_FOREACH(sharedClique orphan, orphans) { + // find the last variable that was eliminated + Index key = (*orphan)->frontals().back(); +#ifndef NDEBUG +// typename BayesNet::const_iterator it = orphan->end(); +// const Conditional& lastConditional = **(--it); +// typename Conditional::const_iterator keyit = lastConditional.endParents(); +// const Index lastKey = *(--keyit); +// assert(key == lastKey); +#endif + // retrieve the cached factor and add to boundary + cachedBoundary.push_back(boost::dynamic_pointer_cast(orphan->cachedFactor())); + if(debug) { cout << "Cached factor for variable " << key; orphan->cachedFactor()->print(""); } + } + + return cachedBoundary; +} + +template +boost::shared_ptr > ISAM2::recalculate(const FastSet& markedKeys, const FastSet& structuralKeys, const vector& newKeys, const FactorGraph::shared_ptr newFactors) { + + static const bool debug = ISDEBUG("ISAM2 recalculate"); + + // Input: BayesTree(this), newFactors + +//#define PRINT_STATS // figures for paper, disable for timing +#ifdef PRINT_STATS + static int counter = 0; + int maxClique = 0; + double avgClique = 0; + int numCliques = 0; + int nnzR = 0; + if (counter>0) { // cannot call on empty tree + GaussianISAM2_P::CliqueData cdata = this->getCliqueData(); + GaussianISAM2_P::CliqueStats cstats = cdata.getStats(); + maxClique = cstats.maxConditionalSize; + avgClique = cstats.avgConditionalSize; + numCliques = cdata.conditionalSizes.size(); + nnzR = calculate_nnz(this->root()); + } + counter++; +#endif + + FastSet affectedStructuralKeys; + if(structuralLast) { + tic(0, "affectedStructuralKeys"); + affectedStructuralKeys.insert(structuralKeys.begin(), structuralKeys.end()); + // For each structural variable, collect the variables up the path to the root, + // which will be constrained to the back of the ordering. + BOOST_FOREACH(Index key, structuralKeys) { + sharedClique clique = this->nodes_[key]; + while(clique) { + affectedStructuralKeys.insert((*clique)->beginFrontals(), (*clique)->endFrontals()); + clique = clique->parent_.lock(); + } + } + toc(0, "affectedStructuralKeys"); + } + +// if(debug) newFactors->print("Recalculating factors: "); + if(debug) { + cout << "markedKeys: "; + BOOST_FOREACH(const Index key, markedKeys) { cout << key << " "; } + cout << endl; + cout << "newKeys: "; + BOOST_FOREACH(const Index key, newKeys) { cout << key << " "; } + cout << endl; + cout << "structuralKeys: "; + BOOST_FOREACH(const Index key, structuralKeys) { cout << key << " "; } + cout << endl; + cout << "affectedStructuralKeys: "; + BOOST_FOREACH(const Index key, affectedStructuralKeys) { cout << key << " "; } + cout << endl; + } + + // 1. Remove top of Bayes tree and convert to a factor graph: + // (a) For each affected variable, remove the corresponding clique and all parents up to the root. + // (b) Store orphaned sub-trees \BayesTree_{O} of removed cliques. + tic(1, "removetop"); + Cliques orphans; + BayesNet affectedBayesNet; + this->removeTop(markedKeys, affectedBayesNet, orphans); + toc(1, "removetop"); + + if(debug) affectedBayesNet.print("Removed top: "); + if(debug) orphans.print("Orphans: "); + + // FactorGraph factors(affectedBayesNet); + // bug was here: we cannot reuse the original factors, because then the cached factors get messed up + // [all the necessary data is actually contained in the affectedBayesNet, including what was passed in from the boundaries, + // so this would be correct; however, in the process we also generate new cached_ entries that will be wrong (ie. they don't + // contain what would be passed up at a certain point if batch elimination was done, but that's what we need); we could choose + // not to update cached_ from here, but then the new information (and potentially different variable ordering) is not reflected + // in the cached_ values which again will be wrong] + // so instead we have to retrieve the original linearized factors AND add the cached factors from the boundary + + // BEGIN OF COPIED CODE + + // ordering provides all keys in conditionals, there cannot be others because path to root included + tic(2,"affectedKeys"); + FastList affectedKeys = affectedBayesNet.ordering(); + toc(2,"affectedKeys"); + + if(affectedKeys.size() >= theta_.size() * batchThreshold) { + + tic(3,"batch"); + + tic(0,"add keys"); + boost::shared_ptr > affectedKeysSet(new FastSet()); + BOOST_FOREACH(const Ordering::value_type& key_index, ordering_) { affectedKeysSet->insert(key_index.second); } + toc(0,"add keys"); + + tic(1,"reorder"); + tic(1,"CCOLAMD"); + // Do a batch step - reorder and relinearize all variables + vector cmember(theta_.size(), 0); + if(structuralLast) { + if(theta_.size() > affectedStructuralKeys.size()) { + BOOST_FOREACH(Index var, affectedStructuralKeys) { cmember[var] = 1; } + if(latestLast) { BOOST_FOREACH(Index var, newKeys) { cmember[var] = 2; } } + } + } else if(latestLast) { + FastSet newKeysSet(newKeys.begin(), newKeys.end()); + if(theta_.size() > newKeysSet.size()) { + BOOST_FOREACH(Index var, newKeys) { cmember[var] = 1; } + } + } + Permutation::shared_ptr colamd(Inference::PermutationCOLAMD_(variableIndex_, cmember)); + Permutation::shared_ptr colamdInverse(colamd->inverse()); + toc(1,"CCOLAMD"); + + // Reorder + tic(2,"permute global variable index"); + variableIndex_.permute(*colamd); + toc(2,"permute global variable index"); + tic(3,"permute delta"); + delta_.permute(*colamd); + toc(3,"permute delta"); + tic(4,"permute ordering"); + ordering_.permuteWithInverse(*colamdInverse); + toc(4,"permute ordering"); + toc(1,"reorder"); + + tic(2,"linearize"); + GaussianFactorGraph factors(*nonlinearFactors_.linearize(theta_, ordering_)); + toc(2,"linearize"); + + tic(5,"eliminate"); + GaussianJunctionTree jt(factors, variableIndex_); + sharedClique newRoot = jt.eliminate(EliminateQR, true); + if(debug) newRoot->print("Eliminated: "); + toc(5,"eliminate"); + + tic(6,"insert"); + BayesTree::clear(); + assert(!this->root_); + this->insert(newRoot); + assert(this->root_); + toc(6,"insert"); + + toc(3,"batch"); + + lastAffectedMarkedCount = markedKeys.size(); + lastAffectedVariableCount = affectedKeysSet->size(); + lastAffectedFactorCount = factors.size(); + + return affectedKeysSet; + + } else { + + tic(4,"incremental"); + + FastList affectedAndNewKeys; + affectedAndNewKeys.insert(affectedAndNewKeys.end(), affectedKeys.begin(), affectedKeys.end()); + affectedAndNewKeys.insert(affectedAndNewKeys.end(), newKeys.begin(), newKeys.end()); + tic(1,"relinearizeAffected"); + GaussianFactorGraph factors(*relinearizeAffectedFactors(affectedAndNewKeys)); + toc(1,"relinearizeAffected"); + +#ifndef NDEBUG +#ifndef SEPARATE_STEPS + // The relinearized variables should not appear anywhere in the orphans + BOOST_FOREACH(boost::shared_ptr::Clique> clique, orphans) { + BOOST_FOREACH(const Index key, (*clique)->frontals()) { + assert(lastRelinVariables_[key] == false); + } + } +#endif +#endif + + // if(debug) factors.print("Affected factors: "); + if(debug) { cout << "Affected keys: "; BOOST_FOREACH(const Index key, affectedKeys) { cout << key << " "; } cout << endl; } + + lastAffectedMarkedCount = markedKeys.size(); + lastAffectedVariableCount = affectedKeys.size(); + lastAffectedFactorCount = factors.size(); + +#ifdef PRINT_STATS + // output for generating figures + cout << "linear: #markedKeys: " << markedKeys.size() << " #affectedVariables: " << affectedKeys.size() + << " #affectedFactors: " << factors.size() << " maxCliqueSize: " << maxClique + << " 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); + if(debug) cachedBoundary.print("Boundary factors: "); + factors.reserve(factors.size() + cachedBoundary.size()); + // Copy so that we can later permute factors + BOOST_FOREACH(const CacheFactor::shared_ptr& cached, cachedBoundary) { +#ifndef NDEBUG +#ifndef SEPARATE_STEPS + BOOST_FOREACH(const Index key, *cached) { + assert(lastRelinVariables_[key] == false); + } +#endif +#endif + factors.push_back(GaussianFactor::shared_ptr(new CacheFactor(*cached))); + } + // factors.push_back(cachedBoundary); + toc(2,"cached"); + + // END OF COPIED CODE + + + // 2. Add the new factors \Factors' into the resulting factor graph + tic(3,"newfactors"); + if (newFactors) { +#ifndef NDEBUG + BOOST_FOREACH(const GaussianFactor::shared_ptr& newFactor, *newFactors) { + bool found = false; + BOOST_FOREACH(const GaussianFactor::shared_ptr& affectedFactor, factors) { + if(newFactor->equals(*affectedFactor, 1e-6)) + found = true; + } + assert(found); + } +#endif + //factors.push_back(*newFactors); + } + toc(3,"newfactors"); + + // 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"); + + //#define PRESORT_ALPHA + + tic(1,"select affected variables"); + // 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 + +#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"); + + // We now need to permute everything according this partial reordering: the + // delta vector, the global ordering, and the factors we're about to + // re-eliminate. The reordered variables are also mentioned in the + // 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"); + + 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"); + + 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) { + assert(!this->root_); + this->insert(newRoot); + } + 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); + } + toc(1,"permute"); + tic(2,"insert"); + // add orphans to the bottom of the new tree + BOOST_FOREACH(sharedClique orphan, orphans) { + // Because the affectedKeysSelector is sorted, the orphan separator keys + // will be sorted correctly according to the new elimination order after + // applying the permutation, so findParentClique, which looks for the + // lowest-ordered parent, will still work. + Index parentRepresentative = Base::findParentClique((*orphan)->parents()); + sharedClique parent = (*this)[parentRepresentative]; + parent->children_ += orphan; + orphan->parent_ = parent; // set new parent! + } + toc(2,"insert"); + toc(7,"orphans"); + + toc(4,"incremental"); + + return affectedKeysSet; + } + + // Output: BayesTree(this) + +// boost::shared_ptr > affectedKeysSet(new set()); +// 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( + const NonlinearFactorGraph& newFactors, const Values& newTheta, + double wildfire_threshold, double relinearize_threshold, bool relinearize, bool force_relinearize) { + + static const bool debug = ISDEBUG("ISAM2 update"); + static const bool verbose = ISDEBUG("ISAM2 update verbose"); + if(disableReordering) { wildfire_threshold = -1.0; relinearize_threshold = 0.0; } + + static int count = 0; + count++; + + lastAffectedVariableCount = 0; + lastAffectedFactorCount = 0; + lastAffectedCliqueCount = 0; + lastAffectedMarkedCount = 0; + lastBacksubVariableCount = 0; + lastNnzTop = 0; + + if(verbose) { + cout << "ISAM2::update\n"; + this->print("ISAM2: "); + } + + tic(1,"push_back factors"); + // 1. Add any new factors \Factors:=\Factors\cup\Factors'. + if(debug || verbose) newFactors.print("The new factors are: "); + nonlinearFactors_.push_back(newFactors); + toc(1,"push_back factors"); + + tic(2,"add new variables"); + // 2. Initialize any new variables \Theta_{new} and add \Theta:=\Theta\cup\Theta_{new}. + Impl::AddVariables(newTheta, theta_, delta_, ordering_, Base::nodes_); + toc(2,"add new variables"); + + tic(3,"gather involved keys"); + // 3. Mark linear update + FastSet markedKeys; + FastSet structuralKeys; + vector newKeys; newKeys.reserve(newFactors.size() * 6); + BOOST_FOREACH(const typename NonlinearFactor::shared_ptr& factor, newFactors) { + BOOST_FOREACH(const Symbol& key, factor->keys()) { + Index var = ordering_[key]; + markedKeys.insert(var); + if(structuralLast) structuralKeys.insert(var); + newKeys.push_back(var); + } + } + toc(3,"gather involved keys"); + +#ifdef SEPARATE_STEPS // original algorithm from paper: separate relin and optimize + + // todo: kaess - don't need linear factors here, just to update variableIndex + boost::shared_ptr linearFactors = newFactors.linearize(theta_, ordering_); + variableIndex_.augment(*linearFactors); + + boost::shared_ptr > replacedKeys_todo = recalculate(markedKeys, newKeys, linearFactors); + markedKeys.clear(); + vector none(variableIndex_.size(), false); + optimize2(this->root(), wildfire_threshold, none, delta_); +#endif + + vector markedRelinMask(ordering_.nVars(), false); + bool relinAny = false; + // Check relinearization if we're at a 10th step, or we are using a looser loop relin threshold + if (force_relinearize || (relinearize && count%10 == 0)) { // todo: every n steps + tic(4,"gather relinearize keys"); + // 4. Mark keys in \Delta above threshold \beta: J=\{\Delta_{j}\in\Delta|\Delta_{j}\geq\beta\}. + for(Index var=0; var(); + if(maxDelta >= relinearize_threshold) { + markedRelinMask[var] = true; + markedKeys.insert(var); + if(!relinAny) 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 + // richard commented these out since now using an array to mark keys + //affectedKeys.sort(); // remove duplicates + //affectedKeys.unique(); + // merge with markedKeys + } + // richard commented these out since now using an array to mark keys + //markedKeys.splice(markedKeys.begin(), affectedKeys, affectedKeys.begin(), affectedKeys.end()); + //markedKeys.sort(); // remove duplicates + //markedKeys.unique(); +// BOOST_FOREACH(const Index var, affectedKeys) { +// markedKeys.push_back(var); +// } + 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); +// theta_ = theta_.expmap(deltaMarked); + } + toc(6,"expmap"); + +#ifndef NDEBUG + lastRelinVariables_ = markedRelinMask; +#endif + +#ifndef SEPARATE_STEPS + tic(7,"linearize new"); + tic(1,"linearize"); + // 7. Linearize new factors + FactorGraph::shared_ptr linearFactors = newFactors.linearize(theta_, ordering_); + toc(1,"linearize"); + + tic(2,"augment VI"); + // Augment the variable index with the new factors + variableIndex_.augment(*linearFactors); + toc(2,"augment VI"); + toc(7,"linearize new"); + + tic(8,"recalculate"); + // 8. Redo top of Bayes tree + if(markedKeys.size() > 0 || newKeys.size() > 0) + replacedKeys = recalculate(markedKeys, structuralKeys, newKeys, linearFactors); + toc(8,"recalculate"); +#else + vector empty; + boost::shared_ptr > replacedKeys = recalculate(markedKeys, empty); +#endif + + tic(9,"solve"); + // 9. Solve + if (wildfire_threshold<=0.) { + VectorValues newDelta(theta_.dims(ordering_)); + optimize2(this->root(), newDelta); + if(debug) newDelta.print("newDelta: "); + assert(newDelta.size() == delta_.size()); + delta_.permutation() = Permutation::Identity(delta_.size()); + delta_.container() = newDelta; + lastBacksubVariableCount = theta_.size(); + +//#ifndef NDEBUG +// FactorGraph linearfullJ = *nonlinearFactors_.linearize(theta_, ordering_); +// VectorValues deltafullJ = optimize(*GenericSequentialSolver(linearfullJ).eliminate()); +// FactorGraph linearfullH = +// *nonlinearFactors_.linearize(theta_, ordering_)->template convertCastFactors >(); +// VectorValues deltafullH = optimize(*GenericSequentialSolver(linearfullH).eliminate()); +// if(!assert_equal(deltafullJ, newDelta, 1e-2)) +// throw runtime_error("iSAM2 does not agree with full Jacobian solver"); +// if(!assert_equal(deltafullH, newDelta, 1e-2)) +// throw runtime_error("iSAM2 does not agree with full Hessian solver"); + //#endif + + } else { + vector replacedKeysMask(variableIndex_.size(), false); + if(replacedKeys) { + BOOST_FOREACH(const Index var, *replacedKeys) { + replacedKeysMask[var] = true; } } + lastBacksubVariableCount = optimize2(this->root(), wildfire_threshold, replacedKeysMask, delta_); // modifies delta_ + +#ifndef NDEBUG + for(size_t j=0; j).all()); +#endif + } + toc(9,"solve"); +} + +/* ************************************************************************* */ +template +Values ISAM2::calculateEstimate() const { + Values ret(theta_); + vector mask(ordering_.nVars(), true); + _SelectiveExpmap selectiveExpmap(delta_, ordering_, mask); + ret.apply(selectiveExpmap); + return ret; +} + +/* ************************************************************************* */ +template +Values ISAM2::calculateBestEstimate() const { + VectorValues delta(theta_.dims(ordering_)); + optimize2(this->root(), delta); + return theta_.expmap(delta, ordering_); +} + +} +/// namespace gtsam diff --git a/gtsam/inference/ISAM2.h b/gtsam/inference/ISAM2.h new file mode 100644 index 000000000..40cd083a8 --- /dev/null +++ b/gtsam/inference/ISAM2.h @@ -0,0 +1,135 @@ +/** + * @file ISAM2.h + * @brief Incremental update functionality (ISAM2) for BayesTree, with fluid relinearization. + * @author Michael Kaess + */ + +// \callgraph + +#pragma once + +#include +#include +#include +//#include +//#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace gtsam { + +//typedef std::vector CachedFactors; + +template +class ISAM2: public BayesTree { + +protected: + + // current linearization point + VALUES theta_; + + // VariableIndex lets us look up factors by involved variable and keeps track of dimensions + VariableIndex variableIndex_; + + // the linear solution, an update to the estimate in theta + VectorValues deltaUnpermuted_; + + // The residual permutation through which the deltaUnpermuted_ is + // referenced. Permuting the VectorVALUES is slow, so for performance the + // permutation is applied at access time instead of to the VectorVALUES + // itself. + Permuted delta_; + + // for keeping all original nonlinear factors + NonlinearFactorGraph nonlinearFactors_; + + // The "ordering" allows converting Symbols to Index (integer) keys. We + // keep it up to date as we add and reorder variables. + Ordering ordering_; + + // cached intermediate results for restarting computation in the middle + // CachedFactors cached_; + +#ifndef NDEBUG + std::vector lastRelinVariables_; +#endif + +public: + + typedef BayesTree Base; + typedef ISAM2 This; + + /** Create an empty Bayes Tree */ + ISAM2(); + + // /** Create a Bayes Tree from a Bayes Net */ + // ISAM2(const NonlinearFactorGraph& fg, const Ordering& ordering, const VALUES& config); + + /** Destructor */ + virtual ~ISAM2() {} + + typedef typename BayesTree::sharedClique sharedClique; + typedef typename BayesTree::Cliques Cliques; + typedef JacobianFactor CacheFactor; + + /** + * ISAM2. + */ + void update(const NonlinearFactorGraph& newFactors, const VALUES& newTheta, + double wildfire_threshold = 0., double relinearize_threshold = 0., bool relinearize = true, + bool force_relinearize = false); + + // needed to create initial estimates + const VALUES& getLinearizationPoint() const {return theta_;} + + // estimate based on incomplete delta (threshold!) + VALUES calculateEstimate() const; + + // estimate based on full delta (note that this is based on the current linearization point) + VALUES calculateBestEstimate() const; + + const Permuted& getDelta() const { return delta_; } + + const NonlinearFactorGraph& getFactorsUnsafe() const { return nonlinearFactors_; } + + const Ordering& getOrdering() const { return ordering_; } + + size_t lastAffectedVariableCount; + size_t lastAffectedFactorCount; + size_t lastAffectedCliqueCount; + size_t lastAffectedMarkedCount; + size_t lastBacksubVariableCount; + size_t lastNnzTop; + + boost::shared_ptr > replacedKeys; + +private: + + FastList getAffectedFactors(const FastList& keys) const; + 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()); + // void linear_update(const GaussianFactorGraph& newFactors); + void find_all(sharedClique clique, FastSet& keys, const std::vector& marked); // helper function + +public: + + struct Impl { + static void AddVariables(const VALUES& newTheta, VALUES& theta, Permuted& delta, Ordering& ordering, typename Base::Nodes& nodes); + }; + +}; // ISAM2 + +} /// namespace gtsam diff --git a/gtsam/inference/Makefile.am b/gtsam/inference/Makefile.am index 6bd273b06..084da71a0 100644 --- a/gtsam/inference/Makefile.am +++ b/gtsam/inference/Makefile.am @@ -34,6 +34,7 @@ headers += EliminationTree.h EliminationTree-inl.h headers += BayesNet.h BayesNet-inl.h headers += BayesTree.h BayesTree-inl.h headers += ISAM.h ISAM-inl.h +headers += ISAM2.h ISAM2-inl.h ISAM2-impl-inl.h check_PROGRAMS += tests/testInference check_PROGRAMS += tests/testFactorGraph check_PROGRAMS += tests/testFactorGraph diff --git a/gtsam/nonlinear/GaussianISAM2.cpp b/gtsam/nonlinear/GaussianISAM2.cpp new file mode 100644 index 000000000..bf3a91b0a --- /dev/null +++ b/gtsam/nonlinear/GaussianISAM2.cpp @@ -0,0 +1,149 @@ +/** + * @file GaussianISAM2 + * @brief Full non-linear ISAM + * @author Michael Kaess + */ + +#include +#include +#include + +using namespace std; +using namespace gtsam; + +#include + +namespace gtsam { + +// Explicitly instantiate so we don't have to include everywhere +template class ISAM2; + +/* ************************************************************************* */ +void optimize2(const GaussianISAM2::sharedClique& clique, double threshold, + vector& changed, const vector& replaced, Permuted& delta, int& count) { + // if none of the variables in this clique (frontal and separator!) changed + // significantly, then by the running intersection property, none of the + // cliques in the children need to be processed + + // Are any clique variables part of the tree that has been redone? + bool cliqueReplaced = replaced[(*clique)->frontals().front()]; +#ifndef NDEBUG + BOOST_FOREACH(Index frontal, (*clique)->frontals()) { + assert(cliqueReplaced == replaced[frontal]); + } +#endif + + // If not redone, then has one of the separator variables changed significantly? + bool recalculate = cliqueReplaced; + if(!recalculate) { + BOOST_FOREACH(Index frontal, (*clique)->frontals()) { + if(changed[frontal]) { + recalculate = true; + break; + } + } + } + + // Solve clique if it was replaced, or if any parents were changed above the + // threshold or themselves replaced. + if(recalculate) { + + // Temporary copy of the original values, to check how much they change + vector originalValues((*clique)->nrFrontals()); + GaussianISAM2::ConditionalType::const_iterator it; + for(it = (*clique)->beginFrontals(); it!=(*clique)->endFrontals(); it++) { + originalValues[it - (*clique)->beginFrontals()] = delta[*it]; + } + + // Back-substitute + (*clique)->rhs(delta); + (*clique)->solveInPlace(delta); + count += (*clique)->nrFrontals(); + + // Whether the values changed above a threshold, or always true if the + // clique was replaced. + bool valuesChanged = cliqueReplaced; + for(it = (*clique)->beginFrontals(); it!=(*clique)->endFrontals(); it++) { + if(!valuesChanged) { + const Vector& oldValue(originalValues[it - (*clique)->beginFrontals()]); + const VectorValues::mapped_type& newValue(delta[*it]); + if((oldValue - newValue).lpNorm() >= threshold) { + valuesChanged = true; + break; + } + } else + break; + } + + // If the values were above the threshold or this clique was replaced + if(valuesChanged) { + // Set changed flag for each frontal variable and leave the new values + BOOST_FOREACH(Index frontal, (*clique)->frontals()) { + changed[frontal] = true; + } + } else { + // Replace with the old values + for(it = (*clique)->beginFrontals(); it!=(*clique)->endFrontals(); it++) { + delta[*it] = originalValues[it - (*clique)->beginFrontals()]; + } + } + + // Recurse to children + BOOST_FOREACH(const GaussianISAM2::sharedClique& child, clique->children_) { + optimize2(child, threshold, changed, replaced, delta, count); + } + } +} + +/* ************************************************************************* */ +// fast full version without threshold +void optimize2(const GaussianISAM2::sharedClique& clique, VectorValues& delta) { + + // parents are assumed to already be solved and available in result + (*clique)->rhs(delta); + (*clique)->solveInPlace(delta); + + // Solve chilren recursively + BOOST_FOREACH(const GaussianISAM2::sharedClique& child, clique->children_) { + optimize2(child, delta); + } +} + +///* ************************************************************************* */ +//boost::shared_ptr optimize2(const GaussianISAM2::sharedClique& root) { +// boost::shared_ptr delta(new VectorValues()); +// set changed; +// // starting from the root, call optimize on each conditional +// optimize2(root, delta); +// return delta; +//} + +/* ************************************************************************* */ +int optimize2(const GaussianISAM2::sharedClique& root, double threshold, const vector& keys, Permuted& delta) { + vector changed(keys.size(), false); + int count = 0; + // starting from the root, call optimize on each conditional + optimize2(root, threshold, changed, keys, delta, count); + return count; +} + +/* ************************************************************************* */ +void nnz_internal(const GaussianISAM2::sharedClique& clique, int& result) { + int dimR = (*clique)->dim(); + int dimSep = (*clique)->get_S().cols() - dimR; + result += ((dimR+1)*dimR)/2 + dimSep*dimR; + // traverse the children + BOOST_FOREACH(const GaussianISAM2::sharedClique& child, clique->children_) { + nnz_internal(child, result); + } +} + +/* ************************************************************************* */ +int calculate_nnz(const GaussianISAM2::sharedClique& clique) { + int result = 0; + // starting from the root, add up entries of frontal and conditional matrices of each conditional + nnz_internal(clique, result); + return result; +} + +} /// namespace gtsam diff --git a/gtsam/nonlinear/GaussianISAM2.h b/gtsam/nonlinear/GaussianISAM2.h new file mode 100644 index 000000000..a2a5bc710 --- /dev/null +++ b/gtsam/nonlinear/GaussianISAM2.h @@ -0,0 +1,39 @@ +/** + * @file GaussianISAM + * @brief Full non-linear ISAM. + * @author Michael Kaess + */ + +// \callgraph + +#pragma once + +#include +#include +#include +#include +#include + +namespace gtsam { + + typedef ISAM2 GaussianISAM2; + typedef ISAM2 GaussianISAM2_P; + + // optimize the BayesTree, starting from the root + void optimize2(const GaussianISAM2::sharedClique& root, VectorValues& delta); + + // optimize the BayesTree, starting from the root; "replaced" needs to contain + // all variables that are contained in the top of the Bayes tree that has been + // redone; "delta" is the current solution, an offset from the linearization + // point; "threshold" is the maximum change against the PREVIOUS delta for + // non-replaced variables that can be ignored, ie. the old delta entry is kept + // and recursive backsubstitution might eventually stop if none of the changed + // variables are contained in the subtree. + // returns the number of variables that were solved for + int optimize2(const GaussianISAM2::sharedClique& root, + double threshold, const std::vector& replaced, Permuted& delta); + + // calculate the number of non-zero entries for the tree starting at clique (use root for complete matrix) + int calculate_nnz(const GaussianISAM2::sharedClique& clique); + +}/// namespace gtsam diff --git a/gtsam/nonlinear/Makefile.am b/gtsam/nonlinear/Makefile.am index 7e3919b0b..7e9ac429c 100644 --- a/gtsam/nonlinear/Makefile.am +++ b/gtsam/nonlinear/Makefile.am @@ -27,8 +27,9 @@ headers += NonlinearFactor.h sources += NonlinearOptimizer.cpp Ordering.cpp check_PROGRAMS += tests/testKey tests/testOrdering -# Nonlinear iSAM +# Nonlinear iSAM(2) headers += NonlinearISAM.h NonlinearISAM-inl.h +sources += GaussianISAM2.cpp # Nonlinear constraints headers += NonlinearEquality.h NonlinearConstraint.h diff --git a/tests/Makefile.am b/tests/Makefile.am index 8898d8648..30e738fa5 100644 --- a/tests/Makefile.am +++ b/tests/Makefile.am @@ -21,6 +21,7 @@ check_PROGRAMS += testNonlinearISAM check_PROGRAMS += testBoundingConstraint check_PROGRAMS += testNonlinearEqualityConstraint check_PROGRAMS += testPose2SLAMwSPCG +check_PROGRAMS += testGaussianISAM2 # only if serialization is available if ENABLE_SERIALIZATION diff --git a/tests/testGaussianISAM2.cpp b/tests/testGaussianISAM2.cpp new file mode 100644 index 000000000..a95bbc34d --- /dev/null +++ b/tests/testGaussianISAM2.cpp @@ -0,0 +1,248 @@ +/** + * @file testGaussianISAM2.cpp + * @brief Unit tests for GaussianISAM2 + * @author Michael Kaess + */ + +#include +#include // for operator += +using namespace boost::assign; + +#include + +#define GTSAM_MAGIC_KEY + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +using namespace std; +using namespace gtsam; +using namespace example; +using boost::shared_ptr; + +const double tol = 1e-4; + +/* ************************************************************************* */ +TEST(ISAM2, AddVariables) { + + // Create initial state + planarSLAM::Values theta; + theta.insert(planarSLAM::PoseKey(0), Pose2(.1, .2, .3)); + theta.insert(planarSLAM::PointKey(0), Point2(.4, .5)); + planarSLAM::Values newTheta; + newTheta.insert(planarSLAM::PoseKey(1), Pose2(.6, .7, .8)); + + VectorValues deltaUnpermuted; + deltaUnpermuted.reserve(2, 5); + { Vector a(3); a << .1, .2, .3; deltaUnpermuted.push_back_preallocated(a); } + { Vector a(2); a << .4, .5; deltaUnpermuted.push_back_preallocated(a); } + + Permutation permutation(2); + permutation[0] = 1; + permutation[1] = 0; + + Permuted delta(permutation, deltaUnpermuted); + + Ordering ordering; ordering += planarSLAM::PointKey(0), planarSLAM::PoseKey(0); + + ISAM2::Nodes nodes(2); + + // Verify initial state + LONGS_EQUAL(0, ordering[planarSLAM::PointKey(0)]); + LONGS_EQUAL(1, ordering[planarSLAM::PoseKey(0)]); + EXPECT(assert_equal(deltaUnpermuted[1], delta[ordering[planarSLAM::PointKey(0)]])); + EXPECT(assert_equal(deltaUnpermuted[0], delta[ordering[planarSLAM::PoseKey(0)]])); + + // Create expected state + planarSLAM::Values thetaExpected; + thetaExpected.insert(planarSLAM::PoseKey(0), Pose2(.1, .2, .3)); + thetaExpected.insert(planarSLAM::PointKey(0), Point2(.4, .5)); + thetaExpected.insert(planarSLAM::PoseKey(1), Pose2(.6, .7, .8)); + + VectorValues deltaUnpermutedExpected; + deltaUnpermutedExpected.reserve(3, 8); + { Vector a(3); a << .1, .2, .3; deltaUnpermutedExpected.push_back_preallocated(a); } + { Vector a(2); a << .4, .5; deltaUnpermutedExpected.push_back_preallocated(a); } + { Vector a(3); a << 0, 0, 0; deltaUnpermutedExpected.push_back_preallocated(a); } + + Permutation permutationExpected(3); + permutationExpected[0] = 1; + permutationExpected[1] = 0; + permutationExpected[2] = 2; + + Permuted deltaExpected(permutationExpected, deltaUnpermutedExpected); + + Ordering orderingExpected; orderingExpected += planarSLAM::PointKey(0), planarSLAM::PoseKey(0), planarSLAM::PoseKey(1); + + ISAM2::Nodes nodesExpected( + 3, ISAM2::sharedClique()); + + // Expand initial state + ISAM2::Impl::AddVariables(newTheta, theta, delta, ordering, nodes); + + EXPECT(assert_equal(thetaExpected, theta)); + EXPECT(assert_equal(deltaUnpermutedExpected, deltaUnpermuted)); + EXPECT(assert_equal(deltaExpected.permutation(), delta.permutation())); + EXPECT(assert_equal(orderingExpected, ordering)); +} + +/* ************************************************************************* */ +TEST(ISAM2, optimize2) { + + // Create initialization + planarSLAM::Values theta; + theta.insert(planarSLAM::PoseKey(0), Pose2(0.01, 0.01, 0.01)); + + // Create conditional + Vector d(3); d << -0.1, -0.1, -0.31831; + Matrix R(3,3); R << + 10, 0.0, 0.0, + 0.0, 10, 0.0, + 0.0, 0.0, 31.8309886; + GaussianConditional::shared_ptr conditional(new GaussianConditional(0, d, R, Vector::Ones(3))); + + // Create ordering + Ordering ordering; ordering += planarSLAM::PoseKey(0); + + // Expected vector + VectorValues expected(1, 3); + conditional->rhs(expected); + conditional->solveInPlace(expected); + + // Clique + GaussianISAM2::sharedClique clique(new GaussianISAM2::Clique(conditional)); + VectorValues actual(theta.dims(ordering)); + conditional->rhs(actual); + optimize2(clique, actual); + +// expected.print("expected: "); +// actual.print("actual: "); + EXPECT(assert_equal(expected, actual)); +} + +/* ************************************************************************* */ +bool isam_check(const planarSLAM::Graph& fullgraph, const planarSLAM::Values& fullinit, const GaussianISAM2_P& isam) { + planarSLAM::Values actual = isam.calculateEstimate(); + Ordering ordering = isam.getOrdering(); // *fullgraph.orderingCOLAMD(fullinit).first; + GaussianFactorGraph linearized = *fullgraph.linearize(fullinit, ordering); +// linearized.print("Expected linearized: "); + GaussianBayesNet gbn = *GaussianSequentialSolver(linearized).eliminate(); +// gbn.print("Expected bayesnet: "); + VectorValues delta = optimize(gbn); + planarSLAM::Values expected = fullinit.expmap(delta, ordering); + + return assert_equal(expected, actual); +} + +/* ************************************************************************* */ +TEST(ISAM2, slamlike_solution) +{ + + // Pose and landmark key types from planarSLAM + typedef planarSLAM::PoseKey PoseKey; + typedef planarSLAM::PointKey PointKey; + + // Set up parameters + double wildfire = 0.001; + SharedDiagonal odoNoise = sharedSigmas(Vector_(3, 0.1, 0.1, M_PI/100.0)); + SharedDiagonal brNoise = sharedSigmas(Vector_(2, M_PI/100.0, 0.1)); + + // These variables will be reused and accumulate factors and values + GaussianISAM2_P isam; + planarSLAM::Values fullinit; + planarSLAM::Graph fullgraph; + + // i keeps track of the time step + size_t i = 0; + + // Add a prior at time 0 and update isam + { + planarSLAM::Graph newfactors; + newfactors.addPrior(0, Pose2(0.0, 0.0, 0.0), odoNoise); + fullgraph.push_back(newfactors); + + planarSLAM::Values init; + init.insert(PoseKey(0), Pose2(0.01, 0.01, 0.01)); + fullinit.insert(PoseKey(0), Pose2(0.01, 0.01, 0.01)); + + isam.update(newfactors, init, wildfire, 0.0, false); + } + + EXPECT(isam_check(fullgraph, fullinit, isam)); + + // Add odometry from time 0 to time 5 + for( ; i<5; ++i) { + planarSLAM::Graph newfactors; + newfactors.addOdometry(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise); + fullgraph.push_back(newfactors); + + planarSLAM::Values init; + init.insert(PoseKey(i+1), Pose2(double(i+1)+0.1, -0.1, 0.01)); + fullinit.insert(PoseKey(i+1), Pose2(double(i+1)+0.1, -0.1, 0.01)); + + isam.update(newfactors, init, wildfire, 0.0, false); + } + + // Add odometry from time 5 to 6 and landmark measurement at time 5 + { + planarSLAM::Graph newfactors; + newfactors.addOdometry(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise); + newfactors.addBearingRange(i, 0, Rot2::fromAngle(M_PI/4.0), 5.0, brNoise); + newfactors.addBearingRange(i, 1, Rot2::fromAngle(-M_PI/4.0), 5.0, brNoise); + fullgraph.push_back(newfactors); + + planarSLAM::Values init; + init.insert(PoseKey(i+1), Pose2(1.01, 0.01, 0.01)); + init.insert(PointKey(0), Point2(5.0/sqrt(2.0), 5.0/sqrt(2.0))); + init.insert(PointKey(1), Point2(5.0/sqrt(2.0), -5.0/sqrt(2.0))); + fullinit.insert(PoseKey(i+1), Pose2(1.01, 0.01, 0.01)); + fullinit.insert(PointKey(0), Point2(5.0/sqrt(2.0), 5.0/sqrt(2.0))); + fullinit.insert(PointKey(1), Point2(5.0/sqrt(2.0), -5.0/sqrt(2.0))); + + isam.update(newfactors, init, wildfire, 0.0, false); + ++ i; + } + + // Add odometry from time 6 to time 10 + for( ; i<10; ++i) { + planarSLAM::Graph newfactors; + newfactors.addOdometry(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise); + fullgraph.push_back(newfactors); + + planarSLAM::Values init; + init.insert(PoseKey(i+1), Pose2(double(i+1)+0.1, -0.1, 0.01)); + fullinit.insert(PoseKey(i+1), Pose2(double(i+1)+0.1, -0.1, 0.01)); + + isam.update(newfactors, init, wildfire, 0.0, false); + } + + // Add odometry from time 10 to 11 and landmark measurement at time 10 + { + planarSLAM::Graph newfactors; + newfactors.addOdometry(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise); + newfactors.addBearingRange(i, 0, Rot2::fromAngle(M_PI/4.0 + M_PI/16.0), 4.5, brNoise); + newfactors.addBearingRange(i, 1, Rot2::fromAngle(-M_PI/4.0 + M_PI/16.0), 4.5, brNoise); + fullgraph.push_back(newfactors); + + planarSLAM::Values init; + init.insert(PoseKey(i+1), Pose2(6.9, 0.1, 0.01)); + fullinit.insert(PoseKey(i+1), Pose2(6.9, 0.1, 0.01)); + + isam.update(newfactors, init, wildfire, 0.0, false); + ++ i; + } + + // Compare solutions + EXPECT(isam_check(fullgraph, fullinit, isam)); +} + +/* ************************************************************************* */ +int main() { TestResult tr; return TestRegistry::runAllTests(tr);} +/* ************************************************************************* */