From b825655ba662a32875a57e41658afb75fe9cf8da Mon Sep 17 00:00:00 2001 From: Michael Kaess Date: Thu, 9 Sep 2010 23:54:39 +0000 Subject: [PATCH] update+relin combined for speed; new backsub/threshold confirmed to yield correct result --- inference/ISAM2-inl.h | 718 ++++++++++++++++++++++------------------- inference/ISAM2.h | 97 +++--- slam/GaussianISAM2.cpp | 2 +- 3 files changed, 438 insertions(+), 379 deletions(-) diff --git a/inference/ISAM2-inl.h b/inference/ISAM2-inl.h index 4e4e28b50..a7f2ae87c 100644 --- a/inference/ISAM2-inl.h +++ b/inference/ISAM2-inl.h @@ -83,7 +83,7 @@ void tictoc_print() { timing.print(); } #else - void tictoc_print() {} +void tictoc_print() {} double tic(string id) {return 0.;} double toc(string id) {return 0.;} #endif @@ -91,225 +91,360 @@ double toc(string id) {return 0.;} namespace gtsam { - using namespace std; +using namespace std; - // from inference-inl.h - need to additionally return the newly created factor for caching - boost::shared_ptr _eliminateOne(FactorGraph& graph, CachedFactors& cached, const Symbol& key) { +// from inference-inl.h - need to additionally return the newly created factor for caching +boost::shared_ptr _eliminateOne(FactorGraph& graph, CachedFactors& cached, const Symbol& key) { - // combine the factors of all nodes connected to the variable to be eliminated - // if no factors are connected to key, returns an empty factor - tic("eliminate_removeandcombinefactors"); - boost::shared_ptr joint_factor = removeAndCombineFactors(graph,key); - toc("eliminate_removeandcombinefactors"); + // combine the factors of all nodes connected to the variable to be eliminated + // if no factors are connected to key, returns an empty factor + tic("eliminate_removeandcombinefactors"); + boost::shared_ptr joint_factor = removeAndCombineFactors(graph,key); + toc("eliminate_removeandcombinefactors"); - // eliminate that joint factor - boost::shared_ptr factor; - boost::shared_ptr conditional; - tic("eliminate_eliminate"); - boost::tie(conditional, factor) = joint_factor->eliminate(key); - toc("eliminate_eliminate"); + // eliminate that joint factor + boost::shared_ptr factor; + boost::shared_ptr conditional; + tic("eliminate_eliminate"); + boost::tie(conditional, factor) = joint_factor->eliminate(key); + toc("eliminate_eliminate"); - // ADDED: remember the intermediate result to be able to later restart computation in the middle - cached[key] = factor; + // ADDED: remember the intermediate result to be able to later restart computation in the middle + cached[key] = factor; - // add new factor on separator back into the graph - if (!factor->empty()) graph.push_back(factor); + // add new factor on separator back into the graph + if (!factor->empty()) graph.push_back(factor); - // return the conditional Gaussian - return conditional; + // return the conditional Gaussian + return conditional; +} + +// from GaussianFactorGraph.cpp, see _eliminateOne above +GaussianBayesNet _eliminate(FactorGraph& graph, CachedFactors& cached, const Ordering& ordering) { + GaussianBayesNet chordalBayesNet; // empty + BOOST_FOREACH(const Symbol& key, ordering) { + GaussianConditional::shared_ptr cg = _eliminateOne(graph, cached, key); + chordalBayesNet.push_back(cg); } + return chordalBayesNet; +} - // from GaussianFactorGraph.cpp, see _eliminateOne above - GaussianBayesNet _eliminate(FactorGraph& graph, CachedFactors& cached, const Ordering& ordering) { - GaussianBayesNet chordalBayesNet; // empty - BOOST_FOREACH(const Symbol& key, ordering) { - GaussianConditional::shared_ptr cg = _eliminateOne(graph, cached, key); - chordalBayesNet.push_back(cg); - } - return chordalBayesNet; +// special const version used in constructor below +GaussianBayesNet _eliminate_const(const FactorGraph& graph, CachedFactors& cached, const Ordering& ordering) { + // make a copy that can be modified locally + FactorGraph graph_ignored = graph; + return _eliminate(graph_ignored, cached, ordering); +} + +/** Create an empty Bayes Tree */ +template +ISAM2::ISAM2() : BayesTree() {} + +/** Create a Bayes Tree from a nonlinear factor graph */ +template +ISAM2::ISAM2(const NonlinearFactorGraph& nlfg, const Ordering& ordering, const Config& config) +: BayesTree(nlfg.linearize(config)->eliminate(ordering)), + theta_(config), delta_(VectorConfig()), nonlinearFactors_(nlfg) { + // todo: repeats calculation above, just to set "cached" + // De-referencing shared pointer can be quite expensive because creates temporary + _eliminate_const(*nlfg.linearize(config), cached_, ordering); +} + +/* ************************************************************************* */ +template +list ISAM2::getAffectedFactors(const list& keys) const { + FactorGraph > allAffected; + list indices; + BOOST_FOREACH(const Symbol& key, keys) { + const list l = nonlinearFactors_.factors(key); + indices.insert(indices.begin(), l.begin(), l.end()); } + indices.sort(); + indices.unique(); + return indices; +} - // special const version used in constructor below - GaussianBayesNet _eliminate_const(const FactorGraph& graph, CachedFactors& cached, const Ordering& ordering) { - // make a copy that can be modified locally - FactorGraph graph_ignored = graph; - return _eliminate(graph_ignored, cached, ordering); - } +/* ************************************************************************* */ +// retrieve all factors that ONLY contain the affected variables +// (note that the remaining stuff is summarized in the cached factors) +template +boost::shared_ptr ISAM2::relinearizeAffectedFactors +(const set& affectedKeys) const { - /** Create an empty Bayes Tree */ - template - ISAM2::ISAM2() : BayesTree() {} + list affectedKeysList; // todo: shouldn't have to convert back to list... + affectedKeysList.insert(affectedKeysList.begin(), affectedKeys.begin(), affectedKeys.end()); + list candidates = getAffectedFactors(affectedKeysList); - /** Create a Bayes Tree from a nonlinear factor graph */ - template - ISAM2::ISAM2(const NonlinearFactorGraph& nlfg, const Ordering& ordering, const Config& config) - : BayesTree(nlfg.linearize(config)->eliminate(ordering)), - theta_(config), delta_(VectorConfig()), nonlinearFactors_(nlfg) { - // todo: repeats calculation above, just to set "cached" - // De-referencing shared pointer can be quite expensive because creates temporary - _eliminate_const(*nlfg.linearize(config), cached_, ordering); - } + NonlinearFactorGraph nonlinearAffectedFactors; - /* ************************************************************************* */ - template - list ISAM2::getAffectedFactors(const list& keys) const { - FactorGraph > allAffected; - list indices; - BOOST_FOREACH(const Symbol& key, keys) { - const list l = nonlinearFactors_.factors(key); - indices.insert(indices.begin(), l.begin(), l.end()); - } - indices.sort(); - indices.unique(); - return indices; - } - - /* ************************************************************************* */ - // retrieve all factors that ONLY contain the affected variables - // (note that the remaining stuff is summarized in the cached factors) - template - boost::shared_ptr ISAM2::relinearizeAffectedFactors - (const set& affectedKeys) const { - - list affectedKeysList; // todo: shouldn't have to convert back to list... - affectedKeysList.insert(affectedKeysList.begin(), affectedKeys.begin(), affectedKeys.end()); - list candidates = getAffectedFactors(affectedKeysList); - - NonlinearFactorGraph nonlinearAffectedFactors; - - BOOST_FOREACH(size_t idx, candidates) { - bool inside = true; - BOOST_FOREACH(const Symbol& key, nonlinearFactors_[idx]->keys()) { - if (affectedKeys.find(key) == affectedKeys.end()) { - inside = false; - break; - } + BOOST_FOREACH(size_t idx, candidates) { + bool inside = true; + BOOST_FOREACH(const Symbol& key, nonlinearFactors_[idx]->keys()) { + if (affectedKeys.find(key) == affectedKeys.end()) { + inside = false; + break; } - if (inside) - nonlinearAffectedFactors.push_back(nonlinearFactors_[idx]); } - - // TODO: temporary might be expensive, return shared pointer ? - return nonlinearAffectedFactors.linearize(theta_); + if (inside) + nonlinearAffectedFactors.push_back(nonlinearFactors_[idx]); } - /* ************************************************************************* */ - // find intermediate (linearized) factors from cache that are passed into the affected area - template - FactorGraph ISAM2::getCachedBoundaryFactors(Cliques& orphans) { - FactorGraph cachedBoundary; + // TODO: temporary might be expensive, return shared pointer ? + return nonlinearAffectedFactors.linearize(theta_); +} - BOOST_FOREACH(sharedClique orphan, orphans) { - // find the last variable that was eliminated - const Symbol& key = orphan->ordering().back(); - // retrieve the cached factor and add to boundary - cachedBoundary.push_back(cached_[key]); - } +/* ************************************************************************* */ +// find intermediate (linearized) factors from cache that are passed into the affected area +template +FactorGraph ISAM2::getCachedBoundaryFactors(Cliques& orphans) { + FactorGraph cachedBoundary; - return cachedBoundary; + BOOST_FOREACH(sharedClique orphan, orphans) { + // find the last variable that was eliminated + const Symbol& key = orphan->ordering().back(); + // retrieve the cached factor and add to boundary + cachedBoundary.push_back(cached_[key]); } - /* ************************************************************************* */ - template - void ISAM2::linear_update(const FactorGraph& newFactors) { + return cachedBoundary; +} - // Input: BayesTree(this), newFactors +/* ************************************************************************* */ +template +void ISAM2::recalculate(const list& markedKeys, const FactorGraph* newFactors) { -//#define PRINT_STATS // figures for paper, disable for timing + // 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++; + 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 - // 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. - const list markedKeys = newFactors.keys(); + // 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("re-removetop"); + Cliques orphans; + BayesNet affectedBayesNet; + this->removeTop(markedKeys, affectedBayesNet, orphans); + toc("re-removetop"); + + // 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 + + tic("re-lookup"); + // ordering provides all keys in conditionals, there cannot be others because path to root included + set affectedKeys; + list tmp = affectedBayesNet.ordering(); + affectedKeys.insert(tmp.begin(), tmp.end()); + + FactorGraph factors(*relinearizeAffectedFactors(affectedKeys)); // todo: no need to relinearize here, should have cached linearized factors + + 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 + + toc("re-lookup"); + + tic("re-cached"); + // add the cached intermediate results from the boundary of the orphans ... + FactorGraph cachedBoundary = getCachedBoundaryFactors(orphans); + factors.push_back(cachedBoundary); + toc("re-cached"); + + // END OF COPIED CODE + + + // 2. Add the new factors \Factors' into the resulting factor graph + tic("re-newfactors"); + if (newFactors) { + factors.push_back(*newFactors); + } + toc("re-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]) + + // create an ordering for the new and contaminated factors + // markedKeys are passed in: those variables will be forced to the end in the ordering + set markedKeysSet; + markedKeysSet.insert(markedKeys.begin(), markedKeys.end()); + Ordering ordering = factors.getConstrainedOrdering(markedKeysSet); // intelligent ordering + // Ordering ordering = factors.getOrdering(); // original ordering, yields in bad performance + + // eliminate into a Bayes net + tic("eliminate"); + BayesNet bayesNet = _eliminate(factors, cached_, ordering); + toc("eliminate"); + + tic("re-assemble"); + // Create Index from ordering + IndexTable index(ordering); + + // insert conditionals back in, straight into the topless bayesTree + typename BayesNet::const_reverse_iterator rit; + for ( rit=bayesNet.rbegin(); rit != bayesNet.rend(); ++rit ) + this->insert(*rit, index); + + // Save number of affectedCliques + lastAffectedCliqueCount = this->size(); + tic("re-assemble"); + + // 4. Insert the orphans back into the new Bayes tree. + + tic("re-orphans"); + // add orphans to the bottom of the new tree + BOOST_FOREACH(sharedClique orphan, orphans) { + Symbol parentRepresentative = findParentClique(orphan->separator_, index); + sharedClique parent = (*this)[parentRepresentative]; + parent->children_ += orphan; + orphan->parent_ = parent; // set new parent! + } + toc("re-orphans"); + + // Output: BayesTree(this) +} + +/* ************************************************************************* */ +template +void ISAM2::linear_update(const FactorGraph& 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, list& keys, const list& marked) { + // does the separator contain any of the variables? + bool found = false; + BOOST_FOREACH(const Symbol& key, clique->separator_) { + if (find(marked.begin(), marked.end(), key) != marked.end()) + found = true; + } + if (found) { + // then add this clique + keys.push_back(clique->keys().front()); + } + BOOST_FOREACH(const sharedClique& child, clique->children_) { + find_all(child, keys, marked); + } +} + +/* ************************************************************************* */ +template +list ISAM2::fluid_relinearization(double relinearize_threshold) { + + // Input: nonlinear factors factors_, linearization point theta_, Bayes tree (this), delta_ + + // 1. Mark variables in \Delta above threshold \beta: J=\{\Delta_{j}\in\Delta|\Delta_{j}\geq\beta\}. + list marked; + VectorConfig deltaMarked; + for (VectorConfig::const_iterator it = delta_.begin(); it!=delta_.end(); it++) { + Symbol key = it->first; + Vector v = it->second; + if (max(abs(v)) >= relinearize_threshold) { + marked.push_back(key); + deltaMarked.insert(key, v); + } + } + + if (marked.size()>0) { + + // 2. Update linearization point for marked variables: \Theta_{J}:=\Theta_{J}+\Delta_{J}. + theta_ = theta_.expmap(deltaMarked); + + // 3. Mark all cliques that involve marked variables \Theta_{J} and all their ancestors. + + // mark all cliques that involve marked variables + list affectedSymbols(marked); // add all marked + tic("nonlin-find_all"); + find_all(this->root(), affectedSymbols, marked); // add other cliques that have the marked ones in the separator + affectedSymbols.sort(); // remove duplicates + affectedSymbols.unique(); + toc("nonlin-find_all"); + + // 4. From the leaves to the top, if a clique is marked: + // re-linearize the original factors in \Factors associated with the clique, + // add the cached marginal factors from its children, and re-eliminate. + + // todo: for simplicity, currently simply remove the top and recreate it using the original ordering + //recalculate(affectedSymbols); + return affectedSymbols; + +#if 0 + + tic("nonlin-mess"); Cliques orphans; BayesNet affectedBayesNet; - this->removeTop(markedKeys, affectedBayesNet, orphans); + this->removeTop(affectedSymbols, affectedBayesNet, orphans); + // remember original ordering + // todo Ordering original_ordering = affectedBayesNet.ordering(); // does not yield original ordering... + FactorGraph tmp_factors(affectedBayesNet); // so instead we recalculate an acceptable ordering here + Ordering original_ordering = tmp_factors.getOrdering(); // todo - remove multiple lines up to here - // 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 + boost::shared_ptr factors; - // BEGIN OF COPIED CODE - - tic("linear_lookup1"); // ordering provides all keys in conditionals, there cannot be others because path to root included set affectedKeys; list tmp = affectedBayesNet.ordering(); affectedKeys.insert(tmp.begin(), tmp.end()); + toc("nonlin-mess"); - FactorGraph factors(*relinearizeAffectedFactors(affectedKeys)); // todo: no need to relinearize here, should have cached linearized factors + tic("nonlin_relin"); + factors = relinearizeAffectedFactors(affectedKeys); + toc("nonlin_relin"); - lastAffectedMarkedCount = markedKeys.size(); - lastAffectedVariableCount = affectedKeys.size(); - lastAffectedFactorCount = factors.size(); + lastNonlinearMarkedCount = marked.size(); + lastNonlinearAffectedVariableCount = affectedKeys.size(); + lastNonlinearAffectedFactorCount = 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 - - toc("linear_lookup1"); + // cout << "nonlinear: #marked: " << marked.size() << " #affected: " << affectedKeys.size() << " #factors: " << factors->size() << endl; // add the cached intermediate results from the boundary of the orphans ... FactorGraph cachedBoundary = getCachedBoundaryFactors(orphans); - factors.push_back(cachedBoundary); - - // END OF COPIED CODE - - - // 2. Add the new factors \Factors' into the resulting factor graph - factors.push_back(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]) - - // create an ordering for the new and contaminated factors - // markedKeys are passed in: those variables will be forced to the end in the ordering - set markedKeysSet; - markedKeysSet.insert(markedKeys.begin(), markedKeys.end()); - Ordering ordering = factors.getConstrainedOrdering(markedKeysSet); // intelligent ordering -// Ordering ordering = factors.getOrdering(); // original ordering, yields in bad performance + factors->push_back(cachedBoundary); // eliminate into a Bayes net - tic("linear_eliminate"); - BayesNet bayesNet = _eliminate(factors, cached_, ordering); - toc("linear_eliminate"); + tic("nonlin_eliminate"); + BayesNet bayesNet = _eliminate(*factors, cached_, original_ordering); + toc("nonlin_eliminate"); // Create Index from ordering - IndexTable index(ordering); + IndexTable index(original_ordering); // insert conditionals back in, straight into the topless bayesTree typename BayesNet::const_reverse_iterator rit; for ( rit=bayesNet.rbegin(); rit != bayesNet.rend(); ++rit ) this->insert(*rit, index); - // Save number of affectedCliques - lastAffectedCliqueCount = this->size(); - - // 4. Insert the orphans back into the new Bayes tree. - // add orphans to the bottom of the new tree BOOST_FOREACH(sharedClique orphan, orphans) { Symbol parentRepresentative = findParentClique(orphan->separator_, index); @@ -317,179 +452,102 @@ namespace gtsam { parent->children_ += orphan; orphan->parent_ = parent; // set new parent! } +#endif - // Output: BayesTree(this) + // Output: updated Bayes tree (this), updated linearization point theta_ } - /* ************************************************************************* */ - // find all variables that are directly connected by a measurement to one of the marked variables - template - void ISAM2::find_all(sharedClique clique, list& keys, const list& marked) { - // does the separator contain any of the variables? - bool found = false; - BOOST_FOREACH(const Symbol& key, clique->separator_) { - if (find(marked.begin(), marked.end(), key) != marked.end()) - found = true; - } - if (found) { - // then add this clique - keys.push_back(clique->keys().front()); - } - BOOST_FOREACH(const sharedClique& child, clique->children_) { - find_all(child, keys, marked); - } + list empty; + return empty; + +} + +/* ************************************************************************* */ +template +void ISAM2::update( + const NonlinearFactorGraph& newFactors, const Config& newTheta, + double wildfire_threshold, double relinearize_threshold, bool relinearize) { + + lastAffectedVariableCount = 0; + lastAffectedFactorCount = 0; + lastAffectedCliqueCount = 0; + lastAffectedMarkedCount = 0; + lastNonlinearMarkedCount = 0; + lastNonlinearAffectedVariableCount = 0; + lastNonlinearAffectedFactorCount = 0; + + tic("all"); + + tic("step1"); + // 1. Add any new factors \Factors:=\Factors\cup\Factors'. + nonlinearFactors_.push_back(newFactors); + toc("step1"); + + tic("step2"); + // 2. Initialize any new variables \Theta_{new} and add \Theta:=\Theta\cup\Theta_{new}. + theta_.insert(newTheta); + toc("step2"); + + tic("step3"); + // 3. Linearize new factor + // boost::shared_ptr linearFactors = newFactors.linearize(theta_); + toc("step3"); + +#if 0 // original algorithm + tic("step4"); + // 4. Linear iSAM step (alg 3) + linear_update(*linearFactors); // in: this + toc("step4"); + + tic("step5"); + // 5. Calculate Delta (alg 0) + delta_ = optimize2(*this, wildfire_threshold); + toc("step5"); + + tic("step6"); + // 6. Iterate Algorithm 4 until no more re-linearizations occur + if (relinearize) { + fluid_relinearization(relinearize_threshold); // in: delta_, theta_, nonlinearFactors_, this } + toc("step6"); - /* ************************************************************************* */ - template - void ISAM2::fluid_relinearization(double relinearize_threshold) { + // todo: not part of algorithm in paper: linearization point and delta_ do not fit... have to update delta again + delta_ = optimize2(*this, wildfire_threshold); +#else // new algorithm - // Input: nonlinear factors factors_, linearization point theta_, Bayes tree (this), delta_ + tic("step4B"); + // 4B. Mark linear update + list markedKeys = newFactors.keys(); + toc("step4B"); - // 1. Mark variables in \Delta above threshold \beta: J=\{\Delta_{j}\in\Delta|\Delta_{j}\geq\beta\}. - list marked; - VectorConfig deltaMarked; - for (VectorConfig::const_iterator it = delta_.begin(); it!=delta_.end(); it++) { - Symbol key = it->first; - Vector v = it->second; - if (max(abs(v)) >= relinearize_threshold) { - marked.push_back(key); - deltaMarked.insert(key, v); - } - } + tic("step5B"); + // 5B. Mark nonlinear update + if (relinearize) { + list markedRelin = fluid_relinearization(relinearize_threshold); // in: delta_, theta_, nonlinearFactors_, this - if (marked.size()>0) { - - // 2. Update linearization point for marked variables: \Theta_{J}:=\Theta_{J}+\Delta_{J}. - theta_ = theta_.expmap(deltaMarked); - - // 3. Mark all cliques that involve marked variables \Theta_{J} and all their ancestors. - - // mark all cliques that involve marked variables - list affectedSymbols(marked); // add all marked - tic("nonlin-find_all"); - find_all(this->root(), affectedSymbols, marked); // add other cliques that have the marked ones in the separator - affectedSymbols.sort(); // remove duplicates - affectedSymbols.unique(); - toc("nonlin-find_all"); - - // 4. From the leaves to the top, if a clique is marked: - // re-linearize the original factors in \Factors associated with the clique, - // add the cached marginal factors from its children, and re-eliminate. - - // todo: for simplicity, currently simply remove the top and recreate it using the original ordering - - tic("nonlin-mess"); - Cliques orphans; - BayesNet affectedBayesNet; - this->removeTop(affectedSymbols, affectedBayesNet, orphans); - // remember original ordering -// todo Ordering original_ordering = affectedBayesNet.ordering(); // does not yield original ordering... - FactorGraph tmp_factors(affectedBayesNet); // so instead we recalculate an acceptable ordering here - Ordering original_ordering = tmp_factors.getOrdering(); // todo - remove multiple lines up to here - - boost::shared_ptr factors; - - // ordering provides all keys in conditionals, there cannot be others because path to root included - set affectedKeys; - list tmp = affectedBayesNet.ordering(); - affectedKeys.insert(tmp.begin(), tmp.end()); - toc("nonlin-mess"); - - tic("nonlin_relin"); - factors = relinearizeAffectedFactors(affectedKeys); - toc("nonlin_relin"); - - lastNonlinearMarkedCount = marked.size(); - lastNonlinearAffectedVariableCount = affectedKeys.size(); - lastNonlinearAffectedFactorCount = factors->size(); - -// cout << "nonlinear: #marked: " << marked.size() << " #affected: " << affectedKeys.size() << " #factors: " << factors->size() << endl; - - // add the cached intermediate results from the boundary of the orphans ... - FactorGraph cachedBoundary = getCachedBoundaryFactors(orphans); - factors->push_back(cachedBoundary); - - // eliminate into a Bayes net - tic("nonlin_eliminate"); - BayesNet bayesNet = _eliminate(*factors, cached_, original_ordering); - toc("nonlin_eliminate"); - - // Create Index from ordering - IndexTable index(original_ordering); - - // insert conditionals back in, straight into the topless bayesTree - typename BayesNet::const_reverse_iterator rit; - for ( rit=bayesNet.rbegin(); rit != bayesNet.rend(); ++rit ) - this->insert(*rit, index); - - // add orphans to the bottom of the new tree - BOOST_FOREACH(sharedClique orphan, orphans) { - Symbol parentRepresentative = findParentClique(orphan->separator_, index); - sharedClique parent = (*this)[parentRepresentative]; - parent->children_ += orphan; - orphan->parent_ = parent; // set new parent! - } - - // Output: updated Bayes tree (this), updated linearization point theta_ - } + // merge with markedKeys + markedKeys.splice(markedKeys.begin(), markedRelin, markedRelin.begin(), markedRelin.end()); + markedKeys.sort(); // remove duplicates + markedKeys.unique(); } + toc("step5B"); - /* ************************************************************************* */ - template - void ISAM2::update( - const NonlinearFactorGraph& newFactors, const Config& newTheta, - double wildfire_threshold, double relinearize_threshold, bool relinearize) { + tic("step6B"); + // 6B. Redo top of Bayes tree + boost::shared_ptr linearFactors = newFactors.linearize(theta_); + recalculate(markedKeys, &(*linearFactors)); + toc("step6B"); - lastAffectedVariableCount = 0; - lastAffectedFactorCount = 0; - lastAffectedCliqueCount = 0; - lastAffectedMarkedCount = 0; - lastNonlinearMarkedCount = 0; - lastNonlinearAffectedVariableCount = 0; - lastNonlinearAffectedFactorCount = 0; + tic("step7B"); + // 7B. Solve + delta_ = optimize2(*this, wildfire_threshold); + toc("step7B"); - tic("all"); +#endif + toc("all"); - tic("step1"); - // 1. Add any new factors \Factors:=\Factors\cup\Factors'. - nonlinearFactors_.push_back(newFactors); - toc("step1"); - - tic("step2"); - // 2. Initialize any new variables \Theta_{new} and add \Theta:=\Theta\cup\Theta_{new}. - theta_.insert(newTheta); - toc("step2"); - - tic("step3"); - // 3. Linearize new factor - boost::shared_ptr linearFactors = newFactors.linearize(theta_); - toc("step3"); - - tic("step4"); - // 4. Linear iSAM step (alg 3) - linear_update(*linearFactors); // in: this - toc("step4"); - - tic("step5"); - // 5. Calculate Delta (alg 0) - delta_ = optimize2(*this, wildfire_threshold); - toc("step5"); - - tic("step6"); - // 6. Iterate Algorithm 4 until no more re-linearizations occur - if (relinearize) { - fluid_relinearization(relinearize_threshold); // in: delta_, theta_, nonlinearFactors_, this - } - toc("step6"); - - // todo: not part of algorithm in paper: linearization point and delta_ do not fit... have to update delta again - delta_ = optimize2(*this, wildfire_threshold); - - toc("all"); - - tictoc_print(); - } + tictoc_print(); +} /* ************************************************************************* */ diff --git a/inference/ISAM2.h b/inference/ISAM2.h index d842d4d0c..cc0d482e0 100644 --- a/inference/ISAM2.h +++ b/inference/ISAM2.h @@ -25,75 +25,76 @@ namespace gtsam { - typedef SymbolMap CachedFactors; +typedef SymbolMap CachedFactors; - template - class ISAM2: public BayesTree { +template +class ISAM2: public BayesTree { - protected: +protected: - // current linearization point - Config theta_; + // current linearization point + Config theta_; - // the linear solution, an update to the estimate in theta - VectorConfig delta_; + // the linear solution, an update to the estimate in theta + VectorConfig delta_; - // for keeping all original nonlinear factors - NonlinearFactorGraph nonlinearFactors_; + // for keeping all original nonlinear factors + NonlinearFactorGraph nonlinearFactors_; - // cached intermediate results for restarting computation in the middle - CachedFactors cached_; + // cached intermediate results for restarting computation in the middle + CachedFactors cached_; - public: +public: - /** Create an empty Bayes Tree */ - ISAM2(); + /** Create an empty Bayes Tree */ + ISAM2(); - /** Create a Bayes Tree from a Bayes Net */ - ISAM2(const NonlinearFactorGraph& fg, const Ordering& ordering, const Config& config); + /** Create a Bayes Tree from a Bayes Net */ + ISAM2(const NonlinearFactorGraph& fg, const Ordering& ordering, const Config& config); - /** Destructor */ - virtual ~ISAM2() {} + /** Destructor */ + virtual ~ISAM2() {} - typedef typename BayesTree::sharedClique sharedClique; + typedef typename BayesTree::sharedClique sharedClique; - typedef typename BayesTree::Cliques Cliques; + typedef typename BayesTree::Cliques Cliques; - /** - * ISAM2. - */ - void update(const NonlinearFactorGraph& newFactors, const Config& newTheta, - double wildfire_threshold = 0., double relinearize_threshold = 0., bool relinearize = true); + /** + * ISAM2. + */ + void update(const NonlinearFactorGraph& newFactors, const Config& newTheta, + double wildfire_threshold = 0., double relinearize_threshold = 0., bool relinearize = true); - // needed to create initial estimates - const Config getLinearizationPoint() const {return theta_;} + // needed to create initial estimates + const Config getLinearizationPoint() const {return theta_;} - // estimate based on incomplete delta (threshold!) - const Config calculateEstimate() const {return theta_.expmap(delta_);} + // estimate based on incomplete delta (threshold!) + const Config calculateEstimate() const {return theta_.expmap(delta_);} - // estimate based on full delta (note that this is based on the current linearization point) - const Config calculateBestEstimate() const {return theta_.expmap(optimize2(*this, 0.));} + // estimate based on full delta (note that this is based on the current linearization point) + const Config calculateBestEstimate() const {return theta_.expmap(optimize2(*this, 0.));} - const NonlinearFactorGraph& getFactorsUnsafe() const { return nonlinearFactors_; } + const NonlinearFactorGraph& getFactorsUnsafe() const { return nonlinearFactors_; } - size_t lastAffectedVariableCount; - size_t lastAffectedFactorCount; - size_t lastAffectedCliqueCount; - size_t lastAffectedMarkedCount; - size_t lastNonlinearMarkedCount; - size_t lastNonlinearAffectedVariableCount; - size_t lastNonlinearAffectedFactorCount; + size_t lastAffectedVariableCount; + size_t lastAffectedFactorCount; + size_t lastAffectedCliqueCount; + size_t lastAffectedMarkedCount; + size_t lastNonlinearMarkedCount; + size_t lastNonlinearAffectedVariableCount; + size_t lastNonlinearAffectedFactorCount; - private: +private: - std::list getAffectedFactors(const std::list& keys) const; - boost::shared_ptr relinearizeAffectedFactors(const std::set& affectedKeys) const; - FactorGraph getCachedBoundaryFactors(Cliques& orphans); + std::list getAffectedFactors(const std::list& keys) const; + boost::shared_ptr relinearizeAffectedFactors(const std::set& affectedKeys) const; + FactorGraph getCachedBoundaryFactors(Cliques& orphans); - void linear_update(const FactorGraph& newFactors); - void find_all(sharedClique clique, std::list& keys, const std::list& marked); // helper function - void fluid_relinearization(double relinearize_threshold); + void recalculate(const std::list& markedKeys, const FactorGraph* newFactors = NULL); + void linear_update(const FactorGraph& newFactors); + void find_all(sharedClique clique, std::list& keys, const std::list& marked); // helper function + std::list fluid_relinearization(double relinearize_threshold); - }; // ISAM2 +}; // ISAM2 } /// namespace gtsam diff --git a/slam/GaussianISAM2.cpp b/slam/GaussianISAM2.cpp index fdb65b671..83556f840 100644 --- a/slam/GaussianISAM2.cpp +++ b/slam/GaussianISAM2.cpp @@ -63,7 +63,7 @@ void optimize2(const GaussianISAM2::sharedClique& clique, double threshold, set< } /* ************************************************************************* */ -// fast version without threshold +// fast full version without threshold void optimize2(const GaussianISAM2::sharedClique& clique, VectorConfig& result) { // parents are assumed to already be solved and available in result GaussianISAM2::Clique::const_reverse_iterator it;