diff --git a/gtsam/inference/BayesTree.h b/gtsam/inference/BayesTree.h index 1b9ddf7ec..9d632ff06 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_; 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/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..eb2285b28 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,468 @@ 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) {} + + // 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: "); + } + + if (debug || verbose) { + newFactors.print("The new factors are: "); + } + } + + // 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, + GaussianFactorGraph* linearFactors, + VariableIndex* variableIndex, + 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. + *newFactorsIndices = nonlinearFactors->add_factors( + newFactors, params_.findUnusedFactorSlots); + + // 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); + *keysWithRemovedFactors = removedFactors.keys(); + } + + // 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 : 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(*unusedKeys, unusedKeys->end())); + } + + // Calculate nonlinear error + void error(const NonlinearFactorGraph& nonlinearFactors, + const Values& estimate, boost::optional* result) const { + gttic(error); + result->reset(nonlinearFactors.error(estimate)); + } + + // Mark linear update + void gatherInvolvedKeys(const NonlinearFactorGraph& newFactors, + const NonlinearFactorGraph& nonlinearFactors, + 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(keysWithRemovedFactors.begin(), + keysWithRemovedFactors.end()); + + // 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()); + } + } + } + + // Update detail, unused, and observed keys from markedKeys + void updateKeys(const KeySet& markedKeys, ISAM2Result* result) const { + gttic(updateKeys); + // Observed keys for detailed results + if (result->detail && params_.enableDetailedResults) { + for (Key key : markedKeys) { + result->detail->variableStatus[key].isObserved = true; + } + } + + 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 + 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; + } + + // Mark keys in \Delta above threshold \beta: + KeySet gatherRelinearizeKeys(const ISAM2::Roots& roots, + const VectorValues& delta, + const KeySet& fixedVariables, + KeySet* markedKeys) 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); + } + } + + // 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 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); + + // Relinearization-involved keys for detailed results + if (detail && params_.enableDetailedResults) { + KeySet involvedRelinKeys; + for (const auto& root : roots) + root->findAll(relinKeys, &involvedRelinKeys); + for (Key key : involvedRelinKeys) { + if (!detail->variableStatus[key].isAboveRelinThreshold) { + detail->variableStatus[key].isRelinearizeInvolved = true; + detail->variableStatus[key].isRelinearized = true; + } + } + } + } + + /** + * 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 + */ + 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; +#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_(); + } + } + } + + // 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); + } + } + } + + static void LogRecalculateKeys(const ISAM2Result& result) { + 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; + } + } + + static FactorIndexSet GetAffectedFactors(const KeyList& keys, + const VariableIndex& variableIndex) { + gttic(GetAffectedFactors); + 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 + static GaussianFactorGraph GetCachedBoundaryFactors( + const ISAM2::Cliques& orphans) { + GaussianFactorGraph cachedBoundary; + + for (const auto& orphan : orphans) { + // retrieve the cached factor and add to boundary + cachedBoundary.push_back(orphan->cachedFactor()); + } + + return cachedBoundary; + } +}; + +} // namespace gtsam diff --git a/gtsam/nonlinear/ISAM2.cpp b/gtsam/nonlinear/ISAM2.cpp index c38faae11..f56b23777 100644 --- a/gtsam/nonlinear/ISAM2.cpp +++ b/gtsam/nonlinear/ISAM2.cpp @@ -16,24 +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 @@ -44,35 +36,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,40 +59,12 @@ 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); +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 @@ -138,7 +73,7 @@ GaussianFactorGraph::shared_ptr ISAM2::relinearizeAffectedFactors( gttoc(affectedKeysSet); gttic(check_candidates_and_linearize); - auto linearized = boost::make_shared(); + GaussianFactorGraph linearized; for (const FactorIndex idx : candidates) { bool inside = true; bool useCachedLinear = params_.cacheLinearizedFactors; @@ -156,10 +91,10 @@ GaussianFactorGraph::shared_ptr ISAM2::relinearizeAffectedFactors( assert(linearFactors_[idx]); assert(linearFactors_[idx]->keys() == nonlinearFactors_[idx]->keys()); #endif - linearized->push_back(linearFactors_[idx]); + linearized.push_back(linearFactors_[idx]); } else { auto linearFactor = nonlinearFactors_[idx]->linearize(theta_); - linearized->push_back(linearFactor); + linearized.push_back(linearFactor); if (params_.cacheLinearizedFactors) { #ifdef GTSAM_EXTRA_CONSISTENCY_CHECKS assert(linearFactors_[idx]->keys() == linearFactor->keys()); @@ -175,325 +110,276 @@ GaussianFactorGraph::shared_ptr ISAM2::relinearizeAffectedFactors( } /* ************************************************************************* */ -// find intermediate (linearized) factors from cache that are passed into the -// affected area +void ISAM2::recalculate(const ISAM2UpdateParams& updateParams, + const KeySet& relinKeys, ISAM2Result* result) { + gttic(recalculate); + UpdateImpl::LogRecalculateKeys(*result); -GaussianFactorGraph ISAM2::getCachedBoundaryFactors(const Cliques& orphans) { - GaussianFactorGraph cachedBoundary; + 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); - for (sharedClique orphan : orphans) { - // retrieve the cached factor and add to boundary - cachedBoundary.push_back(orphan->cachedFactor()); + // 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()); } - - 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. +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; + } + } +} + +/* ************************************************************************* */ +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"); - // 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 + // 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) { - cout << "markedKeys: "; - for (const Key key : markedKeys) { - cout << key << " "; + factors.print("Relinearized factors: "); + std::cout << "Affected keys: "; + for (const Key key : affectedKeys) { + std::cout << key << " "; } - cout << endl; - cout << "observedKeys: "; - for (const Key key : observedKeys) { - cout << key << " "; - } - cout << endl; + std::cout << std::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 + // Reeliminated keys for detailed results if (params_.enableDetailedResults) { - for (const sharedNode& root : this->roots()) - for (Key var : *root->conditional()) - result->detail->variableStatus[var].inRootClique = true; + for (Key key : affectedAndNewKeys) { + result->detail->variableStatus[key].isReeliminated = true; + } } - return affectedKeysSet; + 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 } /* ************************************************************************* */ -void ISAM2::addVariables(const Values& newTheta) { - const bool debug = ISDEBUG("ISAM2 AddVariables"); +void ISAM2::addVariables(const Values& newTheta, + ISAM2Result::DetailedResults* detail) { + gttic(addNewVariables); 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()); RgProd_.insert(newTheta.zeroVectors()); + + // New keys for detailed results + if (detail && params_.enableDetailedResults) { + for (Key key : newTheta.keys()) { + detail->variableStatus[key].isNew = true; + } + } } /* ************************************************************************* */ void ISAM2::removeVariables(const KeySet& unusedKeys) { + gttic(removeVariables); + variableIndex_.removeUnusedVariables(unusedKeys.begin(), unusedKeys.end()); for (Key key : unusedKeys) { delta_.erase(key); @@ -506,36 +392,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 +400,6 @@ ISAM2Result ISAM2::update( const boost::optional >& noRelinKeys, const boost::optional >& extraReelimKeys, bool force_relinearize) { - ISAM2UpdateParams params; params.constrainedKeys = constrainedKeys; params.extraReelimKeys = extraReelimKeys; @@ -557,281 +412,66 @@ 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); - - if (verbose) { - cout << "ISAM2::update\n"; - this->print("ISAM2: "); - } + this->update_count_ += 1; + UpdateImpl::LogStartingUpdate(newFactors, *this); + ISAM2Result result(params_.enableDetailedResults); + UpdateImpl update(params_, updateParams); // Update delta if we need it to check relinearization later - if (relinearizeThisStep) { - gttic(updateDelta); - updateDelta(kDisableReordering); - gttoc(updateDelta); - } + if (update.relinarizationNeeded(update_count_)) + updateDelta(updateParams.forceFullSolve); - gttic(push_back_factors); // 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.newFactorsIndices, + &result.keysWithRemovedFactors); + update.computeUnusedKeys(newFactors, variableIndex_, + result.keysWithRemovedFactors, &result.unusedKeys); - // 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); - - gttic(evaluate_error_before); + addVariables(newTheta, result.details()); if (params_.evaluateNonlinearError) - result.errorBefore.reset(nonlinearFactors_.error(calculateEstimate())); - gttoc(evaluate_error_before); + update.error(nonlinearFactors_, calculateEstimate(), &result.errorBefore); - 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()); - } - } + update.gatherInvolvedKeys(newFactors, nonlinearFactors_, + result.keysWithRemovedFactors, &result.markedKeys); + update.updateKeys(result.markedKeys, &result); - // 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); - - // 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); + result.variablesRelinearized = 0; + if (update.relinarizationNeeded(update_count_)) { // 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. + relinKeys = update.gatherRelinearizeKeys(roots_, delta_, fixedVariables_, + &result.markedKeys); + update.recordRelinearizeDetail(relinKeys, result.details()); 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; - } - } - } + // 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_); } - 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(); - } else { - result.variablesRelinearized = 0; + result.variablesRelinearized = result.markedKeys.size(); } - 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); - - // Update replaced keys mask (accumulates until back-substitution takes place) - if (replacedKeys) - deltaReplacedMask_.insert(replacedKeys->begin(), replacedKeys->end()); - gttoc(recalculate); - - // Update data structures to remove unused keys - if (!unusedKeys.empty()) { - gttic(remove_variables); - removeVariables(unusedKeys); - gttoc(remove_variables); - } + // 8. Redo top of Bayes tree and update data structures + recalculate(updateParams, relinKeys, &result); + if (!result.unusedKeys.empty()) removeVariables(result.unusedKeys); 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; } @@ -966,7 +606,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 +680,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 +697,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 +707,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 +724,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 @@ -1113,6 +754,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 +798,13 @@ 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/ISAM2.h b/gtsam/nonlinear/ISAM2.h index 60cdc45fe..9f33e757f 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 */ @@ -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); @@ -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 */ @@ -292,40 +281,39 @@ 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); + + // 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); + /** * 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* detail = 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 +322,3 @@ template <> struct traits : public Testable {}; } // namespace gtsam - -#include 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 */ 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/gtsam/nonlinear/ISAM2Result.h b/gtsam/nonlinear/ISAM2Result.h index 763a5e198..3953c760b 100644 --- a/gtsam/nonlinear/ISAM2Result.h +++ b/gtsam/nonlinear/ISAM2Result.h @@ -24,8 +24,8 @@ #include #include -#include #include +#include #include @@ -96,7 +96,22 @@ struct GTSAM_EXPORT ISAM2Result { */ FactorIndices newFactorsIndices; - /** A struct holding detailed results, which must be enabled with + /** Unused keys, and indices for unused keys, + * i.e., keys that are empty now and do not appear in the new factors. + */ + KeySet unusedKeys; + + /** 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. */ struct DetailedResults { @@ -132,15 +147,24 @@ struct GTSAM_EXPORT ISAM2Result { inRootClique(false) {} }; - /** The status of each variable during this update, see VariableStatus. - */ - FastMap variableStatus; + using StatusMap = FastMap; + + /// The status of each variable during this update, see VariableStatus. + StatusMap variableStatus; }; /** Detailed results, if enabled by ISAM2Params::enableDetailedResults. See * 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 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/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 */ 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()) { diff --git a/tests/testGaussianISAM2.cpp b/tests/testGaussianISAM2.cpp index 2b6809ada..68d10bb7b 100644 --- a/tests/testGaussianISAM2.cpp +++ b/tests/testGaussianISAM2.cpp @@ -4,7 +4,7 @@ * @author Michael Kaess */ -#include +#include #include #include @@ -14,7 +14,6 @@ #include #include #include -#include #include #include #include @@ -23,10 +22,12 @@ #include #include #include + +#include + #include -#include -using namespace boost::assign; #include +using namespace boost::assign; namespace br { using namespace boost::adaptors; using namespace boost::range; } using namespace std; @@ -34,7 +35,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); @@ -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; @@ -282,34 +284,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, Zero, model); - nonlinearFactors += NonlinearFactor::shared_ptr(); - nonlinearFactors += PriorFactor(11, Zero, model); - - NonlinearFactorGraph newFactors; - newFactors += PriorFactor(1, Zero, model); - newFactors += PriorFactor(2, Zero, model); - - NonlinearFactorGraph expectedNonlinearFactors; - expectedNonlinearFactors += PriorFactor(10, Zero, model); - expectedNonlinearFactors += PriorFactor(1, Zero, model); - expectedNonlinearFactors += PriorFactor(11, Zero, model); - expectedNonlinearFactors += PriorFactor(2, Zero, model); - - const FactorIndices expectedNewFactorIndices = list_of(1)(3); - - FactorIndices actualNewFactorIndices; - - ISAM2::Impl::AddFactorsStep1(newFactors, true, &nonlinearFactors, &actualNewFactorIndices); - - EXPECT(assert_equal(expectedNonlinearFactors, nonlinearFactors)); - EXPECT(assert_container_equality(expectedNewFactorIndices, actualNewFactorIndices)); -} - /* ************************************************************************* */ TEST(ISAM2, simple) { @@ -692,25 +666,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); @@ -719,29 +692,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); @@ -750,38 +722,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); @@ -790,24 +761,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);