From bf904f9ff88b754015d7a8f1252738f5a97cbd69 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Fri, 31 May 2019 00:40:01 -0400 Subject: [PATCH 01/21] moved gradientAtZero helper to clique class, where it belongs --- gtsam/nonlinear/ISAM2.cpp | 29 ++++------------------------- gtsam/nonlinear/ISAM2Clique.cpp | 28 +++++++++++++++++++++++++--- gtsam/nonlinear/ISAM2Clique.h | 5 ++++- 3 files changed, 33 insertions(+), 29 deletions(-) diff --git a/gtsam/nonlinear/ISAM2.cpp b/gtsam/nonlinear/ISAM2.cpp index c01d1c536..988e6779f 100644 --- a/gtsam/nonlinear/ISAM2.cpp +++ b/gtsam/nonlinear/ISAM2.cpp @@ -1113,6 +1113,8 @@ void ISAM2::updateDelta(bool forceFullSolve) const { doglegResult .dx_d; // Copy the VectorValues containing with the linear solution gttoc(Copy_dx_d); + } else { + throw std::runtime_error("iSAM2: unknown ISAM2Params type"); } } @@ -1155,37 +1157,14 @@ double ISAM2::error(const VectorValues& x) const { return GaussianFactorGraph(*this).error(x); } -/* ************************************************************************* */ -static void gradientAtZeroTreeAdder(const boost::shared_ptr& root, - VectorValues* g) { - // Loop through variables in each clique, adding contributions - DenseIndex variablePosition = 0; - for (GaussianConditional::const_iterator jit = root->conditional()->begin(); - jit != root->conditional()->end(); ++jit) { - const DenseIndex dim = root->conditional()->getDim(jit); - pair pos_ins = g->tryInsert( - *jit, root->gradientContribution().segment(variablePosition, dim)); - if (!pos_ins.second) - pos_ins.first->second += - root->gradientContribution().segment(variablePosition, dim); - variablePosition += dim; - } - - // Recursively add contributions from children - typedef boost::shared_ptr sharedClique; - for (const sharedClique& child : root->children) { - gradientAtZeroTreeAdder(child, g); - } -} - /* ************************************************************************* */ VectorValues ISAM2::gradientAtZero() const { // Create result VectorValues g; // Sum up contributions for each clique - for (const ISAM2::sharedClique& root : this->roots()) - gradientAtZeroTreeAdder(root, &g); + for (const auto& root : this->roots()) + root->addGradientAtZero(&g); return g; } diff --git a/gtsam/nonlinear/ISAM2Clique.cpp b/gtsam/nonlinear/ISAM2Clique.cpp index 96212c923..f7a1c14a0 100644 --- a/gtsam/nonlinear/ISAM2Clique.cpp +++ b/gtsam/nonlinear/ISAM2Clique.cpp @@ -19,7 +19,9 @@ #include #include #include + #include +#include using namespace std; @@ -304,7 +306,7 @@ 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()) { + for (Key key : conditional_->parents()) { if (markedMask.exists(key)) { found = true; break; @@ -312,14 +314,34 @@ void ISAM2Clique::findAll(const KeySet& markedMask, KeySet* keys) const { } if (found) { // then add this clique - keys->insert(conditional()->beginFrontals(), conditional()->endFrontals()); + keys->insert(conditional_->beginFrontals(), conditional_->endFrontals()); if (debug) print("Key(s) marked in clique "); - if (debug) cout << "so marking key " << conditional()->front() << endl; + if (debug) cout << "so marking key " << conditional_->front() << endl; } for (const auto& child : children) { child->findAll(markedMask, keys); } } +/* ************************************************************************* */ +void ISAM2Clique::addGradientAtZero(VectorValues* g) const { + // Loop through variables in each clique, adding contributions + DenseIndex position = 0; + for (auto it = conditional_->begin(); it != conditional_->end(); ++it) { + const DenseIndex dim = conditional_->getDim(it); + const Vector contribution = gradientContribution_.segment(position, dim); + VectorValues::iterator values_it; + bool success; + std::tie(values_it, success) = g->tryInsert(*it, contribution); + if (!success) values_it->second += contribution; + position += dim; + } + + // Recursively add contributions from children + for (const auto& child : children) { + child->addGradientAtZero(g); + } +} + /* ************************************************************************* */ } // namespace gtsam diff --git a/gtsam/nonlinear/ISAM2Clique.h b/gtsam/nonlinear/ISAM2Clique.h index 3c53e3d72..53bdaec81 100644 --- a/gtsam/nonlinear/ISAM2Clique.h +++ b/gtsam/nonlinear/ISAM2Clique.h @@ -75,9 +75,12 @@ class GTSAM_EXPORT ISAM2Clique /** Access the cached factor */ Base::FactorType::shared_ptr& cachedFactor() { return cachedFactor_; } - /** Access the gradient contribution */ + /// Access the gradient contribution const Vector& gradientContribution() const { return gradientContribution_; } + /// Recursively add gradient at zero to g + void addGradientAtZero(VectorValues* g) const; + bool equals(const This& other, double tol = 1e-9) const; /** print this node */ From 0a95ac292f31998da53962e35c33c5da514c57ae Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Fri, 31 May 2019 13:42:32 -0400 Subject: [PATCH 02/21] Removed useless typedef, add empty() --- gtsam/inference/EliminationTree-inst.h | 2 +- gtsam/inference/Ordering.cpp | 2 +- gtsam/inference/VariableIndex-inl.h | 4 +-- gtsam/inference/VariableIndex.h | 43 ++++++++++++++------------ gtsam_unstable/discrete/CSP.cpp | 2 +- 5 files changed, 29 insertions(+), 24 deletions(-) diff --git a/gtsam/inference/EliminationTree-inst.h b/gtsam/inference/EliminationTree-inst.h index 33ba6e18d..3390a3aac 100644 --- a/gtsam/inference/EliminationTree-inst.h +++ b/gtsam/inference/EliminationTree-inst.h @@ -98,7 +98,7 @@ namespace gtsam { for (size_t j = 0; j < n; j++) { // Retrieve the factors involving this variable and create the current node - const VariableIndex::Factors& factors = structure[order[j]]; + const FactorIndices& factors = structure[order[j]]; const sharedNode node = boost::make_shared(); node->key = order[j]; diff --git a/gtsam/inference/Ordering.cpp b/gtsam/inference/Ordering.cpp index 1165b4a0f..da61ca57e 100644 --- a/gtsam/inference/Ordering.cpp +++ b/gtsam/inference/Ordering.cpp @@ -79,7 +79,7 @@ Ordering Ordering::ColamdConstrained(const VariableIndex& variableIndex, size_t index = 0; for (auto key_factors: variableIndex) { // Arrange factor indices into COLAMD format - const VariableIndex::Factors& column = key_factors.second; + const FactorIndices& column = key_factors.second; for(size_t factorIndex: column) { A[count++] = (int) factorIndex; // copy sparse column } diff --git a/gtsam/inference/VariableIndex-inl.h b/gtsam/inference/VariableIndex-inl.h index bc8100e4a..727ef8fd8 100644 --- a/gtsam/inference/VariableIndex-inl.h +++ b/gtsam/inference/VariableIndex-inl.h @@ -67,8 +67,8 @@ void VariableIndex::remove(ITERATOR firstFactor, ITERATOR lastFactor, "Internal error, requested inconsistent number of factor indices and factors in VariableIndex::remove"); if (factors[i]) { for(Key j: *factors[i]) { - Factors& factorEntries = internalAt(j); - Factors::iterator entry = std::find(factorEntries.begin(), + FactorIndices& factorEntries = internalAt(j); + auto entry = std::find(factorEntries.begin(), factorEntries.end(), *factorIndex); if (entry == factorEntries.end()) throw std::invalid_argument( diff --git a/gtsam/inference/VariableIndex.h b/gtsam/inference/VariableIndex.h index a96a53289..0d7320428 100644 --- a/gtsam/inference/VariableIndex.h +++ b/gtsam/inference/VariableIndex.h @@ -41,26 +41,22 @@ namespace gtsam { * \nosubgrouping */ class GTSAM_EXPORT VariableIndex { -public: - + public: typedef boost::shared_ptr shared_ptr; - typedef FactorIndices Factors; - typedef Factors::iterator Factor_iterator; - typedef Factors::const_iterator Factor_const_iterator; + typedef FactorIndices::iterator Factor_iterator; + typedef FactorIndices::const_iterator Factor_const_iterator; -protected: - typedef FastMap KeyMap; + protected: + typedef FastMap KeyMap; KeyMap index_; - size_t nFactors_; // Number of factors in the original factor graph. - size_t nEntries_; // Sum of involved variable counts of each factor. + size_t nFactors_; // Number of factors in the original factor graph. + size_t nEntries_; // Sum of involved variable counts of each factor. -public: + public: typedef KeyMap::const_iterator const_iterator; typedef KeyMap::const_iterator iterator; typedef KeyMap::value_type value_type; -public: - /// @name Standard Constructors /// @{ @@ -71,8 +67,10 @@ public: * Create a VariableIndex that computes and stores the block column structure * of a factor graph. */ - template - VariableIndex(const FG& factorGraph) : nFactors_(0), nEntries_(0) { augment(factorGraph); } + template + explicit VariableIndex(const FG& factorGraph) : nFactors_(0), nEntries_(0) { + augment(factorGraph); + } /// @} /// @name Standard Interface @@ -88,7 +86,7 @@ public: size_t nEntries() const { return nEntries_; } /// Access a list of factors by variable - const Factors& operator[](Key variable) const { + const FactorIndices& operator[](Key variable) const { KeyMap::const_iterator item = index_.find(variable); if(item == index_.end()) throw std::invalid_argument("Requested non-existent variable from VariableIndex"); @@ -96,6 +94,11 @@ public: return item->second; } + /// Return true if no factors associated with a variable + const bool empty(Key variable) const { + return (*this)[variable].empty(); + } + /// @} /// @name Testable /// @{ @@ -166,16 +169,18 @@ protected: Factor_const_iterator factorsEnd(Key variable) const { return internalAt(variable).end(); } /// Internal version of 'at' that asserts existence - const Factors& internalAt(Key variable) const { + const FactorIndices& internalAt(Key variable) const { const KeyMap::const_iterator item = index_.find(variable); assert(item != index_.end()); - return item->second; } + return item->second; + } /// Internal version of 'at' that asserts existence - Factors& internalAt(Key variable) { + FactorIndices& internalAt(Key variable) { const KeyMap::iterator item = index_.find(variable); assert(item != index_.end()); - return item->second; } + return item->second; + } /// @} }; diff --git a/gtsam_unstable/discrete/CSP.cpp b/gtsam_unstable/discrete/CSP.cpp index cf5abdcb1..0223250b5 100644 --- a/gtsam_unstable/discrete/CSP.cpp +++ b/gtsam_unstable/discrete/CSP.cpp @@ -43,7 +43,7 @@ namespace gtsam { // keep track of which domains changed changed[v] = false; // loop over all factors/constraints for variable v - const VariableIndex::Factors& factors = index[v]; + const FactorIndices& factors = index[v]; for(size_t f: factors) { // if not already a singleton if (!domains[v].isSingleton()) { From 26f96f70919b8465576b8c8a144580f593240f74 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Fri, 31 May 2019 17:31:59 -0400 Subject: [PATCH 03/21] added timing --- gtsam/inference/BayesTree-inst.h | 1 + 1 file changed, 1 insertion(+) diff --git a/gtsam/inference/BayesTree-inst.h b/gtsam/inference/BayesTree-inst.h index 9935776a5..16a11cc34 100644 --- a/gtsam/inference/BayesTree-inst.h +++ b/gtsam/inference/BayesTree-inst.h @@ -462,6 +462,7 @@ namespace gtsam { template void BayesTree::removeTop(const KeyVector& keys, BayesNetType& bn, Cliques& orphans) { + gttic(removetop); // process each key of the new factor for(const Key& j: keys) { From 5f8a00fa53d5b3bef521d9bcee9e305347b6a597 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Fri, 31 May 2019 17:32:16 -0400 Subject: [PATCH 04/21] made Roots type public --- gtsam/inference/BayesTree.h | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/gtsam/inference/BayesTree.h b/gtsam/inference/BayesTree.h index 892ac5f31..c221a5ed6 100644 --- a/gtsam/inference/BayesTree.h +++ b/gtsam/inference/BayesTree.h @@ -89,14 +89,14 @@ namespace gtsam { /** Map from keys to Clique */ typedef ConcurrentMap Nodes; + /** Root cliques */ + typedef FastVector Roots; + protected: /** Map from indices to Clique */ Nodes nodes_; - /** Root cliques */ - typedef FastVector Roots; - /** Root cliques */ Roots roots_; From 1a29ab5533a18066a7ce12549a40a8bcf8609f52 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Fri, 31 May 2019 17:33:16 -0400 Subject: [PATCH 05/21] Refactored iSAM2::update to reveal what is changed when --- gtsam/nonlinear/ISAM2-impl.cpp | 150 +---- gtsam/nonlinear/ISAM2-impl.h | 843 ++++++++++++++++++++++++++-- gtsam/nonlinear/ISAM2.cpp | 752 ++----------------------- gtsam/nonlinear/ISAM2.h | 55 +- gtsam/nonlinear/ISAM2Result.h | 23 +- gtsam/nonlinear/ISAM2UpdateParams.h | 16 +- tests/testGaussianISAM2.cpp | 13 +- 7 files changed, 915 insertions(+), 937 deletions(-) diff --git a/gtsam/nonlinear/ISAM2-impl.cpp b/gtsam/nonlinear/ISAM2-impl.cpp index 11be14c44..a809cbc08 100644 --- a/gtsam/nonlinear/ISAM2-impl.cpp +++ b/gtsam/nonlinear/ISAM2-impl.cpp @@ -31,150 +31,6 @@ using namespace std; namespace gtsam { -/* ************************************************************************* */ -void ISAM2::Impl::AddFactorsStep1(const NonlinearFactorGraph& newFactors, - bool useUnusedSlots, - NonlinearFactorGraph* nonlinearFactors, - FactorIndices* newFactorIndices) { - newFactorIndices->resize(newFactors.size()); - - if (useUnusedSlots) { - size_t globalFactorIndex = 0; - for (size_t newFactorIndex = 0; newFactorIndex < newFactors.size(); - ++newFactorIndex) { - // Loop to find the next available factor slot - do { - // If we need to add more factors than we have room for, resize - // nonlinearFactors, filling the new slots with NULL factors. Otherwise, - // check if the current factor in nonlinearFactors is already used, and - // 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]) - ++globalFactorIndex; - else - break; - } while (true); - - // Use the current slot, updating nonlinearFactors and newFactorSlots. - (*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); - } -} - -/* ************************************************************************* */ -KeySet ISAM2::Impl::CheckRelinearizationFull( - const VectorValues& delta, - const ISAM2Params::RelinearizationThreshold& relinearizeThreshold) { - KeySet relinKeys; - - if (const double* threshold = boost::get(&relinearizeThreshold)) { - for (const VectorValues::KeyValuePair& key_delta : delta) { - double maxDelta = key_delta.second.lpNorm(); - if (maxDelta >= *threshold) relinKeys.insert(key_delta.first); - } - } else if (const FastMap* thresholds = - boost::get >(&relinearizeThreshold)) { - for (const VectorValues::KeyValuePair& key_delta : delta) { - const Vector& threshold = - thresholds->find(Symbol(key_delta.first).chr())->second; - if (threshold.rows() != key_delta.second.rows()) - throw std::invalid_argument( - "Relinearization threshold vector dimensionality for '" + - std::string(1, Symbol(key_delta.first).chr()) + - "' passed into iSAM2 parameters does not match actual variable " - "dimensionality."); - if ((key_delta.second.array().abs() > threshold.array()).any()) - relinKeys.insert(key_delta.first); - } - } - - return relinKeys; -} - -/* ************************************************************************* */ -static void CheckRelinearizationRecursiveDouble( - double threshold, const VectorValues& delta, - const ISAM2::sharedClique& clique, KeySet* relinKeys) { - // Check the current clique for relinearization - bool relinearize = false; - for (Key var : *clique->conditional()) { - double maxDelta = delta[var].lpNorm(); - if (maxDelta >= threshold) { - relinKeys->insert(var); - relinearize = true; - } - } - - // If this node was relinearized, also check its children - if (relinearize) { - for (const ISAM2::sharedClique& child : clique->children) { - CheckRelinearizationRecursiveDouble(threshold, delta, child, relinKeys); - } - } -} - -/* ************************************************************************* */ -static void CheckRelinearizationRecursiveMap( - const FastMap& thresholds, const VectorValues& delta, - const ISAM2::sharedClique& clique, KeySet* relinKeys) { - // Check the current clique for relinearization - bool relinearize = false; - for (Key var : *clique->conditional()) { - // Find the threshold for this variable type - const Vector& threshold = thresholds.find(Symbol(var).chr())->second; - - const Vector& deltaVar = delta[var]; - - // Verify the threshold vector matches the actual variable size - if (threshold.rows() != deltaVar.rows()) - throw std::invalid_argument( - "Relinearization threshold vector dimensionality for '" + - std::string(1, Symbol(var).chr()) + - "' passed into iSAM2 parameters does not match actual variable " - "dimensionality."); - - // Check for relinearization - if ((deltaVar.array().abs() > threshold.array()).any()) { - relinKeys->insert(var); - relinearize = true; - } - } - - // If this node was relinearized, also check its children - if (relinearize) { - for (const ISAM2::sharedClique& child : clique->children) { - CheckRelinearizationRecursiveMap(thresholds, delta, child, relinKeys); - } - } -} - -/* ************************************************************************* */ -KeySet ISAM2::Impl::CheckRelinearizationPartial( - const ISAM2::Roots& roots, const VectorValues& delta, - const ISAM2Params::RelinearizationThreshold& relinearizeThreshold) { - KeySet relinKeys; - for (const ISAM2::sharedClique& root : roots) { - if (relinearizeThreshold.type() == typeid(double)) - CheckRelinearizationRecursiveDouble( - boost::get(relinearizeThreshold), delta, root, &relinKeys); - else if (relinearizeThreshold.type() == typeid(FastMap)) - CheckRelinearizationRecursiveMap( - boost::get >(relinearizeThreshold), delta, root, - &relinKeys); - } - return relinKeys; -} - /* ************************************************************************* */ namespace internal { inline static void optimizeInPlace(const ISAM2::sharedClique& clique, @@ -189,7 +45,7 @@ inline static void optimizeInPlace(const ISAM2::sharedClique& clique, } // namespace internal /* ************************************************************************* */ -size_t ISAM2::Impl::UpdateGaussNewtonDelta(const ISAM2::Roots& roots, +size_t DeltaImpl::UpdateGaussNewtonDelta(const ISAM2::Roots& roots, const KeySet& replacedKeys, double wildfireThreshold, VectorValues* delta) { @@ -272,7 +128,7 @@ void updateRgProd(const ISAM2::sharedClique& clique, const KeySet& replacedKeys, } // namespace internal /* ************************************************************************* */ -size_t ISAM2::Impl::UpdateRgProd(const ISAM2::Roots& roots, +size_t DeltaImpl::UpdateRgProd(const ISAM2::Roots& roots, const KeySet& replacedKeys, const VectorValues& gradAtZero, VectorValues* RgProd) { @@ -287,7 +143,7 @@ size_t ISAM2::Impl::UpdateRgProd(const ISAM2::Roots& roots, } /* ************************************************************************* */ -VectorValues ISAM2::Impl::ComputeGradientSearch(const VectorValues& gradAtZero, +VectorValues DeltaImpl::ComputeGradientSearch(const VectorValues& gradAtZero, const VectorValues& RgProd) { // Compute gradient squared-magnitude const double gradientSqNorm = gradAtZero.dot(gradAtZero); diff --git a/gtsam/nonlinear/ISAM2-impl.h b/gtsam/nonlinear/ISAM2-impl.h index 8a30fb8cd..f4dfcb99b 100644 --- a/gtsam/nonlinear/ISAM2-impl.h +++ b/gtsam/nonlinear/ISAM2-impl.h @@ -11,18 +11,65 @@ /** * @file ISAM2-impl.h - * @brief Incremental update functionality (ISAM2) for BayesTree, with fluid relinearization. - * @author Michael Kaess, Richard Roberts + * @brief Incremental update functionality (ISAM2) for BayesTree, with fluid + * relinearization. + * @author Michael Kaess, Richard Roberts, Frank Dellaert */ #pragma once -#include #include +#include + +#include +#include // We need the inst file because we'll make a special JT templated on ISAM2 +#include +#include +#include +#include + +#include +#include +namespace br { +using namespace boost::range; +using namespace boost::adaptors; +} // namespace br + +#include +#include +#include +#include namespace gtsam { -struct GTSAM_EXPORT ISAM2::Impl { +/* ************************************************************************* */ +// Special BayesTree class that uses ISAM2 cliques - this is the result of +// reeliminating ISAM2 subtrees. +class ISAM2BayesTree : public ISAM2::Base { + public: + typedef ISAM2::Base Base; + typedef ISAM2BayesTree This; + typedef boost::shared_ptr shared_ptr; + + ISAM2BayesTree() {} +}; + +/* ************************************************************************* */ +// Special JunctionTree class that produces ISAM2 BayesTree cliques, used for +// reeliminating ISAM2 subtrees. +class ISAM2JunctionTree + : public JunctionTree { + public: + typedef JunctionTree Base; + typedef ISAM2JunctionTree This; + typedef boost::shared_ptr shared_ptr; + + explicit ISAM2JunctionTree(const GaussianEliminationTree& eliminationTree) + : Base(eliminationTree) {} +}; + +/* ************************************************************************* */ +struct GTSAM_EXPORT DeltaImpl { struct GTSAM_EXPORT PartialSolveResult { ISAM2::sharedClique bayesTree; }; @@ -34,57 +81,771 @@ struct GTSAM_EXPORT ISAM2::Impl { boost::optional > constrainedKeys; }; - /// 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); - - /** - * Find the set of variables to be relinearized according to relinearizeThreshold. - * Any variables in the VectorValues delta whose vector magnitude is greater than - * or equal to relinearizeThreshold are returned. - * @param delta The linear delta to check against the threshold - * @param keyFormatter Formatter for printing nonlinear keys during debugging - * @return The set of variable indices in delta whose magnitude is greater than or - * equal to relinearizeThreshold - */ - static KeySet CheckRelinearizationFull(const VectorValues& delta, - const ISAM2Params::RelinearizationThreshold& relinearizeThreshold); - - /** - * Find the set of variables to be relinearized according to relinearizeThreshold. - * This check is performed recursively, starting at the top of the tree. Once a - * variable in the tree does not need to be relinearized, no further checks in - * that branch are performed. This is an approximation of the Full version, designed - * to save time at the expense of accuracy. - * @param delta The linear delta to check against the threshold - * @param keyFormatter Formatter for printing nonlinear keys during debugging - * @return The set of variable indices in delta whose magnitude is greater than or - * equal to relinearizeThreshold - */ - static KeySet CheckRelinearizationPartial(const ISAM2::Roots& roots, - const VectorValues& delta, const ISAM2Params::RelinearizationThreshold& relinearizeThreshold); - /** * Update the Newton's method step point, using wildfire */ static size_t UpdateGaussNewtonDelta(const ISAM2::Roots& roots, - const KeySet& replacedKeys, double wildfireThreshold, VectorValues* delta); + const KeySet& replacedKeys, + double wildfireThreshold, + VectorValues* delta); /** * Update the RgProd (R*g) incrementally taking into account which variables * have been recalculated in \c replacedKeys. Only used in Dogleg. */ - static size_t UpdateRgProd(const ISAM2::Roots& roots, const KeySet& replacedKeys, - const VectorValues& gradAtZero, VectorValues* RgProd); + 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. */ static VectorValues ComputeGradientSearch(const VectorValues& gradAtZero, const VectorValues& RgProd); - }; -} +/* ************************************************************************* */ +/** + * Implementation functions for update method + * All of the methods below have clear inputs and outputs, even if not + * functional: iSAM2 is inherintly imperative. + */ +struct GTSAM_EXPORT UpdateImpl { + const ISAM2Params& params_; + const ISAM2UpdateParams& updateParams_; + UpdateImpl(const ISAM2Params& params, const ISAM2UpdateParams& updateParams) + : params_(params), updateParams_(updateParams) {} + + /// 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) { + newFactorIndices->resize(newFactors.size()); + + if (useUnusedSlots) { + size_t globalFactorIndex = 0; + for (size_t newFactorIndex = 0; newFactorIndex < newFactors.size(); + ++newFactorIndex) { + // Loop to find the next available factor slot + do { + // If we need to add more factors than we have room for, resize + // nonlinearFactors, filling the new slots with NULL factors. + // Otherwise, check if the current factor in nonlinearFactors is + // already used, and 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]) + ++globalFactorIndex; + else + break; + } while (true); + + // Use the current slot, updating nonlinearFactors and newFactorSlots. + (*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); + } + } + + // 1. Add any new factors \Factors:=\Factors\cup\Factors'. + void pushBackFactors(const NonlinearFactorGraph& newFactors, + NonlinearFactorGraph* nonlinearFactors, + GaussianFactorGraph* linearFactors, + VariableIndex* variableIndex, + ISAM2Result* result) const { + gttic(pushBackFactors); + const bool debug = ISDEBUG("ISAM2 update"); + const bool verbose = ISDEBUG("ISAM2 update verbose"); + + // Add the new factor indices to the result struct + if (debug || verbose) newFactors.print("The new factors are: "); + AddFactorsStep1(newFactors, params_.findUnusedFactorSlots, nonlinearFactors, + &result->newFactorsIndices); + + // Remove the removed factors + NonlinearFactorGraph removedFactors; + removedFactors.reserve(updateParams_.removeFactorIndices.size()); + for (const auto index : updateParams_.removeFactorIndices) { + removedFactors.push_back(nonlinearFactors->at(index)); + nonlinearFactors->remove(index); + if (params_.cacheLinearizedFactors) linearFactors->remove(index); + } + + // Remove removed factors from the variable index so we do not attempt to + // relinearize them + variableIndex->remove(updateParams_.removeFactorIndices.begin(), + updateParams_.removeFactorIndices.end(), + removedFactors); + result->keysWithRemovedFactors = removedFactors.keys(); + + // Compute unused keys and indices + // Get keys from removed factors and new factors, and compute unused keys, + // i.e., keys that are empty now and do not appear in the new factors. + KeySet removedAndEmpty; + for (Key key : result->keysWithRemovedFactors) { + if (variableIndex->empty(key)) + removedAndEmpty.insert(removedAndEmpty.end(), key); + } + KeySet newFactorSymbKeys = newFactors.keys(); + std::set_difference( + removedAndEmpty.begin(), removedAndEmpty.end(), + newFactorSymbKeys.begin(), newFactorSymbKeys.end(), + std::inserter(result->unusedKeys, result->unusedKeys.end())); + + // Get indices for unused keys + for (Key key : result->unusedKeys) { + result->unusedIndices.insert(result->unusedIndices.end(), key); + } + } + + // 3. Mark linear update + void gatherInvolvedKeys(const NonlinearFactorGraph& newFactors, + const NonlinearFactorGraph& nonlinearFactors, + ISAM2Result* result) const { + gttic(gatherInvolvedKeys); + result->markedKeys = newFactors.keys(); // Get keys from new factors + // Also mark keys involved in removed factors + result->markedKeys.insert(result->keysWithRemovedFactors.begin(), + result->keysWithRemovedFactors.end()); + + // Also mark any provided extra re-eliminate keys + if (updateParams_.extraReelimKeys) { + for (Key key : *updateParams_.extraReelimKeys) { + result->markedKeys.insert(key); + } + } + // Also, keys that were not observed in existing factors, but whose affected + // keys have been extended now (e.g. smart factors) + if (updateParams_.newAffectedKeys) { + for (const auto& factorAddedKeys : *updateParams_.newAffectedKeys) { + const auto factorIdx = factorAddedKeys.first; + const auto& affectedKeys = nonlinearFactors.at(factorIdx)->keys(); + result->markedKeys.insert(affectedKeys.begin(), affectedKeys.end()); + } + } + + // Observed keys for detailed results + if (params_.enableDetailedResults) { + for (Key key : result->markedKeys) { + result->detail->variableStatus[key].isObserved = true; + } + } + + for (Key index : result->markedKeys) { + // Only add if not unused + if (result->unusedIndices.find(index) == result->unusedIndices.end()) + // Make a copy of these, as we'll soon add to them + result->observedKeys.push_back(index); + } + } + + static void CheckRelinearizationRecursiveMap( + const FastMap& thresholds, const VectorValues& delta, + const ISAM2::sharedClique& clique, KeySet* relinKeys) { + // Check the current clique for relinearization + bool relinearize = false; + for (Key var : *clique->conditional()) { + // Find the threshold for this variable type + const Vector& threshold = thresholds.find(Symbol(var).chr())->second; + + const Vector& deltaVar = delta[var]; + + // Verify the threshold vector matches the actual variable size + if (threshold.rows() != deltaVar.rows()) + throw std::invalid_argument( + "Relinearization threshold vector dimensionality for '" + + std::string(1, Symbol(var).chr()) + + "' passed into iSAM2 parameters does not match actual variable " + "dimensionality."); + + // Check for relinearization + if ((deltaVar.array().abs() > threshold.array()).any()) { + relinKeys->insert(var); + relinearize = true; + } + } + + // If this node was relinearized, also check its children + if (relinearize) { + for (const ISAM2::sharedClique& child : clique->children) { + CheckRelinearizationRecursiveMap(thresholds, delta, child, relinKeys); + } + } + } + + static void CheckRelinearizationRecursiveDouble( + double threshold, const VectorValues& delta, + const ISAM2::sharedClique& clique, KeySet* relinKeys) { + // Check the current clique for relinearization + bool relinearize = false; + for (Key var : *clique->conditional()) { + double maxDelta = delta[var].lpNorm(); + if (maxDelta >= threshold) { + relinKeys->insert(var); + relinearize = true; + } + } + + // If this node was relinearized, also check its children + if (relinearize) { + for (const ISAM2::sharedClique& child : clique->children) { + CheckRelinearizationRecursiveDouble(threshold, delta, child, relinKeys); + } + } + } + + /** + * Find the set of variables to be relinearized according to + * relinearizeThreshold. This check is performed recursively, starting at the + * top of the tree. Once a variable in the tree does not need to be + * relinearized, no further checks in that branch are performed. This is an + * approximation of the Full version, designed to save time at the expense of + * accuracy. + * @param delta The linear delta to check against the threshold + * @param keyFormatter Formatter for printing nonlinear keys during debugging + * @return The set of variable indices in delta whose magnitude is greater + * than or equal to relinearizeThreshold + */ + static KeySet CheckRelinearizationPartial( + const ISAM2::Roots& roots, const VectorValues& delta, + const ISAM2Params::RelinearizationThreshold& relinearizeThreshold) { + KeySet relinKeys; + for (const ISAM2::sharedClique& root : roots) { + if (relinearizeThreshold.type() == typeid(double)) + CheckRelinearizationRecursiveDouble( + boost::get(relinearizeThreshold), delta, root, &relinKeys); + else if (relinearizeThreshold.type() == typeid(FastMap)) + CheckRelinearizationRecursiveMap( + boost::get >(relinearizeThreshold), delta, + root, &relinKeys); + } + return relinKeys; + } + + /** + * Find the set of variables to be relinearized according to + * relinearizeThreshold. Any variables in the VectorValues delta whose vector + * magnitude is greater than or equal to relinearizeThreshold are returned. + * @param delta The linear delta to check against the threshold + * @param keyFormatter Formatter for printing nonlinear keys during debugging + * @return The set of variable indices in delta whose magnitude is greater + * than or equal to relinearizeThreshold + */ + static KeySet CheckRelinearizationFull( + const VectorValues& delta, + const ISAM2Params::RelinearizationThreshold& relinearizeThreshold) { + KeySet relinKeys; + + if (const double* threshold = boost::get(&relinearizeThreshold)) { + for (const VectorValues::KeyValuePair& key_delta : delta) { + double maxDelta = key_delta.second.lpNorm(); + if (maxDelta >= *threshold) relinKeys.insert(key_delta.first); + } + } else if (const FastMap* thresholds = + boost::get >(&relinearizeThreshold)) { + for (const VectorValues::KeyValuePair& key_delta : delta) { + const Vector& threshold = + thresholds->find(Symbol(key_delta.first).chr())->second; + if (threshold.rows() != key_delta.second.rows()) + throw std::invalid_argument( + "Relinearization threshold vector dimensionality for '" + + std::string(1, Symbol(key_delta.first).chr()) + + "' passed into iSAM2 parameters does not match actual variable " + "dimensionality."); + if ((key_delta.second.array().abs() > threshold.array()).any()) + relinKeys.insert(key_delta.first); + } + } + + return relinKeys; + } + + /** + * 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 VectorValues& delta, const KeySet& mask, + Values* theta) const { + 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_(); + } + } + } + + KeySet relinearize(const ISAM2::Roots& roots, const VectorValues& delta, + const KeySet& fixedVariables, Values* theta, + ISAM2Result* result) const { + KeySet relinKeys; + gttic(gather_relinearize_keys); + // 4. Mark keys in \Delta above threshold \beta: + // J=\{\Delta_{j}\in\Delta|\Delta_{j}\geq\beta\}. + if (params_.enablePartialRelinearizationCheck) + relinKeys = CheckRelinearizationPartial(roots, delta, + params_.relinearizeThreshold); + else + relinKeys = CheckRelinearizationFull(delta, params_.relinearizeThreshold); + if (updateParams_.forceFullSolve) + relinKeys = + CheckRelinearizationFull(delta, 0.0); // This is used for debugging + + // Remove from relinKeys any keys whose linearization points are fixed + for (Key key : fixedVariables) { + relinKeys.erase(key); + } + if (updateParams_.noRelinKeys) { + for (Key key : *updateParams_.noRelinKeys) { + relinKeys.erase(key); + } + } + + // Above relin threshold keys for detailed results + if (params_.enableDetailedResults) { + for (Key key : relinKeys) { + result->detail->variableStatus[key].isAboveRelinThreshold = true; + result->detail->variableStatus[key].isRelinearized = true; + } + } + + // Add the variables being relinearized to the marked keys + KeySet markedRelinMask; + for (const Key key : relinKeys) markedRelinMask.insert(key); + result->markedKeys.insert(relinKeys.begin(), relinKeys.end()); + gttoc(gather_relinearize_keys); + + gttic(fluid_find_all); + // 5. Mark all cliques that involve marked variables \Theta_{J} and all + // their ancestors. + if (!relinKeys.empty()) { + for (const auto& root : roots) + // add other cliques that have the marked ones in the separator + root->findAll(markedRelinMask, &result->markedKeys); + + // Relin involved keys for detailed results + if (params_.enableDetailedResults) { + KeySet involvedRelinKeys; + for (const auto& root : roots) + root->findAll(markedRelinMask, &involvedRelinKeys); + for (Key key : involvedRelinKeys) { + if (!result->detail->variableStatus[key].isAboveRelinThreshold) { + result->detail->variableStatus[key].isRelinearizeInvolved = true; + result->detail->variableStatus[key].isRelinearized = true; + } + } + } + } + gttoc(fluid_find_all); + + gttic(expmap); + // 6. Update linearization point for marked variables: + // \Theta_{J}:=\Theta_{J}+\Delta_{J}. + if (!relinKeys.empty()) expmapMasked(delta, markedRelinMask, theta); + gttoc(expmap); + + result->variablesRelinearized = result->markedKeys.size(); + return relinKeys; + } + + // 7. Linearize new factors + void linearizeNewFactors(const NonlinearFactorGraph& newFactors, + const Values& theta, size_t numNonlinearFactors, + const FactorIndices& newFactorsIndices, + GaussianFactorGraph* linearFactors) const { + gttic(linearizeNewFactors); + auto linearized = newFactors.linearize(theta); + if (params_.findUnusedFactorSlots) { + linearFactors->resize(numNonlinearFactors); + for (size_t i = 0; i < newFactors.size(); ++i) + (*linearFactors)[newFactorsIndices[i]] = (*linearized)[i]; + } else { + linearFactors->push_back(*linearized); + } + assert(linearFactors->size() == numNonlinearFactors); + } + + void augmentVariableIndex(const NonlinearFactorGraph& newFactors, + const FactorIndices& newFactorsIndices, + VariableIndex* variableIndex) const { + gttic(augmentVariableIndex); + // Augment the variable index with the new factors + if (params_.findUnusedFactorSlots) + variableIndex->augment(newFactors, newFactorsIndices); + else + variableIndex->augment(newFactors); + + // Augment it with existing factors which now affect to more variables: + if (updateParams_.newAffectedKeys) { + for (const auto& factorAddedKeys : *updateParams_.newAffectedKeys) { + const auto factorIdx = factorAddedKeys.first; + variableIndex->augmentExistingFactor(factorIdx, factorAddedKeys.second); + } + } + } + + void logRecalculateKeys(const ISAM2Result& result) const { + const bool debug = ISDEBUG("ISAM2 recalculate"); + + if (debug) { + std::cout << "markedKeys: "; + for (const Key key : result.markedKeys) { + std::cout << key << " "; + } + std::cout << std::endl; + std::cout << "observedKeys: "; + for (const Key key : result.observedKeys) { + std::cout << key << " "; + } + std::cout << std::endl; + } + } + + FactorIndexSet getAffectedFactors(const KeyList& keys, + const VariableIndex& variableIndex) const { + FactorIndexSet indices; + for (const Key key : keys) { + const FactorIndices& factors(variableIndex[key]); + indices.insert(factors.begin(), factors.end()); + } + return indices; + } + + // find intermediate (linearized) factors from cache that are passed into the + // affected area + GaussianFactorGraph getCachedBoundaryFactors( + const ISAM2::Cliques& orphans) const { + GaussianFactorGraph cachedBoundary; + + for (const auto& orphan : orphans) { + // retrieve the cached factor and add to boundary + cachedBoundary.push_back(orphan->cachedFactor()); + } + + return cachedBoundary; + } + + // retrieve all factors that ONLY contain the affected variables + // (note that the remaining stuff is summarized in the cached factors) + GaussianFactorGraph relinearizeAffectedFactors( + const FastList& affectedKeys, const KeySet& relinKeys, + const NonlinearFactorGraph& nonlinearFactors, + const VariableIndex& variableIndex, const Values& theta, + GaussianFactorGraph* linearFactors) const { + gttic(getAffectedFactors); + FactorIndexSet candidates = getAffectedFactors(affectedKeys, variableIndex); + gttoc(getAffectedFactors); + + gttic(affectedKeysSet); + // for fast lookup below + KeySet affectedKeysSet; + affectedKeysSet.insert(affectedKeys.begin(), affectedKeys.end()); + gttoc(affectedKeysSet); + + gttic(check_candidates_and_linearize); + GaussianFactorGraph linearized; + for (const FactorIndex idx : candidates) { + bool inside = true; + bool useCachedLinear = params_.cacheLinearizedFactors; + for (Key key : nonlinearFactors[idx]->keys()) { + if (affectedKeysSet.find(key) == affectedKeysSet.end()) { + inside = false; + break; + } + if (useCachedLinear && relinKeys.find(key) != relinKeys.end()) + useCachedLinear = false; + } + if (inside) { + if (useCachedLinear) { +#ifdef GTSAM_EXTRA_CONSISTENCY_CHECKS + assert((*linearFactors)[idx]); + assert((*linearFactors)[idx]->keys() == + nonlinearFactors[idx]->keys()); +#endif + linearized.push_back((*linearFactors)[idx]); + } else { + auto linearFactor = nonlinearFactors[idx]->linearize(theta); + linearized.push_back(linearFactor); + if (params_.cacheLinearizedFactors) { +#ifdef GTSAM_EXTRA_CONSISTENCY_CHECKS + assert((*linearFactors)[idx]->keys() == linearFactor->keys()); +#endif + (*linearFactors)[idx] = linearFactor; + } + } + } + } + gttoc(check_candidates_and_linearize); + + return linearized; + } + + KeySet recalculate(const Values& theta, const VariableIndex& variableIndex, + const NonlinearFactorGraph& nonlinearFactors, + const GaussianBayesNet& affectedBayesNet, + const ISAM2::Cliques& orphans, const KeySet& relinKeys, + GaussianFactorGraph* linearFactors, ISAM2::Roots* roots, + ISAM2::Nodes* nodes, ISAM2Result* result) const { + gttic(recalculate); + logRecalculateKeys(*result); + + // FactorGraph factors(affectedBayesNet); + // bug was here: we cannot reuse the original factors, because then the + // cached factors get messed up [all the necessary data is actually + // contained in the affectedBayesNet, including what was passed in from the + // boundaries, + // so this would be correct; however, in the process we also generate new + // cached_ entries that will be wrong (ie. they don't contain what would be + // passed up at a certain point if batch elimination was done, but that's + // what we need); we could choose not to update cached_ from here, but then + // the new information (and potentially different variable ordering) is not + // reflected in the cached_ values which again will be wrong] + // so instead we have to retrieve the original linearized factors AND add + // the cached factors from the boundary + + // BEGIN OF COPIED CODE + + // ordering provides all keys in conditionals, there cannot be others + // because path to root included + gttic(affectedKeys); + FastList affectedKeys; + for (const auto& conditional : affectedBayesNet) + affectedKeys.insert(affectedKeys.end(), conditional->beginFrontals(), + conditional->endFrontals()); + gttoc(affectedKeys); + + KeySet affectedKeysSet; // Will return this result + + static const double kBatchThreshold = 0.65; + + if (affectedKeys.size() >= theta.size() * kBatchThreshold) { + // Do a batch step - reorder and relinearize all variables + gttic(batch); + + gttic(add_keys); + br::copy(variableIndex | br::map_keys, + std::inserter(affectedKeysSet, affectedKeysSet.end())); + + // Removed unused keys: + VariableIndex affectedFactorsVarIndex = variableIndex; + + affectedFactorsVarIndex.removeUnusedVariables( + result->unusedIndices.begin(), result->unusedIndices.end()); + + for (const Key key : result->unusedIndices) { + affectedKeysSet.erase(key); + } + gttoc(add_keys); + + gttic(ordering); + Ordering order; + if (updateParams_.constrainedKeys) { + order = Ordering::ColamdConstrained(affectedFactorsVarIndex, + *updateParams_.constrainedKeys); + } else { + if (theta.size() > result->observedKeys.size()) { + // Only if some variables are unconstrained + FastMap constraintGroups; + for (Key var : result->observedKeys) constraintGroups[var] = 1; + order = Ordering::ColamdConstrained(affectedFactorsVarIndex, + constraintGroups); + } else { + order = Ordering::Colamd(affectedFactorsVarIndex); + } + } + gttoc(ordering); + + gttic(linearize); + GaussianFactorGraph linearized = *nonlinearFactors.linearize(theta); + if (params_.cacheLinearizedFactors) *linearFactors = linearized; + gttoc(linearize); + + gttic(eliminate); + ISAM2BayesTree::shared_ptr bayesTree = + ISAM2JunctionTree(GaussianEliminationTree( + linearized, affectedFactorsVarIndex, order)) + .eliminate(params_.getEliminationFunction()) + .first; + gttoc(eliminate); + + gttic(insert); + roots->clear(); + roots->insert(roots->end(), bayesTree->roots().begin(), + bayesTree->roots().end()); + nodes->clear(); + nodes->insert(bayesTree->nodes().begin(), bayesTree->nodes().end()); + gttoc(insert); + + result->variablesReeliminated = affectedKeysSet.size(); + result->factorsRecalculated = nonlinearFactors.size(); + + // Reeliminated keys for detailed results + if (params_.enableDetailedResults) { + for (Key key : theta.keys()) { + result->detail->variableStatus[key].isReeliminated = true; + } + } + + gttoc(batch); + + } else { + gttic(incremental); + const bool debug = ISDEBUG("ISAM2 recalculate"); + + // 2. Add the new factors \Factors' into the resulting factor graph + FastList affectedAndNewKeys; + affectedAndNewKeys.insert(affectedAndNewKeys.end(), affectedKeys.begin(), + affectedKeys.end()); + affectedAndNewKeys.insert(affectedAndNewKeys.end(), + result->observedKeys.begin(), + result->observedKeys.end()); + gttic(relinearizeAffected); + GaussianFactorGraph factors = relinearizeAffectedFactors( + affectedAndNewKeys, relinKeys, nonlinearFactors, variableIndex, theta, + linearFactors); + gttoc(relinearizeAffected); + + if (debug) { + factors.print("Relinearized factors: "); + std::cout << "Affected keys: "; + for (const Key key : affectedKeys) { + std::cout << key << " "; + } + std::cout << std::endl; + } + + // Reeliminated keys for detailed results + if (params_.enableDetailedResults) { + for (Key key : affectedAndNewKeys) { + result->detail->variableStatus[key].isReeliminated = true; + } + } + + result->variablesReeliminated = affectedAndNewKeys.size(); + result->factorsRecalculated = factors.size(); + + gttic(cached); + // add the cached intermediate results from the boundary of the orphans + // ... + GaussianFactorGraph cachedBoundary = getCachedBoundaryFactors(orphans); + if (debug) cachedBoundary.print("Boundary factors: "); + factors.push_back(cachedBoundary); + gttoc(cached); + + gttic(orphans); + // Add the orphaned subtrees + for (const auto& orphan : orphans) + factors += + boost::make_shared >(orphan); + gttoc(orphans); + + // END OF COPIED CODE + + // 3. Re-order and eliminate the factor graph into a Bayes net (Algorithm + // [alg:eliminate]), and re-assemble into a new Bayes tree (Algorithm + // [alg:BayesTree]) + + gttic(reorder_and_eliminate); + + gttic(list_to_set); + // create a partial reordering for the new and contaminated factors + // result->markedKeys are passed in: those variables will be forced to the + // end in the ordering + affectedKeysSet.insert(result->markedKeys.begin(), + result->markedKeys.end()); + affectedKeysSet.insert(affectedKeys.begin(), affectedKeys.end()); + gttoc(list_to_set); + + VariableIndex affectedFactorsVarIndex(factors); + + gttic(ordering_constraints); + // Create ordering constraints + FastMap constraintGroups; + if (updateParams_.constrainedKeys) { + constraintGroups = *updateParams_.constrainedKeys; + } else { + constraintGroups = FastMap(); + const int group = + result->observedKeys.size() < affectedFactorsVarIndex.size() ? 1 + : 0; + for (Key var : result->observedKeys) + constraintGroups.insert(std::make_pair(var, group)); + } + + // Remove unaffected keys from the constraints + for (FastMap::iterator iter = constraintGroups.begin(); + iter != constraintGroups.end(); + /*Incremented in loop ++iter*/) { + if (result->unusedIndices.exists(iter->first) || + !affectedKeysSet.exists(iter->first)) + constraintGroups.erase(iter++); + else + ++iter; + } + gttoc(ordering_constraints); + + // Generate ordering + gttic(Ordering); + Ordering ordering = Ordering::ColamdConstrained(affectedFactorsVarIndex, + constraintGroups); + gttoc(Ordering); + + ISAM2BayesTree::shared_ptr bayesTree = + ISAM2JunctionTree(GaussianEliminationTree( + factors, affectedFactorsVarIndex, ordering)) + .eliminate(params_.getEliminationFunction()) + .first; + + gttoc(reorder_and_eliminate); + + gttic(reassemble); + roots->insert(roots->end(), bayesTree->roots().begin(), + bayesTree->roots().end()); + nodes->insert(bayesTree->nodes().begin(), bayesTree->nodes().end()); + gttoc(reassemble); + + // 4. The orphans have already been inserted during elimination + + gttoc(incremental); + } + + // Root clique variables for detailed results + if (params_.enableDetailedResults) { + for (const auto& root : *roots) + for (Key var : *root->conditional()) + result->detail->variableStatus[var].inRootClique = true; + } + + return affectedKeysSet; + } +}; +/* ************************************************************************* */ + +} // namespace gtsam diff --git a/gtsam/nonlinear/ISAM2.cpp b/gtsam/nonlinear/ISAM2.cpp index 988e6779f..0ce60fd6f 100644 --- a/gtsam/nonlinear/ISAM2.cpp +++ b/gtsam/nonlinear/ISAM2.cpp @@ -16,26 +16,16 @@ * @author Michael Kaess, Richard Roberts, Frank Dellaert */ +#include #include +#include #include #include #include -#include // We need the inst file because we'll make a special JT templated on ISAM2 -#include #include -#include -#include -namespace br { -using namespace boost::range; -using namespace boost::adaptors; -} // namespace br - -#include -#include #include -#include using namespace std; @@ -44,35 +34,6 @@ namespace gtsam { // Instantiate base class template class BayesTree; -static const bool kDisableReordering = false; -static const double kBatchThreshold = 0.65; - -/* ************************************************************************* */ -// Special BayesTree class that uses ISAM2 cliques - this is the result of -// reeliminating ISAM2 subtrees. -class ISAM2BayesTree : public ISAM2::Base { - public: - typedef ISAM2::Base Base; - typedef ISAM2BayesTree This; - typedef boost::shared_ptr shared_ptr; - - ISAM2BayesTree() {} -}; - -/* ************************************************************************* */ -// Special JunctionTree class that produces ISAM2 BayesTree cliques, used for -// reeliminating ISAM2 subtrees. -class ISAM2JunctionTree - : public JunctionTree { - public: - typedef JunctionTree Base; - typedef ISAM2JunctionTree This; - typedef boost::shared_ptr shared_ptr; - - explicit ISAM2JunctionTree(const GaussianEliminationTree& eliminationTree) - : Base(eliminationTree) {} -}; - /* ************************************************************************* */ ISAM2::ISAM2(const ISAM2Params& params) : params_(params), update_count_(0) { if (params_.optimizationParams.type() == typeid(ISAM2DoglegParams)) @@ -96,392 +57,11 @@ bool ISAM2::equals(const ISAM2& other, double tol) const { } /* ************************************************************************* */ -FactorIndexSet ISAM2::getAffectedFactors(const KeyList& keys) const { - static const bool debug = false; - if (debug) cout << "Getting affected factors for "; - if (debug) { - for (const Key key : keys) { - cout << key << " "; - } - } - if (debug) cout << endl; - - FactorIndexSet indices; - for (const Key key : keys) { - const VariableIndex::Factors& factors(variableIndex_[key]); - indices.insert(factors.begin(), factors.end()); - } - if (debug) cout << "Affected factors are: "; - if (debug) { - for (const auto index : indices) { - cout << index << " "; - } - } - if (debug) cout << endl; - return indices; -} - -/* ************************************************************************* */ -// retrieve all factors that ONLY contain the affected variables -// (note that the remaining stuff is summarized in the cached factors) - -GaussianFactorGraph::shared_ptr ISAM2::relinearizeAffectedFactors( - const FastList& affectedKeys, const KeySet& relinKeys) const { - gttic(getAffectedFactors); - FactorIndexSet candidates = getAffectedFactors(affectedKeys); - gttoc(getAffectedFactors); - - gttic(affectedKeysSet); - // for fast lookup below - KeySet affectedKeysSet; - affectedKeysSet.insert(affectedKeys.begin(), affectedKeys.end()); - gttoc(affectedKeysSet); - - gttic(check_candidates_and_linearize); - auto linearized = boost::make_shared(); - for (const FactorIndex idx : candidates) { - bool inside = true; - bool useCachedLinear = params_.cacheLinearizedFactors; - for (Key key : nonlinearFactors_[idx]->keys()) { - if (affectedKeysSet.find(key) == affectedKeysSet.end()) { - inside = false; - break; - } - if (useCachedLinear && relinKeys.find(key) != relinKeys.end()) - useCachedLinear = false; - } - if (inside) { - if (useCachedLinear) { -#ifdef GTSAM_EXTRA_CONSISTENCY_CHECKS - assert(linearFactors_[idx]); - assert(linearFactors_[idx]->keys() == nonlinearFactors_[idx]->keys()); -#endif - linearized->push_back(linearFactors_[idx]); - } else { - auto linearFactor = nonlinearFactors_[idx]->linearize(theta_); - linearized->push_back(linearFactor); - if (params_.cacheLinearizedFactors) { -#ifdef GTSAM_EXTRA_CONSISTENCY_CHECKS - assert(linearFactors_[idx]->keys() == linearFactor->keys()); -#endif - linearFactors_[idx] = linearFactor; - } - } - } - } - gttoc(check_candidates_and_linearize); - - return linearized; -} - -/* ************************************************************************* */ -// find intermediate (linearized) factors from cache that are passed into the -// affected area - -GaussianFactorGraph ISAM2::getCachedBoundaryFactors(const Cliques& orphans) { - GaussianFactorGraph cachedBoundary; - - for (sharedClique orphan : orphans) { - // retrieve the cached factor and add to boundary - cachedBoundary.push_back(orphan->cachedFactor()); - } - - return cachedBoundary; -} - -/* ************************************************************************* */ -boost::shared_ptr ISAM2::recalculate( - const KeySet& markedKeys, const KeySet& relinKeys, - const KeyVector& observedKeys, const KeySet& unusedIndices, - const boost::optional >& constrainKeys, - ISAM2Result* result) { - // TODO(dellaert): new factors are linearized twice, - // the newFactors passed in are not used. - - const bool debug = ISDEBUG("ISAM2 recalculate"); - - // Input: BayesTree(this), newFactors - -// figures for paper, disable for timing -#ifdef PRINT_STATS - static int counter = 0; - int maxClique = 0; - double avgClique = 0; - int numCliques = 0; - int nnzR = 0; - if (counter > 0) { // cannot call on empty tree - GaussianISAM2_P::CliqueData cdata = this->getCliqueData(); - GaussianISAM2_P::CliqueStats cstats = cdata.getStats(); - maxClique = cstats.maxCONDITIONALSize; - avgClique = cstats.avgCONDITIONALSize; - numCliques = cdata.conditionalSizes.size(); - nnzR = calculate_nnz(this->root()); - } - counter++; -#endif - - if (debug) { - cout << "markedKeys: "; - for (const Key key : markedKeys) { - cout << key << " "; - } - cout << endl; - cout << "observedKeys: "; - for (const Key key : observedKeys) { - cout << key << " "; - } - cout << endl; - } - - // 1. Remove top of Bayes tree and convert to a factor graph: - // (a) For each affected variable, remove the corresponding clique and all - // parents up to the root. (b) Store orphaned sub-trees \BayesTree_{O} of - // removed cliques. - gttic(removetop); - Cliques orphans; - GaussianBayesNet affectedBayesNet; - this->removeTop(KeyVector(markedKeys.begin(), markedKeys.end()), - affectedBayesNet, orphans); - gttoc(removetop); - - // FactorGraph factors(affectedBayesNet); - // bug was here: we cannot reuse the original factors, because then the cached - // factors get messed up [all the necessary data is actually contained in the - // affectedBayesNet, including what was passed in from the boundaries, - // so this would be correct; however, in the process we also generate new - // cached_ entries that will be wrong (ie. they don't contain what would be - // passed up at a certain point if batch elimination was done, but that's - // what we need); we could choose not to update cached_ from here, but then - // the new information (and potentially different variable ordering) is not - // reflected in the cached_ values which again will be wrong] - // so instead we have to retrieve the original linearized factors AND add the - // cached factors from the boundary - - // BEGIN OF COPIED CODE - - // ordering provides all keys in conditionals, there cannot be others because - // path to root included - gttic(affectedKeys); - FastList affectedKeys; - for (const auto& conditional : affectedBayesNet) - affectedKeys.insert(affectedKeys.end(), conditional->beginFrontals(), - conditional->endFrontals()); - gttoc(affectedKeys); - - boost::shared_ptr affectedKeysSet( - new KeySet()); // Will return this result - - if (affectedKeys.size() >= theta_.size() * kBatchThreshold) { - // Do a batch step - reorder and relinearize all variables - gttic(batch); - - gttic(add_keys); - br::copy(variableIndex_ | br::map_keys, - std::inserter(*affectedKeysSet, affectedKeysSet->end())); - - // Removed unused keys: - VariableIndex affectedFactorsVarIndex = variableIndex_; - - affectedFactorsVarIndex.removeUnusedVariables(unusedIndices.begin(), - unusedIndices.end()); - - for (const Key key : unusedIndices) { - affectedKeysSet->erase(key); - } - gttoc(add_keys); - - gttic(ordering); - Ordering order; - if (constrainKeys) { - order = - Ordering::ColamdConstrained(affectedFactorsVarIndex, *constrainKeys); - } else { - if (theta_.size() > observedKeys.size()) { - // Only if some variables are unconstrained - FastMap constraintGroups; - for (Key var : observedKeys) constraintGroups[var] = 1; - order = Ordering::ColamdConstrained(affectedFactorsVarIndex, - constraintGroups); - } else { - order = Ordering::Colamd(affectedFactorsVarIndex); - } - } - gttoc(ordering); - - gttic(linearize); - GaussianFactorGraph linearized = *nonlinearFactors_.linearize(theta_); - if (params_.cacheLinearizedFactors) linearFactors_ = linearized; - gttoc(linearize); - - gttic(eliminate); - ISAM2BayesTree::shared_ptr bayesTree = - ISAM2JunctionTree( - GaussianEliminationTree(linearized, affectedFactorsVarIndex, order)) - .eliminate(params_.getEliminationFunction()) - .first; - gttoc(eliminate); - - gttic(insert); - this->clear(); - this->roots_.insert(this->roots_.end(), bayesTree->roots().begin(), - bayesTree->roots().end()); - this->nodes_.insert(bayesTree->nodes().begin(), bayesTree->nodes().end()); - gttoc(insert); - - result->variablesReeliminated = affectedKeysSet->size(); - result->factorsRecalculated = nonlinearFactors_.size(); - - lastAffectedMarkedCount = markedKeys.size(); - lastAffectedVariableCount = affectedKeysSet->size(); - lastAffectedFactorCount = linearized.size(); - - // Reeliminated keys for detailed results - if (params_.enableDetailedResults) { - for (Key key : theta_.keys()) { - result->detail->variableStatus[key].isReeliminated = true; - } - } - - gttoc(batch); - - } else { - gttic(incremental); - - // 2. Add the new factors \Factors' into the resulting factor graph - FastList affectedAndNewKeys; - affectedAndNewKeys.insert(affectedAndNewKeys.end(), affectedKeys.begin(), - affectedKeys.end()); - affectedAndNewKeys.insert(affectedAndNewKeys.end(), observedKeys.begin(), - observedKeys.end()); - gttic(relinearizeAffected); - GaussianFactorGraph factors( - *relinearizeAffectedFactors(affectedAndNewKeys, relinKeys)); - if (debug) factors.print("Relinearized factors: "); - gttoc(relinearizeAffected); - - if (debug) { - cout << "Affected keys: "; - for (const Key key : affectedKeys) { - cout << key << " "; - } - cout << endl; - } - - // Reeliminated keys for detailed results - if (params_.enableDetailedResults) { - for (Key key : affectedAndNewKeys) { - result->detail->variableStatus[key].isReeliminated = true; - } - } - - result->variablesReeliminated = affectedAndNewKeys.size(); - result->factorsRecalculated = factors.size(); - lastAffectedMarkedCount = markedKeys.size(); - lastAffectedVariableCount = affectedKeys.size(); - lastAffectedFactorCount = factors.size(); - -#ifdef PRINT_STATS - // output for generating figures - cout << "linear: #markedKeys: " << markedKeys.size() - << " #affectedVariables: " << affectedKeys.size() - << " #affectedFactors: " << factors.size() - << " maxCliqueSize: " << maxClique << " avgCliqueSize: " << avgClique - << " #Cliques: " << numCliques << " nnzR: " << nnzR << endl; -#endif - - gttic(cached); - // add the cached intermediate results from the boundary of the orphans ... - GaussianFactorGraph cachedBoundary = getCachedBoundaryFactors(orphans); - if (debug) cachedBoundary.print("Boundary factors: "); - factors.push_back(cachedBoundary); - gttoc(cached); - - gttic(orphans); - // Add the orphaned subtrees - for (const sharedClique& orphan : orphans) - factors += boost::make_shared >(orphan); - gttoc(orphans); - - // END OF COPIED CODE - - // 3. Re-order and eliminate the factor graph into a Bayes net (Algorithm - // [alg:eliminate]), and re-assemble into a new Bayes tree (Algorithm - // [alg:BayesTree]) - - gttic(reorder_and_eliminate); - - gttic(list_to_set); - // create a partial reordering for the new and contaminated factors - // markedKeys are passed in: those variables will be forced to the end in - // the ordering - affectedKeysSet->insert(markedKeys.begin(), markedKeys.end()); - affectedKeysSet->insert(affectedKeys.begin(), affectedKeys.end()); - gttoc(list_to_set); - - VariableIndex affectedFactorsVarIndex(factors); - - gttic(ordering_constraints); - // Create ordering constraints - FastMap constraintGroups; - if (constrainKeys) { - constraintGroups = *constrainKeys; - } else { - constraintGroups = FastMap(); - const int group = - observedKeys.size() < affectedFactorsVarIndex.size() ? 1 : 0; - for (Key var : observedKeys) - constraintGroups.insert(make_pair(var, group)); - } - - // Remove unaffected keys from the constraints - for (FastMap::iterator iter = constraintGroups.begin(); - iter != constraintGroups.end(); - /*Incremented in loop ++iter*/) { - if (unusedIndices.exists(iter->first) || - !affectedKeysSet->exists(iter->first)) - constraintGroups.erase(iter++); - else - ++iter; - } - gttoc(ordering_constraints); - - // Generate ordering - gttic(Ordering); - Ordering ordering = - Ordering::ColamdConstrained(affectedFactorsVarIndex, constraintGroups); - gttoc(Ordering); - - ISAM2BayesTree::shared_ptr bayesTree = - ISAM2JunctionTree( - GaussianEliminationTree(factors, affectedFactorsVarIndex, ordering)) - .eliminate(params_.getEliminationFunction()) - .first; - - gttoc(reorder_and_eliminate); - - gttic(reassemble); - this->roots_.insert(this->roots_.end(), bayesTree->roots().begin(), - bayesTree->roots().end()); - this->nodes_.insert(bayesTree->nodes().begin(), bayesTree->nodes().end()); - gttoc(reassemble); - - // 4. The orphans have already been inserted during elimination - - gttoc(incremental); - } - - // Root clique variables for detailed results - if (params_.enableDetailedResults) { - for (const sharedNode& root : this->roots()) - for (Key var : *root->conditional()) - result->detail->variableStatus[var].inRootClique = true; - } - - return affectedKeysSet; -} - -/* ************************************************************************* */ -void ISAM2::addVariables(const Values& newTheta) { +void ISAM2::addVariables( + const Values& newTheta, + ISAM2Result::DetailedResults::StatusMap* variableStatus) { + gttic(addNewVariables); + // \Theta:=\Theta\cup\Theta_{new}. const bool debug = ISDEBUG("ISAM2 AddVariables"); theta_.insert(newTheta); @@ -490,6 +70,13 @@ void ISAM2::addVariables(const Values& newTheta) { delta_.insert(newTheta.zeroVectors()); deltaNewton_.insert(newTheta.zeroVectors()); RgProd_.insert(newTheta.zeroVectors()); + + // New keys for detailed results + if (variableStatus && params_.enableDetailedResults) { + for (Key key : newTheta.keys()) { + (*variableStatus)[key].isNew = true; + } + } } /* ************************************************************************* */ @@ -506,36 +93,6 @@ void ISAM2::removeVariables(const KeySet& unusedKeys) { } } -/* ************************************************************************* */ -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, @@ -544,7 +101,6 @@ ISAM2Result ISAM2::update( const boost::optional >& noRelinKeys, const boost::optional >& extraReelimKeys, bool force_relinearize) { - ISAM2UpdateParams params; params.constrainedKeys = constrainedKeys; params.extraReelimKeys = extraReelimKeys; @@ -557,28 +113,21 @@ ISAM2Result ISAM2::update( /* ************************************************************************* */ ISAM2Result ISAM2::update(const NonlinearFactorGraph& newFactors, - const Values& newTheta, - const ISAM2UpdateParams& updateParams) { - const bool debug = ISDEBUG("ISAM2 update"); - const bool verbose = ISDEBUG("ISAM2 update verbose"); - + const Values& newTheta, + const ISAM2UpdateParams& updateParams) { gttic(ISAM2_update); this->update_count_++; - lastAffectedVariableCount = 0; - lastAffectedFactorCount = 0; - lastAffectedCliqueCount = 0; - lastAffectedMarkedCount = 0; - lastBacksubVariableCount = 0; - lastNnzTop = 0; ISAM2Result result; if (params_.enableDetailedResults) result.detail = ISAM2Result::DetailedResults(); const bool relinearizeThisStep = - updateParams.force_relinearize || (params_.enableRelinearization && - update_count_ % params_.relinearizeSkip == 0); + updateParams.force_relinearize || + (params_.enableRelinearization && + update_count_ % params_.relinearizeSkip == 0); + const bool verbose = ISDEBUG("ISAM2 update verbose"); if (verbose) { cout << "ISAM2::update\n"; this->print("ISAM2: "); @@ -586,243 +135,66 @@ ISAM2Result ISAM2::update(const NonlinearFactorGraph& newFactors, // Update delta if we need it to check relinearization later if (relinearizeThisStep) { - gttic(updateDelta); - updateDelta(kDisableReordering); - gttoc(updateDelta); + updateDelta(updateParams.forceFullSolve); } - gttic(push_back_factors); + UpdateImpl update(params_, updateParams); + // 1. Add any new factors \Factors:=\Factors\cup\Factors'. - // 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); + update.pushBackFactors(newFactors, &nonlinearFactors_, &linearFactors_, + &variableIndex_, &result); - // Remove the removed factors - NonlinearFactorGraph removeFactors; - removeFactors.reserve(updateParams.removeFactorIndices.size()); - for (const auto index : updateParams.removeFactorIndices) { - removeFactors.push_back(nonlinearFactors_[index]); - nonlinearFactors_.remove(index); - if (params_.cacheLinearizedFactors) linearFactors_.remove(index); - } - - // Remove removed factors from the variable index so we do not attempt to - // relinearize them - variableIndex_.remove(updateParams.removeFactorIndices.begin(), - updateParams.removeFactorIndices.end(), - removeFactors); - - // Compute unused keys and indices - KeySet unusedKeys; - KeySet unusedIndices; - { - // Get keys from removed factors and new factors, and compute unused keys, - // i.e., keys that are empty now and do not appear in the new factors. - KeySet removedAndEmpty; - for (Key key : removeFactors.keys()) { - if (variableIndex_[key].empty()) - removedAndEmpty.insert(removedAndEmpty.end(), key); - } - KeySet newFactorSymbKeys = newFactors.keys(); - std::set_difference(removedAndEmpty.begin(), removedAndEmpty.end(), - newFactorSymbKeys.begin(), newFactorSymbKeys.end(), - std::inserter(unusedKeys, unusedKeys.end())); - - // Get indices for unused keys - for (Key key : unusedKeys) { - unusedIndices.insert(unusedIndices.end(), key); - } - } - gttoc(push_back_factors); - - gttic(add_new_variables); // 2. Initialize any new variables \Theta_{new} and add - // \Theta:=\Theta\cup\Theta_{new}. - addVariables(newTheta); - // New keys for detailed results - if (params_.enableDetailedResults) { - for (Key key : newTheta.keys()) { - result.detail->variableStatus[key].isNew = true; - } - } - gttoc(add_new_variables); + addVariables(newTheta, result.detail ? &result.detail->variableStatus : 0); gttic(evaluate_error_before); if (params_.evaluateNonlinearError) result.errorBefore.reset(nonlinearFactors_.error(calculateEstimate())); gttoc(evaluate_error_before); - gttic(gather_involved_keys); // 3. Mark linear update - KeySet markedKeys = newFactors.keys(); // Get keys from new factors - // Also mark keys involved in removed factors - { - KeySet markedRemoveKeys = - removeFactors.keys(); // Get keys involved in removed factors - markedKeys.insert( - markedRemoveKeys.begin(), - markedRemoveKeys.end()); // Add to the overall set of marked keys - } - // Also mark any provided extra re-eliminate keys - if (updateParams.extraReelimKeys) { - for (Key key : *updateParams.extraReelimKeys) { - markedKeys.insert(key); - } - } - // Also, keys that were not observed in existing factors, but whose affected - // keys have been extended now (e.g. smart factors) - if (updateParams.newAffectedKeys) { - for (const auto &factorAddedKeys : *updateParams.newAffectedKeys) { - const auto factorIdx = factorAddedKeys.first; - const auto& affectedKeys = nonlinearFactors_.at(factorIdx)->keys(); - markedKeys.insert(affectedKeys.begin(),affectedKeys.end()); - } - } - - // Observed keys for detailed results - if (params_.enableDetailedResults) { - for (Key key : markedKeys) { - result.detail->variableStatus[key].isObserved = true; - } - } - - KeyVector observedKeys; - for (Key index : markedKeys) { - // Only add if not unused - if (unusedIndices.find(index) == unusedIndices.end()) - // Make a copy of these, as we'll soon add to them - observedKeys.push_back(index); - } - gttoc(gather_involved_keys); + update.gatherInvolvedKeys(newFactors, nonlinearFactors_, &result); // Check relinearization if we're at the nth step, or we are using a looser // loop relin threshold KeySet relinKeys; if (relinearizeThisStep) { - gttic(gather_relinearize_keys); - // 4. Mark keys in \Delta above threshold \beta: - // J=\{\Delta_{j}\in\Delta|\Delta_{j}\geq\beta\}. - if (params_.enablePartialRelinearizationCheck) - relinKeys = Impl::CheckRelinearizationPartial( - roots_, delta_, params_.relinearizeThreshold); - else - relinKeys = - Impl::CheckRelinearizationFull(delta_, params_.relinearizeThreshold); - if (kDisableReordering) - relinKeys = Impl::CheckRelinearizationFull( - delta_, 0.0); // This is used for debugging - - // Remove from relinKeys any keys whose linearization points are fixed - for (Key key : fixedVariables_) { - relinKeys.erase(key); - } - if (updateParams.noRelinKeys) { - for (Key key : *updateParams.noRelinKeys) { - relinKeys.erase(key); - } - } - - // Above relin threshold keys for detailed results - if (params_.enableDetailedResults) { - for (Key key : relinKeys) { - result.detail->variableStatus[key].isAboveRelinThreshold = true; - result.detail->variableStatus[key].isRelinearized = true; - } - } - - // Add the variables being relinearized to the marked keys - KeySet markedRelinMask; - for (const Key key : relinKeys) markedRelinMask.insert(key); - markedKeys.insert(relinKeys.begin(), relinKeys.end()); - gttoc(gather_relinearize_keys); - - gttic(fluid_find_all); - // 5. Mark all cliques that involve marked variables \Theta_{J} and all - // their ancestors. - if (!relinKeys.empty()) { - for (const sharedClique& root : roots_) - // add other cliques that have the marked ones in the separator - root->findAll(markedRelinMask, &markedKeys); - - // Relin involved keys for detailed results - if (params_.enableDetailedResults) { - KeySet involvedRelinKeys; - for (const sharedClique& root : roots_) - root->findAll(markedRelinMask, &involvedRelinKeys); - for (Key key : involvedRelinKeys) { - if (!result.detail->variableStatus[key].isAboveRelinThreshold) { - result.detail->variableStatus[key].isRelinearizeInvolved = true; - result.detail->variableStatus[key].isRelinearized = true; - } - } - } - } - gttoc(fluid_find_all); - - gttic(expmap); - // 6. Update linearization point for marked variables: - // \Theta_{J}:=\Theta_{J}+\Delta_{J}. - if (!relinKeys.empty()) expmapMasked(markedRelinMask); - gttoc(expmap); - - result.variablesRelinearized = markedKeys.size(); + relinKeys = + update.relinearize(roots_, delta_, fixedVariables_, &theta_, &result); } else { result.variablesRelinearized = 0; } - gttic(linearize_new); // 7. Linearize new factors - if (params_.cacheLinearizedFactors) { - gttic(linearize); - auto linearFactors = newFactors.linearize(theta_); - if (params_.findUnusedFactorSlots) { - linearFactors_.resize(nonlinearFactors_.size()); - for (size_t newFactorI = 0; newFactorI < newFactors.size(); ++newFactorI) - linearFactors_[result.newFactorsIndices[newFactorI]] = - (*linearFactors)[newFactorI]; - } else { - linearFactors_.push_back(*linearFactors); - } - assert(nonlinearFactors_.size() == linearFactors_.size()); - gttoc(linearize); - } - gttoc(linearize_new); + update.linearizeNewFactors(newFactors, theta_, nonlinearFactors_.size(), + result.newFactorsIndices, &linearFactors_); + update.augmentVariableIndex(newFactors, result.newFactorsIndices, + &variableIndex_); - gttic(augment_VI); - // Augment the variable index with the new factors - if (params_.findUnusedFactorSlots) - variableIndex_.augment(newFactors, result.newFactorsIndices); - else - variableIndex_.augment(newFactors); - - // Augment it with existing factors which now affect to more variables: - if (updateParams.newAffectedKeys) { - for (const auto &factorAddedKeys : *updateParams.newAffectedKeys) { - const auto factorIdx = factorAddedKeys.first; - variableIndex_.augmentExistingFactor( - factorIdx, factorAddedKeys.second); - } - } - gttoc(augment_VI); - - gttic(recalculate); // 8. Redo top of Bayes tree - boost::shared_ptr replacedKeys; - if (!markedKeys.empty() || !observedKeys.empty()) - replacedKeys = recalculate( - markedKeys, relinKeys, observedKeys, unusedIndices, - updateParams.constrainedKeys, &result); + if (!result.markedKeys.empty() || !result.observedKeys.empty()) { + // Remove top of Bayes tree and convert to a factor graph: + // (a) For each affected variable, remove the corresponding clique and all + // parents up to the root. (b) Store orphaned sub-trees \BayesTree_{O} of + // removed cliques. + GaussianBayesNet affectedBayesNet; + Cliques orphans; + this->removeTop( + KeyVector(result.markedKeys.begin(), result.markedKeys.end()), + affectedBayesNet, orphans); - // Update replaced keys mask (accumulates until back-substitution takes place) - if (replacedKeys) - deltaReplacedMask_.insert(replacedKeys->begin(), replacedKeys->end()); - gttoc(recalculate); + KeySet affectedKeysSet = update.recalculate( + theta_, variableIndex_, nonlinearFactors_, affectedBayesNet, orphans, + relinKeys, &linearFactors_, &roots_, &nodes_, &result); + // Update replaced keys mask (accumulates until back-substitution takes + // place) + deltaReplacedMask_.insert(affectedKeysSet.begin(), affectedKeysSet.end()); + } // Update data structures to remove unused keys - if (!unusedKeys.empty()) { + if (!result.unusedKeys.empty()) { gttic(remove_variables); - removeVariables(unusedKeys); + removeVariables(result.unusedKeys); gttoc(remove_variables); } result.cliques = this->nodes().size(); @@ -966,7 +338,7 @@ void ISAM2::marginalizeLeaves( } } // Create factor graph from factor indices - for (const auto index: factorsFromMarginalizedInClique_step1) { + for (const auto index : factorsFromMarginalizedInClique_step1) { graph.push_back(nonlinearFactors_[index]->linearize(theta_)); } @@ -1040,7 +412,7 @@ void ISAM2::marginalizeLeaves( // Remove the factors to remove that have been summarized in the newly-added // marginal factors NonlinearFactorGraph removedFactors; - for (const auto index: factorIndicesToRemove) { + for (const auto index : factorIndicesToRemove) { removedFactors.push_back(nonlinearFactors_[index]); nonlinearFactors_.remove(index); if (params_.cacheLinearizedFactors) linearFactors_.remove(index); @@ -1057,6 +429,7 @@ void ISAM2::marginalizeLeaves( } /* ************************************************************************* */ +// Marked const but actually changes mutable delta void ISAM2::updateDelta(bool forceFullSolve) const { gttic(updateDelta); if (params_.optimizationParams.type() == typeid(ISAM2GaussNewtonParams)) { @@ -1066,8 +439,8 @@ void ISAM2::updateDelta(bool forceFullSolve) const { const double effectiveWildfireThreshold = forceFullSolve ? 0.0 : gaussNewtonParams.wildfireThreshold; gttic(Wildfire_update); - lastBacksubVariableCount = Impl::UpdateGaussNewtonDelta( - roots_, deltaReplacedMask_, effectiveWildfireThreshold, &delta_); + DeltaImpl::UpdateGaussNewtonDelta(roots_, deltaReplacedMask_, + effectiveWildfireThreshold, &delta_); deltaReplacedMask_.clear(); gttoc(Wildfire_update); @@ -1083,15 +456,15 @@ void ISAM2::updateDelta(bool forceFullSolve) const { // Compute Newton's method step gttic(Wildfire_update); - lastBacksubVariableCount = Impl::UpdateGaussNewtonDelta( + DeltaImpl::UpdateGaussNewtonDelta( roots_, deltaReplacedMask_, effectiveWildfireThreshold, &deltaNewton_); gttoc(Wildfire_update); // Compute steepest descent step const VectorValues gradAtZero = this->gradientAtZero(); // Compute gradient - Impl::UpdateRgProd(roots_, deltaReplacedMask_, gradAtZero, - &RgProd_); // Update RgProd - const VectorValues dx_u = Impl::ComputeGradientSearch( + DeltaImpl::UpdateRgProd(roots_, deltaReplacedMask_, gradAtZero, + &RgProd_); // Update RgProd + const VectorValues dx_u = DeltaImpl::ComputeGradientSearch( gradAtZero, RgProd_); // Compute gradient search point // Clear replaced keys mask because now we've updated deltaNewton_ and @@ -1163,8 +536,7 @@ VectorValues ISAM2::gradientAtZero() const { VectorValues g; // Sum up contributions for each clique - for (const auto& root : this->roots()) - root->addGradientAtZero(&g); + for (const auto& root : this->roots()) root->addGradientAtZero(&g); return g; } diff --git a/gtsam/nonlinear/ISAM2.h b/gtsam/nonlinear/ISAM2.h index 60cdc45fe..b0eb4693b 100644 --- a/gtsam/nonlinear/ISAM2.h +++ b/gtsam/nonlinear/ISAM2.h @@ -20,12 +20,12 @@ #pragma once +#include +#include #include #include -#include #include #include -#include #include @@ -73,8 +73,8 @@ class GTSAM_EXPORT ISAM2 : public BayesTree { * This is \c mutable because it is used internally to not update delta_ * until it is needed. */ - mutable KeySet - deltaReplacedMask_; // TODO(dellaert): Make sure accessed in the right way + mutable KeySet deltaReplacedMask_; // TODO(dellaert): Make sure accessed in + // the right way /** All original nonlinear factors are stored here to use during * relinearization */ @@ -175,9 +175,9 @@ class GTSAM_EXPORT ISAM2 : public BayesTree { * @return An ISAM2Result struct containing information about the update * @note No default parameters to avoid ambiguous call errors. */ - virtual ISAM2Result update( - const NonlinearFactorGraph& newFactors, const Values& newTheta, - const ISAM2UpdateParams& updateParams); + virtual ISAM2Result update(const NonlinearFactorGraph& newFactors, + const Values& newTheta, + const ISAM2UpdateParams& updateParams); /** Marginalize out variables listed in leafKeys. These keys must be leaves * in the BayesTree. Throws MarginalizeNonleafException if non-leaves are @@ -226,7 +226,6 @@ class GTSAM_EXPORT ISAM2 : public BayesTree { return traits::Retract(theta_.at(key), delta); } - /** Compute an estimate for a single variable using its incomplete linear * delta computed during the last update. This is faster than calling the * no-argument version of calculateEstimate, which operates on all variables. @@ -243,9 +242,6 @@ class GTSAM_EXPORT ISAM2 : public BayesTree { /// @name Public members for non-typical usage /// @{ - /** Internal implementation functions */ - struct Impl; - /** Compute an estimate using a complete delta computed by a full * back-substitution. */ @@ -268,13 +264,6 @@ class GTSAM_EXPORT ISAM2 : public BayesTree { /** Access the nonlinear variable index */ const KeySet& getFixedVariables() const { return fixedVariables_; } - size_t lastAffectedVariableCount; - size_t lastAffectedFactorCount; - size_t lastAffectedCliqueCount; - size_t lastAffectedMarkedCount; - mutable size_t lastBacksubVariableCount; - size_t lastNnzTop; - const ISAM2Params& params() const { return params_; } /** prints out clique statistics */ @@ -295,37 +284,17 @@ class GTSAM_EXPORT ISAM2 : public BayesTree { /** * 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 + * @param variableStatus optional detailed result structure */ - void addVariables(const Values& newTheta); + void addVariables( + const Values& newTheta, + ISAM2Result::DetailedResults::StatusMap* variableStatus = 0); /** * 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); - - FactorIndexSet getAffectedFactors(const FastList& keys) const; - GaussianFactorGraph::shared_ptr relinearizeAffectedFactors( - const FastList& affectedKeys, const KeySet& relinKeys) const; - GaussianFactorGraph getCachedBoundaryFactors(const Cliques& orphans); - - virtual boost::shared_ptr recalculate( - const KeySet& markedKeys, const KeySet& relinKeys, - const KeyVector& observedKeys, const KeySet& unusedIndices, - const boost::optional >& constrainKeys, - ISAM2Result* result); - void updateDelta(bool forceFullSolve = false) const; }; // ISAM2 @@ -334,5 +303,3 @@ template <> struct traits : public Testable {}; } // namespace gtsam - -#include diff --git a/gtsam/nonlinear/ISAM2Result.h b/gtsam/nonlinear/ISAM2Result.h index 763a5e198..d49c92627 100644 --- a/gtsam/nonlinear/ISAM2Result.h +++ b/gtsam/nonlinear/ISAM2Result.h @@ -24,8 +24,8 @@ #include #include -#include #include +#include #include @@ -96,6 +96,20 @@ struct GTSAM_EXPORT ISAM2Result { */ FactorIndices newFactorsIndices; + /** Unused keys, and indices for unused keys. TODO(frank): the same?? + * i.e., keys that are empty now and do not appear in the new factors. + */ + KeySet unusedKeys, unusedIndices; + + /** keys for variables that were observed, i.e., not unused. */ + KeyVector observedKeys; + + /** Keys of variables that had factors removed. */ + KeySet keysWithRemovedFactors; + + /** All keys that were marked during the update process. */ + KeySet markedKeys; + /** A struct holding detailed results, which must be enabled with * ISAM2Params::enableDetailedResults. */ @@ -132,9 +146,10 @@ struct GTSAM_EXPORT ISAM2Result { inRootClique(false) {} }; - /** The status of each variable during this update, see VariableStatus. - */ - FastMap variableStatus; + typedef FastMap StatusMap; + + /// The status of each variable during this update, see VariableStatus. + StatusMap variableStatus; }; /** Detailed results, if enabled by ISAM2Params::enableDetailedResults. See diff --git a/gtsam/nonlinear/ISAM2UpdateParams.h b/gtsam/nonlinear/ISAM2UpdateParams.h index 77f722b12..884ac7922 100644 --- a/gtsam/nonlinear/ISAM2UpdateParams.h +++ b/gtsam/nonlinear/ISAM2UpdateParams.h @@ -16,11 +16,11 @@ #pragma once -#include #include -#include // GTSAM_EXPORT -#include // Key, KeySet -#include //FactorIndices +#include // GTSAM_EXPORT +#include // Key, KeySet +#include //FactorIndices +#include namespace gtsam { @@ -63,8 +63,12 @@ struct GTSAM_EXPORT ISAM2UpdateParams { * depend on Keys `X(2)`, `X(3)`. Next call to ISAM2::update() must include * its `newAffectedKeys` field with the map `13 -> {X(2), X(3)}`. */ - boost::optional> newAffectedKeys{boost::none}; + boost::optional> newAffectedKeys{boost::none}; + /** By default, iSAM2 uses a wildfire update scheme that stops updating when + * the deltas become too small down in the tree. This flagg forces a full + * solve instead. */ + bool forceFullSolve{false}; }; -} // namespace gtsam +} // namespace gtsam diff --git a/tests/testGaussianISAM2.cpp b/tests/testGaussianISAM2.cpp index 2b6809ada..d8b829872 100644 --- a/tests/testGaussianISAM2.cpp +++ b/tests/testGaussianISAM2.cpp @@ -4,7 +4,8 @@ * @author Michael Kaess */ -#include +#include +#include #include #include @@ -14,7 +15,6 @@ #include #include #include -#include #include #include #include @@ -23,10 +23,13 @@ #include #include #include -#include #include -using namespace boost::assign; + +#include + +#include #include +using namespace boost::assign; namespace br { using namespace boost::adaptors; using namespace boost::range; } using namespace std; @@ -304,7 +307,7 @@ TEST(ISAM2, AddFactorsStep1) FactorIndices actualNewFactorIndices; - ISAM2::Impl::AddFactorsStep1(newFactors, true, &nonlinearFactors, &actualNewFactorIndices); + UpdateImpl::AddFactorsStep1(newFactors, true, &nonlinearFactors, &actualNewFactorIndices); EXPECT(assert_equal(expectedNonlinearFactors, nonlinearFactors)); EXPECT(assert_container_equality(expectedNewFactorIndices, actualNewFactorIndices)); From 5a819645a46d9a5e3af9aed64755671430383689 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Fri, 31 May 2019 20:10:44 -0400 Subject: [PATCH 06/21] Fixed print when no noise model --- gtsam/slam/PriorFactor.h | 10 ++++++++-- 1 file changed, 8 insertions(+), 2 deletions(-) diff --git a/gtsam/slam/PriorFactor.h b/gtsam/slam/PriorFactor.h index 8bb7a3ab8..5b085652f 100644 --- a/gtsam/slam/PriorFactor.h +++ b/gtsam/slam/PriorFactor.h @@ -18,6 +18,8 @@ #include #include +#include + namespace gtsam { /** @@ -70,10 +72,14 @@ namespace gtsam { /** implement functions needed for Testable */ /** print */ - virtual void print(const std::string& s, const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { + virtual void print(const std::string& s, const KeyFormatter& keyFormatter = + DefaultKeyFormatter) const { std::cout << s << "PriorFactor on " << keyFormatter(this->key()) << "\n"; traits::Print(prior_, " prior mean: "); - this->noiseModel_->print(" noise model: "); + if (this->noiseModel_) + this->noiseModel_->print(" noise model: "); + else + std::cout << "no noise model" << std::endl; } /** equals */ From ecacda68c04313a4e9ce5a687b8439fdeaa64f61 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 1 Jun 2019 13:45:07 -0400 Subject: [PATCH 07/21] Further refactored pushNewFactors --- gtsam/nonlinear/ISAM2-impl.h | 65 +++++++++++++++++++++--------------- gtsam/nonlinear/ISAM2.cpp | 8 +---- tests/testGaussianISAM2.cpp | 9 +++-- 3 files changed, 45 insertions(+), 37 deletions(-) diff --git a/gtsam/nonlinear/ISAM2-impl.h b/gtsam/nonlinear/ISAM2-impl.h index f4dfcb99b..352d26c8a 100644 --- a/gtsam/nonlinear/ISAM2-impl.h +++ b/gtsam/nonlinear/ISAM2-impl.h @@ -117,17 +117,33 @@ struct GTSAM_EXPORT UpdateImpl { UpdateImpl(const ISAM2Params& params, const ISAM2UpdateParams& updateParams) : params_(params), updateParams_(updateParams) {} + // Provide some debugging information at the start of update + static void LogStartingUpdate(const NonlinearFactorGraph& newFactors, + const ISAM2& isam2) { + gttic(pushBackFactors); + const bool debug = ISDEBUG("ISAM2 update"); + const bool verbose = ISDEBUG("ISAM2 update verbose"); + + if (verbose) { + std::cout << "ISAM2::update\n"; + isam2.print("ISAM2: "); + } + + // Add the new factor indices to the result struct + if (debug || verbose) { + newFactors.print("The new factors are: "); + } + } + /// 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) { - newFactorIndices->resize(newFactors.size()); + FactorIndices addFactorsStep1(const NonlinearFactorGraph& newFactors, + NonlinearFactorGraph* nonlinearFactors) const { + FactorIndices newFactorIndices(newFactors.size()); - if (useUnusedSlots) { + if (params_.findUnusedFactorSlots) { size_t globalFactorIndex = 0; for (size_t newFactorIndex = 0; newFactorIndex < newFactors.size(); ++newFactorIndex) { @@ -150,14 +166,15 @@ struct GTSAM_EXPORT UpdateImpl { // Use the current slot, updating nonlinearFactors and newFactorSlots. (*nonlinearFactors)[globalFactorIndex] = newFactors[newFactorIndex]; - (*newFactorIndices)[newFactorIndex] = globalFactorIndex; + 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(); + newFactorIndices[i] = i + nonlinearFactors->size(); nonlinearFactors->push_back(newFactors); } + return newFactorIndices; } // 1. Add any new factors \Factors:=\Factors\cup\Factors'. @@ -167,13 +184,9 @@ struct GTSAM_EXPORT UpdateImpl { VariableIndex* variableIndex, ISAM2Result* result) const { gttic(pushBackFactors); - const bool debug = ISDEBUG("ISAM2 update"); - const bool verbose = ISDEBUG("ISAM2 update verbose"); // Add the new factor indices to the result struct - if (debug || verbose) newFactors.print("The new factors are: "); - AddFactorsStep1(newFactors, params_.findUnusedFactorSlots, nonlinearFactors, - &result->newFactorsIndices); + result->newFactorsIndices = addFactorsStep1(newFactors, nonlinearFactors); // Remove the removed factors NonlinearFactorGraph removedFactors; @@ -378,8 +391,8 @@ struct GTSAM_EXPORT UpdateImpl { * \c mask. Values are expmapped in-place. * \param mask Mask on linear indices, only \c true entries are expmapped */ - void expmapMasked(const VectorValues& delta, const KeySet& mask, - Values* theta) const { + static void ExpmapMasked(const VectorValues& delta, const KeySet& mask, + Values* theta) { assert(theta->size() == delta.size()); Values::iterator key_value; VectorValues::const_iterator key_delta; @@ -468,7 +481,7 @@ struct GTSAM_EXPORT UpdateImpl { gttic(expmap); // 6. Update linearization point for marked variables: // \Theta_{J}:=\Theta_{J}+\Delta_{J}. - if (!relinKeys.empty()) expmapMasked(delta, markedRelinMask, theta); + if (!relinKeys.empty()) ExpmapMasked(delta, markedRelinMask, theta); gttoc(expmap); result->variablesRelinearized = result->markedKeys.size(); @@ -511,7 +524,7 @@ struct GTSAM_EXPORT UpdateImpl { } } - void logRecalculateKeys(const ISAM2Result& result) const { + static void LogRecalculateKeys(const ISAM2Result& result) { const bool debug = ISDEBUG("ISAM2 recalculate"); if (debug) { @@ -528,8 +541,9 @@ struct GTSAM_EXPORT UpdateImpl { } } - FactorIndexSet getAffectedFactors(const KeyList& keys, - const VariableIndex& variableIndex) const { + static FactorIndexSet GetAffectedFactors(const KeyList& keys, + const VariableIndex& variableIndex) { + gttic(GetAffectedFactors); FactorIndexSet indices; for (const Key key : keys) { const FactorIndices& factors(variableIndex[key]); @@ -540,8 +554,8 @@ struct GTSAM_EXPORT UpdateImpl { // find intermediate (linearized) factors from cache that are passed into the // affected area - GaussianFactorGraph getCachedBoundaryFactors( - const ISAM2::Cliques& orphans) const { + static GaussianFactorGraph GetCachedBoundaryFactors( + const ISAM2::Cliques& orphans) { GaussianFactorGraph cachedBoundary; for (const auto& orphan : orphans) { @@ -559,9 +573,7 @@ struct GTSAM_EXPORT UpdateImpl { const NonlinearFactorGraph& nonlinearFactors, const VariableIndex& variableIndex, const Values& theta, GaussianFactorGraph* linearFactors) const { - gttic(getAffectedFactors); - FactorIndexSet candidates = getAffectedFactors(affectedKeys, variableIndex); - gttoc(getAffectedFactors); + FactorIndexSet candidates = GetAffectedFactors(affectedKeys, variableIndex); gttic(affectedKeysSet); // for fast lookup below @@ -614,7 +626,7 @@ struct GTSAM_EXPORT UpdateImpl { GaussianFactorGraph* linearFactors, ISAM2::Roots* roots, ISAM2::Nodes* nodes, ISAM2Result* result) const { gttic(recalculate); - logRecalculateKeys(*result); + LogRecalculateKeys(*result); // FactorGraph factors(affectedBayesNet); // bug was here: we cannot reuse the original factors, because then the @@ -754,7 +766,7 @@ struct GTSAM_EXPORT UpdateImpl { gttic(cached); // add the cached intermediate results from the boundary of the orphans // ... - GaussianFactorGraph cachedBoundary = getCachedBoundaryFactors(orphans); + GaussianFactorGraph cachedBoundary = GetCachedBoundaryFactors(orphans); if (debug) cachedBoundary.print("Boundary factors: "); factors.push_back(cachedBoundary); gttoc(cached); @@ -846,6 +858,5 @@ struct GTSAM_EXPORT UpdateImpl { return affectedKeysSet; } }; -/* ************************************************************************* */ } // namespace gtsam diff --git a/gtsam/nonlinear/ISAM2.cpp b/gtsam/nonlinear/ISAM2.cpp index 0ce60fd6f..d6a48c6d5 100644 --- a/gtsam/nonlinear/ISAM2.cpp +++ b/gtsam/nonlinear/ISAM2.cpp @@ -116,8 +116,8 @@ ISAM2Result ISAM2::update(const NonlinearFactorGraph& newFactors, const Values& newTheta, const ISAM2UpdateParams& updateParams) { gttic(ISAM2_update); - this->update_count_++; + UpdateImpl::LogStartingUpdate(newFactors, *this); ISAM2Result result; if (params_.enableDetailedResults) @@ -127,12 +127,6 @@ ISAM2Result ISAM2::update(const NonlinearFactorGraph& newFactors, (params_.enableRelinearization && update_count_ % params_.relinearizeSkip == 0); - const bool verbose = ISDEBUG("ISAM2 update verbose"); - if (verbose) { - cout << "ISAM2::update\n"; - this->print("ISAM2: "); - } - // Update delta if we need it to check relinearization later if (relinearizeThisStep) { updateDelta(updateParams.forceFullSolve); diff --git a/tests/testGaussianISAM2.cpp b/tests/testGaussianISAM2.cpp index d8b829872..3f8619a77 100644 --- a/tests/testGaussianISAM2.cpp +++ b/tests/testGaussianISAM2.cpp @@ -305,9 +305,12 @@ TEST(ISAM2, AddFactorsStep1) const FactorIndices expectedNewFactorIndices = list_of(1)(3); - FactorIndices actualNewFactorIndices; - - UpdateImpl::AddFactorsStep1(newFactors, true, &nonlinearFactors, &actualNewFactorIndices); + ISAM2Params params; + ISAM2UpdateParams updateParams; + params.findUnusedFactorSlots = true; + UpdateImpl update(params, updateParams); + FactorIndices actualNewFactorIndices = + update.addFactorsStep1(newFactors, &nonlinearFactors); EXPECT(assert_equal(expectedNonlinearFactors, nonlinearFactors)); EXPECT(assert_container_equality(expectedNewFactorIndices, actualNewFactorIndices)); From 4b405728a789ff43b299cbc96b3b36e813400ca3 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 1 Jun 2019 13:53:00 -0400 Subject: [PATCH 08/21] Get rid of deprecated LieScalar --- tests/testGaussianISAM2.cpp | 148 +++++++++++++++++------------------- 1 file changed, 71 insertions(+), 77 deletions(-) diff --git a/tests/testGaussianISAM2.cpp b/tests/testGaussianISAM2.cpp index 3f8619a77..8f01d22aa 100644 --- a/tests/testGaussianISAM2.cpp +++ b/tests/testGaussianISAM2.cpp @@ -23,7 +23,6 @@ #include #include #include -#include #include @@ -37,7 +36,6 @@ using namespace gtsam; using boost::shared_ptr; static const SharedNoiseModel model; -static const LieScalar Zero(0); // SETDEBUG("ISAM2 update", true); // SETDEBUG("ISAM2 update verbose", true); @@ -286,22 +284,21 @@ bool isam_check(const NonlinearFactorGraph& fullgraph, const Values& fullinit, c } /* ************************************************************************* */ -TEST(ISAM2, AddFactorsStep1) -{ +TEST(ISAM2, AddFactorsStep1) { NonlinearFactorGraph nonlinearFactors; - nonlinearFactors += PriorFactor(10, Zero, model); + nonlinearFactors += PriorFactor(10, 0.0, model); nonlinearFactors += NonlinearFactor::shared_ptr(); - nonlinearFactors += PriorFactor(11, Zero, model); + nonlinearFactors += PriorFactor(11, 0.0, model); NonlinearFactorGraph newFactors; - newFactors += PriorFactor(1, Zero, model); - newFactors += PriorFactor(2, Zero, model); + newFactors += PriorFactor(1, 0.0, model); + newFactors += PriorFactor(2, 0.0, model); NonlinearFactorGraph expectedNonlinearFactors; - expectedNonlinearFactors += PriorFactor(10, Zero, model); - expectedNonlinearFactors += PriorFactor(1, Zero, model); - expectedNonlinearFactors += PriorFactor(11, Zero, model); - expectedNonlinearFactors += PriorFactor(2, Zero, model); + expectedNonlinearFactors += PriorFactor(10, 0.0, model); + expectedNonlinearFactors += PriorFactor(1, 0.0, model); + expectedNonlinearFactors += PriorFactor(11, 0.0, model); + expectedNonlinearFactors += PriorFactor(2, 0.0, model); const FactorIndices expectedNewFactorIndices = list_of(1)(3); @@ -313,7 +310,8 @@ TEST(ISAM2, AddFactorsStep1) update.addFactorsStep1(newFactors, &nonlinearFactors); EXPECT(assert_equal(expectedNonlinearFactors, nonlinearFactors)); - EXPECT(assert_container_equality(expectedNewFactorIndices, actualNewFactorIndices)); + EXPECT(assert_container_equality(expectedNewFactorIndices, + actualNewFactorIndices)); } /* ************************************************************************* */ @@ -698,25 +696,24 @@ namespace { } /* ************************************************************************* */ -TEST(ISAM2, marginalizeLeaves1) -{ +TEST(ISAM2, marginalizeLeaves1) { ISAM2 isam; NonlinearFactorGraph factors; - factors += PriorFactor(0, Zero, model); + factors += PriorFactor(0, 0.0, model); - factors += BetweenFactor(0, 1, Zero, model); - factors += BetweenFactor(1, 2, Zero, model); - factors += BetweenFactor(0, 2, Zero, model); + factors += BetweenFactor(0, 1, 0.0, model); + factors += BetweenFactor(1, 2, 0.0, model); + factors += BetweenFactor(0, 2, 0.0, model); Values values; - values.insert(0, Zero); - values.insert(1, Zero); - values.insert(2, Zero); + values.insert(0, 0.0); + values.insert(1, 0.0); + values.insert(2, 0.0); - FastMap constrainedKeys; - constrainedKeys.insert(make_pair(0,0)); - constrainedKeys.insert(make_pair(1,1)); - constrainedKeys.insert(make_pair(2,2)); + FastMap constrainedKeys; + constrainedKeys.insert(make_pair(0, 0)); + constrainedKeys.insert(make_pair(1, 1)); + constrainedKeys.insert(make_pair(2, 2)); isam.update(factors, values, FactorIndices(), constrainedKeys); @@ -725,29 +722,28 @@ TEST(ISAM2, marginalizeLeaves1) } /* ************************************************************************* */ -TEST(ISAM2, marginalizeLeaves2) -{ +TEST(ISAM2, marginalizeLeaves2) { ISAM2 isam; NonlinearFactorGraph factors; - factors += PriorFactor(0, Zero, model); + factors += PriorFactor(0, 0.0, model); - factors += BetweenFactor(0, 1, Zero, model); - factors += BetweenFactor(1, 2, Zero, model); - factors += BetweenFactor(0, 2, Zero, model); - factors += BetweenFactor(2, 3, Zero, model); + factors += BetweenFactor(0, 1, 0.0, model); + factors += BetweenFactor(1, 2, 0.0, model); + factors += BetweenFactor(0, 2, 0.0, model); + factors += BetweenFactor(2, 3, 0.0, model); Values values; - values.insert(0, Zero); - values.insert(1, Zero); - values.insert(2, Zero); - values.insert(3, Zero); + values.insert(0, 0.0); + values.insert(1, 0.0); + values.insert(2, 0.0); + values.insert(3, 0.0); - FastMap constrainedKeys; - constrainedKeys.insert(make_pair(0,0)); - constrainedKeys.insert(make_pair(1,1)); - constrainedKeys.insert(make_pair(2,2)); - constrainedKeys.insert(make_pair(3,3)); + FastMap constrainedKeys; + constrainedKeys.insert(make_pair(0, 0)); + constrainedKeys.insert(make_pair(1, 1)); + constrainedKeys.insert(make_pair(2, 2)); + constrainedKeys.insert(make_pair(3, 3)); isam.update(factors, values, FactorIndices(), constrainedKeys); @@ -756,38 +752,37 @@ TEST(ISAM2, marginalizeLeaves2) } /* ************************************************************************* */ -TEST(ISAM2, marginalizeLeaves3) -{ +TEST(ISAM2, marginalizeLeaves3) { ISAM2 isam; NonlinearFactorGraph factors; - factors += PriorFactor(0, Zero, model); + factors += PriorFactor(0, 0.0, model); - factors += BetweenFactor(0, 1, Zero, model); - factors += BetweenFactor(1, 2, Zero, model); - factors += BetweenFactor(0, 2, Zero, model); + factors += BetweenFactor(0, 1, 0.0, model); + factors += BetweenFactor(1, 2, 0.0, model); + factors += BetweenFactor(0, 2, 0.0, model); - factors += BetweenFactor(2, 3, Zero, model); + factors += BetweenFactor(2, 3, 0.0, model); - factors += BetweenFactor(3, 4, Zero, model); - factors += BetweenFactor(4, 5, Zero, model); - factors += BetweenFactor(3, 5, Zero, model); + factors += BetweenFactor(3, 4, 0.0, model); + factors += BetweenFactor(4, 5, 0.0, model); + factors += BetweenFactor(3, 5, 0.0, model); Values values; - values.insert(0, Zero); - values.insert(1, Zero); - values.insert(2, Zero); - values.insert(3, Zero); - values.insert(4, Zero); - values.insert(5, Zero); + values.insert(0, 0.0); + values.insert(1, 0.0); + values.insert(2, 0.0); + values.insert(3, 0.0); + values.insert(4, 0.0); + values.insert(5, 0.0); - FastMap constrainedKeys; - constrainedKeys.insert(make_pair(0,0)); - constrainedKeys.insert(make_pair(1,1)); - constrainedKeys.insert(make_pair(2,2)); - constrainedKeys.insert(make_pair(3,3)); - constrainedKeys.insert(make_pair(4,4)); - constrainedKeys.insert(make_pair(5,5)); + FastMap constrainedKeys; + constrainedKeys.insert(make_pair(0, 0)); + constrainedKeys.insert(make_pair(1, 1)); + constrainedKeys.insert(make_pair(2, 2)); + constrainedKeys.insert(make_pair(3, 3)); + constrainedKeys.insert(make_pair(4, 4)); + constrainedKeys.insert(make_pair(5, 5)); isam.update(factors, values, FactorIndices(), constrainedKeys); @@ -796,24 +791,23 @@ TEST(ISAM2, marginalizeLeaves3) } /* ************************************************************************* */ -TEST(ISAM2, marginalizeLeaves4) -{ +TEST(ISAM2, marginalizeLeaves4) { ISAM2 isam; NonlinearFactorGraph factors; - factors += PriorFactor(0, Zero, model); - factors += BetweenFactor(0, 2, Zero, model); - factors += BetweenFactor(1, 2, Zero, model); + factors += PriorFactor(0, 0.0, model); + factors += BetweenFactor(0, 2, 0.0, model); + factors += BetweenFactor(1, 2, 0.0, model); Values values; - values.insert(0, Zero); - values.insert(1, Zero); - values.insert(2, Zero); + values.insert(0, 0.0); + values.insert(1, 0.0); + values.insert(2, 0.0); - FastMap constrainedKeys; - constrainedKeys.insert(make_pair(0,0)); - constrainedKeys.insert(make_pair(1,1)); - constrainedKeys.insert(make_pair(2,2)); + FastMap constrainedKeys; + constrainedKeys.insert(make_pair(0, 0)); + constrainedKeys.insert(make_pair(1, 1)); + constrainedKeys.insert(make_pair(2, 2)); isam.update(factors, values, FactorIndices(), constrainedKeys); From 42b5f8163311979b2fcd2de0a5c2f1355d23242f Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 1 Jun 2019 17:49:05 -0400 Subject: [PATCH 09/21] Now using add_factors --- gtsam/nonlinear/ISAM2-impl.h | 50 +++++------------------------------- tests/testGaussianISAM2.cpp | 31 ---------------------- 2 files changed, 6 insertions(+), 75 deletions(-) diff --git a/gtsam/nonlinear/ISAM2-impl.h b/gtsam/nonlinear/ISAM2-impl.h index 352d26c8a..8a7db626b 100644 --- a/gtsam/nonlinear/ISAM2-impl.h +++ b/gtsam/nonlinear/ISAM2-impl.h @@ -135,48 +135,6 @@ struct GTSAM_EXPORT UpdateImpl { } } - /// 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. - FactorIndices addFactorsStep1(const NonlinearFactorGraph& newFactors, - NonlinearFactorGraph* nonlinearFactors) const { - FactorIndices newFactorIndices(newFactors.size()); - - if (params_.findUnusedFactorSlots) { - size_t globalFactorIndex = 0; - for (size_t newFactorIndex = 0; newFactorIndex < newFactors.size(); - ++newFactorIndex) { - // Loop to find the next available factor slot - do { - // If we need to add more factors than we have room for, resize - // nonlinearFactors, filling the new slots with NULL factors. - // Otherwise, check if the current factor in nonlinearFactors is - // already used, and 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]) - ++globalFactorIndex; - else - break; - } while (true); - - // Use the current slot, updating nonlinearFactors and newFactorSlots. - (*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); - } - return newFactorIndices; - } - // 1. Add any new factors \Factors:=\Factors\cup\Factors'. void pushBackFactors(const NonlinearFactorGraph& newFactors, NonlinearFactorGraph* nonlinearFactors, @@ -185,8 +143,12 @@ struct GTSAM_EXPORT UpdateImpl { ISAM2Result* result) const { gttic(pushBackFactors); - // Add the new factor indices to the result struct - result->newFactorsIndices = addFactorsStep1(newFactors, nonlinearFactors); + // 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. + result->newFactorsIndices = nonlinearFactors->add_factors( + newFactors, params_.findUnusedFactorSlots); // Remove the removed factors NonlinearFactorGraph removedFactors; diff --git a/tests/testGaussianISAM2.cpp b/tests/testGaussianISAM2.cpp index 8f01d22aa..52938d8db 100644 --- a/tests/testGaussianISAM2.cpp +++ b/tests/testGaussianISAM2.cpp @@ -283,37 +283,6 @@ bool isam_check(const NonlinearFactorGraph& fullgraph, const Values& fullinit, c return nodeGradientsOk && expectedGradOk && totalGradOk && isamEqual && isamTreeEqual && consistent; } -/* ************************************************************************* */ -TEST(ISAM2, AddFactorsStep1) { - NonlinearFactorGraph nonlinearFactors; - nonlinearFactors += PriorFactor(10, 0.0, model); - nonlinearFactors += NonlinearFactor::shared_ptr(); - nonlinearFactors += PriorFactor(11, 0.0, model); - - NonlinearFactorGraph newFactors; - newFactors += PriorFactor(1, 0.0, model); - newFactors += PriorFactor(2, 0.0, model); - - NonlinearFactorGraph expectedNonlinearFactors; - expectedNonlinearFactors += PriorFactor(10, 0.0, model); - expectedNonlinearFactors += PriorFactor(1, 0.0, model); - expectedNonlinearFactors += PriorFactor(11, 0.0, model); - expectedNonlinearFactors += PriorFactor(2, 0.0, model); - - const FactorIndices expectedNewFactorIndices = list_of(1)(3); - - ISAM2Params params; - ISAM2UpdateParams updateParams; - params.findUnusedFactorSlots = true; - UpdateImpl update(params, updateParams); - FactorIndices actualNewFactorIndices = - update.addFactorsStep1(newFactors, &nonlinearFactors); - - EXPECT(assert_equal(expectedNonlinearFactors, nonlinearFactors)); - EXPECT(assert_container_equality(expectedNewFactorIndices, - actualNewFactorIndices)); -} - /* ************************************************************************* */ TEST(ISAM2, simple) { From 696d8076d1cf5cb630306c469421045124910e1f Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 1 Jun 2019 23:55:50 -0400 Subject: [PATCH 10/21] Removed unusedIndices --- gtsam/nonlinear/ISAM2-impl.h | 13 ++++--------- gtsam/nonlinear/ISAM2Result.h | 4 ++-- 2 files changed, 6 insertions(+), 11 deletions(-) diff --git a/gtsam/nonlinear/ISAM2-impl.h b/gtsam/nonlinear/ISAM2-impl.h index 8a7db626b..f2592f5a1 100644 --- a/gtsam/nonlinear/ISAM2-impl.h +++ b/gtsam/nonlinear/ISAM2-impl.h @@ -179,11 +179,6 @@ struct GTSAM_EXPORT UpdateImpl { removedAndEmpty.begin(), removedAndEmpty.end(), newFactorSymbKeys.begin(), newFactorSymbKeys.end(), std::inserter(result->unusedKeys, result->unusedKeys.end())); - - // Get indices for unused keys - for (Key key : result->unusedKeys) { - result->unusedIndices.insert(result->unusedIndices.end(), key); - } } // 3. Mark linear update @@ -221,7 +216,7 @@ struct GTSAM_EXPORT UpdateImpl { for (Key index : result->markedKeys) { // Only add if not unused - if (result->unusedIndices.find(index) == result->unusedIndices.end()) + if (result->unusedKeys.find(index) == result->unusedKeys.end()) // Make a copy of these, as we'll soon add to them result->observedKeys.push_back(index); } @@ -631,9 +626,9 @@ struct GTSAM_EXPORT UpdateImpl { VariableIndex affectedFactorsVarIndex = variableIndex; affectedFactorsVarIndex.removeUnusedVariables( - result->unusedIndices.begin(), result->unusedIndices.end()); + result->unusedKeys.begin(), result->unusedKeys.end()); - for (const Key key : result->unusedIndices) { + for (const Key key : result->unusedKeys) { affectedKeysSet.erase(key); } gttoc(add_keys); @@ -777,7 +772,7 @@ struct GTSAM_EXPORT UpdateImpl { for (FastMap::iterator iter = constraintGroups.begin(); iter != constraintGroups.end(); /*Incremented in loop ++iter*/) { - if (result->unusedIndices.exists(iter->first) || + if (result->unusedKeys.exists(iter->first) || !affectedKeysSet.exists(iter->first)) constraintGroups.erase(iter++); else diff --git a/gtsam/nonlinear/ISAM2Result.h b/gtsam/nonlinear/ISAM2Result.h index d49c92627..17968453d 100644 --- a/gtsam/nonlinear/ISAM2Result.h +++ b/gtsam/nonlinear/ISAM2Result.h @@ -96,10 +96,10 @@ struct GTSAM_EXPORT ISAM2Result { */ FactorIndices newFactorsIndices; - /** Unused keys, and indices for unused keys. TODO(frank): the same?? + /** Unused keys, and indices for unused keys, * i.e., keys that are empty now and do not appear in the new factors. */ - KeySet unusedKeys, unusedIndices; + KeySet unusedKeys; /** keys for variables that were observed, i.e., not unused. */ KeyVector observedKeys; From 8b01b81027adadd5d2c002331e6f95acd04c356b Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 2 Jun 2019 00:32:06 -0400 Subject: [PATCH 11/21] Split up relinearize again for clarity --- gtsam/nonlinear/ISAM2-impl.h | 146 ++++++++++++++++------------------- gtsam/nonlinear/ISAM2.cpp | 21 +++-- tests/testGaussianISAM2.cpp | 1 - 3 files changed, 84 insertions(+), 84 deletions(-) diff --git a/gtsam/nonlinear/ISAM2-impl.h b/gtsam/nonlinear/ISAM2-impl.h index f2592f5a1..91cdd8c2e 100644 --- a/gtsam/nonlinear/ISAM2-impl.h +++ b/gtsam/nonlinear/ISAM2-impl.h @@ -135,7 +135,7 @@ struct GTSAM_EXPORT UpdateImpl { } } - // 1. Add any new factors \Factors:=\Factors\cup\Factors'. + // Add any new factors \Factors:=\Factors\cup\Factors'. void pushBackFactors(const NonlinearFactorGraph& newFactors, NonlinearFactorGraph* nonlinearFactors, GaussianFactorGraph* linearFactors, @@ -181,7 +181,7 @@ struct GTSAM_EXPORT UpdateImpl { std::inserter(result->unusedKeys, result->unusedKeys.end())); } - // 3. Mark linear update + // Mark linear update void gatherInvolvedKeys(const NonlinearFactorGraph& newFactors, const NonlinearFactorGraph& nonlinearFactors, ISAM2Result* result) const { @@ -197,6 +197,7 @@ struct GTSAM_EXPORT UpdateImpl { result->markedKeys.insert(key); } } + // Also, keys that were not observed in existing factors, but whose affected // keys have been extended now (e.g. smart factors) if (updateParams_.newAffectedKeys) { @@ -343,6 +344,67 @@ struct GTSAM_EXPORT UpdateImpl { return relinKeys; } + // Mark keys in \Delta above threshold \beta: + KeySet gatherRelinearizeKeys(const ISAM2::Roots& roots, + const VectorValues& delta, + const KeySet& fixedVariables, + ISAM2Result* result) const { + gttic(gatherRelinearizeKeys); + // J=\{\Delta_{j}\in\Delta|\Delta_{j}\geq\beta\}. + KeySet relinKeys = + params_.enablePartialRelinearizationCheck + ? CheckRelinearizationPartial(roots, delta, + params_.relinearizeThreshold) + : CheckRelinearizationFull(delta, params_.relinearizeThreshold); + if (updateParams_.forceFullSolve) + relinKeys = CheckRelinearizationFull(delta, 0.0); // for debugging + + // Remove from relinKeys any keys whose linearization points are fixed + for (Key key : fixedVariables) { + relinKeys.erase(key); + } + if (updateParams_.noRelinKeys) { + for (Key key : *updateParams_.noRelinKeys) { + relinKeys.erase(key); + } + } + + // Record above relinerization threshold keys in detailed results + if (params_.enableDetailedResults) { + for (Key key : relinKeys) { + result->detail->variableStatus[key].isAboveRelinThreshold = true; + result->detail->variableStatus[key].isRelinearized = true; + } + } + + // Add the variables being relinearized to the marked keys + result->markedKeys.insert(relinKeys.begin(), relinKeys.end()); + return relinKeys; + } + + // Mark all cliques that involve marked variables \Theta_{J} and all + // their ancestors. + void fluidFindAll(const ISAM2::Roots& roots, const KeySet& relinKeys, + ISAM2Result* result) const { + gttic(fluidFindAll); + for (const auto& root : roots) + // add other cliques that have the marked ones in the separator + root->findAll(relinKeys, &result->markedKeys); + + // Relinearization-involved keys for detailed results + if (params_.enableDetailedResults) { + KeySet involvedRelinKeys; + for (const auto& root : roots) + root->findAll(relinKeys, &involvedRelinKeys); + for (Key key : involvedRelinKeys) { + if (!result->detail->variableStatus[key].isAboveRelinThreshold) { + result->detail->variableStatus[key].isRelinearizeInvolved = true; + result->detail->variableStatus[key].isRelinearized = true; + } + } + } + } + /** * Apply expmap to the given values, but only for indices appearing in * \c mask. Values are expmapped in-place. @@ -350,6 +412,7 @@ struct GTSAM_EXPORT UpdateImpl { */ static void ExpmapMasked(const VectorValues& delta, const KeySet& mask, Values* theta) { + gttic(ExpmapMasked); assert(theta->size() == delta.size()); Values::iterator key_value; VectorValues::const_iterator key_delta; @@ -372,80 +435,7 @@ struct GTSAM_EXPORT UpdateImpl { } } - KeySet relinearize(const ISAM2::Roots& roots, const VectorValues& delta, - const KeySet& fixedVariables, Values* theta, - ISAM2Result* result) const { - KeySet relinKeys; - gttic(gather_relinearize_keys); - // 4. Mark keys in \Delta above threshold \beta: - // J=\{\Delta_{j}\in\Delta|\Delta_{j}\geq\beta\}. - if (params_.enablePartialRelinearizationCheck) - relinKeys = CheckRelinearizationPartial(roots, delta, - params_.relinearizeThreshold); - else - relinKeys = CheckRelinearizationFull(delta, params_.relinearizeThreshold); - if (updateParams_.forceFullSolve) - relinKeys = - CheckRelinearizationFull(delta, 0.0); // This is used for debugging - - // Remove from relinKeys any keys whose linearization points are fixed - for (Key key : fixedVariables) { - relinKeys.erase(key); - } - if (updateParams_.noRelinKeys) { - for (Key key : *updateParams_.noRelinKeys) { - relinKeys.erase(key); - } - } - - // Above relin threshold keys for detailed results - if (params_.enableDetailedResults) { - for (Key key : relinKeys) { - result->detail->variableStatus[key].isAboveRelinThreshold = true; - result->detail->variableStatus[key].isRelinearized = true; - } - } - - // Add the variables being relinearized to the marked keys - KeySet markedRelinMask; - for (const Key key : relinKeys) markedRelinMask.insert(key); - result->markedKeys.insert(relinKeys.begin(), relinKeys.end()); - gttoc(gather_relinearize_keys); - - gttic(fluid_find_all); - // 5. Mark all cliques that involve marked variables \Theta_{J} and all - // their ancestors. - if (!relinKeys.empty()) { - for (const auto& root : roots) - // add other cliques that have the marked ones in the separator - root->findAll(markedRelinMask, &result->markedKeys); - - // Relin involved keys for detailed results - if (params_.enableDetailedResults) { - KeySet involvedRelinKeys; - for (const auto& root : roots) - root->findAll(markedRelinMask, &involvedRelinKeys); - for (Key key : involvedRelinKeys) { - if (!result->detail->variableStatus[key].isAboveRelinThreshold) { - result->detail->variableStatus[key].isRelinearizeInvolved = true; - result->detail->variableStatus[key].isRelinearized = true; - } - } - } - } - gttoc(fluid_find_all); - - gttic(expmap); - // 6. Update linearization point for marked variables: - // \Theta_{J}:=\Theta_{J}+\Delta_{J}. - if (!relinKeys.empty()) ExpmapMasked(delta, markedRelinMask, theta); - gttoc(expmap); - - result->variablesRelinearized = result->markedKeys.size(); - return relinKeys; - } - - // 7. Linearize new factors + // Linearize new factors void linearizeNewFactors(const NonlinearFactorGraph& newFactors, const Values& theta, size_t numNonlinearFactors, const FactorIndices& newFactorsIndices, @@ -625,8 +615,8 @@ struct GTSAM_EXPORT UpdateImpl { // Removed unused keys: VariableIndex affectedFactorsVarIndex = variableIndex; - affectedFactorsVarIndex.removeUnusedVariables( - result->unusedKeys.begin(), result->unusedKeys.end()); + affectedFactorsVarIndex.removeUnusedVariables(result->unusedKeys.begin(), + result->unusedKeys.end()); for (const Key key : result->unusedKeys) { affectedKeysSet.erase(key); diff --git a/gtsam/nonlinear/ISAM2.cpp b/gtsam/nonlinear/ISAM2.cpp index d6a48c6d5..cce698be4 100644 --- a/gtsam/nonlinear/ISAM2.cpp +++ b/gtsam/nonlinear/ISAM2.cpp @@ -119,9 +119,6 @@ ISAM2Result ISAM2::update(const NonlinearFactorGraph& newFactors, this->update_count_++; UpdateImpl::LogStartingUpdate(newFactors, *this); - ISAM2Result result; - if (params_.enableDetailedResults) - result.detail = ISAM2Result::DetailedResults(); const bool relinearizeThisStep = updateParams.force_relinearize || (params_.enableRelinearization && @@ -132,6 +129,9 @@ ISAM2Result ISAM2::update(const NonlinearFactorGraph& newFactors, updateDelta(updateParams.forceFullSolve); } + ISAM2Result result; + if (params_.enableDetailedResults) + result.detail = ISAM2Result::DetailedResults(); UpdateImpl update(params_, updateParams); // 1. Add any new factors \Factors:=\Factors\cup\Factors'. @@ -150,14 +150,25 @@ ISAM2Result ISAM2::update(const NonlinearFactorGraph& newFactors, update.gatherInvolvedKeys(newFactors, nonlinearFactors_, &result); // Check relinearization if we're at the nth step, or we are using a looser - // loop relin threshold + // loop relinerization threshold. KeySet relinKeys; if (relinearizeThisStep) { + // 4. Mark keys in \Delta above threshold \beta: relinKeys = - update.relinearize(roots_, delta_, fixedVariables_, &theta_, &result); + update.gatherRelinearizeKeys(roots_, delta_, fixedVariables_, &result); + if (!relinKeys.empty()) { + // 5. Mark all cliques that involve marked variables \Theta_{J} and all + // their ancestors. + update.fluidFindAll(roots_, relinKeys, &result); + // 6. Update linearization point for marked variables: + // \Theta_{J}:=\Theta_{J}+\Delta_{J}. + UpdateImpl::ExpmapMasked(delta_, relinKeys, &theta_); + } + result.variablesRelinearized = result.markedKeys.size(); } else { result.variablesRelinearized = 0; } + // TODO(frank): should be result.variablesRelinearized = relinKeys.size(); ? // 7. Linearize new factors update.linearizeNewFactors(newFactors, theta_, nonlinearFactors_.size(), diff --git a/tests/testGaussianISAM2.cpp b/tests/testGaussianISAM2.cpp index 52938d8db..871e4bc18 100644 --- a/tests/testGaussianISAM2.cpp +++ b/tests/testGaussianISAM2.cpp @@ -5,7 +5,6 @@ */ #include -#include #include #include From 38d59eed68e3c5fea07391f880f9ad8d9908d769 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 2 Jun 2019 01:09:01 -0400 Subject: [PATCH 12/21] Split recalculate for clarity --- gtsam/nonlinear/ISAM2-impl.h | 408 ++++++++++++++++++----------------- 1 file changed, 212 insertions(+), 196 deletions(-) diff --git a/gtsam/nonlinear/ISAM2-impl.h b/gtsam/nonlinear/ISAM2-impl.h index 91cdd8c2e..65441da0e 100644 --- a/gtsam/nonlinear/ISAM2-impl.h +++ b/gtsam/nonlinear/ISAM2-impl.h @@ -566,6 +566,205 @@ struct GTSAM_EXPORT UpdateImpl { return linearized; } + // Do a batch step - reorder and relinearize all variables + void recalculateBatch(const Values& theta, const VariableIndex& variableIndex, + const NonlinearFactorGraph& nonlinearFactors, + GaussianFactorGraph* linearFactors, + KeySet* affectedKeysSet, ISAM2::Roots* roots, + ISAM2::Nodes* nodes, ISAM2Result* result) const { + gttic(batch); + + gttic(add_keys); + br::copy(variableIndex | br::map_keys, + std::inserter(*affectedKeysSet, affectedKeysSet->end())); + + // Removed unused keys: + VariableIndex affectedFactorsVarIndex = variableIndex; + + affectedFactorsVarIndex.removeUnusedVariables(result->unusedKeys.begin(), + result->unusedKeys.end()); + + for (const Key key : result->unusedKeys) { + affectedKeysSet->erase(key); + } + gttoc(add_keys); + + gttic(ordering); + Ordering order; + if (updateParams_.constrainedKeys) { + order = Ordering::ColamdConstrained(affectedFactorsVarIndex, + *updateParams_.constrainedKeys); + } else { + if (theta.size() > result->observedKeys.size()) { + // Only if some variables are unconstrained + FastMap constraintGroups; + for (Key var : result->observedKeys) constraintGroups[var] = 1; + order = Ordering::ColamdConstrained(affectedFactorsVarIndex, + constraintGroups); + } else { + order = Ordering::Colamd(affectedFactorsVarIndex); + } + } + gttoc(ordering); + + gttic(linearize); + GaussianFactorGraph linearized = *nonlinearFactors.linearize(theta); + if (params_.cacheLinearizedFactors) *linearFactors = linearized; + gttoc(linearize); + + gttic(eliminate); + ISAM2BayesTree::shared_ptr bayesTree = + ISAM2JunctionTree( + GaussianEliminationTree(linearized, affectedFactorsVarIndex, order)) + .eliminate(params_.getEliminationFunction()) + .first; + gttoc(eliminate); + + gttic(insert); + roots->clear(); + roots->insert(roots->end(), bayesTree->roots().begin(), + bayesTree->roots().end()); + nodes->clear(); + nodes->insert(bayesTree->nodes().begin(), bayesTree->nodes().end()); + gttoc(insert); + + result->variablesReeliminated = affectedKeysSet->size(); + result->factorsRecalculated = nonlinearFactors.size(); + + // Reeliminated keys for detailed results + if (params_.enableDetailedResults) { + for (Key key : theta.keys()) { + result->detail->variableStatus[key].isReeliminated = true; + } + } + } + + void recalculateIncremental(const Values& theta, + const VariableIndex& variableIndex, + const NonlinearFactorGraph& nonlinearFactors, + const ISAM2::Cliques& orphans, + const KeySet& relinKeys, + GaussianFactorGraph* linearFactors, + const FastList& affectedKeys, + KeySet* affectedKeysSet, ISAM2::Roots* roots, + ISAM2::Nodes* nodes, ISAM2Result* result) const { + gttic(incremental); + const bool debug = ISDEBUG("ISAM2 recalculate"); + + // 2. Add the new factors \Factors' into the resulting factor graph + FastList affectedAndNewKeys; + affectedAndNewKeys.insert(affectedAndNewKeys.end(), affectedKeys.begin(), + affectedKeys.end()); + affectedAndNewKeys.insert(affectedAndNewKeys.end(), + result->observedKeys.begin(), + result->observedKeys.end()); + gttic(relinearizeAffected); + GaussianFactorGraph factors = relinearizeAffectedFactors( + affectedAndNewKeys, relinKeys, nonlinearFactors, variableIndex, theta, + linearFactors); + gttoc(relinearizeAffected); + + if (debug) { + factors.print("Relinearized factors: "); + std::cout << "Affected keys: "; + for (const Key key : affectedKeys) { + std::cout << key << " "; + } + std::cout << std::endl; + } + + // Reeliminated keys for detailed results + if (params_.enableDetailedResults) { + for (Key key : affectedAndNewKeys) { + result->detail->variableStatus[key].isReeliminated = true; + } + } + + result->variablesReeliminated = affectedAndNewKeys.size(); + result->factorsRecalculated = factors.size(); + + gttic(cached); + // add the cached intermediate results from the boundary of the orphans + // ... + GaussianFactorGraph cachedBoundary = GetCachedBoundaryFactors(orphans); + if (debug) cachedBoundary.print("Boundary factors: "); + factors.push_back(cachedBoundary); + gttoc(cached); + + gttic(orphans); + // Add the orphaned subtrees + for (const auto& orphan : orphans) + factors += + boost::make_shared >(orphan); + gttoc(orphans); + + // END OF COPIED CODE + + // 3. Re-order and eliminate the factor graph into a Bayes net (Algorithm + // [alg:eliminate]), and re-assemble into a new Bayes tree (Algorithm + // [alg:BayesTree]) + + gttic(reorder_and_eliminate); + + gttic(list_to_set); + // create a partial reordering for the new and contaminated factors + // result->markedKeys are passed in: those variables will be forced to the + // end in the ordering + affectedKeysSet->insert(result->markedKeys.begin(), + result->markedKeys.end()); + affectedKeysSet->insert(affectedKeys.begin(), affectedKeys.end()); + gttoc(list_to_set); + + VariableIndex affectedFactorsVarIndex(factors); + + gttic(ordering_constraints); + // Create ordering constraints + FastMap constraintGroups; + if (updateParams_.constrainedKeys) { + constraintGroups = *updateParams_.constrainedKeys; + } else { + constraintGroups = FastMap(); + const int group = + result->observedKeys.size() < affectedFactorsVarIndex.size() ? 1 : 0; + for (Key var : result->observedKeys) + constraintGroups.insert(std::make_pair(var, group)); + } + + // Remove unaffected keys from the constraints + for (FastMap::iterator iter = constraintGroups.begin(); + iter != constraintGroups.end(); + /*Incremented in loop ++iter*/) { + if (result->unusedKeys.exists(iter->first) || + !affectedKeysSet->exists(iter->first)) + constraintGroups.erase(iter++); + else + ++iter; + } + gttoc(ordering_constraints); + + // Generate ordering + gttic(Ordering); + Ordering ordering = + Ordering::ColamdConstrained(affectedFactorsVarIndex, constraintGroups); + gttoc(Ordering); + + ISAM2BayesTree::shared_ptr bayesTree = + ISAM2JunctionTree( + GaussianEliminationTree(factors, affectedFactorsVarIndex, ordering)) + .eliminate(params_.getEliminationFunction()) + .first; + + gttoc(reorder_and_eliminate); + + gttic(reassemble); + roots->insert(roots->end(), bayesTree->roots().begin(), + bayesTree->roots().end()); + nodes->insert(bayesTree->nodes().begin(), bayesTree->nodes().end()); + gttoc(reassemble); + + // 4. The orphans have already been inserted during elimination + } + KeySet recalculate(const Values& theta, const VariableIndex& variableIndex, const NonlinearFactorGraph& nonlinearFactors, const GaussianBayesNet& affectedBayesNet, @@ -579,15 +778,14 @@ struct GTSAM_EXPORT UpdateImpl { // bug was here: we cannot reuse the original factors, because then the // cached factors get messed up [all the necessary data is actually // contained in the affectedBayesNet, including what was passed in from the - // boundaries, - // so this would be correct; however, in the process we also generate new - // cached_ entries that will be wrong (ie. they don't contain what would be - // passed up at a certain point if batch elimination was done, but that's - // what we need); we could choose not to update cached_ from here, but then - // the new information (and potentially different variable ordering) is not - // reflected in the cached_ values which again will be wrong] - // so instead we have to retrieve the original linearized factors AND add - // the cached factors from the boundary + // boundaries, so this would be correct; however, in the process we also + // generate new cached_ entries that will be wrong (ie. they don't contain + // what would be passed up at a certain point if batch elimination was done, + // but that's what we need); we could choose not to update cached_ from + // here, but then the new information (and potentially different variable + // ordering) is not reflected in the cached_ values which again will be + // wrong] so instead we have to retrieve the original linearized factors AND + // add the cached factors from the boundary // BEGIN OF COPIED CODE @@ -603,196 +801,14 @@ struct GTSAM_EXPORT UpdateImpl { KeySet affectedKeysSet; // Will return this result static const double kBatchThreshold = 0.65; - if (affectedKeys.size() >= theta.size() * kBatchThreshold) { // Do a batch step - reorder and relinearize all variables - gttic(batch); - - gttic(add_keys); - br::copy(variableIndex | br::map_keys, - std::inserter(affectedKeysSet, affectedKeysSet.end())); - - // Removed unused keys: - VariableIndex affectedFactorsVarIndex = variableIndex; - - affectedFactorsVarIndex.removeUnusedVariables(result->unusedKeys.begin(), - result->unusedKeys.end()); - - for (const Key key : result->unusedKeys) { - affectedKeysSet.erase(key); - } - gttoc(add_keys); - - gttic(ordering); - Ordering order; - if (updateParams_.constrainedKeys) { - order = Ordering::ColamdConstrained(affectedFactorsVarIndex, - *updateParams_.constrainedKeys); - } else { - if (theta.size() > result->observedKeys.size()) { - // Only if some variables are unconstrained - FastMap constraintGroups; - for (Key var : result->observedKeys) constraintGroups[var] = 1; - order = Ordering::ColamdConstrained(affectedFactorsVarIndex, - constraintGroups); - } else { - order = Ordering::Colamd(affectedFactorsVarIndex); - } - } - gttoc(ordering); - - gttic(linearize); - GaussianFactorGraph linearized = *nonlinearFactors.linearize(theta); - if (params_.cacheLinearizedFactors) *linearFactors = linearized; - gttoc(linearize); - - gttic(eliminate); - ISAM2BayesTree::shared_ptr bayesTree = - ISAM2JunctionTree(GaussianEliminationTree( - linearized, affectedFactorsVarIndex, order)) - .eliminate(params_.getEliminationFunction()) - .first; - gttoc(eliminate); - - gttic(insert); - roots->clear(); - roots->insert(roots->end(), bayesTree->roots().begin(), - bayesTree->roots().end()); - nodes->clear(); - nodes->insert(bayesTree->nodes().begin(), bayesTree->nodes().end()); - gttoc(insert); - - result->variablesReeliminated = affectedKeysSet.size(); - result->factorsRecalculated = nonlinearFactors.size(); - - // Reeliminated keys for detailed results - if (params_.enableDetailedResults) { - for (Key key : theta.keys()) { - result->detail->variableStatus[key].isReeliminated = true; - } - } - - gttoc(batch); - + recalculateBatch(theta, variableIndex, nonlinearFactors, linearFactors, + &affectedKeysSet, roots, nodes, result); } else { - gttic(incremental); - const bool debug = ISDEBUG("ISAM2 recalculate"); - - // 2. Add the new factors \Factors' into the resulting factor graph - FastList affectedAndNewKeys; - affectedAndNewKeys.insert(affectedAndNewKeys.end(), affectedKeys.begin(), - affectedKeys.end()); - affectedAndNewKeys.insert(affectedAndNewKeys.end(), - result->observedKeys.begin(), - result->observedKeys.end()); - gttic(relinearizeAffected); - GaussianFactorGraph factors = relinearizeAffectedFactors( - affectedAndNewKeys, relinKeys, nonlinearFactors, variableIndex, theta, - linearFactors); - gttoc(relinearizeAffected); - - if (debug) { - factors.print("Relinearized factors: "); - std::cout << "Affected keys: "; - for (const Key key : affectedKeys) { - std::cout << key << " "; - } - std::cout << std::endl; - } - - // Reeliminated keys for detailed results - if (params_.enableDetailedResults) { - for (Key key : affectedAndNewKeys) { - result->detail->variableStatus[key].isReeliminated = true; - } - } - - result->variablesReeliminated = affectedAndNewKeys.size(); - result->factorsRecalculated = factors.size(); - - gttic(cached); - // add the cached intermediate results from the boundary of the orphans - // ... - GaussianFactorGraph cachedBoundary = GetCachedBoundaryFactors(orphans); - if (debug) cachedBoundary.print("Boundary factors: "); - factors.push_back(cachedBoundary); - gttoc(cached); - - gttic(orphans); - // Add the orphaned subtrees - for (const auto& orphan : orphans) - factors += - boost::make_shared >(orphan); - gttoc(orphans); - - // END OF COPIED CODE - - // 3. Re-order and eliminate the factor graph into a Bayes net (Algorithm - // [alg:eliminate]), and re-assemble into a new Bayes tree (Algorithm - // [alg:BayesTree]) - - gttic(reorder_and_eliminate); - - gttic(list_to_set); - // create a partial reordering for the new and contaminated factors - // result->markedKeys are passed in: those variables will be forced to the - // end in the ordering - affectedKeysSet.insert(result->markedKeys.begin(), - result->markedKeys.end()); - affectedKeysSet.insert(affectedKeys.begin(), affectedKeys.end()); - gttoc(list_to_set); - - VariableIndex affectedFactorsVarIndex(factors); - - gttic(ordering_constraints); - // Create ordering constraints - FastMap constraintGroups; - if (updateParams_.constrainedKeys) { - constraintGroups = *updateParams_.constrainedKeys; - } else { - constraintGroups = FastMap(); - const int group = - result->observedKeys.size() < affectedFactorsVarIndex.size() ? 1 - : 0; - for (Key var : result->observedKeys) - constraintGroups.insert(std::make_pair(var, group)); - } - - // Remove unaffected keys from the constraints - for (FastMap::iterator iter = constraintGroups.begin(); - iter != constraintGroups.end(); - /*Incremented in loop ++iter*/) { - if (result->unusedKeys.exists(iter->first) || - !affectedKeysSet.exists(iter->first)) - constraintGroups.erase(iter++); - else - ++iter; - } - gttoc(ordering_constraints); - - // Generate ordering - gttic(Ordering); - Ordering ordering = Ordering::ColamdConstrained(affectedFactorsVarIndex, - constraintGroups); - gttoc(Ordering); - - ISAM2BayesTree::shared_ptr bayesTree = - ISAM2JunctionTree(GaussianEliminationTree( - factors, affectedFactorsVarIndex, ordering)) - .eliminate(params_.getEliminationFunction()) - .first; - - gttoc(reorder_and_eliminate); - - gttic(reassemble); - roots->insert(roots->end(), bayesTree->roots().begin(), - bayesTree->roots().end()); - nodes->insert(bayesTree->nodes().begin(), bayesTree->nodes().end()); - gttoc(reassemble); - - // 4. The orphans have already been inserted during elimination - - gttoc(incremental); + recalculateIncremental(theta, variableIndex, nonlinearFactors, orphans, + relinKeys, linearFactors, affectedKeys, + &affectedKeysSet, roots, nodes, result); } // Root clique variables for detailed results From 1757198463c252f0406ee9f2c8863f974b5cf801 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 2 Jun 2019 01:51:14 -0400 Subject: [PATCH 13/21] Moved recalculate back to iSAM2.cpp --- gtsam/nonlinear/ISAM2-impl.h | 308 ----------------------------------- gtsam/nonlinear/ISAM2.cpp | 294 ++++++++++++++++++++++++++++++++- gtsam/nonlinear/ISAM2.h | 21 +++ 3 files changed, 312 insertions(+), 311 deletions(-) diff --git a/gtsam/nonlinear/ISAM2-impl.h b/gtsam/nonlinear/ISAM2-impl.h index 65441da0e..b6e5663bf 100644 --- a/gtsam/nonlinear/ISAM2-impl.h +++ b/gtsam/nonlinear/ISAM2-impl.h @@ -512,314 +512,6 @@ struct GTSAM_EXPORT UpdateImpl { return cachedBoundary; } - - // retrieve all factors that ONLY contain the affected variables - // (note that the remaining stuff is summarized in the cached factors) - GaussianFactorGraph relinearizeAffectedFactors( - const FastList& affectedKeys, const KeySet& relinKeys, - const NonlinearFactorGraph& nonlinearFactors, - const VariableIndex& variableIndex, const Values& theta, - GaussianFactorGraph* linearFactors) const { - FactorIndexSet candidates = GetAffectedFactors(affectedKeys, variableIndex); - - gttic(affectedKeysSet); - // for fast lookup below - KeySet affectedKeysSet; - affectedKeysSet.insert(affectedKeys.begin(), affectedKeys.end()); - gttoc(affectedKeysSet); - - gttic(check_candidates_and_linearize); - GaussianFactorGraph linearized; - for (const FactorIndex idx : candidates) { - bool inside = true; - bool useCachedLinear = params_.cacheLinearizedFactors; - for (Key key : nonlinearFactors[idx]->keys()) { - if (affectedKeysSet.find(key) == affectedKeysSet.end()) { - inside = false; - break; - } - if (useCachedLinear && relinKeys.find(key) != relinKeys.end()) - useCachedLinear = false; - } - if (inside) { - if (useCachedLinear) { -#ifdef GTSAM_EXTRA_CONSISTENCY_CHECKS - assert((*linearFactors)[idx]); - assert((*linearFactors)[idx]->keys() == - nonlinearFactors[idx]->keys()); -#endif - linearized.push_back((*linearFactors)[idx]); - } else { - auto linearFactor = nonlinearFactors[idx]->linearize(theta); - linearized.push_back(linearFactor); - if (params_.cacheLinearizedFactors) { -#ifdef GTSAM_EXTRA_CONSISTENCY_CHECKS - assert((*linearFactors)[idx]->keys() == linearFactor->keys()); -#endif - (*linearFactors)[idx] = linearFactor; - } - } - } - } - gttoc(check_candidates_and_linearize); - - return linearized; - } - - // Do a batch step - reorder and relinearize all variables - void recalculateBatch(const Values& theta, const VariableIndex& variableIndex, - const NonlinearFactorGraph& nonlinearFactors, - GaussianFactorGraph* linearFactors, - KeySet* affectedKeysSet, ISAM2::Roots* roots, - ISAM2::Nodes* nodes, ISAM2Result* result) const { - gttic(batch); - - gttic(add_keys); - br::copy(variableIndex | br::map_keys, - std::inserter(*affectedKeysSet, affectedKeysSet->end())); - - // Removed unused keys: - VariableIndex affectedFactorsVarIndex = variableIndex; - - affectedFactorsVarIndex.removeUnusedVariables(result->unusedKeys.begin(), - result->unusedKeys.end()); - - for (const Key key : result->unusedKeys) { - affectedKeysSet->erase(key); - } - gttoc(add_keys); - - gttic(ordering); - Ordering order; - if (updateParams_.constrainedKeys) { - order = Ordering::ColamdConstrained(affectedFactorsVarIndex, - *updateParams_.constrainedKeys); - } else { - if (theta.size() > result->observedKeys.size()) { - // Only if some variables are unconstrained - FastMap constraintGroups; - for (Key var : result->observedKeys) constraintGroups[var] = 1; - order = Ordering::ColamdConstrained(affectedFactorsVarIndex, - constraintGroups); - } else { - order = Ordering::Colamd(affectedFactorsVarIndex); - } - } - gttoc(ordering); - - gttic(linearize); - GaussianFactorGraph linearized = *nonlinearFactors.linearize(theta); - if (params_.cacheLinearizedFactors) *linearFactors = linearized; - gttoc(linearize); - - gttic(eliminate); - ISAM2BayesTree::shared_ptr bayesTree = - ISAM2JunctionTree( - GaussianEliminationTree(linearized, affectedFactorsVarIndex, order)) - .eliminate(params_.getEliminationFunction()) - .first; - gttoc(eliminate); - - gttic(insert); - roots->clear(); - roots->insert(roots->end(), bayesTree->roots().begin(), - bayesTree->roots().end()); - nodes->clear(); - nodes->insert(bayesTree->nodes().begin(), bayesTree->nodes().end()); - gttoc(insert); - - result->variablesReeliminated = affectedKeysSet->size(); - result->factorsRecalculated = nonlinearFactors.size(); - - // Reeliminated keys for detailed results - if (params_.enableDetailedResults) { - for (Key key : theta.keys()) { - result->detail->variableStatus[key].isReeliminated = true; - } - } - } - - void recalculateIncremental(const Values& theta, - const VariableIndex& variableIndex, - const NonlinearFactorGraph& nonlinearFactors, - const ISAM2::Cliques& orphans, - const KeySet& relinKeys, - GaussianFactorGraph* linearFactors, - const FastList& affectedKeys, - KeySet* affectedKeysSet, ISAM2::Roots* roots, - ISAM2::Nodes* nodes, ISAM2Result* result) const { - gttic(incremental); - const bool debug = ISDEBUG("ISAM2 recalculate"); - - // 2. Add the new factors \Factors' into the resulting factor graph - FastList affectedAndNewKeys; - affectedAndNewKeys.insert(affectedAndNewKeys.end(), affectedKeys.begin(), - affectedKeys.end()); - affectedAndNewKeys.insert(affectedAndNewKeys.end(), - result->observedKeys.begin(), - result->observedKeys.end()); - gttic(relinearizeAffected); - GaussianFactorGraph factors = relinearizeAffectedFactors( - affectedAndNewKeys, relinKeys, nonlinearFactors, variableIndex, theta, - linearFactors); - gttoc(relinearizeAffected); - - if (debug) { - factors.print("Relinearized factors: "); - std::cout << "Affected keys: "; - for (const Key key : affectedKeys) { - std::cout << key << " "; - } - std::cout << std::endl; - } - - // Reeliminated keys for detailed results - if (params_.enableDetailedResults) { - for (Key key : affectedAndNewKeys) { - result->detail->variableStatus[key].isReeliminated = true; - } - } - - result->variablesReeliminated = affectedAndNewKeys.size(); - result->factorsRecalculated = factors.size(); - - gttic(cached); - // add the cached intermediate results from the boundary of the orphans - // ... - GaussianFactorGraph cachedBoundary = GetCachedBoundaryFactors(orphans); - if (debug) cachedBoundary.print("Boundary factors: "); - factors.push_back(cachedBoundary); - gttoc(cached); - - gttic(orphans); - // Add the orphaned subtrees - for (const auto& orphan : orphans) - factors += - boost::make_shared >(orphan); - gttoc(orphans); - - // END OF COPIED CODE - - // 3. Re-order and eliminate the factor graph into a Bayes net (Algorithm - // [alg:eliminate]), and re-assemble into a new Bayes tree (Algorithm - // [alg:BayesTree]) - - gttic(reorder_and_eliminate); - - gttic(list_to_set); - // create a partial reordering for the new and contaminated factors - // result->markedKeys are passed in: those variables will be forced to the - // end in the ordering - affectedKeysSet->insert(result->markedKeys.begin(), - result->markedKeys.end()); - affectedKeysSet->insert(affectedKeys.begin(), affectedKeys.end()); - gttoc(list_to_set); - - VariableIndex affectedFactorsVarIndex(factors); - - gttic(ordering_constraints); - // Create ordering constraints - FastMap constraintGroups; - if (updateParams_.constrainedKeys) { - constraintGroups = *updateParams_.constrainedKeys; - } else { - constraintGroups = FastMap(); - const int group = - result->observedKeys.size() < affectedFactorsVarIndex.size() ? 1 : 0; - for (Key var : result->observedKeys) - constraintGroups.insert(std::make_pair(var, group)); - } - - // Remove unaffected keys from the constraints - for (FastMap::iterator iter = constraintGroups.begin(); - iter != constraintGroups.end(); - /*Incremented in loop ++iter*/) { - if (result->unusedKeys.exists(iter->first) || - !affectedKeysSet->exists(iter->first)) - constraintGroups.erase(iter++); - else - ++iter; - } - gttoc(ordering_constraints); - - // Generate ordering - gttic(Ordering); - Ordering ordering = - Ordering::ColamdConstrained(affectedFactorsVarIndex, constraintGroups); - gttoc(Ordering); - - ISAM2BayesTree::shared_ptr bayesTree = - ISAM2JunctionTree( - GaussianEliminationTree(factors, affectedFactorsVarIndex, ordering)) - .eliminate(params_.getEliminationFunction()) - .first; - - gttoc(reorder_and_eliminate); - - gttic(reassemble); - roots->insert(roots->end(), bayesTree->roots().begin(), - bayesTree->roots().end()); - nodes->insert(bayesTree->nodes().begin(), bayesTree->nodes().end()); - gttoc(reassemble); - - // 4. The orphans have already been inserted during elimination - } - - KeySet recalculate(const Values& theta, const VariableIndex& variableIndex, - const NonlinearFactorGraph& nonlinearFactors, - const GaussianBayesNet& affectedBayesNet, - const ISAM2::Cliques& orphans, const KeySet& relinKeys, - GaussianFactorGraph* linearFactors, ISAM2::Roots* roots, - ISAM2::Nodes* nodes, ISAM2Result* result) const { - gttic(recalculate); - LogRecalculateKeys(*result); - - // FactorGraph factors(affectedBayesNet); - // bug was here: we cannot reuse the original factors, because then the - // cached factors get messed up [all the necessary data is actually - // contained in the affectedBayesNet, including what was passed in from the - // boundaries, so this would be correct; however, in the process we also - // generate new cached_ entries that will be wrong (ie. they don't contain - // what would be passed up at a certain point if batch elimination was done, - // but that's what we need); we could choose not to update cached_ from - // here, but then the new information (and potentially different variable - // ordering) is not reflected in the cached_ values which again will be - // wrong] so instead we have to retrieve the original linearized factors AND - // add the cached factors from the boundary - - // BEGIN OF COPIED CODE - - // ordering provides all keys in conditionals, there cannot be others - // because path to root included - gttic(affectedKeys); - FastList affectedKeys; - for (const auto& conditional : affectedBayesNet) - affectedKeys.insert(affectedKeys.end(), conditional->beginFrontals(), - conditional->endFrontals()); - gttoc(affectedKeys); - - KeySet affectedKeysSet; // Will return this result - - static const double kBatchThreshold = 0.65; - if (affectedKeys.size() >= theta.size() * kBatchThreshold) { - // Do a batch step - reorder and relinearize all variables - recalculateBatch(theta, variableIndex, nonlinearFactors, linearFactors, - &affectedKeysSet, roots, nodes, result); - } else { - recalculateIncremental(theta, variableIndex, nonlinearFactors, orphans, - relinKeys, linearFactors, affectedKeys, - &affectedKeysSet, roots, nodes, result); - } - - // Root clique variables for detailed results - if (params_.enableDetailedResults) { - for (const auto& root : *roots) - for (Key var : *root->conditional()) - result->detail->variableStatus[var].inRootClique = true; - } - - return affectedKeysSet; - } }; } // namespace gtsam diff --git a/gtsam/nonlinear/ISAM2.cpp b/gtsam/nonlinear/ISAM2.cpp index cce698be4..c1ff1ad02 100644 --- a/gtsam/nonlinear/ISAM2.cpp +++ b/gtsam/nonlinear/ISAM2.cpp @@ -25,7 +25,9 @@ #include #include +#include #include +#include using namespace std; @@ -56,6 +58,293 @@ bool ISAM2::equals(const ISAM2& other, double tol) const { fixedVariables_ == other.fixedVariables_; } +/* ************************************************************************* */ +void ISAM2::recalculateBatch(const ISAM2UpdateParams& updateParams, + KeySet* affectedKeysSet, ISAM2Result* result) { + gttic(recalculateBatch); + + gttic(add_keys); + br::copy(variableIndex_ | br::map_keys, + std::inserter(*affectedKeysSet, affectedKeysSet->end())); + + // Removed unused keys: + VariableIndex affectedFactorsVarIndex = variableIndex_; + + affectedFactorsVarIndex.removeUnusedVariables(result->unusedKeys.begin(), + result->unusedKeys.end()); + + for (const Key key : result->unusedKeys) { + affectedKeysSet->erase(key); + } + gttoc(add_keys); + + gttic(ordering); + Ordering order; + if (updateParams.constrainedKeys) { + order = Ordering::ColamdConstrained(affectedFactorsVarIndex, + *updateParams.constrainedKeys); + } else { + if (theta_.size() > result->observedKeys.size()) { + // Only if some variables are unconstrained + FastMap constraintGroups; + for (Key var : result->observedKeys) constraintGroups[var] = 1; + order = Ordering::ColamdConstrained(affectedFactorsVarIndex, + constraintGroups); + } else { + order = Ordering::Colamd(affectedFactorsVarIndex); + } + } + gttoc(ordering); + + gttic(linearize); + auto linearized = nonlinearFactors_.linearize(theta_); + if (params_.cacheLinearizedFactors) linearFactors_ = *linearized; + gttoc(linearize); + + gttic(eliminate); + ISAM2BayesTree::shared_ptr bayesTree = + ISAM2JunctionTree( + GaussianEliminationTree(*linearized, affectedFactorsVarIndex, order)) + .eliminate(params_.getEliminationFunction()) + .first; + gttoc(eliminate); + + gttic(insert); + roots_.clear(); + roots_.insert(roots_.end(), bayesTree->roots().begin(), + bayesTree->roots().end()); + nodes_.clear(); + nodes_.insert(bayesTree->nodes().begin(), bayesTree->nodes().end()); + gttoc(insert); + + result->variablesReeliminated = affectedKeysSet->size(); + result->factorsRecalculated = nonlinearFactors_.size(); + + // Reeliminated keys for detailed results + if (params_.enableDetailedResults) { + for (Key key : theta_.keys()) { + result->detail->variableStatus[key].isReeliminated = true; + } + } +} + +/* ************************************************************************* */ +GaussianFactorGraph ISAM2::relinearizeAffectedFactors( + const ISAM2UpdateParams& updateParams, const FastList& affectedKeys, + const KeySet& relinKeys) { + gttic(relinearizeAffectedFactors); + FactorIndexSet candidates = + UpdateImpl::GetAffectedFactors(affectedKeys, variableIndex_); + + gttic(affectedKeysSet); + // for fast lookup below + KeySet affectedKeysSet; + affectedKeysSet.insert(affectedKeys.begin(), affectedKeys.end()); + gttoc(affectedKeysSet); + + gttic(check_candidates_and_linearize); + GaussianFactorGraph linearized; + for (const FactorIndex idx : candidates) { + bool inside = true; + bool useCachedLinear = params_.cacheLinearizedFactors; + for (Key key : nonlinearFactors_[idx]->keys()) { + if (affectedKeysSet.find(key) == affectedKeysSet.end()) { + inside = false; + break; + } + if (useCachedLinear && relinKeys.find(key) != relinKeys.end()) + useCachedLinear = false; + } + if (inside) { + if (useCachedLinear) { +#ifdef GTSAM_EXTRA_CONSISTENCY_CHECKS + assert(linearFactors_[idx]); + assert(linearFactors_[idx]->keys() == nonlinearFactors_[idx]->keys()); +#endif + linearized.push_back(linearFactors_[idx]); + } else { + auto linearFactor = nonlinearFactors_[idx]->linearize(theta_); + linearized.push_back(linearFactor); + if (params_.cacheLinearizedFactors) { +#ifdef GTSAM_EXTRA_CONSISTENCY_CHECKS + assert(linearFactors_[idx]->keys() == linearFactor->keys()); +#endif + linearFactors_[idx] = linearFactor; + } + } + } + } + gttoc(check_candidates_and_linearize); + + return linearized; +} + +/* ************************************************************************* */ +void ISAM2::recalculateIncremental(const ISAM2UpdateParams& updateParams, + const KeySet& relinKeys, + const FastList& affectedKeys, + KeySet* affectedKeysSet, Cliques* orphans, + ISAM2Result* result) { + gttic(recalculateIncremental); + const bool debug = ISDEBUG("ISAM2 recalculate"); + + // 2. Add the new factors \Factors' into the resulting factor graph + FastList affectedAndNewKeys; + affectedAndNewKeys.insert(affectedAndNewKeys.end(), affectedKeys.begin(), + affectedKeys.end()); + affectedAndNewKeys.insert(affectedAndNewKeys.end(), + result->observedKeys.begin(), + result->observedKeys.end()); + GaussianFactorGraph factors = + relinearizeAffectedFactors(updateParams, affectedAndNewKeys, relinKeys); + + if (debug) { + factors.print("Relinearized factors: "); + std::cout << "Affected keys: "; + for (const Key key : affectedKeys) { + std::cout << key << " "; + } + std::cout << std::endl; + } + + // Reeliminated keys for detailed results + if (params_.enableDetailedResults) { + for (Key key : affectedAndNewKeys) { + result->detail->variableStatus[key].isReeliminated = true; + } + } + + result->variablesReeliminated = affectedAndNewKeys.size(); + result->factorsRecalculated = factors.size(); + + gttic(cached); + // Add the cached intermediate results from the boundary of the orphans... + GaussianFactorGraph cachedBoundary = + UpdateImpl::GetCachedBoundaryFactors(*orphans); + if (debug) cachedBoundary.print("Boundary factors: "); + factors.push_back(cachedBoundary); + gttoc(cached); + + gttic(orphans); + // Add the orphaned subtrees + for (const auto& orphan : *orphans) + factors += + boost::make_shared >(orphan); + gttoc(orphans); + + // 3. Re-order and eliminate the factor graph into a Bayes net (Algorithm + // [alg:eliminate]), and re-assemble into a new Bayes tree (Algorithm + // [alg:BayesTree]) + + gttic(reorder_and_eliminate); + + gttic(list_to_set); + // create a partial reordering for the new and contaminated factors + // result->markedKeys are passed in: those variables will be forced to the + // end in the ordering + affectedKeysSet->insert(result->markedKeys.begin(), result->markedKeys.end()); + affectedKeysSet->insert(affectedKeys.begin(), affectedKeys.end()); + gttoc(list_to_set); + + VariableIndex affectedFactorsVarIndex(factors); + + gttic(ordering_constraints); + // Create ordering constraints + FastMap constraintGroups; + if (updateParams.constrainedKeys) { + constraintGroups = *updateParams.constrainedKeys; + } else { + constraintGroups = FastMap(); + const int group = + result->observedKeys.size() < affectedFactorsVarIndex.size() ? 1 : 0; + for (Key var : result->observedKeys) + constraintGroups.insert(std::make_pair(var, group)); + } + + // Remove unaffected keys from the constraints + for (FastMap::iterator iter = constraintGroups.begin(); + iter != constraintGroups.end(); + /*Incremented in loop ++iter*/) { + if (result->unusedKeys.exists(iter->first) || + !affectedKeysSet->exists(iter->first)) + constraintGroups.erase(iter++); + else + ++iter; + } + gttoc(ordering_constraints); + + // Generate ordering + gttic(Ordering); + const Ordering ordering = + Ordering::ColamdConstrained(affectedFactorsVarIndex, constraintGroups); + gttoc(Ordering); + + // Do elimination + GaussianEliminationTree etree(factors, affectedFactorsVarIndex, ordering); + auto bayesTree = ISAM2JunctionTree(etree) + .eliminate(params_.getEliminationFunction()) + .first; + gttoc(reorder_and_eliminate); + + gttic(reassemble); + roots_.insert(roots_.end(), bayesTree->roots().begin(), + bayesTree->roots().end()); + nodes_.insert(bayesTree->nodes().begin(), bayesTree->nodes().end()); + gttoc(reassemble); + + // 4. The orphans have already been inserted during elimination +} + +/* ************************************************************************* */ +KeySet ISAM2::recalculate(const ISAM2UpdateParams& updateParams, + const GaussianBayesNet& affectedBayesNet, + const KeySet& relinKeys, Cliques* orphans, + ISAM2Result* result) { + gttic(recalculate); + UpdateImpl::LogRecalculateKeys(*result); + + // FactorGraph factors(affectedBayesNet); + // bug was here: we cannot reuse the original factors, because then the + // cached factors get messed up [all the necessary data is actually + // contained in the affectedBayesNet, including what was passed in from the + // boundaries, so this would be correct; however, in the process we also + // generate new cached_ entries that will be wrong (ie. they don't contain + // what would be passed up at a certain point if batch elimination was done, + // but that's what we need); we could choose not to update cached_ from + // here, but then the new information (and potentially different variable + // ordering) is not reflected in the cached_ values which again will be + // wrong] so instead we have to retrieve the original linearized factors AND + // add the cached factors from the boundary + + // ordering provides all keys in conditionals, there cannot be others + // because path to root included + gttic(affectedKeys); + FastList affectedKeys; + for (const auto& conditional : affectedBayesNet) + affectedKeys.insert(affectedKeys.end(), conditional->beginFrontals(), + conditional->endFrontals()); + gttoc(affectedKeys); + + KeySet affectedKeysSet; // Will return this result + + static const double kBatchThreshold = 0.65; + if (affectedKeys.size() >= theta_.size() * kBatchThreshold) { + // Do a batch step - reorder and relinearize all variables + recalculateBatch(updateParams, &affectedKeysSet, result); + } else { + recalculateIncremental(updateParams, relinKeys, affectedKeys, + &affectedKeysSet, orphans, result); + } + + // Root clique variables for detailed results + if (params_.enableDetailedResults) { + for (const auto& root : roots_) + for (Key var : *root->conditional()) + result->detail->variableStatus[var].inRootClique = true; + } + + return affectedKeysSet; +} /* ************************************************************************* */ void ISAM2::addVariables( const Values& newTheta, @@ -188,9 +477,8 @@ ISAM2Result ISAM2::update(const NonlinearFactorGraph& newFactors, KeyVector(result.markedKeys.begin(), result.markedKeys.end()), affectedBayesNet, orphans); - KeySet affectedKeysSet = update.recalculate( - theta_, variableIndex_, nonlinearFactors_, affectedBayesNet, orphans, - relinKeys, &linearFactors_, &roots_, &nodes_, &result); + KeySet affectedKeysSet = recalculate(updateParams, affectedBayesNet, + relinKeys, &orphans, &result); // Update replaced keys mask (accumulates until back-substitution takes // place) deltaReplacedMask_.insert(affectedKeysSet.begin(), affectedKeysSet.end()); diff --git a/gtsam/nonlinear/ISAM2.h b/gtsam/nonlinear/ISAM2.h index b0eb4693b..8d8299d81 100644 --- a/gtsam/nonlinear/ISAM2.h +++ b/gtsam/nonlinear/ISAM2.h @@ -281,6 +281,27 @@ class GTSAM_EXPORT ISAM2 : public BayesTree { /// @} protected: +// Do a batch step - reorder and relinearize all variables + void recalculateBatch(const ISAM2UpdateParams& updateParams, + KeySet* affectedKeysSet, ISAM2Result* result); + + // retrieve all factors that ONLY contain the affected variables + // (note that the remaining stuff is summarized in the cached factors) + GaussianFactorGraph relinearizeAffectedFactors( + const ISAM2UpdateParams& updateParams, const FastList& affectedKeys, + const KeySet& relinKeys); + + void recalculateIncremental(const ISAM2UpdateParams& updateParams, + const KeySet& relinKeys, + const FastList& affectedKeys, + KeySet* affectedKeysSet, Cliques* orphans, + ISAM2Result* result); + + KeySet recalculate(const ISAM2UpdateParams& updateParams, + const GaussianBayesNet& affectedBayesNet, + const KeySet& relinKeys, Cliques* orphans, + ISAM2Result* result); + /** * Add new variables to the ISAM2 system. * @param newTheta Initial values for new variables From 3b51bae5c1a4d4ff9a97f012bd9960f38450659a Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 2 Jun 2019 16:36:36 -0400 Subject: [PATCH 14/21] made markedKeys explicit --- gtsam/nonlinear/ISAM2-impl.h | 53 ++++++++++++++++++++---------------- gtsam/nonlinear/ISAM2.cpp | 22 ++++++++------- gtsam/nonlinear/ISAM2.h | 2 +- 3 files changed, 42 insertions(+), 35 deletions(-) diff --git a/gtsam/nonlinear/ISAM2-impl.h b/gtsam/nonlinear/ISAM2-impl.h index b6e5663bf..7e17ec785 100644 --- a/gtsam/nonlinear/ISAM2-impl.h +++ b/gtsam/nonlinear/ISAM2-impl.h @@ -140,14 +140,15 @@ struct GTSAM_EXPORT UpdateImpl { NonlinearFactorGraph* nonlinearFactors, GaussianFactorGraph* linearFactors, VariableIndex* variableIndex, - ISAM2Result* result) const { + FactorIndices* newFactorsIndices, + KeySet* keysWithRemovedFactors) const { gttic(pushBackFactors); // 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. - result->newFactorsIndices = nonlinearFactors->add_factors( + *newFactorsIndices = nonlinearFactors->add_factors( newFactors, params_.findUnusedFactorSlots); // Remove the removed factors @@ -164,37 +165,41 @@ struct GTSAM_EXPORT UpdateImpl { variableIndex->remove(updateParams_.removeFactorIndices.begin(), updateParams_.removeFactorIndices.end(), removedFactors); - result->keysWithRemovedFactors = removedFactors.keys(); + *keysWithRemovedFactors = removedFactors.keys(); + } - // Compute unused keys and indices - // Get keys from removed factors and new factors, and compute unused keys, - // i.e., keys that are empty now and do not appear in the new factors. + // Get keys from removed factors and new factors, and compute unused keys, + // i.e., keys that are empty now and do not appear in the new factors. + void computeUnusedKeys(const NonlinearFactorGraph& newFactors, + const VariableIndex& variableIndex, + const KeySet& keysWithRemovedFactors, + KeySet* unusedKeys) const { + gttic(computeUnusedKeys); KeySet removedAndEmpty; - for (Key key : result->keysWithRemovedFactors) { - if (variableIndex->empty(key)) + for (Key key : keysWithRemovedFactors) { + if (variableIndex.empty(key)) removedAndEmpty.insert(removedAndEmpty.end(), key); } KeySet newFactorSymbKeys = newFactors.keys(); - std::set_difference( - removedAndEmpty.begin(), removedAndEmpty.end(), - newFactorSymbKeys.begin(), newFactorSymbKeys.end(), - std::inserter(result->unusedKeys, result->unusedKeys.end())); + std::set_difference(removedAndEmpty.begin(), removedAndEmpty.end(), + newFactorSymbKeys.begin(), newFactorSymbKeys.end(), + std::inserter(*unusedKeys, unusedKeys->end())); } // Mark linear update void gatherInvolvedKeys(const NonlinearFactorGraph& newFactors, const NonlinearFactorGraph& nonlinearFactors, - ISAM2Result* result) const { + KeySet* markedKeys, ISAM2Result* result) const { gttic(gatherInvolvedKeys); - result->markedKeys = newFactors.keys(); // Get keys from new factors + *markedKeys = newFactors.keys(); // Get keys from new factors // Also mark keys involved in removed factors - result->markedKeys.insert(result->keysWithRemovedFactors.begin(), - result->keysWithRemovedFactors.end()); + markedKeys->insert(result->keysWithRemovedFactors.begin(), + result->keysWithRemovedFactors.end()); // Also mark any provided extra re-eliminate keys if (updateParams_.extraReelimKeys) { for (Key key : *updateParams_.extraReelimKeys) { - result->markedKeys.insert(key); + markedKeys->insert(key); } } @@ -204,18 +209,18 @@ struct GTSAM_EXPORT UpdateImpl { for (const auto& factorAddedKeys : *updateParams_.newAffectedKeys) { const auto factorIdx = factorAddedKeys.first; const auto& affectedKeys = nonlinearFactors.at(factorIdx)->keys(); - result->markedKeys.insert(affectedKeys.begin(), affectedKeys.end()); + markedKeys->insert(affectedKeys.begin(), affectedKeys.end()); } } // Observed keys for detailed results if (params_.enableDetailedResults) { - for (Key key : result->markedKeys) { + for (Key key : *markedKeys) { result->detail->variableStatus[key].isObserved = true; } } - for (Key index : result->markedKeys) { + for (Key index : *markedKeys) { // Only add if not unused if (result->unusedKeys.find(index) == result->unusedKeys.end()) // Make a copy of these, as we'll soon add to them @@ -347,7 +352,7 @@ struct GTSAM_EXPORT UpdateImpl { // Mark keys in \Delta above threshold \beta: KeySet gatherRelinearizeKeys(const ISAM2::Roots& roots, const VectorValues& delta, - const KeySet& fixedVariables, + const KeySet& fixedVariables, KeySet* markedKeys, ISAM2Result* result) const { gttic(gatherRelinearizeKeys); // J=\{\Delta_{j}\in\Delta|\Delta_{j}\geq\beta\}. @@ -378,18 +383,18 @@ struct GTSAM_EXPORT UpdateImpl { } // Add the variables being relinearized to the marked keys - result->markedKeys.insert(relinKeys.begin(), relinKeys.end()); + markedKeys->insert(relinKeys.begin(), relinKeys.end()); return relinKeys; } // Mark all cliques that involve marked variables \Theta_{J} and all // their ancestors. void fluidFindAll(const ISAM2::Roots& roots, const KeySet& relinKeys, - ISAM2Result* result) const { + KeySet* markedKeys, ISAM2Result* result) const { gttic(fluidFindAll); for (const auto& root : roots) // add other cliques that have the marked ones in the separator - root->findAll(relinKeys, &result->markedKeys); + root->findAll(relinKeys, markedKeys); // Relinearization-involved keys for detailed results if (params_.enableDetailedResults) { diff --git a/gtsam/nonlinear/ISAM2.cpp b/gtsam/nonlinear/ISAM2.cpp index c1ff1ad02..ce73706a0 100644 --- a/gtsam/nonlinear/ISAM2.cpp +++ b/gtsam/nonlinear/ISAM2.cpp @@ -350,11 +350,9 @@ void ISAM2::addVariables( const Values& newTheta, ISAM2Result::DetailedResults::StatusMap* variableStatus) { gttic(addNewVariables); - // \Theta:=\Theta\cup\Theta_{new}. - const bool debug = ISDEBUG("ISAM2 AddVariables"); theta_.insert(newTheta); - if (debug) newTheta.print("The new variables are: "); + if (ISDEBUG("ISAM2 AddVariables")) newTheta.print("The new variables are: "); // Add zeros into the VectorValues delta_.insert(newTheta.zeroVectors()); deltaNewton_.insert(newTheta.zeroVectors()); @@ -425,9 +423,13 @@ ISAM2Result ISAM2::update(const NonlinearFactorGraph& newFactors, // 1. Add any new factors \Factors:=\Factors\cup\Factors'. update.pushBackFactors(newFactors, &nonlinearFactors_, &linearFactors_, - &variableIndex_, &result); + &variableIndex_, &result.newFactorsIndices, + &result.keysWithRemovedFactors); + update.computeUnusedKeys(newFactors, variableIndex_, + result.keysWithRemovedFactors, &result.unusedKeys); // 2. Initialize any new variables \Theta_{new} and add + // \Theta:=\Theta\cup\Theta_{new}. addVariables(newTheta, result.detail ? &result.detail->variableStatus : 0); gttic(evaluate_error_before); @@ -436,19 +438,20 @@ ISAM2Result ISAM2::update(const NonlinearFactorGraph& newFactors, gttoc(evaluate_error_before); // 3. Mark linear update - update.gatherInvolvedKeys(newFactors, nonlinearFactors_, &result); + update.gatherInvolvedKeys(newFactors, nonlinearFactors_, &result.markedKeys, + &result); // Check relinearization if we're at the nth step, or we are using a looser // loop relinerization threshold. KeySet relinKeys; if (relinearizeThisStep) { // 4. Mark keys in \Delta above threshold \beta: - relinKeys = - update.gatherRelinearizeKeys(roots_, delta_, fixedVariables_, &result); + relinKeys = update.gatherRelinearizeKeys(roots_, delta_, fixedVariables_, + &result.markedKeys, &result); if (!relinKeys.empty()) { // 5. Mark all cliques that involve marked variables \Theta_{J} and all // their ancestors. - update.fluidFindAll(roots_, relinKeys, &result); + update.fluidFindAll(roots_, relinKeys, &result.markedKeys, &result); // 6. Update linearization point for marked variables: // \Theta_{J}:=\Theta_{J}+\Delta_{J}. UpdateImpl::ExpmapMasked(delta_, relinKeys, &theta_); @@ -479,8 +482,7 @@ ISAM2Result ISAM2::update(const NonlinearFactorGraph& newFactors, KeySet affectedKeysSet = recalculate(updateParams, affectedBayesNet, relinKeys, &orphans, &result); - // Update replaced keys mask (accumulates until back-substitution takes - // place) + // Update replaced keys mask (accumulates until back-substitution happens) deltaReplacedMask_.insert(affectedKeysSet.begin(), affectedKeysSet.end()); } diff --git a/gtsam/nonlinear/ISAM2.h b/gtsam/nonlinear/ISAM2.h index 8d8299d81..e2b9a18e8 100644 --- a/gtsam/nonlinear/ISAM2.h +++ b/gtsam/nonlinear/ISAM2.h @@ -281,7 +281,7 @@ class GTSAM_EXPORT ISAM2 : public BayesTree { /// @} protected: -// Do a batch step - reorder and relinearize all variables + // Do a batch step - reorder and relinearize all variables void recalculateBatch(const ISAM2UpdateParams& updateParams, KeySet* affectedKeysSet, ISAM2Result* result); From a9e955d56e08a7fb21255132f21d4f560c8e74d8 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Mon, 3 Jun 2019 12:33:23 -0400 Subject: [PATCH 15/21] using rather than typedef --- gtsam/nonlinear/ISAM2.h | 10 +++++----- gtsam/nonlinear/ISAM2Result.h | 5 +++-- 2 files changed, 8 insertions(+), 7 deletions(-) diff --git a/gtsam/nonlinear/ISAM2.h b/gtsam/nonlinear/ISAM2.h index e2b9a18e8..19199b22d 100644 --- a/gtsam/nonlinear/ISAM2.h +++ b/gtsam/nonlinear/ISAM2.h @@ -97,11 +97,11 @@ class GTSAM_EXPORT ISAM2 : public BayesTree { ///< periodic relinearization public: - typedef ISAM2 This; ///< This class - typedef BayesTree Base; ///< The BayesTree base class - typedef Base::Clique Clique; ///< A clique - typedef Base::sharedClique sharedClique; ///< Shared pointer to a clique - typedef Base::Cliques Cliques; ///< List of Clique typedef from base class + using This = ISAM2; ///< This class + using Base = BayesTree; ///< The BayesTree base class + using Clique = Base::Clique; ///< A clique + using sharedClique = Base::sharedClique; ///< Shared pointer to a clique + using Cliques = Base::Cliques; ///< List of Cliques /** Create an empty ISAM2 instance */ explicit ISAM2(const ISAM2Params& params); diff --git a/gtsam/nonlinear/ISAM2Result.h b/gtsam/nonlinear/ISAM2Result.h index 17968453d..1287b0e1a 100644 --- a/gtsam/nonlinear/ISAM2Result.h +++ b/gtsam/nonlinear/ISAM2Result.h @@ -110,7 +110,8 @@ struct GTSAM_EXPORT ISAM2Result { /** All keys that were marked during the update process. */ KeySet markedKeys; - /** A struct holding detailed results, which must be enabled with + /** + * A struct holding detailed results, which must be enabled with * ISAM2Params::enableDetailedResults. */ struct DetailedResults { @@ -146,7 +147,7 @@ struct GTSAM_EXPORT ISAM2Result { inRootClique(false) {} }; - typedef FastMap StatusMap; + using StatusMap = FastMap; /// The status of each variable during this update, see VariableStatus. StatusMap variableStatus; From 62233395b2d9337b1a06e7f94c23e0e19c3cc97c Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Mon, 3 Jun 2019 13:39:16 -0400 Subject: [PATCH 16/21] made gatherInvolvedKeys more explicit --- gtsam/nonlinear/ISAM2-impl.h | 39 +++++++++++++++++++++--------------- gtsam/nonlinear/ISAM2.cpp | 5 +++-- 2 files changed, 26 insertions(+), 18 deletions(-) diff --git a/gtsam/nonlinear/ISAM2-impl.h b/gtsam/nonlinear/ISAM2-impl.h index 7e17ec785..0fb70558e 100644 --- a/gtsam/nonlinear/ISAM2-impl.h +++ b/gtsam/nonlinear/ISAM2-impl.h @@ -129,7 +129,6 @@ struct GTSAM_EXPORT UpdateImpl { isam2.print("ISAM2: "); } - // Add the new factor indices to the result struct if (debug || verbose) { newFactors.print("The new factors are: "); } @@ -189,12 +188,13 @@ struct GTSAM_EXPORT UpdateImpl { // Mark linear update void gatherInvolvedKeys(const NonlinearFactorGraph& newFactors, const NonlinearFactorGraph& nonlinearFactors, - KeySet* markedKeys, ISAM2Result* result) const { + const KeySet& keysWithRemovedFactors, + KeySet* markedKeys) const { gttic(gatherInvolvedKeys); *markedKeys = newFactors.keys(); // Get keys from new factors // Also mark keys involved in removed factors - markedKeys->insert(result->keysWithRemovedFactors.begin(), - result->keysWithRemovedFactors.end()); + markedKeys->insert(keysWithRemovedFactors.begin(), + keysWithRemovedFactors.end()); // Also mark any provided extra re-eliminate keys if (updateParams_.extraReelimKeys) { @@ -212,15 +212,19 @@ struct GTSAM_EXPORT UpdateImpl { markedKeys->insert(affectedKeys.begin(), affectedKeys.end()); } } + } + // Update detail, unused, and observed keys from markedKeys + void updateKeys(const KeySet& markedKeys, ISAM2Result* result) const { + gttic(updateKeys); // Observed keys for detailed results if (params_.enableDetailedResults) { - for (Key key : *markedKeys) { + for (Key key : markedKeys) { result->detail->variableStatus[key].isObserved = true; } } - for (Key index : *markedKeys) { + for (Key index : markedKeys) { // Only add if not unused if (result->unusedKeys.find(index) == result->unusedKeys.end()) // Make a copy of these, as we'll soon add to them @@ -285,13 +289,14 @@ struct GTSAM_EXPORT UpdateImpl { /** * Find the set of variables to be relinearized according to - * relinearizeThreshold. This check is performed recursively, starting at the - * top of the tree. Once a variable in the tree does not need to be + * relinearizeThreshold. This check is performed recursively, starting at + * the top of the tree. Once a variable in the tree does not need to be * relinearized, no further checks in that branch are performed. This is an - * approximation of the Full version, designed to save time at the expense of - * accuracy. + * approximation of the Full version, designed to save time at the expense + * of accuracy. * @param delta The linear delta to check against the threshold - * @param keyFormatter Formatter for printing nonlinear keys during debugging + * @param keyFormatter Formatter for printing nonlinear keys during + * debugging * @return The set of variable indices in delta whose magnitude is greater * than or equal to relinearizeThreshold */ @@ -313,10 +318,12 @@ struct GTSAM_EXPORT UpdateImpl { /** * Find the set of variables to be relinearized according to - * relinearizeThreshold. Any variables in the VectorValues delta whose vector - * magnitude is greater than or equal to relinearizeThreshold are returned. + * relinearizeThreshold. Any variables in the VectorValues delta whose + * vector magnitude is greater than or equal to relinearizeThreshold are + * returned. * @param delta The linear delta to check against the threshold - * @param keyFormatter Formatter for printing nonlinear keys during debugging + * @param keyFormatter Formatter for printing nonlinear keys during + * debugging * @return The set of variable indices in delta whose magnitude is greater * than or equal to relinearizeThreshold */ @@ -504,8 +511,8 @@ struct GTSAM_EXPORT UpdateImpl { return indices; } - // find intermediate (linearized) factors from cache that are passed into the - // affected area + // find intermediate (linearized) factors from cache that are passed into + // the affected area static GaussianFactorGraph GetCachedBoundaryFactors( const ISAM2::Cliques& orphans) { GaussianFactorGraph cachedBoundary; diff --git a/gtsam/nonlinear/ISAM2.cpp b/gtsam/nonlinear/ISAM2.cpp index ce73706a0..1d1447e92 100644 --- a/gtsam/nonlinear/ISAM2.cpp +++ b/gtsam/nonlinear/ISAM2.cpp @@ -438,8 +438,9 @@ ISAM2Result ISAM2::update(const NonlinearFactorGraph& newFactors, gttoc(evaluate_error_before); // 3. Mark linear update - update.gatherInvolvedKeys(newFactors, nonlinearFactors_, &result.markedKeys, - &result); + update.gatherInvolvedKeys(newFactors, nonlinearFactors_, + result.keysWithRemovedFactors, &result.markedKeys); + update.updateKeys(result.markedKeys, &result); // Check relinearization if we're at the nth step, or we are using a looser // loop relinerization threshold. From 3ab9a1e3cc3e7382d987a79b58a79ea2b4b0284d Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Mon, 3 Jun 2019 13:53:37 -0400 Subject: [PATCH 17/21] Made detail handling more explicit in update --- gtsam/nonlinear/ISAM2-impl.h | 36 +++++++++++++++++++---------------- gtsam/nonlinear/ISAM2.cpp | 19 +++++++++--------- gtsam/nonlinear/ISAM2.h | 5 ++--- gtsam/nonlinear/ISAM2Params.h | 6 +++--- tests/testGaussianISAM2.cpp | 6 ++++-- 5 files changed, 39 insertions(+), 33 deletions(-) diff --git a/gtsam/nonlinear/ISAM2-impl.h b/gtsam/nonlinear/ISAM2-impl.h index 0fb70558e..f668e12c7 100644 --- a/gtsam/nonlinear/ISAM2-impl.h +++ b/gtsam/nonlinear/ISAM2-impl.h @@ -218,7 +218,7 @@ struct GTSAM_EXPORT UpdateImpl { void updateKeys(const KeySet& markedKeys, ISAM2Result* result) const { gttic(updateKeys); // Observed keys for detailed results - if (params_.enableDetailedResults) { + if (result->detail && params_.enableDetailedResults) { for (Key key : markedKeys) { result->detail->variableStatus[key].isObserved = true; } @@ -359,8 +359,8 @@ struct GTSAM_EXPORT UpdateImpl { // Mark keys in \Delta above threshold \beta: KeySet gatherRelinearizeKeys(const ISAM2::Roots& roots, const VectorValues& delta, - const KeySet& fixedVariables, KeySet* markedKeys, - ISAM2Result* result) const { + const KeySet& fixedVariables, + KeySet* markedKeys) const { gttic(gatherRelinearizeKeys); // J=\{\Delta_{j}\in\Delta|\Delta_{j}\geq\beta\}. KeySet relinKeys = @@ -381,37 +381,41 @@ struct GTSAM_EXPORT UpdateImpl { } } - // Record above relinerization threshold keys in detailed results - if (params_.enableDetailedResults) { - for (Key key : relinKeys) { - result->detail->variableStatus[key].isAboveRelinThreshold = true; - result->detail->variableStatus[key].isRelinearized = true; - } - } - // Add the variables being relinearized to the marked keys markedKeys->insert(relinKeys.begin(), relinKeys.end()); return relinKeys; } + // Record relinerization threshold keys in detailed results + void recordRelinearizeDetail(const KeySet& relinKeys, + ISAM2Result::DetailedResults* detail) const { + if (detail && params_.enableDetailedResults) { + for (Key key : relinKeys) { + detail->variableStatus[key].isAboveRelinThreshold = true; + detail->variableStatus[key].isRelinearized = true; + } + } + } + // Mark all cliques that involve marked variables \Theta_{J} and all // their ancestors. void fluidFindAll(const ISAM2::Roots& roots, const KeySet& relinKeys, - KeySet* markedKeys, ISAM2Result* result) const { + KeySet* markedKeys, + ISAM2Result::DetailedResults* detail) const { gttic(fluidFindAll); for (const auto& root : roots) // add other cliques that have the marked ones in the separator root->findAll(relinKeys, markedKeys); // Relinearization-involved keys for detailed results - if (params_.enableDetailedResults) { + if (detail && params_.enableDetailedResults) { KeySet involvedRelinKeys; for (const auto& root : roots) root->findAll(relinKeys, &involvedRelinKeys); for (Key key : involvedRelinKeys) { - if (!result->detail->variableStatus[key].isAboveRelinThreshold) { - result->detail->variableStatus[key].isRelinearizeInvolved = true; - result->detail->variableStatus[key].isRelinearized = true; + if (!detail->variableStatus[key].isAboveRelinThreshold) { + detail->variableStatus[key].isRelinearizeInvolved = true; + detail->variableStatus[key].isRelinearized = true; } } } diff --git a/gtsam/nonlinear/ISAM2.cpp b/gtsam/nonlinear/ISAM2.cpp index 1d1447e92..4e4a507bf 100644 --- a/gtsam/nonlinear/ISAM2.cpp +++ b/gtsam/nonlinear/ISAM2.cpp @@ -337,7 +337,7 @@ KeySet ISAM2::recalculate(const ISAM2UpdateParams& updateParams, } // Root clique variables for detailed results - if (params_.enableDetailedResults) { + if (result->detail && params_.enableDetailedResults) { for (const auto& root : roots_) for (Key var : *root->conditional()) result->detail->variableStatus[var].inRootClique = true; @@ -346,9 +346,8 @@ KeySet ISAM2::recalculate(const ISAM2UpdateParams& updateParams, return affectedKeysSet; } /* ************************************************************************* */ -void ISAM2::addVariables( - const Values& newTheta, - ISAM2Result::DetailedResults::StatusMap* variableStatus) { +void ISAM2::addVariables(const Values& newTheta, + ISAM2Result::DetailedResults* detail) { gttic(addNewVariables); theta_.insert(newTheta); @@ -359,9 +358,9 @@ void ISAM2::addVariables( RgProd_.insert(newTheta.zeroVectors()); // New keys for detailed results - if (variableStatus && params_.enableDetailedResults) { + if (detail && params_.enableDetailedResults) { for (Key key : newTheta.keys()) { - (*variableStatus)[key].isNew = true; + detail->variableStatus[key].isNew = true; } } } @@ -430,7 +429,7 @@ ISAM2Result ISAM2::update(const NonlinearFactorGraph& newFactors, // 2. Initialize any new variables \Theta_{new} and add // \Theta:=\Theta\cup\Theta_{new}. - addVariables(newTheta, result.detail ? &result.detail->variableStatus : 0); + addVariables(newTheta, result.detail.get_ptr()); gttic(evaluate_error_before); if (params_.evaluateNonlinearError) @@ -448,11 +447,13 @@ ISAM2Result ISAM2::update(const NonlinearFactorGraph& newFactors, if (relinearizeThisStep) { // 4. Mark keys in \Delta above threshold \beta: relinKeys = update.gatherRelinearizeKeys(roots_, delta_, fixedVariables_, - &result.markedKeys, &result); + &result.markedKeys); + update.recordRelinearizeDetail(relinKeys, result.detail.get_ptr()); if (!relinKeys.empty()) { // 5. Mark all cliques that involve marked variables \Theta_{J} and all // their ancestors. - update.fluidFindAll(roots_, relinKeys, &result.markedKeys, &result); + update.fluidFindAll(roots_, relinKeys, &result.markedKeys, + result.detail.get_ptr()); // 6. Update linearization point for marked variables: // \Theta_{J}:=\Theta_{J}+\Delta_{J}. UpdateImpl::ExpmapMasked(delta_, relinKeys, &theta_); diff --git a/gtsam/nonlinear/ISAM2.h b/gtsam/nonlinear/ISAM2.h index 19199b22d..bd1477072 100644 --- a/gtsam/nonlinear/ISAM2.h +++ b/gtsam/nonlinear/ISAM2.h @@ -307,9 +307,8 @@ class GTSAM_EXPORT ISAM2 : public BayesTree { * @param newTheta Initial values for new variables * @param variableStatus optional detailed result structure */ - void addVariables( - const Values& newTheta, - ISAM2Result::DetailedResults::StatusMap* variableStatus = 0); + void addVariables(const Values& newTheta, + ISAM2Result::DetailedResults* detail = 0); /** * Remove variables from the ISAM2 system. diff --git a/gtsam/nonlinear/ISAM2Params.h b/gtsam/nonlinear/ISAM2Params.h index afddd1f8e..5cf962e43 100644 --- a/gtsam/nonlinear/ISAM2Params.h +++ b/gtsam/nonlinear/ISAM2Params.h @@ -234,8 +234,8 @@ struct GTSAM_EXPORT ISAM2Params { Factorization _factorization = ISAM2Params::CHOLESKY, bool _cacheLinearizedFactors = true, const KeyFormatter& _keyFormatter = - DefaultKeyFormatter ///< see ISAM2::Params::keyFormatter - ) + DefaultKeyFormatter, ///< see ISAM2::Params::keyFormatter, + bool _enableDetailedResults = false) : optimizationParams(_optimizationParams), relinearizeThreshold(_relinearizeThreshold), relinearizeSkip(_relinearizeSkip), @@ -244,7 +244,7 @@ struct GTSAM_EXPORT ISAM2Params { factorization(_factorization), cacheLinearizedFactors(_cacheLinearizedFactors), keyFormatter(_keyFormatter), - enableDetailedResults(false), + enableDetailedResults(_enableDetailedResults), enablePartialRelinearizationCheck(false), findUnusedFactorSlots(false) {} diff --git a/tests/testGaussianISAM2.cpp b/tests/testGaussianISAM2.cpp index 871e4bc18..68d10bb7b 100644 --- a/tests/testGaussianISAM2.cpp +++ b/tests/testGaussianISAM2.cpp @@ -47,9 +47,11 @@ SharedDiagonal brNoise = noiseModel::Diagonal::Sigmas((Vector(2) << M_PI/100.0, ISAM2 createSlamlikeISAM2( boost::optional init_values = boost::none, boost::optional full_graph = boost::none, - const ISAM2Params& params = ISAM2Params(ISAM2GaussNewtonParams(0.001), 0.0, 0, false, true), + const ISAM2Params& params = ISAM2Params(ISAM2GaussNewtonParams(0.001), 0.0, + 0, false, true, + ISAM2Params::CHOLESKY, true, + DefaultKeyFormatter, true), size_t maxPoses = 10) { - // These variables will be reused and accumulate factors and values ISAM2 isam(params); Values fullinit; From 6f7e92afdc1dfa5b85a73b9861dfec614a956b5f Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Mon, 3 Jun 2019 14:12:31 -0400 Subject: [PATCH 18/21] Make error calculation concise --- gtsam/nonlinear/ISAM2-impl.h | 7 +++++++ gtsam/nonlinear/ISAM2.cpp | 12 ++++-------- 2 files changed, 11 insertions(+), 8 deletions(-) diff --git a/gtsam/nonlinear/ISAM2-impl.h b/gtsam/nonlinear/ISAM2-impl.h index f668e12c7..1dba44713 100644 --- a/gtsam/nonlinear/ISAM2-impl.h +++ b/gtsam/nonlinear/ISAM2-impl.h @@ -185,6 +185,13 @@ struct GTSAM_EXPORT UpdateImpl { std::inserter(*unusedKeys, unusedKeys->end())); } + // Calculate nonlinear error + void error(const NonlinearFactorGraph& nonlinearFactors, + const Values& estimate, boost::optional* error) const { + gttic(error); + error->reset(nonlinearFactors.error(estimate)); + } + // Mark linear update void gatherInvolvedKeys(const NonlinearFactorGraph& newFactors, const NonlinearFactorGraph& nonlinearFactors, diff --git a/gtsam/nonlinear/ISAM2.cpp b/gtsam/nonlinear/ISAM2.cpp index 4e4a507bf..9394b3201 100644 --- a/gtsam/nonlinear/ISAM2.cpp +++ b/gtsam/nonlinear/ISAM2.cpp @@ -367,6 +367,8 @@ void ISAM2::addVariables(const Values& newTheta, /* ************************************************************************* */ void ISAM2::removeVariables(const KeySet& unusedKeys) { + gttic(removeVariables); + variableIndex_.removeUnusedVariables(unusedKeys.begin(), unusedKeys.end()); for (Key key : unusedKeys) { delta_.erase(key); @@ -431,10 +433,8 @@ ISAM2Result ISAM2::update(const NonlinearFactorGraph& newFactors, // \Theta:=\Theta\cup\Theta_{new}. addVariables(newTheta, result.detail.get_ptr()); - gttic(evaluate_error_before); if (params_.evaluateNonlinearError) - result.errorBefore.reset(nonlinearFactors_.error(calculateEstimate())); - gttoc(evaluate_error_before); + update.error(nonlinearFactors_, calculateEstimate(), &result.errorBefore); // 3. Mark linear update update.gatherInvolvedKeys(newFactors, nonlinearFactors_, @@ -490,16 +490,12 @@ ISAM2Result ISAM2::update(const NonlinearFactorGraph& newFactors, // Update data structures to remove unused keys if (!result.unusedKeys.empty()) { - gttic(remove_variables); removeVariables(result.unusedKeys); - gttoc(remove_variables); } result.cliques = this->nodes().size(); - gttic(evaluate_error_after); if (params_.evaluateNonlinearError) - result.errorAfter.reset(nonlinearFactors_.error(calculateEstimate())); - gttoc(evaluate_error_after); + update.error(nonlinearFactors_, calculateEstimate(), &result.errorAfter); return result; } From 18553feb036fa38b1c7a6442f341e87396491c42 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Mon, 3 Jun 2019 14:30:00 -0400 Subject: [PATCH 19/21] Recalculate now removes top --- gtsam/nonlinear/ISAM2-impl.h | 4 +- gtsam/nonlinear/ISAM2.cpp | 109 +++++++++++++++++------------------ gtsam/nonlinear/ISAM2.h | 10 ++-- 3 files changed, 59 insertions(+), 64 deletions(-) diff --git a/gtsam/nonlinear/ISAM2-impl.h b/gtsam/nonlinear/ISAM2-impl.h index 1dba44713..6acdd28d5 100644 --- a/gtsam/nonlinear/ISAM2-impl.h +++ b/gtsam/nonlinear/ISAM2-impl.h @@ -187,9 +187,9 @@ struct GTSAM_EXPORT UpdateImpl { // Calculate nonlinear error void error(const NonlinearFactorGraph& nonlinearFactors, - const Values& estimate, boost::optional* error) const { + const Values& estimate, boost::optional* result) const { gttic(error); - error->reset(nonlinearFactors.error(estimate)); + result->reset(nonlinearFactors.error(estimate)); } // Mark linear update diff --git a/gtsam/nonlinear/ISAM2.cpp b/gtsam/nonlinear/ISAM2.cpp index 9394b3201..8b882f1ed 100644 --- a/gtsam/nonlinear/ISAM2.cpp +++ b/gtsam/nonlinear/ISAM2.cpp @@ -296,54 +296,64 @@ void ISAM2::recalculateIncremental(const ISAM2UpdateParams& updateParams, } /* ************************************************************************* */ -KeySet ISAM2::recalculate(const ISAM2UpdateParams& updateParams, - const GaussianBayesNet& affectedBayesNet, - const KeySet& relinKeys, Cliques* orphans, - ISAM2Result* result) { +void ISAM2::recalculate(const ISAM2UpdateParams& updateParams, + const KeySet& relinKeys, ISAM2Result* result) { gttic(recalculate); UpdateImpl::LogRecalculateKeys(*result); - // FactorGraph factors(affectedBayesNet); - // bug was here: we cannot reuse the original factors, because then the - // cached factors get messed up [all the necessary data is actually - // contained in the affectedBayesNet, including what was passed in from the - // boundaries, so this would be correct; however, in the process we also - // generate new cached_ entries that will be wrong (ie. they don't contain - // what would be passed up at a certain point if batch elimination was done, - // but that's what we need); we could choose not to update cached_ from - // here, but then the new information (and potentially different variable - // ordering) is not reflected in the cached_ values which again will be - // wrong] so instead we have to retrieve the original linearized factors AND - // add the cached factors from the boundary + if (!result->markedKeys.empty() || !result->observedKeys.empty()) { + // Remove top of Bayes tree and convert to a factor graph: + // (a) For each affected variable, remove the corresponding clique and all + // parents up to the root. (b) Store orphaned sub-trees \BayesTree_{O} of + // removed cliques. + GaussianBayesNet affectedBayesNet; + Cliques orphans; + this->removeTop( + KeyVector(result->markedKeys.begin(), result->markedKeys.end()), + &affectedBayesNet, &orphans); - // ordering provides all keys in conditionals, there cannot be others - // because path to root included - gttic(affectedKeys); - FastList affectedKeys; - for (const auto& conditional : affectedBayesNet) - affectedKeys.insert(affectedKeys.end(), conditional->beginFrontals(), - conditional->endFrontals()); - gttoc(affectedKeys); + // FactorGraph factors(affectedBayesNet); + // bug was here: we cannot reuse the original factors, because then the + // cached factors get messed up [all the necessary data is actually + // contained in the affectedBayesNet, including what was passed in from the + // boundaries, so this would be correct; however, in the process we also + // generate new cached_ entries that will be wrong (ie. they don't contain + // what would be passed up at a certain point if batch elimination was done, + // but that's what we need); we could choose not to update cached_ from + // here, but then the new information (and potentially different variable + // ordering) is not reflected in the cached_ values which again will be + // wrong] so instead we have to retrieve the original linearized factors AND + // add the cached factors from the boundary - KeySet affectedKeysSet; // Will return this result + // ordering provides all keys in conditionals, there cannot be others + // because path to root included + gttic(affectedKeys); + FastList affectedKeys; + for (const auto& conditional : affectedBayesNet) + affectedKeys.insert(affectedKeys.end(), conditional->beginFrontals(), + conditional->endFrontals()); + gttoc(affectedKeys); - static const double kBatchThreshold = 0.65; - if (affectedKeys.size() >= theta_.size() * kBatchThreshold) { - // Do a batch step - reorder and relinearize all variables - recalculateBatch(updateParams, &affectedKeysSet, result); - } else { - recalculateIncremental(updateParams, relinKeys, affectedKeys, - &affectedKeysSet, orphans, result); + KeySet affectedKeysSet; + static const double kBatchThreshold = 0.65; + if (affectedKeys.size() >= theta_.size() * kBatchThreshold) { + // Do a batch step - reorder and relinearize all variables + recalculateBatch(updateParams, &affectedKeysSet, result); + } else { + recalculateIncremental(updateParams, relinKeys, affectedKeys, + &affectedKeysSet, &orphans, result); + } + + // Root clique variables for detailed results + if (result->detail && params_.enableDetailedResults) { + for (const auto& root : roots_) + for (Key var : *root->conditional()) + result->detail->variableStatus[var].inRootClique = true; + } + + // Update replaced keys mask (accumulates until back-substitution happens) + deltaReplacedMask_.insert(affectedKeysSet.begin(), affectedKeysSet.end()); } - - // Root clique variables for detailed results - if (result->detail && params_.enableDetailedResults) { - for (const auto& root : roots_) - for (Key var : *root->conditional()) - result->detail->variableStatus[var].inRootClique = true; - } - - return affectedKeysSet; } /* ************************************************************************* */ void ISAM2::addVariables(const Values& newTheta, @@ -471,22 +481,7 @@ ISAM2Result ISAM2::update(const NonlinearFactorGraph& newFactors, &variableIndex_); // 8. Redo top of Bayes tree - if (!result.markedKeys.empty() || !result.observedKeys.empty()) { - // Remove top of Bayes tree and convert to a factor graph: - // (a) For each affected variable, remove the corresponding clique and all - // parents up to the root. (b) Store orphaned sub-trees \BayesTree_{O} of - // removed cliques. - GaussianBayesNet affectedBayesNet; - Cliques orphans; - this->removeTop( - KeyVector(result.markedKeys.begin(), result.markedKeys.end()), - affectedBayesNet, orphans); - - KeySet affectedKeysSet = recalculate(updateParams, affectedBayesNet, - relinKeys, &orphans, &result); - // Update replaced keys mask (accumulates until back-substitution happens) - deltaReplacedMask_.insert(affectedKeysSet.begin(), affectedKeysSet.end()); - } + recalculate(updateParams, relinKeys, &result); // Update data structures to remove unused keys if (!result.unusedKeys.empty()) { diff --git a/gtsam/nonlinear/ISAM2.h b/gtsam/nonlinear/ISAM2.h index bd1477072..ec1b71a72 100644 --- a/gtsam/nonlinear/ISAM2.h +++ b/gtsam/nonlinear/ISAM2.h @@ -296,11 +296,11 @@ class GTSAM_EXPORT ISAM2 : public BayesTree { const FastList& affectedKeys, KeySet* affectedKeysSet, Cliques* orphans, ISAM2Result* result); - - KeySet recalculate(const ISAM2UpdateParams& updateParams, - const GaussianBayesNet& affectedBayesNet, - const KeySet& relinKeys, Cliques* orphans, - ISAM2Result* result); + /** + * Remove marked top and either recalculate in batch or incrementally. + */ + void recalculate(const ISAM2UpdateParams& updateParams, + const KeySet& relinKeys, ISAM2Result* result); /** * Add new variables to the ISAM2 system. From ebae189c7be5cf971de75ab46d5c3b511857f473 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Mon, 3 Jun 2019 16:06:09 -0400 Subject: [PATCH 20/21] Re-ordered functions to minimize diff --- gtsam/nonlinear/ISAM2.cpp | 223 +++++++++++++++++++------------------- gtsam/nonlinear/ISAM2.h | 9 +- 2 files changed, 116 insertions(+), 116 deletions(-) diff --git a/gtsam/nonlinear/ISAM2.cpp b/gtsam/nonlinear/ISAM2.cpp index 8b882f1ed..fa765a109 100644 --- a/gtsam/nonlinear/ISAM2.cpp +++ b/gtsam/nonlinear/ISAM2.cpp @@ -58,6 +58,118 @@ bool ISAM2::equals(const ISAM2& other, double tol) const { fixedVariables_ == other.fixedVariables_; } +/* ************************************************************************* */ +GaussianFactorGraph ISAM2::relinearizeAffectedFactors( + const ISAM2UpdateParams& updateParams, const FastList& affectedKeys, + const KeySet& relinKeys) { + gttic(relinearizeAffectedFactors); + FactorIndexSet candidates = + UpdateImpl::GetAffectedFactors(affectedKeys, variableIndex_); + + gttic(affectedKeysSet); + // for fast lookup below + KeySet affectedKeysSet; + affectedKeysSet.insert(affectedKeys.begin(), affectedKeys.end()); + gttoc(affectedKeysSet); + + gttic(check_candidates_and_linearize); + GaussianFactorGraph linearized; + for (const FactorIndex idx : candidates) { + bool inside = true; + bool useCachedLinear = params_.cacheLinearizedFactors; + for (Key key : nonlinearFactors_[idx]->keys()) { + if (affectedKeysSet.find(key) == affectedKeysSet.end()) { + inside = false; + break; + } + if (useCachedLinear && relinKeys.find(key) != relinKeys.end()) + useCachedLinear = false; + } + if (inside) { + if (useCachedLinear) { +#ifdef GTSAM_EXTRA_CONSISTENCY_CHECKS + assert(linearFactors_[idx]); + assert(linearFactors_[idx]->keys() == nonlinearFactors_[idx]->keys()); +#endif + linearized.push_back(linearFactors_[idx]); + } else { + auto linearFactor = nonlinearFactors_[idx]->linearize(theta_); + linearized.push_back(linearFactor); + if (params_.cacheLinearizedFactors) { +#ifdef GTSAM_EXTRA_CONSISTENCY_CHECKS + assert(linearFactors_[idx]->keys() == linearFactor->keys()); +#endif + linearFactors_[idx] = linearFactor; + } + } + } + } + gttoc(check_candidates_and_linearize); + + return linearized; +} + +/* ************************************************************************* */ +void ISAM2::recalculate(const ISAM2UpdateParams& updateParams, + const KeySet& relinKeys, ISAM2Result* result) { + gttic(recalculate); + UpdateImpl::LogRecalculateKeys(*result); + + if (!result->markedKeys.empty() || !result->observedKeys.empty()) { + // Remove top of Bayes tree and convert to a factor graph: + // (a) For each affected variable, remove the corresponding clique and all + // parents up to the root. (b) Store orphaned sub-trees \BayesTree_{O} of + // removed cliques. + GaussianBayesNet affectedBayesNet; + Cliques orphans; + this->removeTop( + KeyVector(result->markedKeys.begin(), result->markedKeys.end()), + &affectedBayesNet, &orphans); + + // FactorGraph factors(affectedBayesNet); + // bug was here: we cannot reuse the original factors, because then the + // cached factors get messed up [all the necessary data is actually + // contained in the affectedBayesNet, including what was passed in from the + // boundaries, so this would be correct; however, in the process we also + // generate new cached_ entries that will be wrong (ie. they don't contain + // what would be passed up at a certain point if batch elimination was done, + // but that's what we need); we could choose not to update cached_ from + // here, but then the new information (and potentially different variable + // ordering) is not reflected in the cached_ values which again will be + // wrong] so instead we have to retrieve the original linearized factors AND + // add the cached factors from the boundary + + // ordering provides all keys in conditionals, there cannot be others + // because path to root included + gttic(affectedKeys); + FastList affectedKeys; + for (const auto& conditional : affectedBayesNet) + affectedKeys.insert(affectedKeys.end(), conditional->beginFrontals(), + conditional->endFrontals()); + gttoc(affectedKeys); + + KeySet affectedKeysSet; + static const double kBatchThreshold = 0.65; + if (affectedKeys.size() >= theta_.size() * kBatchThreshold) { + // Do a batch step - reorder and relinearize all variables + recalculateBatch(updateParams, &affectedKeysSet, result); + } else { + recalculateIncremental(updateParams, relinKeys, affectedKeys, + &affectedKeysSet, &orphans, result); + } + + // Root clique variables for detailed results + if (result->detail && params_.enableDetailedResults) { + for (const auto& root : roots_) + for (Key var : *root->conditional()) + result->detail->variableStatus[var].inRootClique = true; + } + + // Update replaced keys mask (accumulates until back-substitution happens) + deltaReplacedMask_.insert(affectedKeysSet.begin(), affectedKeysSet.end()); + } +} + /* ************************************************************************* */ void ISAM2::recalculateBatch(const ISAM2UpdateParams& updateParams, KeySet* affectedKeysSet, ISAM2Result* result) { @@ -128,57 +240,6 @@ void ISAM2::recalculateBatch(const ISAM2UpdateParams& updateParams, } } -/* ************************************************************************* */ -GaussianFactorGraph ISAM2::relinearizeAffectedFactors( - const ISAM2UpdateParams& updateParams, const FastList& affectedKeys, - const KeySet& relinKeys) { - gttic(relinearizeAffectedFactors); - FactorIndexSet candidates = - UpdateImpl::GetAffectedFactors(affectedKeys, variableIndex_); - - gttic(affectedKeysSet); - // for fast lookup below - KeySet affectedKeysSet; - affectedKeysSet.insert(affectedKeys.begin(), affectedKeys.end()); - gttoc(affectedKeysSet); - - gttic(check_candidates_and_linearize); - GaussianFactorGraph linearized; - for (const FactorIndex idx : candidates) { - bool inside = true; - bool useCachedLinear = params_.cacheLinearizedFactors; - for (Key key : nonlinearFactors_[idx]->keys()) { - if (affectedKeysSet.find(key) == affectedKeysSet.end()) { - inside = false; - break; - } - if (useCachedLinear && relinKeys.find(key) != relinKeys.end()) - useCachedLinear = false; - } - if (inside) { - if (useCachedLinear) { -#ifdef GTSAM_EXTRA_CONSISTENCY_CHECKS - assert(linearFactors_[idx]); - assert(linearFactors_[idx]->keys() == nonlinearFactors_[idx]->keys()); -#endif - linearized.push_back(linearFactors_[idx]); - } else { - auto linearFactor = nonlinearFactors_[idx]->linearize(theta_); - linearized.push_back(linearFactor); - if (params_.cacheLinearizedFactors) { -#ifdef GTSAM_EXTRA_CONSISTENCY_CHECKS - assert(linearFactors_[idx]->keys() == linearFactor->keys()); -#endif - linearFactors_[idx] = linearFactor; - } - } - } - } - gttoc(check_candidates_and_linearize); - - return linearized; -} - /* ************************************************************************* */ void ISAM2::recalculateIncremental(const ISAM2UpdateParams& updateParams, const KeySet& relinKeys, @@ -295,66 +356,6 @@ void ISAM2::recalculateIncremental(const ISAM2UpdateParams& updateParams, // 4. The orphans have already been inserted during elimination } -/* ************************************************************************* */ -void ISAM2::recalculate(const ISAM2UpdateParams& updateParams, - const KeySet& relinKeys, ISAM2Result* result) { - gttic(recalculate); - UpdateImpl::LogRecalculateKeys(*result); - - if (!result->markedKeys.empty() || !result->observedKeys.empty()) { - // Remove top of Bayes tree and convert to a factor graph: - // (a) For each affected variable, remove the corresponding clique and all - // parents up to the root. (b) Store orphaned sub-trees \BayesTree_{O} of - // removed cliques. - GaussianBayesNet affectedBayesNet; - Cliques orphans; - this->removeTop( - KeyVector(result->markedKeys.begin(), result->markedKeys.end()), - &affectedBayesNet, &orphans); - - // FactorGraph factors(affectedBayesNet); - // bug was here: we cannot reuse the original factors, because then the - // cached factors get messed up [all the necessary data is actually - // contained in the affectedBayesNet, including what was passed in from the - // boundaries, so this would be correct; however, in the process we also - // generate new cached_ entries that will be wrong (ie. they don't contain - // what would be passed up at a certain point if batch elimination was done, - // but that's what we need); we could choose not to update cached_ from - // here, but then the new information (and potentially different variable - // ordering) is not reflected in the cached_ values which again will be - // wrong] so instead we have to retrieve the original linearized factors AND - // add the cached factors from the boundary - - // ordering provides all keys in conditionals, there cannot be others - // because path to root included - gttic(affectedKeys); - FastList affectedKeys; - for (const auto& conditional : affectedBayesNet) - affectedKeys.insert(affectedKeys.end(), conditional->beginFrontals(), - conditional->endFrontals()); - gttoc(affectedKeys); - - KeySet affectedKeysSet; - static const double kBatchThreshold = 0.65; - if (affectedKeys.size() >= theta_.size() * kBatchThreshold) { - // Do a batch step - reorder and relinearize all variables - recalculateBatch(updateParams, &affectedKeysSet, result); - } else { - recalculateIncremental(updateParams, relinKeys, affectedKeys, - &affectedKeysSet, &orphans, result); - } - - // Root clique variables for detailed results - if (result->detail && params_.enableDetailedResults) { - for (const auto& root : roots_) - for (Key var : *root->conditional()) - result->detail->variableStatus[var].inRootClique = true; - } - - // Update replaced keys mask (accumulates until back-substitution happens) - deltaReplacedMask_.insert(affectedKeysSet.begin(), affectedKeysSet.end()); - } -} /* ************************************************************************* */ void ISAM2::addVariables(const Values& newTheta, ISAM2Result::DetailedResults* detail) { diff --git a/gtsam/nonlinear/ISAM2.h b/gtsam/nonlinear/ISAM2.h index ec1b71a72..9f33e757f 100644 --- a/gtsam/nonlinear/ISAM2.h +++ b/gtsam/nonlinear/ISAM2.h @@ -281,6 +281,10 @@ class GTSAM_EXPORT ISAM2 : public BayesTree { /// @} protected: + /// Remove marked top and either recalculate in batch or incrementally. + void recalculate(const ISAM2UpdateParams& updateParams, + const KeySet& relinKeys, ISAM2Result* result); + // Do a batch step - reorder and relinearize all variables void recalculateBatch(const ISAM2UpdateParams& updateParams, KeySet* affectedKeysSet, ISAM2Result* result); @@ -296,11 +300,6 @@ class GTSAM_EXPORT ISAM2 : public BayesTree { const FastList& affectedKeys, KeySet* affectedKeysSet, Cliques* orphans, ISAM2Result* result); - /** - * Remove marked top and either recalculate in batch or incrementally. - */ - void recalculate(const ISAM2UpdateParams& updateParams, - const KeySet& relinKeys, ISAM2Result* result); /** * Add new variables to the ISAM2 system. From d18795e8443ccf6df1b2e7708cfbb40f103131fa Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Mon, 3 Jun 2019 17:29:21 -0400 Subject: [PATCH 21/21] Some last cosmetic changes to make update fit on single screen --- gtsam/nonlinear/ISAM2-impl.h | 16 +++++++++---- gtsam/nonlinear/ISAM2.cpp | 45 ++++++++++------------------------- gtsam/nonlinear/ISAM2Result.h | 8 +++++++ 3 files changed, 32 insertions(+), 37 deletions(-) diff --git a/gtsam/nonlinear/ISAM2-impl.h b/gtsam/nonlinear/ISAM2-impl.h index 6acdd28d5..eb2285b28 100644 --- a/gtsam/nonlinear/ISAM2-impl.h +++ b/gtsam/nonlinear/ISAM2-impl.h @@ -134,6 +134,14 @@ struct GTSAM_EXPORT UpdateImpl { } } + // Check relinearization if we're at the nth step, or we are using a looser + // loop relinerization threshold. + bool relinarizationNeeded(size_t update_count) const { + return updateParams_.force_relinearize || + (params_.enableRelinearization && + update_count % params_.relinearizeSkip == 0); + } + // Add any new factors \Factors:=\Factors\cup\Factors'. void pushBackFactors(const NonlinearFactorGraph& newFactors, NonlinearFactorGraph* nonlinearFactors, @@ -406,10 +414,10 @@ struct GTSAM_EXPORT UpdateImpl { // Mark all cliques that involve marked variables \Theta_{J} and all // their ancestors. - void fluidFindAll(const ISAM2::Roots& roots, const KeySet& relinKeys, - KeySet* markedKeys, - ISAM2Result::DetailedResults* detail) const { - gttic(fluidFindAll); + void findFluid(const ISAM2::Roots& roots, const KeySet& relinKeys, + KeySet* markedKeys, + ISAM2Result::DetailedResults* detail) const { + gttic(findFluid); for (const auto& root : roots) // add other cliques that have the marked ones in the separator root->findAll(relinKeys, markedKeys); diff --git a/gtsam/nonlinear/ISAM2.cpp b/gtsam/nonlinear/ISAM2.cpp index fa765a109..f56b23777 100644 --- a/gtsam/nonlinear/ISAM2.cpp +++ b/gtsam/nonlinear/ISAM2.cpp @@ -415,23 +415,14 @@ ISAM2Result ISAM2::update(const NonlinearFactorGraph& newFactors, const Values& newTheta, const ISAM2UpdateParams& updateParams) { gttic(ISAM2_update); - this->update_count_++; + this->update_count_ += 1; UpdateImpl::LogStartingUpdate(newFactors, *this); - - const bool relinearizeThisStep = - updateParams.force_relinearize || - (params_.enableRelinearization && - update_count_ % params_.relinearizeSkip == 0); + ISAM2Result result(params_.enableDetailedResults); + UpdateImpl update(params_, updateParams); // Update delta if we need it to check relinearization later - if (relinearizeThisStep) { + if (update.relinarizationNeeded(update_count_)) updateDelta(updateParams.forceFullSolve); - } - - ISAM2Result result; - if (params_.enableDetailedResults) - result.detail = ISAM2Result::DetailedResults(); - UpdateImpl update(params_, updateParams); // 1. Add any new factors \Factors:=\Factors\cup\Factors'. update.pushBackFactors(newFactors, &nonlinearFactors_, &linearFactors_, @@ -442,8 +433,7 @@ ISAM2Result ISAM2::update(const NonlinearFactorGraph& newFactors, // 2. Initialize any new variables \Theta_{new} and add // \Theta:=\Theta\cup\Theta_{new}. - addVariables(newTheta, result.detail.get_ptr()); - + addVariables(newTheta, result.details()); if (params_.evaluateNonlinearError) update.error(nonlinearFactors_, calculateEstimate(), &result.errorBefore); @@ -452,28 +442,22 @@ ISAM2Result ISAM2::update(const NonlinearFactorGraph& newFactors, result.keysWithRemovedFactors, &result.markedKeys); update.updateKeys(result.markedKeys, &result); - // Check relinearization if we're at the nth step, or we are using a looser - // loop relinerization threshold. KeySet relinKeys; - if (relinearizeThisStep) { + result.variablesRelinearized = 0; + if (update.relinarizationNeeded(update_count_)) { // 4. Mark keys in \Delta above threshold \beta: relinKeys = update.gatherRelinearizeKeys(roots_, delta_, fixedVariables_, &result.markedKeys); - update.recordRelinearizeDetail(relinKeys, result.detail.get_ptr()); + update.recordRelinearizeDetail(relinKeys, result.details()); if (!relinKeys.empty()) { - // 5. Mark all cliques that involve marked variables \Theta_{J} and all - // their ancestors. - update.fluidFindAll(roots_, relinKeys, &result.markedKeys, - result.detail.get_ptr()); + // 5. Mark cliques that involve marked variables \Theta_{J} and ancestors. + update.findFluid(roots_, relinKeys, &result.markedKeys, result.details()); // 6. Update linearization point for marked variables: // \Theta_{J}:=\Theta_{J}+\Delta_{J}. UpdateImpl::ExpmapMasked(delta_, relinKeys, &theta_); } result.variablesRelinearized = result.markedKeys.size(); - } else { - result.variablesRelinearized = 0; } - // TODO(frank): should be result.variablesRelinearized = relinKeys.size(); ? // 7. Linearize new factors update.linearizeNewFactors(newFactors, theta_, nonlinearFactors_.size(), @@ -481,18 +465,13 @@ ISAM2Result ISAM2::update(const NonlinearFactorGraph& newFactors, update.augmentVariableIndex(newFactors, result.newFactorsIndices, &variableIndex_); - // 8. Redo top of Bayes tree + // 8. Redo top of Bayes tree and update data structures recalculate(updateParams, relinKeys, &result); - - // Update data structures to remove unused keys - if (!result.unusedKeys.empty()) { - removeVariables(result.unusedKeys); - } + if (!result.unusedKeys.empty()) removeVariables(result.unusedKeys); result.cliques = this->nodes().size(); if (params_.evaluateNonlinearError) update.error(nonlinearFactors_, calculateEstimate(), &result.errorAfter); - return result; } diff --git a/gtsam/nonlinear/ISAM2Result.h b/gtsam/nonlinear/ISAM2Result.h index 1287b0e1a..3953c760b 100644 --- a/gtsam/nonlinear/ISAM2Result.h +++ b/gtsam/nonlinear/ISAM2Result.h @@ -157,6 +157,14 @@ struct GTSAM_EXPORT ISAM2Result { * Detail for information about the results data stored here. */ boost::optional detail; + explicit ISAM2Result(bool enableDetailedResults = false) { + if (enableDetailedResults) detail.reset(DetailedResults()); + } + + /// Return pointer to detail, 0 if no detail requested + DetailedResults* details() { return detail.get_ptr(); } + + /// Print results void print(const std::string str = "") const { using std::cout; cout << str << " Reelimintated: " << variablesReeliminated