diff --git a/gtsam/nonlinear/ISAM2.cpp b/gtsam/nonlinear/ISAM2.cpp index 9a510c251..55aab98f5 100644 --- a/gtsam/nonlinear/ISAM2.cpp +++ b/gtsam/nonlinear/ISAM2.cpp @@ -173,25 +173,18 @@ bool ISAM2::equals(const ISAM2& other, double tol) const { } /* ************************************************************************* */ -FastList ISAM2::getAffectedFactors(const FastList& keys) const { +FastSet ISAM2::getAffectedFactors(const FastList& keys) const { static const bool debug = false; if(debug) cout << "Getting affected factors for "; if(debug) { BOOST_FOREACH(const Key key, keys) { cout << key << " "; } } if(debug) cout << endl; NonlinearFactorGraph allAffected; - FastList indices; + FastSet indices; BOOST_FOREACH(const Key key, keys) { -// const list l = nonlinearFactors_.factors(key); -// indices.insert(indices.begin(), l.begin(), l.end()); const VariableIndex::Factors& factors(variableIndex_[key]); - BOOST_FOREACH(size_t factor, factors) { - if(debug) cout << "Variable " << key << " affects factor " << factor << endl; - indices.push_back(factor); - } + indices.insert(factors.begin(), factors.end()); } - indices.sort(); - indices.unique(); if(debug) cout << "Affected factors are: "; if(debug) { BOOST_FOREACH(const size_t index, indices) { cout << index << " "; } } if(debug) cout << endl; @@ -206,7 +199,7 @@ GaussianFactorGraph::shared_ptr ISAM2::relinearizeAffectedFactors(const FastList& affectedKeys, const FastSet& relinKeys) const { gttic(getAffectedFactors); - FastList candidates = getAffectedFactors(affectedKeys); + FastSet candidates = getAffectedFactors(affectedKeys); gttoc(getAffectedFactors); NonlinearFactorGraph nonlinearAffectedFactors; diff --git a/gtsam/nonlinear/ISAM2.h b/gtsam/nonlinear/ISAM2.h index d93957fc3..159f88ad4 100644 --- a/gtsam/nonlinear/ISAM2.h +++ b/gtsam/nonlinear/ISAM2.h @@ -624,7 +624,7 @@ public: private: - FastList getAffectedFactors(const FastList& keys) const; + FastSet getAffectedFactors(const FastList& keys) const; GaussianFactorGraph::shared_ptr relinearizeAffectedFactors(const FastList& affectedKeys, const FastSet& relinKeys) const; GaussianFactorGraph getCachedBoundaryFactors(Cliques& orphans);