diff --git a/inference/ISAM2-inl.h b/inference/ISAM2-inl.h index 4d02246fb..c122c896c 100644 --- a/inference/ISAM2-inl.h +++ b/inference/ISAM2-inl.h @@ -133,152 +133,6 @@ namespace gtsam { } /* ************************************************************************* */ - // todo: will be obsolete soon - template - void ISAM2::update_internal(const NonlinearFactorGraph& newFactors, - const Config& newTheta, Cliques& orphans, double wildfire_threshold, double relinearize_threshold, bool relinearize) { - - // marked_ = nonlinearFactors_.keys(); // debug only //////////// - - // only relinearize if requested in previous step AND necessary (ie. at least one variable changes) - relinearize = true; // todo - switched off - bool relinFromLast = true; //marked_.size() > 0; - - //// 1 - relinearize selected variables - - if (relinFromLast) { - theta_ = theta_.expmap(deltaMarked_); - } - - //// 2 - Add new factors (for later relinearization) - - nonlinearFactors_.push_back(newFactors); - - //// 3 - Initialize new variables - - theta_.insert(newTheta); - thetaFuture_.insert(newTheta); - - //// 4 - Mark affected variables as invalid - - // todo - not in lyx yet: relin requires more than just removing the cliques corresponding to the variables!!! - // It's about factors!!! - - if (relinFromLast) { - // mark variables that have to be removed as invalid (removeFATtop) - // basically calculate all the keys contained in the factors that contain any of the keys... - // the goal is to relinearize all variables directly affected by new factors - list allAffected = getAffectedFactors(marked_); - set accumulate; - BOOST_FOREACH(int idx, allAffected) { - list tmp = nonlinearFactors_[idx]->keys(); - accumulate.insert(tmp.begin(), tmp.end()); - } - marked_.clear(); - marked_.insert(marked_.begin(), accumulate.begin(), accumulate.end()); - } // else: marked_ is empty anyways - - // also mark variables that are affected by new factors as invalid - const list newKeys = newFactors.keys(); - marked_.insert(marked_.begin(), newKeys.begin(), newKeys.end()); - // eliminate duplicates - marked_.sort(); - marked_.unique(); - - //// 5 - removeTop invalidate all cliques involving marked variables - - // remove affected factors - BayesNet affectedBayesNet; - this->removeTop(marked_, affectedBayesNet, orphans); - - //// 6 - find factors connected to affected variables - //// 7 - linearize - - boost::shared_ptr factors; - - if (relinFromLast) { - // 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()); - - // todo - remerge in keys of new factors - affectedKeys.insert(newKeys.begin(), newKeys.end()); - - // Save number of affected variables - lastAffectedVariableCount = affectedKeys.size(); - - factors = relinearizeAffectedFactors(affectedKeys); - - // Save number of affected factors - lastAffectedFactorCount = factors->size(); - - // add the cached intermediate results from the boundary of the orphans ... - FactorGraph cachedBoundary = getCachedBoundaryFactors(orphans); - factors->push_back(cachedBoundary); - } else { - // reuse the old factors - FactorGraph tmp(affectedBayesNet); - factors.reset(new GaussianFactorGraph); - factors->push_back(tmp); - factors->push_back(*newFactors.linearize(theta_)); // avoid temporary ? - } - - //// 8 - eliminate and add orphans back in - - // create an ordering for the new and contaminated factors - // newKeys are passed in: those variables will be forced to the end in the ordering - set newKeysSet; - newKeysSet.insert(newKeys.begin(), newKeys.end()); - Ordering ordering = factors->getConstrainedOrdering(newKeysSet); - - // eliminate into a Bayes net - BayesNet bayesNet = _eliminate(*factors, cached_, ordering); - - // 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(); - - // 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! - } - - //// 9 - update solution - - delta_ = optimize2(*this, wildfire_threshold); - - //// 10 - mark variables, if significant change - - marked_.clear(); - deltaMarked_ = VectorConfig(); // clear - if (relinearize) { // decides about next step!!! - - 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); - } - } - - // not part of the formal algorithm, but needed to allow initialization of new variables outside by the user - thetaFuture_ = thetaFuture_.expmap(deltaMarked_); - } - - } - template void ISAM2::linear_update(const FactorGraph& newFactors) { @@ -291,7 +145,33 @@ namespace gtsam { Cliques orphans; BayesNet affectedBayesNet; this->removeTop(newKeys, affectedBayesNet, orphans); - FactorGraph factors(affectedBayesNet); + + // 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 + 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 + + cout << "linear: #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); @@ -302,7 +182,8 @@ namespace gtsam { // newKeys are passed in: those variables will be forced to the end in the ordering set newKeysSet; newKeysSet.insert(newKeys.begin(), newKeys.end()); - Ordering ordering = factors.getConstrainedOrdering(newKeysSet); + // Ordering ordering = factors.getConstrainedOrdering(newKeysSet); + Ordering ordering = factors.getOrdering(); // todo: back to constrained... // eliminate into a Bayes net BayesNet bayesNet = _eliminate(factors, cached_, ordering); @@ -331,11 +212,13 @@ namespace gtsam { // Output: BayesTree(this) } + /* ************************************************************************* */ + // 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_) { + BOOST_FOREACH(const Symbol& key, clique->keys()) { // todo clique->separator_) { if (find(marked.begin(), marked.end(), key) != marked.end()) found = true; } @@ -348,6 +231,7 @@ namespace gtsam { } } + /* ************************************************************************* */ template void ISAM2::fluid_relinearization(double relinearize_threshold) { @@ -365,74 +249,78 @@ namespace gtsam { } } - // 2. Update linearization point for marked variables: \Theta_{J}:=\Theta_{J}+\Delta_{J}. - theta_ = theta_.expmap(deltaMarked); + if (marked.size()>0) { - // 3. Mark all cliques that involve marked variables \Theta_{J} and all their ancestors. + // 2. Update linearization point for marked variables: \Theta_{J}:=\Theta_{J}+\Delta_{J}. + theta_ = theta_.expmap(deltaMarked); - // mark all cliques that involve marked variables - list affectedSymbols(marked); // add all marked - find_all(this->root(), affectedSymbols, marked); // add other cliques that have the marked ones in the separator + // 3. Mark all cliques that involve marked variables \Theta_{J} and all their ancestors. - // 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. + // mark all cliques that involve marked variables + list affectedSymbols(marked); // add all marked + find_all(this->root(), affectedSymbols, marked); // add other cliques that have the marked ones in the separator + affectedSymbols.sort(); // remove duplicates + affectedSymbols.unique(); - // todo: for simplicity, currently simply remove the top and recreate it using the original ordering + // 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. - Cliques orphans; - BayesNet affectedBayesNet; - this->removeTop(affectedSymbols, affectedBayesNet, orphans); - // remember original ordering - Ordering original_ordering = affectedBayesNet.ordering(); + // todo: for simplicity, currently simply remove the top and recreate it using the original ordering - boost::shared_ptr factors; + Cliques orphans; + BayesNet affectedBayesNet; + this->removeTop(affectedSymbols, affectedBayesNet, orphans); + // remember original ordering + Ordering original_ordering = affectedBayesNet.ordering(); - // 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()); - factors = relinearizeAffectedFactors(affectedKeys); + boost::shared_ptr factors; - // add the cached intermediate results from the boundary of the orphans ... - FactorGraph cachedBoundary = getCachedBoundaryFactors(orphans); - factors->push_back(cachedBoundary); + // 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()); - // eliminate into a Bayes net - BayesNet bayesNet = _eliminate(*factors, cached_, original_ordering); + factors = relinearizeAffectedFactors(affectedKeys); - // Create Index from ordering - IndexTable index(original_ordering); + cout << "nonlinear: #marked: " << marked.size() << " #affected: " << affectedKeys.size() << " #factors: " << factors->size() << endl; - // 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 the cached intermediate results from the boundary of the orphans ... + FactorGraph cachedBoundary = getCachedBoundaryFactors(orphans); + factors->push_back(cachedBoundary); - // 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! + // todo - temporary solution + Ordering ordering = factors->getOrdering(); + + // eliminate into a Bayes net + BayesNet bayesNet = _eliminate(*factors, cached_, ordering); // todo original_ordering); + + // Create Index from ordering + IndexTable index(ordering); // todo 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_ } - - // Output: updated Bayes tree (this), updated linearization point theta_ - } + /* ************************************************************************* */ template void ISAM2::update( const NonlinearFactorGraph& newFactors, const Config& newTheta, double wildfire_threshold, double relinearize_threshold, bool relinearize) { -#if 1 - // old algorithm: - Cliques orphans; - this->update_internal(newFactors, newTheta, orphans, wildfire_threshold, relinearize_threshold, relinearize); - -#else - // 1. Add any new factors \Factors:=\Factors\cup\Factors'. nonlinearFactors_.push_back(newFactors); @@ -452,14 +340,12 @@ namespace gtsam { if (relinearize) fluid_relinearization(relinearize_threshold); // in: delta_, theta_, nonlinearFactors_, this - - // todo: linearization point and delta_ do not fit... have to update delta again + // todo: not part of algorithm in paper: linearization point and delta_ do not fit... have to update delta again delta_ = optimize2(*this, wildfire_threshold); // todo: for getLinearizationPoint(), see ISAM2.h thetaFuture_ = theta_; -#endif } /* ************************************************************************* */ diff --git a/inference/ISAM2.h b/inference/ISAM2.h index 312a0d13f..a6a6bbbb1 100644 --- a/inference/ISAM2.h +++ b/inference/ISAM2.h @@ -71,9 +71,6 @@ namespace gtsam { 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 update_internal(const NonlinearFactorGraph& newFactors, - const Config& newTheta, Cliques& orphans, - double wildfire_threshold, double relinearize_threshold, bool relinearize); void update(const NonlinearFactorGraph& newFactors, const Config& newTheta, double wildfire_threshold = 0., double relinearize_threshold = 0., bool relinearize = true);