From d6edc217abb64ac9e76323d69c38d483c7722649 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 30 Sep 2018 14:11:00 -0400 Subject: [PATCH] Changed several Impl functions to methods in ISAM2 and ISAM2Clique --- gtsam/nonlinear/ISAM2-impl.cpp | 131 ++++---------------------------- gtsam/nonlinear/ISAM2-impl.h | 66 ++-------------- gtsam/nonlinear/ISAM2.cpp | 76 +++++++++++++++--- gtsam/nonlinear/ISAM2.h | 24 ++++++ gtsam/nonlinear/ISAM2Clique.cpp | 27 ++++++- gtsam/nonlinear/ISAM2Clique.h | 21 ++++- tests/testGaussianISAM2.cpp | 2 +- 7 files changed, 154 insertions(+), 193 deletions(-) diff --git a/gtsam/nonlinear/ISAM2-impl.cpp b/gtsam/nonlinear/ISAM2-impl.cpp index bc43c9b9d..8a8813af5 100644 --- a/gtsam/nonlinear/ISAM2-impl.cpp +++ b/gtsam/nonlinear/ISAM2-impl.cpp @@ -31,27 +31,12 @@ using namespace std; namespace gtsam { -/* ************************************************************************* */ -void ISAM2::Impl::AddVariables(const Values& newTheta, Values& theta, - VectorValues& delta, VectorValues& deltaNewton, - VectorValues& RgProd, - const KeyFormatter& keyFormatter) { - const bool debug = ISDEBUG("ISAM2 AddVariables"); - - theta.insert(newTheta); - if (debug) newTheta.print("The new variables are: "); - // Add zeros into the VectorValues - delta.insert(newTheta.zeroVectors()); - deltaNewton.insert(newTheta.zeroVectors()); - RgProd.insert(newTheta.zeroVectors()); -} - /* ************************************************************************* */ void ISAM2::Impl::AddFactorsStep1(const NonlinearFactorGraph& newFactors, bool useUnusedSlots, - NonlinearFactorGraph& nonlinearFactors, - FactorIndices& newFactorIndices) { - newFactorIndices.resize(newFactors.size()); + NonlinearFactorGraph* nonlinearFactors, + FactorIndices* newFactorIndices) { + newFactorIndices->resize(newFactors.size()); if (useUnusedSlots) { size_t globalFactorIndex = 0; @@ -65,44 +50,24 @@ void ISAM2::Impl::AddFactorsStep1(const NonlinearFactorGraph& newFactors, // if so, increase globalFactorIndex. If the current factor in // nonlinearFactors is unused, break out of the loop and use the current // slot. - if (globalFactorIndex >= nonlinearFactors.size()) - nonlinearFactors.resize(nonlinearFactors.size() + newFactors.size() - - newFactorIndex); - else if (nonlinearFactors[globalFactorIndex]) + if (globalFactorIndex >= nonlinearFactors->size()) + nonlinearFactors->resize(nonlinearFactors->size() + + newFactors.size() - newFactorIndex); + else if ((*nonlinearFactors)[globalFactorIndex]) ++globalFactorIndex; else break; } while (true); // Use the current slot, updating nonlinearFactors and newFactorSlots. - nonlinearFactors[globalFactorIndex] = newFactors[newFactorIndex]; - newFactorIndices[newFactorIndex] = globalFactorIndex; + (*nonlinearFactors)[globalFactorIndex] = newFactors[newFactorIndex]; + (*newFactorIndices)[newFactorIndex] = globalFactorIndex; } } else { // We're not looking for unused slots, so just add the factors at the end. for (size_t i = 0; i < newFactors.size(); ++i) - newFactorIndices[i] = i + nonlinearFactors.size(); - nonlinearFactors.push_back(newFactors); - } -} - -/* ************************************************************************* */ -void ISAM2::Impl::RemoveVariables(const KeySet& unusedKeys, - const FastVector& roots, - Values& theta, VariableIndex& variableIndex, - VectorValues& delta, - VectorValues& deltaNewton, - VectorValues& RgProd, KeySet& replacedKeys, - Base::Nodes& nodes, KeySet& fixedVariables) { - variableIndex.removeUnusedVariables(unusedKeys.begin(), unusedKeys.end()); - for (Key key : unusedKeys) { - delta.erase(key); - deltaNewton.erase(key); - RgProd.erase(key); - replacedKeys.erase(key); - nodes.unsafe_erase(key); - theta.erase(key); - fixedVariables.erase(key); + (*newFactorIndices)[i] = i + nonlinearFactors->size(); + nonlinearFactors->push_back(newFactors); } } @@ -195,7 +160,7 @@ static void CheckRelinearizationRecursiveMap( /* ************************************************************************* */ KeySet ISAM2::Impl::CheckRelinearizationPartial( - const FastVector& roots, const VectorValues& delta, + const ISAM2::Roots& roots, const VectorValues& delta, const ISAM2Params::RelinearizationThreshold& relinearizeThreshold) { KeySet relinKeys; for (const ISAM2::sharedClique& root : roots) { @@ -210,71 +175,6 @@ KeySet ISAM2::Impl::CheckRelinearizationPartial( return relinKeys; } -/* ************************************************************************* */ -void ISAM2::Impl::FindAll(ISAM2::sharedClique clique, KeySet& keys, - const KeySet& markedMask) { - static const bool debug = false; - // does the separator contain any of the variables? - bool found = false; - for (Key key : clique->conditional()->parents()) { - if (markedMask.exists(key)) { - found = true; - break; - } - } - if (found) { - // then add this clique - keys.insert(clique->conditional()->beginFrontals(), - clique->conditional()->endFrontals()); - if (debug) clique->print("Key(s) marked in clique "); - if (debug) - cout << "so marking key " << clique->conditional()->front() << endl; - } - for (const ISAM2::sharedClique& child : clique->children) { - FindAll(child, keys, markedMask); - } -} - -/* ************************************************************************* */ -void ISAM2::Impl::ExpmapMasked(const VectorValues& delta, const KeySet& mask, - Values* values, - boost::optional invalidateIfDebug, - const KeyFormatter& keyFormatter) { - // If debugging, invalidate if requested, otherwise do not invalidate. - // Invalidating means setting expmapped entries to Inf, to trigger assertions - // if we try to re-use them. -#ifdef NDEBUG - invalidateIfDebug = boost::none; -#endif - - assert(values->size() == delta.size()); - Values::iterator key_value; - VectorValues::const_iterator key_delta; -#ifdef GTSAM_USE_TBB - for (key_value = values->begin(); key_value != values->end(); ++key_value) { - key_delta = delta.find(key_value->key); -#else - for (key_value = values->begin(), key_delta = delta.begin(); - key_value != values->end(); ++key_value, ++key_delta) { - assert(key_value->key == key_delta->first); -#endif - Key var = key_value->key; - assert(static_cast(delta[var].size()) == key_value->value.dim()); - assert(delta[var].allFinite()); - if (mask.exists(var)) { - Value* retracted = key_value->value.retract_(delta[var]); - key_value->value = *retracted; - retracted->deallocate_(); - if (invalidateIfDebug) - (*invalidateIfDebug)[var].operator=(Vector::Constant( - delta[var].rows(), - numeric_limits::infinity())); // Strange syntax to work - // with clang++ (bug in - // clang?) - } - } -} - /* ************************************************************************* */ namespace internal { inline static void optimizeInPlace(const ISAM2::sharedClique& clique, @@ -289,9 +189,10 @@ inline static void optimizeInPlace(const ISAM2::sharedClique& clique, } // namespace internal /* ************************************************************************* */ -size_t ISAM2::Impl::UpdateGaussNewtonDelta( - const FastVector& roots, const KeySet& replacedKeys, - double wildfireThreshold, VectorValues* delta) { +size_t ISAM2::Impl::UpdateGaussNewtonDelta(const ISAM2::Roots& roots, + const KeySet& replacedKeys, + double wildfireThreshold, + VectorValues* delta) { size_t lastBacksubVariableCount; if (wildfireThreshold <= 0.0) { diff --git a/gtsam/nonlinear/ISAM2-impl.h b/gtsam/nonlinear/ISAM2-impl.h index 0e05ab453..8a30fb8cd 100644 --- a/gtsam/nonlinear/ISAM2-impl.h +++ b/gtsam/nonlinear/ISAM2-impl.h @@ -23,7 +23,6 @@ namespace gtsam { struct GTSAM_EXPORT ISAM2::Impl { - struct GTSAM_EXPORT PartialSolveResult { ISAM2::sharedClique bayesTree; }; @@ -32,35 +31,14 @@ struct GTSAM_EXPORT ISAM2::Impl { size_t nFullSystemVars; enum { /*AS_ADDED,*/ COLAMD } algorithm; enum { NO_CONSTRAINT, CONSTRAIN_LAST } constrain; - boost::optional > constrainedKeys; + boost::optional > constrainedKeys; }; - /** - * Add new variables to the ISAM2 system. - * @param newTheta Initial values for new variables - * @param theta Current solution to be augmented with new initialization - * @param delta Current linear delta to be augmented with zeros - * @param ordering Current ordering to be augmented with new variables - * @param nodes Current BayesTree::Nodes index to be augmented with slots for new variables - * @param keyFormatter Formatter for printing nonlinear keys during debugging - */ - static void AddVariables(const Values& newTheta, Values& theta, VectorValues& delta, - VectorValues& deltaNewton, VectorValues& RgProd, - const KeyFormatter& keyFormatter = DefaultKeyFormatter); - /// Perform the first part of the bookkeeping updates for adding new factors. Adds them to the /// complete list of nonlinear factors, and populates the list of new factor indices, both /// optionally finding and reusing empty factor slots. static void AddFactorsStep1(const NonlinearFactorGraph& newFactors, bool useUnusedSlots, - NonlinearFactorGraph& nonlinearFactors, FactorIndices& newFactorIndices); - - /** - * Remove variables from the ISAM2 system. - */ - static void RemoveVariables(const KeySet& unusedKeys, const FastVector& roots, - Values& theta, VariableIndex& variableIndex, VectorValues& delta, VectorValues& deltaNewton, - VectorValues& RgProd, KeySet& replacedKeys, Base::Nodes& nodes, - KeySet& fixedVariables); + NonlinearFactorGraph* nonlinearFactors, FactorIndices* newFactorIndices); /** * Find the set of variables to be relinearized according to relinearizeThreshold. @@ -85,47 +63,13 @@ struct GTSAM_EXPORT ISAM2::Impl { * @return The set of variable indices in delta whose magnitude is greater than or * equal to relinearizeThreshold */ - static KeySet CheckRelinearizationPartial(const FastVector& roots, + static KeySet CheckRelinearizationPartial(const ISAM2::Roots& roots, const VectorValues& delta, const ISAM2Params::RelinearizationThreshold& relinearizeThreshold); - /** - * Recursively search this clique and its children for marked keys appearing - * in the separator, and add the *frontal* keys of any cliques whose - * separator contains any marked keys to the set \c keys. The purpose of - * this is to discover the cliques that need to be redone due to information - * propagating to them from cliques that directly contain factors being - * relinearized. - * - * The original comment says this finds all variables directly connected to - * the marked ones by measurements. Is this true, because it seems like this - * would also pull in variables indirectly connected through other frontal or - * separator variables? - * - * Alternatively could we trace up towards the root for each variable here? - */ - static void FindAll(ISAM2Clique::shared_ptr clique, KeySet& keys, const KeySet& markedMask); - - /** - * Apply expmap to the given values, but only for indices appearing in - * \c markedRelinMask. Values are expmapped in-place. - * \param delta The linear delta with which to expmap - * \param mask Mask on linear indices, only \c true entries are expmapped - * \param [in, out] values The value to expmap in-place - * \param invalidateIfDebug If this is true, *and* NDEBUG is not defined, - * expmapped deltas will be set to an invalid value (infinity) to catch bugs - * where we might expmap something twice, or expmap it but then not - * recalculate its delta. - * @param keyFormatter Formatter for printing nonlinear keys during debugging - */ - static void ExpmapMasked( - const VectorValues& delta, const KeySet& mask, Values* values, - boost::optional invalidateIfDebug = boost::none, - const KeyFormatter& keyFormatter = DefaultKeyFormatter); - /** * Update the Newton's method step point, using wildfire */ - static size_t UpdateGaussNewtonDelta(const FastVector& roots, + static size_t UpdateGaussNewtonDelta(const ISAM2::Roots& roots, const KeySet& replacedKeys, double wildfireThreshold, VectorValues* delta); /** @@ -134,7 +78,7 @@ struct GTSAM_EXPORT ISAM2::Impl { */ static size_t UpdateRgProd(const ISAM2::Roots& roots, const KeySet& replacedKeys, const VectorValues& gradAtZero, VectorValues* RgProd); - + /** * Compute the gradient-search point. Only used in Dogleg. */ diff --git a/gtsam/nonlinear/ISAM2.cpp b/gtsam/nonlinear/ISAM2.cpp index e543508b2..2f44063ac 100644 --- a/gtsam/nonlinear/ISAM2.cpp +++ b/gtsam/nonlinear/ISAM2.cpp @@ -33,6 +33,7 @@ using namespace boost::adaptors; } // namespace br #include +#include #include #include @@ -484,6 +485,62 @@ boost::shared_ptr ISAM2::recalculate( return affectedKeysSet; } +/* ************************************************************************* */ +void ISAM2::addVariables(const Values& newTheta) { + const bool debug = ISDEBUG("ISAM2 AddVariables"); + + theta_.insert(newTheta); + if (debug) newTheta.print("The new variables are: "); + // Add zeros into the VectorValues + delta_.insert(newTheta.zeroVectors()); + deltaNewton_.insert(newTheta.zeroVectors()); + RgProd_.insert(newTheta.zeroVectors()); +} + +/* ************************************************************************* */ +void ISAM2::removeVariables(const KeySet& unusedKeys) { + variableIndex_.removeUnusedVariables(unusedKeys.begin(), unusedKeys.end()); + for (Key key : unusedKeys) { + delta_.erase(key); + deltaNewton_.erase(key); + RgProd_.erase(key); + deltaReplacedMask_.erase(key); + Base::nodes_.unsafe_erase(key); + theta_.erase(key); + fixedVariables_.erase(key); + } +} + +/* ************************************************************************* */ +void ISAM2::expmapMasked(const KeySet& mask) { + assert(theta_.size() == delta_.size()); + Values::iterator key_value; + VectorValues::const_iterator key_delta; +#ifdef GTSAM_USE_TBB + for (key_value = theta_.begin(); key_value != theta_.end(); ++key_value) { + key_delta = delta_.find(key_value->key); +#else + for (key_value = theta_.begin(), key_delta = delta_.begin(); + key_value != theta_.end(); ++key_value, ++key_delta) { + assert(key_value->key == key_delta->first); +#endif + Key var = key_value->key; + assert(static_cast(delta_[var].size()) == key_value->value.dim()); + assert(delta_[var].allFinite()); + if (mask.exists(var)) { + Value* retracted = key_value->value.retract_(delta_[var]); + key_value->value = *retracted; + retracted->deallocate_(); +#ifndef NDEBUG + // If debugging, invalidate delta_ entries to Inf, to trigger assertions + // if we try to re-use them. + delta_[var] = Vector::Constant(delta_[var].rows(), + numeric_limits::infinity()); +#endif + } + } +} + /* ************************************************************************* */ ISAM2Result ISAM2::update( const NonlinearFactorGraph& newFactors, const Values& newTheta, @@ -529,7 +586,7 @@ ISAM2Result ISAM2::update( // Add the new factor indices to the result struct if (debug || verbose) newFactors.print("The new factors are: "); Impl::AddFactorsStep1(newFactors, params_.findUnusedFactorSlots, - nonlinearFactors_, result.newFactorsIndices); + &nonlinearFactors_, &result.newFactorsIndices); // Remove the removed factors NonlinearFactorGraph removeFactors; @@ -571,7 +628,7 @@ ISAM2Result ISAM2::update( gttic(add_new_variables); // 2. Initialize any new variables \Theta_{new} and add // \Theta:=\Theta\cup\Theta_{new}. - Impl::AddVariables(newTheta, theta_, delta_, deltaNewton_, RgProd_); + addVariables(newTheta); // New keys for detailed results if (params_.enableDetailedResults) { for (Key key : newTheta.keys()) { @@ -669,13 +726,13 @@ ISAM2Result ISAM2::update( if (!relinKeys.empty()) { for (const sharedClique& root : roots_) // add other cliques that have the marked ones in the separator - Impl::FindAll(root, markedKeys, markedRelinMask); + root->findAll(markedRelinMask, &markedKeys); // Relin involved keys for detailed results if (params_.enableDetailedResults) { KeySet involvedRelinKeys; for (const sharedClique& root : roots_) - Impl::FindAll(root, involvedRelinKeys, markedRelinMask); + root->findAll(markedRelinMask, &involvedRelinKeys); for (Key key : involvedRelinKeys) { if (!result.detail->variableStatus[key].isAboveRelinThreshold) { result.detail->variableStatus[key].isRelinearizeInvolved = true; @@ -689,8 +746,7 @@ ISAM2Result ISAM2::update( gttic(expmap); // 6. Update linearization point for marked variables: // \Theta_{J}:=\Theta_{J}+\Delta_{J}. - if (!relinKeys.empty()) - Impl::ExpmapMasked(delta_, markedRelinMask, &theta_, delta_); + if (!relinKeys.empty()) expmapMasked(markedRelinMask); gttoc(expmap); result.variablesRelinearized = markedKeys.size(); @@ -740,9 +796,7 @@ ISAM2Result ISAM2::update( // Update data structures to remove unused keys if (!unusedKeys.empty()) { gttic(remove_variables); - Impl::RemoveVariables(unusedKeys, roots_, theta_, variableIndex_, delta_, - deltaNewton_, RgProd_, deltaReplacedMask_, - Base::nodes_, fixedVariables_); + removeVariables(unusedKeys); gttoc(remove_variables); } result.cliques = this->nodes().size(); @@ -998,9 +1052,7 @@ void ISAM2::marginalizeLeaves( factorIndicesToRemove.end()); // Remove the marginalized variables - Impl::RemoveVariables(KeySet(leafKeys.begin(), leafKeys.end()), roots_, - theta_, variableIndex_, delta_, deltaNewton_, RgProd_, - deltaReplacedMask_, nodes_, fixedVariables_); + removeVariables(KeySet(leafKeys.begin(), leafKeys.end())); } /* ************************************************************************* */ diff --git a/gtsam/nonlinear/ISAM2.h b/gtsam/nonlinear/ISAM2.h index 069d90cc8..2de8eb608 100644 --- a/gtsam/nonlinear/ISAM2.h +++ b/gtsam/nonlinear/ISAM2.h @@ -269,6 +269,29 @@ class GTSAM_EXPORT ISAM2 : public BayesTree { /// @} protected: + /** + * Add new variables to the ISAM2 system. + * @param newTheta Initial values for new variables + * @param theta Current solution to be augmented with new initialization + * @param delta Current linear delta to be augmented with zeros + * @param deltaNewton + * @param RgProd + * @param keyFormatter Formatter for printing nonlinear keys during debugging + */ + void addVariables(const Values& newTheta); + + /** + * Remove variables from the ISAM2 system. + */ + void removeVariables(const KeySet& unusedKeys); + + /** + * Apply expmap to the given values, but only for indices appearing in + * \c mask. Values are expmapped in-place. + * \param mask Mask on linear indices, only \c true entries are expmapped + */ + void expmapMasked(const KeySet& mask); + FastSet getAffectedFactors(const FastList& keys) const; GaussianFactorGraph::shared_ptr relinearizeAffectedFactors( const FastList& affectedKeys, const KeySet& relinKeys) const; @@ -279,6 +302,7 @@ class GTSAM_EXPORT ISAM2 : public BayesTree { const std::vector& observedKeys, const KeySet& unusedIndices, const boost::optional >& constrainKeys, ISAM2Result& result); + void updateDelta(bool forceFullSolve = false) const; }; // ISAM2 diff --git a/gtsam/nonlinear/ISAM2Clique.cpp b/gtsam/nonlinear/ISAM2Clique.cpp index 6e03b0680..c55ca7959 100644 --- a/gtsam/nonlinear/ISAM2Clique.cpp +++ b/gtsam/nonlinear/ISAM2Clique.cpp @@ -15,10 +15,10 @@ * @author Michael Kaess, Richard Roberts, Frank Dellaert */ -#include +#include #include #include -#include +#include #include using namespace std; @@ -299,4 +299,27 @@ size_t ISAM2Clique::calculate_nnz() const { return result; } +/* ************************************************************************* */ +void ISAM2Clique::findAll(const KeySet& markedMask, KeySet* keys) const { + static const bool debug = false; + // does the separator contain any of the variables? + bool found = false; + for (Key key : conditional()->parents()) { + if (markedMask.exists(key)) { + found = true; + break; + } + } + if (found) { + // then add this clique + keys->insert(conditional()->beginFrontals(), conditional()->endFrontals()); + if (debug) print("Key(s) marked in clique "); + if (debug) cout << "so marking key " << conditional()->front() << endl; + } + for (const auto& child : children) { + child->findAll(markedMask, keys); + } +} + +/* ************************************************************************* */ } // namespace gtsam diff --git a/gtsam/nonlinear/ISAM2Clique.h b/gtsam/nonlinear/ISAM2Clique.h index fecc8d335..3c53e3d72 100644 --- a/gtsam/nonlinear/ISAM2Clique.h +++ b/gtsam/nonlinear/ISAM2Clique.h @@ -19,11 +19,11 @@ #pragma once +#include +#include #include #include #include -#include -#include #include namespace gtsam { @@ -99,6 +99,23 @@ class GTSAM_EXPORT ISAM2Clique void nnz_internal(size_t* result) const; size_t calculate_nnz() const; + /** + * Recursively search this clique and its children for marked keys appearing + * in the separator, and add the *frontal* keys of any cliques whose + * separator contains any marked keys to the set \c keys. The purpose of + * this is to discover the cliques that need to be redone due to information + * propagating to them from cliques that directly contain factors being + * relinearized. + * + * The original comment says this finds all variables directly connected to + * the marked ones by measurements. Is this true, because it seems like this + * would also pull in variables indirectly connected through other frontal or + * separator variables? + * + * Alternatively could we trace up towards the root for each variable here? + */ + void findAll(const KeySet& markedMask, KeySet* keys) const; + private: /** * Check if clique was replaced, or if any parents were changed above the diff --git a/tests/testGaussianISAM2.cpp b/tests/testGaussianISAM2.cpp index 681c30587..d6ca896dd 100644 --- a/tests/testGaussianISAM2.cpp +++ b/tests/testGaussianISAM2.cpp @@ -304,7 +304,7 @@ TEST(ISAM2, AddFactorsStep1) FactorIndices actualNewFactorIndices; - ISAM2::Impl::AddFactorsStep1(newFactors, true, nonlinearFactors, actualNewFactorIndices); + ISAM2::Impl::AddFactorsStep1(newFactors, true, &nonlinearFactors, &actualNewFactorIndices); EXPECT(assert_equal(expectedNonlinearFactors, nonlinearFactors)); EXPECT(assert_container_equality(expectedNewFactorIndices, actualNewFactorIndices));