From 7fd8bc1bf5e5039a1b270620b48823298c2c8e10 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 30 Sep 2018 11:19:25 -0400 Subject: [PATCH] Restored non-recursive version, disabled solution pointers back-substitute. --- gtsam/nonlinear/ISAM2-impl.cpp | 2 +- gtsam/nonlinear/ISAM2.cpp | 66 +++++++++++++++++----------------- gtsam/nonlinear/ISAM2.h | 2 ++ 3 files changed, 37 insertions(+), 33 deletions(-) diff --git a/gtsam/nonlinear/ISAM2-impl.cpp b/gtsam/nonlinear/ISAM2-impl.cpp index aa201138c..bc43c9b9d 100644 --- a/gtsam/nonlinear/ISAM2-impl.cpp +++ b/gtsam/nonlinear/ISAM2-impl.cpp @@ -304,7 +304,7 @@ size_t ISAM2::Impl::UpdateGaussNewtonDelta( // Optimize with wildfire lastBacksubVariableCount = 0; for (const ISAM2::sharedClique& root : roots) - lastBacksubVariableCount += optimizeWildfire( + lastBacksubVariableCount += optimizeWildfireNonRecursive( root, wildfireThreshold, replacedKeys, delta); // modifies delta #if !defined(NDEBUG) && defined(GTSAM_EXTRA_CONSISTENCY_CHECKS) diff --git a/gtsam/nonlinear/ISAM2.cpp b/gtsam/nonlinear/ISAM2.cpp index c16fe04da..f2b154cc4 100644 --- a/gtsam/nonlinear/ISAM2.cpp +++ b/gtsam/nonlinear/ISAM2.cpp @@ -213,6 +213,7 @@ bool ISAM2Clique::isDirty(const KeySet& replaced, const KeySet& changed) const { * fast access. */ void ISAM2Clique::fastBackSubstitute(VectorValues* delta) const { +#ifdef USE_BROKEN_FAST_BACKSUBSTITUTE // TODO(gareth): This code shares a lot of logic w/ linearAlgorithms-inst, // potentially refactor @@ -279,6 +280,9 @@ void ISAM2Clique::fastBackSubstitute(VectorValues* delta) const { // Just call plain solve because we couldn't use solution pointers. delta->update(conditional_->solve(*delta)); } +#else + delta->update(conditional_->solve(*delta)); +#endif } /* ************************************************************************* */ @@ -312,6 +316,7 @@ void ISAM2Clique::restoreFromOriginals(const Vector& originalValues, } /* ************************************************************************* */ +// Note: not being used right now in favor of non-recursive version below. void ISAM2Clique::optimizeWildfire(const KeySet& replaced, double threshold, KeySet* changed, VectorValues* delta, size_t* count) const { @@ -320,7 +325,7 @@ void ISAM2Clique::optimizeWildfire(const KeySet& replaced, double threshold, auto originalValues = delta->vector(conditional_->frontals()); // Back-substitute - delta->update(conditional_->solve(*delta)); + fastBackSubstitute(delta); count += conditional_->nrFrontals(); if (valuesChanged(replaced, originalValues, *delta, threshold)) { @@ -336,6 +341,15 @@ void ISAM2Clique::optimizeWildfire(const KeySet& replaced, double threshold, } } +size_t optimizeWildfire(const ISAM2::sharedClique& root, double threshold, + const KeySet& keys, VectorValues* delta) { + KeySet changed; + size_t count = 0; + // starting from the root, call optimize on each conditional + if (root) root->optimizeWildfire(keys, threshold, &changed, delta, &count); + return count; +} + /* ************************************************************************* */ bool ISAM2Clique::optimizeWildfireNode(const KeySet& replaced, double threshold, KeySet* changed, VectorValues* delta, @@ -347,6 +361,7 @@ bool ISAM2Clique::optimizeWildfireNode(const KeySet& replaced, double threshold, // Temporary copy of the original values, to check how much they change auto originalValues = delta->vector(conditional_->frontals()); + // Back-substitute fastBackSubstitute(delta); count += conditional_->nrFrontals(); @@ -360,37 +375,6 @@ bool ISAM2Clique::optimizeWildfireNode(const KeySet& replaced, double threshold, return dirty; } -/* ************************************************************************* */ -void ISAM2Clique::nnz_internal(size_t* result) const { - size_t dimR = conditional_->rows(); - size_t dimSep = conditional_->get_S().cols(); - *result += ((dimR + 1) * dimR) / 2 + dimSep * dimR; - // traverse the children - for (const auto& child : children) { - child->nnz_internal(result); - } -} - -/* ************************************************************************* */ -size_t ISAM2Clique::calculate_nnz() const { - size_t result = 0; - nnz_internal(&result); - return result; -} - -/* ************************************************************************* */ -size_t optimizeWildfire(const ISAM2::sharedClique& root, double threshold, - const KeySet& keys, VectorValues* delta) { - KeySet changed; - size_t count = 0; - // starting from the root, call optimize on each conditional - if (root) root->optimizeWildfire(keys, threshold, &changed, delta, &count); - return count; -} - -/* ************************************************************************* */ -// This version is non-recursive version, but seems to have -// a bug, as was diagnosed with ISAM2Example_SmartFactor. Disabled for now. size_t optimizeWildfireNonRecursive(const ISAM2::sharedClique& root, double threshold, const KeySet& keys, VectorValues* delta) { @@ -417,6 +401,24 @@ size_t optimizeWildfireNonRecursive(const ISAM2::sharedClique& root, return count; } +/* ************************************************************************* */ +void ISAM2Clique::nnz_internal(size_t* result) const { + size_t dimR = conditional_->rows(); + size_t dimSep = conditional_->get_S().cols(); + *result += ((dimR + 1) * dimR) / 2 + dimSep * dimR; + // traverse the children + for (const auto& child : children) { + child->nnz_internal(result); + } +} + +/* ************************************************************************* */ +size_t ISAM2Clique::calculate_nnz() const { + size_t result = 0; + nnz_internal(&result); + return result; +} + /* ************************************************************************* */ ISAM2::ISAM2(const ISAM2Params& params) : params_(params), update_count_(0) { if (params_.optimizationParams.type() == typeid(ISAM2DoglegParams)) diff --git a/gtsam/nonlinear/ISAM2.h b/gtsam/nonlinear/ISAM2.h index b60a24644..eb33038a7 100644 --- a/gtsam/nonlinear/ISAM2.h +++ b/gtsam/nonlinear/ISAM2.h @@ -509,7 +509,9 @@ class GTSAM_EXPORT ISAM2Clique Base::FactorType::shared_ptr cachedFactor_; Vector gradientContribution_; +#ifdef USE_BROKEN_FAST_BACKSUBSTITUTE mutable FastMap solnPointers_; +#endif /// Default constructor ISAM2Clique() : Base() {}