From 1d093e388db66e083784cf062bf48aa0dcd7410d Mon Sep 17 00:00:00 2001 From: Michael Kaess Date: Fri, 22 Jan 2010 06:28:12 +0000 Subject: [PATCH] changed main algorithm to allow recovery of exact solution --- cpp/ISAM2-inl.h | 79 ++++++++++++++++++++++++++++--------------------- cpp/ISAM2.h | 20 +++++++++---- 2 files changed, 60 insertions(+), 39 deletions(-) diff --git a/cpp/ISAM2-inl.h b/cpp/ISAM2-inl.h index f76b88a59..c2c8a8020 100644 --- a/cpp/ISAM2-inl.h +++ b/cpp/ISAM2-inl.h @@ -67,16 +67,16 @@ namespace gtsam { /** 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)), nonlinearFactors_(nlfg), theta_(config) { + : BayesTree(nlfg.linearize(config).eliminate(ordering)), theta_(config), thetaFuture_(config), nonlinearFactors_(nlfg) { // todo: repeats calculation above, just to set "cached" _eliminate_const(nlfg.linearize(config), cached_, ordering); } /* ************************************************************************* */ template - boost::shared_ptr > > + FactorGraph > ISAM2::getAffectedFactors(const list& keys) const { - boost::shared_ptr > > allAffected(new FactorGraph >); + FactorGraph > allAffected; list indices; BOOST_FOREACH(const Symbol& key, keys) { const list l = nonlinearFactors_.factors(key); @@ -85,7 +85,7 @@ namespace gtsam { indices.sort(); indices.unique(); BOOST_FOREACH(int i, indices) { - allAffected->push_back(nonlinearFactors_[i]); + allAffected.push_back(nonlinearFactors_[i]); } return allAffected; } @@ -94,17 +94,19 @@ namespace gtsam { // retrieve all factors that ONLY contain the affected variables // (note that the remaining stuff is summarized in the cached factors) template - FactorGraph ISAM2::relinearizeAffectedFactors(const list& affectedKeys) const { + FactorGraph ISAM2::relinearizeAffectedFactors(const set& affectedKeys) const { - boost::shared_ptr > > candidates = getAffectedFactors(affectedKeys); + list affectedKeysList; // todo: shouldn't have to convert back to list... + affectedKeysList.insert(affectedKeysList.begin(), affectedKeys.begin(), affectedKeys.end()); + FactorGraph > candidates = getAffectedFactors(affectedKeysList); NonlinearFactorGraph nonlinearAffectedFactors; typename FactorGraph >::const_iterator it; - for(it = candidates->begin(); it != candidates->end(); it++) { + for(it = candidates.begin(); it != candidates.end(); it++) { bool inside = true; BOOST_FOREACH(const Symbol& key, (*it)->keys()) { - if (find(affectedKeys.begin(), affectedKeys.end(), key) == affectedKeys.end()) { + if (affectedKeys.find(key) == affectedKeys.end()) { inside = false; break; } @@ -135,26 +137,32 @@ namespace gtsam { /* ************************************************************************* */ template void ISAM2::update_internal(const NonlinearFactorGraph& newFactors, - const Config& theta_new, Cliques& orphans, double wildfire_threshold, double relinearize_threshold) { + const Config& newTheta, Cliques& orphans, double wildfire_threshold, double relinearize_threshold) { + // marked_ = nonlinearFactors_.keys(); // debug only //////////// - // todo - debug only - // marked_ = nonlinearFactors_.keys(); + //// 1 - relinearize selected variables + + theta_ = expmap(theta_, deltaMarked_); + + //// 2 - Add new factors (for later relinearization) - //// 1 - Remember the new factors for later relinearization nonlinearFactors_.push_back(newFactors); - //// 2 - add in new information - // add new variables - theta_.insert(theta_new); + //// 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!!! // 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 - boost::shared_ptr > > allAffected = getAffectedFactors(marked_); - marked_ = allAffected->keys(); + FactorGraph > allAffected = getAffectedFactors(marked_); + marked_ = allAffected.keys(); // merge keys of new factors with mask const list newKeys = newFactors.keys(); @@ -163,23 +171,27 @@ namespace gtsam { marked_.sort(); marked_.unique(); - //// 4 - removeTop invalidate all cliques involving marked variables + //// 5 - removeTop invalidate all cliques involving marked variables // remove affected factors BayesNet affectedBayesNet; this->removeTop(marked_, affectedBayesNet, orphans); - //// 3 - find factors connected to affected variables - //// 4 - linearize + //// 6 - find factors connected to affected variables + //// 7 - linearize // ordering provides all keys in conditionals, there cannot be others because path to root included - list affectedKeys = affectedBayesNet.ordering(); + set affectedKeys; + list tmp = affectedBayesNet.ordering(); + affectedKeys.insert(tmp.begin(), tmp.end()); // todo - remerge in keys of new factors - affectedKeys.insert(affectedKeys.begin(), newKeys.begin(), newKeys.end()); + affectedKeys.insert(newKeys.begin(), newKeys.end()); +#if 0 // no longer needed for set // eliminate duplicates affectedKeys.sort(); affectedKeys.unique(); +#endif FactorGraph factors = relinearizeAffectedFactors(affectedKeys); @@ -187,7 +199,7 @@ namespace gtsam { FactorGraph cachedBoundary = getCachedBoundaryFactors(orphans); factors.push_back(cachedBoundary); - //// 5 - eliminate and add orphans back in + //// 8 - eliminate and add orphans back in // create an ordering for the new and contaminated factors Ordering ordering = factors.getOrdering(); @@ -211,36 +223,35 @@ namespace gtsam { orphan->parent_ = parent; // set new parent! } - //// 6 - update solution + //// 9 - update solution - VectorConfig delta = optimize2(*this, wildfire_threshold); + delta_ = optimize2(*this, wildfire_threshold); - //// 7 - mark variables, if significant change + //// 10 - mark variables, if significant change marked_.clear(); - VectorConfig deltaMarked; - for (VectorConfig::const_iterator it = delta.begin(); it!=delta.end(); it++) { + deltaMarked_ = VectorConfig(); // clear + 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); + deltaMarked_.insert(key, v); } } - //// 8 - relinearize selected variables - - theta_ = expmap(theta_, deltaMarked); + // not part of the formal algorithm, but needed to allow initialization of new variables outside by the user + thetaFuture_ = expmap(thetaFuture_, deltaMarked_); } template void ISAM2::update( - const NonlinearFactorGraph& newFactors, const Config& config, + const NonlinearFactorGraph& newFactors, const Config& newTheta, double wildfire_threshold, double relinearize_threshold) { Cliques orphans; - this->update_internal(newFactors, config, orphans, wildfire_threshold, relinearize_threshold); + this->update_internal(newFactors, newTheta, orphans, wildfire_threshold, relinearize_threshold); } diff --git a/cpp/ISAM2.h b/cpp/ISAM2.h index 41ed12cab..702ccf885 100644 --- a/cpp/ISAM2.h +++ b/cpp/ISAM2.h @@ -34,6 +34,7 @@ namespace gtsam { // current linearization point Config theta_; + Config thetaFuture_; // lin point of next iteration // for keeping all original nonlinear factors NonlinearFactorGraph nonlinearFactors_; @@ -41,6 +42,10 @@ namespace gtsam { // cached intermediate results for restarting computation in the middle CachedFactors cached_; + // the linear solution, an update to the estimate in theta + VectorConfig delta_; + VectorConfig deltaMarked_; + // variables that have been updated, requiring the corresponding factors to be relinearized std::list marked_; @@ -64,19 +69,24 @@ namespace gtsam { * ISAM2. (update_internal provides access to list of orphans for drawing purposes) */ void update_internal(const NonlinearFactorGraph& newFactors, - const Config& config, Cliques& orphans, + const Config& newTheta, Cliques& orphans, double wildfire_threshold, double relinearize_threshold); - void update(const NonlinearFactorGraph& newFactors, const Config& config, + void update(const NonlinearFactorGraph& newFactors, const Config& newTheta, double wildfire_threshold = 0., double relinearize_threshold = 0.); - const Config estimate() const {return theta_;} + // needed to create initial estimates (note that this will be the linearization point in the next step!) + const Config getLinearizationPoint() const {return thetaFuture_;} + // estimate based on incomplete delta (threshold!) + const Config calculateEstimate() const {return expmap(theta_, delta_);} + // estimate based on full delta (note that this is based on the actual current linearization point) + const Config calculateBestEstimate() const {return expmap(theta_, optimize2(*this, 0.));} const std::list& getMarked() const { return marked_; } private: - boost::shared_ptr > > getAffectedFactors(const std::list& keys) const; - FactorGraph relinearizeAffectedFactors(const std::list& affectedKeys) const; + FactorGraph > getAffectedFactors(const std::list& keys) const; + FactorGraph relinearizeAffectedFactors(const std::set& affectedKeys) const; FactorGraph getCachedBoundaryFactors(Cliques& orphans); }; // ISAM2