diff --git a/cpp/GaussianISAM2.cpp b/cpp/GaussianISAM2.cpp index 951777416..6fcd7b3a3 100644 --- a/cpp/GaussianISAM2.cpp +++ b/cpp/GaussianISAM2.cpp @@ -18,47 +18,38 @@ using namespace gtsam; namespace gtsam { /* ************************************************************************* */ -void optimize2(const GaussianISAM2::sharedClique& clique, VectorConfig& result) { +void optimize2(const GaussianISAM2::sharedClique& clique, double threshold, VectorConfig& result) { + bool process_children = false; // parents are assumed to already be solved and available in result GaussianISAM2::Clique::const_reverse_iterator it; for (it = clique->rbegin(); it!=clique->rend(); it++) { GaussianConditional::shared_ptr cg = *it; Vector x = cg->solve(result); // Solve for that variable + if (max(abs(x)) >= threshold) { + process_children = true; + } result.insert(cg->key(), x); // store result in partial solution } - BOOST_FOREACH(GaussianISAM2::sharedClique child, clique->children_) { - optimize2(child, result); + if (process_children) { + BOOST_FOREACH(GaussianISAM2::sharedClique child, clique->children_) { + optimize2(child, threshold, result); + } } } /* ************************************************************************* */ -VectorConfig optimize2(const GaussianISAM2& bayesTree) { +VectorConfig optimize2(const GaussianISAM2& bayesTree, double threshold) { VectorConfig result; // starting from the root, call optimize on each conditional - optimize2(bayesTree.root(), result); + optimize2(bayesTree.root(), threshold, result); return result; } -#if 0 /* ************************************************************************* */ -void optimize2(const GaussianISAM2_P::sharedClique& clique, VectorConfig& result) { - // parents are assumed to already be solved and available in result - GaussianISAM2_P::Clique::const_reverse_iterator it; - for (it = clique->rbegin(); it!=clique->rend(); it++) { - GaussianConditional::shared_ptr cg = *it; - result.insert(cg->key(), cg->solve(result)); // store result in partial solution - } - BOOST_FOREACH(GaussianISAM2_P::sharedClique child, clique->children_) { - optimize2(child, result); - } -} -#endif - -/* ************************************************************************* */ -VectorConfig optimize2(const GaussianISAM2_P& bayesTree) { +VectorConfig optimize2(const GaussianISAM2_P& bayesTree, double threshold) { VectorConfig result; // starting from the root, call optimize on each conditional - optimize2(bayesTree.root(), result); + optimize2(bayesTree.root(), threshold, result); return result; } diff --git a/cpp/GaussianISAM2.h b/cpp/GaussianISAM2.h index 7f6bc661e..61c2de016 100644 --- a/cpp/GaussianISAM2.h +++ b/cpp/GaussianISAM2.h @@ -19,10 +19,10 @@ namespace gtsam { typedef ISAM2 GaussianISAM2; // recursively optimize this conditional and all subtrees - void optimize2(const GaussianISAM2::sharedClique& clique, VectorConfig& result); + void optimize2(const GaussianISAM2::sharedClique& clique, double threshold, VectorConfig& result); // optimize the BayesTree, starting from the root - VectorConfig optimize2(const GaussianISAM2& bayesTree); + VectorConfig optimize2(const GaussianISAM2& bayesTree, double threshold = 0.); // todo: copy'n'paste to avoid template hell @@ -33,6 +33,6 @@ namespace gtsam { // void optimize2(const GaussianISAM2_P::sharedClique& clique, VectorConfig& result); // optimize the BayesTree, starting from the root - VectorConfig optimize2(const GaussianISAM2_P& bayesTree); + VectorConfig optimize2(const GaussianISAM2_P& bayesTree, double threshold = 0.); }/// namespace gtsam diff --git a/cpp/ISAM2-inl.h b/cpp/ISAM2-inl.h index 350f7ad88..f2d92f864 100644 --- a/cpp/ISAM2-inl.h +++ b/cpp/ISAM2-inl.h @@ -113,7 +113,7 @@ namespace gtsam { nonlinearAffectedFactors.push_back(*it); } - return nonlinearAffectedFactors.linearize(linPoint_); // todo: use shared_ptr to avoid copying? + return nonlinearAffectedFactors.linearize(linPoint_); } /* ************************************************************************* */ @@ -129,78 +129,61 @@ namespace gtsam { cachedBoundary.push_back(cached_[key]); } - return cachedBoundary; // todo: use shared_ptr to avoid copying? + return cachedBoundary; } /* ************************************************************************* */ template void ISAM2::update_internal(const NonlinearFactorGraph& newFactors, - const Config& config, Cliques& orphans) { + const Config& config, Cliques& orphans, double wildfire_threshold, double relinearize_threshold) { - list newFactorsKeys = newFactors.keys(); - -#if 1 // 0=relinearize all in each step - - // relinearize all keys that are in newFactors, and already exist (not new variables!) - list keysToRelinearize; - BOOST_FOREACH(const Symbol& key, newFactorsKeys) { - if (nonlinearFactors_.involves(key)) - keysToRelinearize.push_back(key); - } - - // 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(keysToRelinearize); - list keysToBeRemoved = allAffected->keys(); - -#else - // debug only: full relinearization in each step - list keysToRelinearize = nonlinearFactors_.keys(); - list keysToBeRemoved = nonlinearFactors_.keys(); -#endif - - // remove affected factors - BayesNet affectedBayesNet; - this->removeTop(keysToBeRemoved, affectedBayesNet, orphans); - - // selectively update the linearization point - VectorConfig selected_delta; - BOOST_FOREACH(const Symbol& key, keysToRelinearize) { - if (delta_.contains(key)) // after constructor call, delta is empty... - selected_delta.insert(key, delta_[key]); - } - linPoint_ = expmap(linPoint_, selected_delta); - - // relinearize the affected factors ... - list affectedKeys = affectedBayesNet.ordering(); // all keys in conditionals, there cannot be others because path to root included - FactorGraph factors = relinearizeAffectedFactors(affectedKeys); - - // ... add the cached intermediate results from the boundary of the orphans ... - FactorGraph cachedBoundary = getCachedBoundaryFactors(orphans); - factors.push_back(cachedBoundary); + //// 1 - add in new information // add new variables linPoint_.insert(config); - // ... and finally add the new linearized factors themselves - FactorGraph newFactorsLinearized = newFactors.linearize(linPoint_); - factors.push_back(newFactorsLinearized); - - // create an ordering for the new and contaminated factors - Ordering ordering; - if (true) { - ordering = factors.getOrdering(); - } else { - list keys = factors.keys(); - keys.sort(); // todo: correct sorting order? - ordering = keys; - } - - // eliminate into a Bayes net - BayesNet bayesNet = _eliminate(factors, cached_, ordering); // remember the new factors for later relinearization nonlinearFactors_.push_back(newFactors); + // merge keys of new factors with mask + const list newKeys = newFactors.keys(); + marked_.insert(marked_.begin(), newKeys.begin(), newKeys.end()); + // eliminate duplicates + marked_.sort(); + marked_.unique(); + + //// 2 - invalidate all cliques involving marked variables + + // remove affected factors + BayesNet affectedBayesNet; + this->removeTop(marked_, affectedBayesNet, orphans); + + //// 3 - find factors connected to marked variables + //// 4 - linearize + + // ordering provides all keys in conditionals, there cannot be others because path to root included + list affectedKeys = affectedBayesNet.ordering(); + + // todo - remerge in keys of new factors + affectedKeys.insert(affectedKeys.begin(), newKeys.begin(), newKeys.end()); + // eliminate duplicates + affectedKeys.sort(); + affectedKeys.unique(); + + FactorGraph factors = relinearizeAffectedFactors(affectedKeys); + + // add the cached intermediate results from the boundary of the orphans ... + FactorGraph cachedBoundary = getCachedBoundaryFactors(orphans); + factors.push_back(cachedBoundary); + + //// 5 - eliminate and add orphans back in + + // create an ordering for the new and contaminated factors + Ordering ordering = factors.getOrdering(); + + // eliminate into a Bayes net + BayesNet bayesNet = _eliminate(factors, cached_, ordering); + // insert conditionals back in, straight into the topless bayesTree typename BayesNet::const_reverse_iterator rit; for ( rit=bayesNet.rbegin(); rit != bayesNet.rend(); ++rit ) { @@ -209,23 +192,43 @@ namespace gtsam { // add orphans to the bottom of the new tree BOOST_FOREACH(sharedClique orphan, orphans) { - Symbol key = findParentClique(orphan->separator_, ordering); sharedClique parent = (*this)[key]; - parent->children_ += orphan; orphan->parent_ = parent; // set new parent! } - // update solution - todo: potentially expensive - delta_ = optimize2(*this); + //// 6 - update solution + + VectorConfig delta = optimize2(*this, wildfire_threshold); + + //// 7 - mark variables, if significant change + + marked_.clear(); + 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); + } + } + + //// 8 - relinearize selected variables + + linPoint_ = expmap(linPoint_, deltaMarked); + } template - void ISAM2::update(const NonlinearFactorGraph& newFactors, const Config& config) { + void ISAM2::update( + const NonlinearFactorGraph& newFactors, const Config& config, + double wildfire_threshold, double relinearize_threshold) { Cliques orphans; - this->update_internal(newFactors, config, orphans); + this->update_internal(newFactors, config, orphans, wildfire_threshold, relinearize_threshold); + } /* ************************************************************************* */ diff --git a/cpp/ISAM2.h b/cpp/ISAM2.h index 8381a8cbb..bb8f3b255 100644 --- a/cpp/ISAM2.h +++ b/cpp/ISAM2.h @@ -34,15 +34,15 @@ namespace gtsam { // current linearization point Config linPoint_; - // most recent solution - VectorConfig delta_; - // for keeping all original nonlinear factors NonlinearFactorGraph nonlinearFactors_; // cached intermediate results for restarting computation in the middle CachedFactors cached_; + // variables that have been updated, requiring the corresponding factors to be relinearized + std::list marked_; + public: /** Create an empty Bayes Tree */ @@ -62,11 +62,13 @@ 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); - void update(const NonlinearFactorGraph& newFactors, const Config& config); + void update_internal(const NonlinearFactorGraph& newFactors, + const Config& config, Cliques& orphans, + double wildfire_threshold, double relinearize_threshold); + void update(const NonlinearFactorGraph& newFactors, const Config& config, + double wildfire_threshold = 0., double relinearize_threshold = 0.); - const Config estimate() const {return expmap(linPoint_, delta_);} - const Config linearizationPoint() const {return linPoint_;} + const Config estimate() const {return linPoint_;} private: diff --git a/cpp/Vector.cpp b/cpp/Vector.cpp index 9ded0917d..9149e62d2 100644 --- a/cpp/Vector.cpp +++ b/cpp/Vector.cpp @@ -211,6 +211,13 @@ namespace gtsam { return s; } + /* ************************************************************************* */ + Vector abs(const Vector& v) { + Vector s(v.size()); + transform(v.begin(), v.end(), s.begin(), ::fabs); + return s; + } + /* ************************************************************************* */ double max(const Vector &a) { return *(std::max_element(a.begin(), a.end())); diff --git a/cpp/Vector.h b/cpp/Vector.h index 02f5930e8..6a02d1312 100644 --- a/cpp/Vector.h +++ b/cpp/Vector.h @@ -184,6 +184,13 @@ Vector reciprocal(const Vector &a); */ Vector esqrt(const Vector& v); +/** + * absolut values of vector elements + * @param a vector + * @return [abs(a(i))] + */ +Vector abs(const Vector& v); + /** * return the max element of a vector * @param a vector