From 7bc4ee65da14c1f081c9d6e1ec1d39795595851c Mon Sep 17 00:00:00 2001 From: Michael Kaess Date: Sat, 23 Jan 2010 00:21:34 +0000 Subject: [PATCH] allow pure linear steps (disabled) --- cpp/ISAM2-inl.h | 96 ++++++++++++++++++++++++++++--------------------- cpp/ISAM2.h | 4 +-- 2 files changed, 58 insertions(+), 42 deletions(-) diff --git a/cpp/ISAM2-inl.h b/cpp/ISAM2-inl.h index 5ad3d3127..cc18f4131 100644 --- a/cpp/ISAM2-inl.h +++ b/cpp/ISAM2-inl.h @@ -132,13 +132,19 @@ namespace gtsam { /* ************************************************************************* */ template void ISAM2::update_internal(const NonlinearFactorGraph& newFactors, - const Config& newTheta, Cliques& orphans, double wildfire_threshold, double relinearize_threshold) { + 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 - theta_ = expmap(theta_, deltaMarked_); + if (relinFromLast) { + theta_ = expmap(theta_, deltaMarked_); + } //// 2 - Add new factors (for later relinearization) @@ -154,18 +160,21 @@ namespace gtsam { // 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 - 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()); + 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 - // merge keys of new factors with mask + // 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 @@ -181,24 +190,28 @@ namespace gtsam { //// 6 - find factors connected to affected variables //// 7 - linearize - // 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; - // todo - remerge in keys of new factors - affectedKeys.insert(newKeys.begin(), newKeys.end()); -#if 0 // no longer needed for set - // eliminate duplicates - affectedKeys.sort(); - affectedKeys.unique(); -#endif + 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()); - FactorGraph factors = relinearizeAffectedFactors(affectedKeys); + // todo - remerge in keys of new factors + affectedKeys.insert(newKeys.begin(), newKeys.end()); - // add the cached intermediate results from the boundary of the orphans ... - FactorGraph cachedBoundary = getCachedBoundaryFactors(orphans); - factors.push_back(cachedBoundary); + factors = relinearizeAffectedFactors(affectedKeys); + + // 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.push_back(tmp); + factors.push_back(newFactors.linearize(theta_)); + } //// 8 - eliminate and add orphans back in @@ -232,27 +245,30 @@ namespace gtsam { marked_.clear(); 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); - } - } + if (relinearize) { // decides about next step!!! - // not part of the formal algorithm, but needed to allow initialization of new variables outside by the user - thetaFuture_ = expmap(thetaFuture_, 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); + } + } + + // 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& newTheta, - double wildfire_threshold, double relinearize_threshold) { + double wildfire_threshold, double relinearize_threshold, bool relinearize) { Cliques orphans; - this->update_internal(newFactors, newTheta, orphans, wildfire_threshold, relinearize_threshold); + this->update_internal(newFactors, newTheta, orphans, wildfire_threshold, relinearize_threshold, relinearize); } diff --git a/cpp/ISAM2.h b/cpp/ISAM2.h index ba0a94c93..b814279e9 100644 --- a/cpp/ISAM2.h +++ b/cpp/ISAM2.h @@ -70,9 +70,9 @@ namespace gtsam { */ void update_internal(const NonlinearFactorGraph& newFactors, const Config& newTheta, Cliques& orphans, - double wildfire_threshold, double relinearize_threshold); + double wildfire_threshold, double relinearize_threshold, bool relinearize); void update(const NonlinearFactorGraph& newFactors, const Config& newTheta, - double wildfire_threshold = 0., double relinearize_threshold = 0.); + double wildfire_threshold = 0., double relinearize_threshold = 0., bool relinearize = true); // needed to create initial estimates (note that this will be the linearization point in the next step!) const Config getLinearizationPoint() const {return thetaFuture_;}