From 5223713c1877be08f30a5b8a3307c282025ad175 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 30 Sep 2018 10:30:27 -0400 Subject: [PATCH] Use VectorValues::vector rather than bespoke code for saving delta values. --- gtsam/nonlinear/ISAM2.cpp | 58 +++++++++++++++------------------------ gtsam/nonlinear/ISAM2.h | 52 ++++++++++++++++------------------- 2 files changed, 46 insertions(+), 64 deletions(-) diff --git a/gtsam/nonlinear/ISAM2.cpp b/gtsam/nonlinear/ISAM2.cpp index bdd6b42bb..599219145 100644 --- a/gtsam/nonlinear/ISAM2.cpp +++ b/gtsam/nonlinear/ISAM2.cpp @@ -85,10 +85,10 @@ class ISAM2JunctionTree }; /* ************************************************************************* */ -std::string ISAM2DoglegParams::adaptationModeTranslator( +string ISAM2DoglegParams::adaptationModeTranslator( const DoglegOptimizerImpl::TrustRegionAdaptationMode& adaptationMode) const { - std::string s; + string s; switch (adaptationMode) { case DoglegOptimizerImpl::SEARCH_EACH_ITERATION: s = "SEARCH_EACH_ITERATION"; @@ -106,8 +106,8 @@ std::string ISAM2DoglegParams::adaptationModeTranslator( /* ************************************************************************* */ DoglegOptimizerImpl::TrustRegionAdaptationMode ISAM2DoglegParams::adaptationModeTranslator( - const std::string& adaptationMode) const { - std::string s = adaptationMode; + const string& adaptationMode) const { + string s = adaptationMode; boost::algorithm::to_upper(s); if (s == "SEARCH_EACH_ITERATION") return DoglegOptimizerImpl::SEARCH_EACH_ITERATION; @@ -120,8 +120,8 @@ ISAM2DoglegParams::adaptationModeTranslator( /* ************************************************************************* */ ISAM2Params::Factorization ISAM2Params::factorizationTranslator( - const std::string& str) { - std::string s = str; + const string& str) { + string s = str; boost::algorithm::to_upper(s); if (s == "QR") return ISAM2Params::QR; if (s == "CHOLESKY") return ISAM2Params::CHOLESKY; @@ -131,9 +131,9 @@ ISAM2Params::Factorization ISAM2Params::factorizationTranslator( } /* ************************************************************************* */ -std::string ISAM2Params::factorizationTranslator( +string ISAM2Params::factorizationTranslator( const ISAM2Params::Factorization& value) { - std::string s; + string s; switch (value) { case ISAM2Params::QR: s = "QR"; @@ -171,13 +171,12 @@ bool ISAM2Clique::equals(const This& other, double tol) const { } /* ************************************************************************* */ -void ISAM2Clique::print(const std::string& s, - const KeyFormatter& formatter) const { +void ISAM2Clique::print(const string& s, const KeyFormatter& formatter) const { Base::print(s, formatter); if (cachedFactor_) cachedFactor_->print(s + "Cached: ", formatter); else - std::cout << s << "Cached empty" << std::endl; + cout << s << "Cached empty" << endl; if (gradientContribution_.rows() != 0) gtsam::print(gradientContribution_, "Gradient contribution: "); } @@ -207,31 +206,16 @@ bool ISAM2Clique::isDirty(const KeySet& replaced, const KeySet& changed) const { } return dirty; } -/* ************************************************************************* */ -FastVector ISAM2Clique::copyRelevantValues( - const VectorValues& delta) const { - FastVector values; - values.reserve(conditional_->nrFrontals()); - for (Key frontal : conditional_->frontals()) { - values.push_back(delta[frontal]); - } - return values; -} /* ************************************************************************* */ bool ISAM2Clique::valuesChanged(const KeySet& replaced, - const FastVector& originalValues, + const Vector& originalValues, const VectorValues& delta, double threshold) const { - if (replaced.exists(conditional_->frontals().front())) return true; - size_t i = 0; - for (Key frontal : conditional_->frontals()) { - const Vector diff = originalValues[i++] - delta[frontal]; - if (diff.lpNorm() >= threshold) { - return true; - } - } - return false; + auto frontals = conditional_->frontals(); + if (replaced.exists(frontals.front())) return true; + auto diff = originalValues - delta.vector(frontals); + return diff.lpNorm() >= threshold; } /* ************************************************************************* */ @@ -243,11 +227,13 @@ void ISAM2Clique::markFrontalsAsChanged(KeySet* changed) const { } /* ************************************************************************* */ -void ISAM2Clique::restoreFromOriginals(const FastVector& originalValues, +void ISAM2Clique::restoreFromOriginals(const Vector& originalValues, VectorValues* delta) const { - size_t i = 0; + size_t pos = 0; for (Key frontal : conditional_->frontals()) { - delta->at(frontal) = originalValues[i++]; + auto v = delta->at(frontal); + v = originalValues.segment(pos, v.size()); + pos += v.size(); } } @@ -257,7 +243,7 @@ void ISAM2Clique::optimizeWildfire(const KeySet& replaced, double threshold, size_t* count) const { if (isDirty(replaced, *changed)) { // Temporary copy of the original values, to check how much they change - auto originalValues = copyRelevantValues(*delta); + auto originalValues = delta->vector(conditional_->frontals()); // Back-substitute delta->update(conditional_->solve(*delta)); @@ -285,7 +271,7 @@ bool ISAM2Clique::optimizeWildfireNode(const KeySet& replaced, double threshold, bool dirty = isDirty(replaced, *changed); if (dirty) { // Temporary copy of the original values, to check how much they change - auto originalValues = copyRelevantValues(*delta); + auto originalValues = delta->vector(conditional_->frontals()); // Back-substitute - special version stores solution pointers in cliques for // fast access. diff --git a/gtsam/nonlinear/ISAM2.h b/gtsam/nonlinear/ISAM2.h index d3904067c..465e5cd7f 100644 --- a/gtsam/nonlinear/ISAM2.h +++ b/gtsam/nonlinear/ISAM2.h @@ -546,36 +546,12 @@ class GTSAM_EXPORT ISAM2Clique void print(const std::string& s = "", const KeyFormatter& formatter = DefaultKeyFormatter) const; - /** - * Check if clique was replaced, or if any parents were changed above the - * threshold or themselves replaced. - */ - bool isDirty(const KeySet& replaced, const KeySet& changed) const; - - /// Copy values in delta pertaining to this clique. - FastVector copyRelevantValues(const VectorValues& delta) const; - - /* - * Check whether the values changed above a threshold, or always true if the - * clique was replaced. - */ - bool valuesChanged(const KeySet& replaced, - const FastVector& originalValues, - const VectorValues& delta, double threshold) const; - - /// Set changed flag for each frontal variable - void markFrontalsAsChanged(KeySet* changed) const; - - /// Restore delta to original values, guided by frontal keys. - void restoreFromOriginals(const FastVector& originalValues, - VectorValues* delta) const; - - void optimizeWildfire(const KeySet& replaced, double threshold, KeySet* changed, - VectorValues* delta, + void optimizeWildfire(const KeySet& replaced, double threshold, + KeySet* changed, VectorValues* delta, size_t* count) const; - bool optimizeWildfireNode(const KeySet& replaced, double threshold, KeySet* changed, - VectorValues* delta, + bool optimizeWildfireNode(const KeySet& replaced, double threshold, + KeySet* changed, VectorValues* delta, size_t* count) const; /** @@ -586,6 +562,26 @@ class GTSAM_EXPORT ISAM2Clique size_t calculate_nnz() const; private: + /** + * Check if clique was replaced, or if any parents were changed above the + * threshold or themselves replaced. + */ + bool isDirty(const KeySet& replaced, const KeySet& changed) const; + + /* + * Check whether the values changed above a threshold, or always true if the + * clique was replaced. + */ + bool valuesChanged(const KeySet& replaced, const Vector& originalValues, + const VectorValues& delta, double threshold) const; + + /// Set changed flag for each frontal variable + void markFrontalsAsChanged(KeySet* changed) const; + + /// Restore delta to original values, guided by frontal keys. + void restoreFromOriginals(const Vector& originalValues, + VectorValues* delta) const; + /** Serialization function */ friend class boost::serialization::access; template