diff --git a/gtsam/nonlinear/ISAM2-impl.cpp b/gtsam/nonlinear/ISAM2-impl.cpp index 11be14c44..a809cbc08 100644 --- a/gtsam/nonlinear/ISAM2-impl.cpp +++ b/gtsam/nonlinear/ISAM2-impl.cpp @@ -31,150 +31,6 @@ using namespace std; namespace gtsam { -/* ************************************************************************* */ -void ISAM2::Impl::AddFactorsStep1(const NonlinearFactorGraph& newFactors, - bool useUnusedSlots, - NonlinearFactorGraph* nonlinearFactors, - FactorIndices* newFactorIndices) { - newFactorIndices->resize(newFactors.size()); - - if (useUnusedSlots) { - size_t globalFactorIndex = 0; - for (size_t newFactorIndex = 0; newFactorIndex < newFactors.size(); - ++newFactorIndex) { - // Loop to find the next available factor slot - do { - // If we need to add more factors than we have room for, resize - // nonlinearFactors, filling the new slots with NULL factors. Otherwise, - // check if the current factor in nonlinearFactors is already used, and - // if so, increase globalFactorIndex. If the current factor in - // nonlinearFactors is unused, break out of the loop and use the current - // slot. - if (globalFactorIndex >= nonlinearFactors->size()) - nonlinearFactors->resize(nonlinearFactors->size() + - newFactors.size() - newFactorIndex); - else if ((*nonlinearFactors)[globalFactorIndex]) - ++globalFactorIndex; - else - break; - } while (true); - - // Use the current slot, updating nonlinearFactors and newFactorSlots. - (*nonlinearFactors)[globalFactorIndex] = newFactors[newFactorIndex]; - (*newFactorIndices)[newFactorIndex] = globalFactorIndex; - } - } else { - // We're not looking for unused slots, so just add the factors at the end. - for (size_t i = 0; i < newFactors.size(); ++i) - (*newFactorIndices)[i] = i + nonlinearFactors->size(); - nonlinearFactors->push_back(newFactors); - } -} - -/* ************************************************************************* */ -KeySet ISAM2::Impl::CheckRelinearizationFull( - const VectorValues& delta, - const ISAM2Params::RelinearizationThreshold& relinearizeThreshold) { - KeySet relinKeys; - - if (const double* threshold = boost::get(&relinearizeThreshold)) { - for (const VectorValues::KeyValuePair& key_delta : delta) { - double maxDelta = key_delta.second.lpNorm(); - if (maxDelta >= *threshold) relinKeys.insert(key_delta.first); - } - } else if (const FastMap* thresholds = - boost::get >(&relinearizeThreshold)) { - for (const VectorValues::KeyValuePair& key_delta : delta) { - const Vector& threshold = - thresholds->find(Symbol(key_delta.first).chr())->second; - if (threshold.rows() != key_delta.second.rows()) - throw std::invalid_argument( - "Relinearization threshold vector dimensionality for '" + - std::string(1, Symbol(key_delta.first).chr()) + - "' passed into iSAM2 parameters does not match actual variable " - "dimensionality."); - if ((key_delta.second.array().abs() > threshold.array()).any()) - relinKeys.insert(key_delta.first); - } - } - - return relinKeys; -} - -/* ************************************************************************* */ -static void CheckRelinearizationRecursiveDouble( - double threshold, const VectorValues& delta, - const ISAM2::sharedClique& clique, KeySet* relinKeys) { - // Check the current clique for relinearization - bool relinearize = false; - for (Key var : *clique->conditional()) { - double maxDelta = delta[var].lpNorm(); - if (maxDelta >= threshold) { - relinKeys->insert(var); - relinearize = true; - } - } - - // If this node was relinearized, also check its children - if (relinearize) { - for (const ISAM2::sharedClique& child : clique->children) { - CheckRelinearizationRecursiveDouble(threshold, delta, child, relinKeys); - } - } -} - -/* ************************************************************************* */ -static void CheckRelinearizationRecursiveMap( - const FastMap& thresholds, const VectorValues& delta, - const ISAM2::sharedClique& clique, KeySet* relinKeys) { - // Check the current clique for relinearization - bool relinearize = false; - for (Key var : *clique->conditional()) { - // Find the threshold for this variable type - const Vector& threshold = thresholds.find(Symbol(var).chr())->second; - - const Vector& deltaVar = delta[var]; - - // Verify the threshold vector matches the actual variable size - if (threshold.rows() != deltaVar.rows()) - throw std::invalid_argument( - "Relinearization threshold vector dimensionality for '" + - std::string(1, Symbol(var).chr()) + - "' passed into iSAM2 parameters does not match actual variable " - "dimensionality."); - - // Check for relinearization - if ((deltaVar.array().abs() > threshold.array()).any()) { - relinKeys->insert(var); - relinearize = true; - } - } - - // If this node was relinearized, also check its children - if (relinearize) { - for (const ISAM2::sharedClique& child : clique->children) { - CheckRelinearizationRecursiveMap(thresholds, delta, child, relinKeys); - } - } -} - -/* ************************************************************************* */ -KeySet ISAM2::Impl::CheckRelinearizationPartial( - const ISAM2::Roots& roots, const VectorValues& delta, - const ISAM2Params::RelinearizationThreshold& relinearizeThreshold) { - KeySet relinKeys; - for (const ISAM2::sharedClique& root : roots) { - if (relinearizeThreshold.type() == typeid(double)) - CheckRelinearizationRecursiveDouble( - boost::get(relinearizeThreshold), delta, root, &relinKeys); - else if (relinearizeThreshold.type() == typeid(FastMap)) - CheckRelinearizationRecursiveMap( - boost::get >(relinearizeThreshold), delta, root, - &relinKeys); - } - return relinKeys; -} - /* ************************************************************************* */ namespace internal { inline static void optimizeInPlace(const ISAM2::sharedClique& clique, @@ -189,7 +45,7 @@ inline static void optimizeInPlace(const ISAM2::sharedClique& clique, } // namespace internal /* ************************************************************************* */ -size_t ISAM2::Impl::UpdateGaussNewtonDelta(const ISAM2::Roots& roots, +size_t DeltaImpl::UpdateGaussNewtonDelta(const ISAM2::Roots& roots, const KeySet& replacedKeys, double wildfireThreshold, VectorValues* delta) { @@ -272,7 +128,7 @@ void updateRgProd(const ISAM2::sharedClique& clique, const KeySet& replacedKeys, } // namespace internal /* ************************************************************************* */ -size_t ISAM2::Impl::UpdateRgProd(const ISAM2::Roots& roots, +size_t DeltaImpl::UpdateRgProd(const ISAM2::Roots& roots, const KeySet& replacedKeys, const VectorValues& gradAtZero, VectorValues* RgProd) { @@ -287,7 +143,7 @@ size_t ISAM2::Impl::UpdateRgProd(const ISAM2::Roots& roots, } /* ************************************************************************* */ -VectorValues ISAM2::Impl::ComputeGradientSearch(const VectorValues& gradAtZero, +VectorValues DeltaImpl::ComputeGradientSearch(const VectorValues& gradAtZero, const VectorValues& RgProd) { // Compute gradient squared-magnitude const double gradientSqNorm = gradAtZero.dot(gradAtZero); diff --git a/gtsam/nonlinear/ISAM2-impl.h b/gtsam/nonlinear/ISAM2-impl.h index 8a30fb8cd..f4dfcb99b 100644 --- a/gtsam/nonlinear/ISAM2-impl.h +++ b/gtsam/nonlinear/ISAM2-impl.h @@ -11,18 +11,65 @@ /** * @file ISAM2-impl.h - * @brief Incremental update functionality (ISAM2) for BayesTree, with fluid relinearization. - * @author Michael Kaess, Richard Roberts + * @brief Incremental update functionality (ISAM2) for BayesTree, with fluid + * relinearization. + * @author Michael Kaess, Richard Roberts, Frank Dellaert */ #pragma once -#include #include +#include + +#include +#include // We need the inst file because we'll make a special JT templated on ISAM2 +#include +#include +#include +#include + +#include +#include +namespace br { +using namespace boost::range; +using namespace boost::adaptors; +} // namespace br + +#include +#include +#include +#include namespace gtsam { -struct GTSAM_EXPORT ISAM2::Impl { +/* ************************************************************************* */ +// Special BayesTree class that uses ISAM2 cliques - this is the result of +// reeliminating ISAM2 subtrees. +class ISAM2BayesTree : public ISAM2::Base { + public: + typedef ISAM2::Base Base; + typedef ISAM2BayesTree This; + typedef boost::shared_ptr shared_ptr; + + ISAM2BayesTree() {} +}; + +/* ************************************************************************* */ +// Special JunctionTree class that produces ISAM2 BayesTree cliques, used for +// reeliminating ISAM2 subtrees. +class ISAM2JunctionTree + : public JunctionTree { + public: + typedef JunctionTree Base; + typedef ISAM2JunctionTree This; + typedef boost::shared_ptr shared_ptr; + + explicit ISAM2JunctionTree(const GaussianEliminationTree& eliminationTree) + : Base(eliminationTree) {} +}; + +/* ************************************************************************* */ +struct GTSAM_EXPORT DeltaImpl { struct GTSAM_EXPORT PartialSolveResult { ISAM2::sharedClique bayesTree; }; @@ -34,57 +81,771 @@ struct GTSAM_EXPORT ISAM2::Impl { boost::optional > constrainedKeys; }; - /// Perform the first part of the bookkeeping updates for adding new factors. Adds them to the - /// complete list of nonlinear factors, and populates the list of new factor indices, both - /// optionally finding and reusing empty factor slots. - static void AddFactorsStep1(const NonlinearFactorGraph& newFactors, bool useUnusedSlots, - NonlinearFactorGraph* nonlinearFactors, FactorIndices* newFactorIndices); - - /** - * Find the set of variables to be relinearized according to relinearizeThreshold. - * Any variables in the VectorValues delta whose vector magnitude is greater than - * or equal to relinearizeThreshold are returned. - * @param delta The linear delta to check against the threshold - * @param keyFormatter Formatter for printing nonlinear keys during debugging - * @return The set of variable indices in delta whose magnitude is greater than or - * equal to relinearizeThreshold - */ - static KeySet CheckRelinearizationFull(const VectorValues& delta, - const ISAM2Params::RelinearizationThreshold& relinearizeThreshold); - - /** - * Find the set of variables to be relinearized according to relinearizeThreshold. - * This check is performed recursively, starting at the top of the tree. Once a - * variable in the tree does not need to be relinearized, no further checks in - * that branch are performed. This is an approximation of the Full version, designed - * to save time at the expense of accuracy. - * @param delta The linear delta to check against the threshold - * @param keyFormatter Formatter for printing nonlinear keys during debugging - * @return The set of variable indices in delta whose magnitude is greater than or - * equal to relinearizeThreshold - */ - static KeySet CheckRelinearizationPartial(const ISAM2::Roots& roots, - const VectorValues& delta, const ISAM2Params::RelinearizationThreshold& relinearizeThreshold); - /** * Update the Newton's method step point, using wildfire */ static size_t UpdateGaussNewtonDelta(const ISAM2::Roots& roots, - const KeySet& replacedKeys, double wildfireThreshold, VectorValues* delta); + const KeySet& replacedKeys, + double wildfireThreshold, + VectorValues* delta); /** * Update the RgProd (R*g) incrementally taking into account which variables * have been recalculated in \c replacedKeys. Only used in Dogleg. */ - static size_t UpdateRgProd(const ISAM2::Roots& roots, const KeySet& replacedKeys, - const VectorValues& gradAtZero, VectorValues* RgProd); + static size_t UpdateRgProd(const ISAM2::Roots& roots, + const KeySet& replacedKeys, + const VectorValues& gradAtZero, + VectorValues* RgProd); /** * Compute the gradient-search point. Only used in Dogleg. */ static VectorValues ComputeGradientSearch(const VectorValues& gradAtZero, const VectorValues& RgProd); - }; -} +/* ************************************************************************* */ +/** + * Implementation functions for update method + * All of the methods below have clear inputs and outputs, even if not + * functional: iSAM2 is inherintly imperative. + */ +struct GTSAM_EXPORT UpdateImpl { + const ISAM2Params& params_; + const ISAM2UpdateParams& updateParams_; + UpdateImpl(const ISAM2Params& params, const ISAM2UpdateParams& updateParams) + : params_(params), updateParams_(updateParams) {} + + /// Perform the first part of the bookkeeping updates for adding new factors. + /// Adds them to the complete list of nonlinear factors, and populates the + /// list of new factor indices, both optionally finding and reusing empty + /// factor slots. + static void AddFactorsStep1(const NonlinearFactorGraph& newFactors, + bool useUnusedSlots, + NonlinearFactorGraph* nonlinearFactors, + FactorIndices* newFactorIndices) { + newFactorIndices->resize(newFactors.size()); + + if (useUnusedSlots) { + size_t globalFactorIndex = 0; + for (size_t newFactorIndex = 0; newFactorIndex < newFactors.size(); + ++newFactorIndex) { + // Loop to find the next available factor slot + do { + // If we need to add more factors than we have room for, resize + // nonlinearFactors, filling the new slots with NULL factors. + // Otherwise, check if the current factor in nonlinearFactors is + // already used, and if so, increase globalFactorIndex. If the + // current factor in nonlinearFactors is unused, break out of the loop + // and use the current slot. + if (globalFactorIndex >= nonlinearFactors->size()) + nonlinearFactors->resize(nonlinearFactors->size() + + newFactors.size() - newFactorIndex); + else if ((*nonlinearFactors)[globalFactorIndex]) + ++globalFactorIndex; + else + break; + } while (true); + + // Use the current slot, updating nonlinearFactors and newFactorSlots. + (*nonlinearFactors)[globalFactorIndex] = newFactors[newFactorIndex]; + (*newFactorIndices)[newFactorIndex] = globalFactorIndex; + } + } else { + // We're not looking for unused slots, so just add the factors at the end. + for (size_t i = 0; i < newFactors.size(); ++i) + (*newFactorIndices)[i] = i + nonlinearFactors->size(); + nonlinearFactors->push_back(newFactors); + } + } + + // 1. Add any new factors \Factors:=\Factors\cup\Factors'. + void pushBackFactors(const NonlinearFactorGraph& newFactors, + NonlinearFactorGraph* nonlinearFactors, + GaussianFactorGraph* linearFactors, + VariableIndex* variableIndex, + ISAM2Result* result) const { + gttic(pushBackFactors); + const bool debug = ISDEBUG("ISAM2 update"); + const bool verbose = ISDEBUG("ISAM2 update verbose"); + + // Add the new factor indices to the result struct + if (debug || verbose) newFactors.print("The new factors are: "); + AddFactorsStep1(newFactors, params_.findUnusedFactorSlots, nonlinearFactors, + &result->newFactorsIndices); + + // Remove the removed factors + NonlinearFactorGraph removedFactors; + removedFactors.reserve(updateParams_.removeFactorIndices.size()); + for (const auto index : updateParams_.removeFactorIndices) { + removedFactors.push_back(nonlinearFactors->at(index)); + nonlinearFactors->remove(index); + if (params_.cacheLinearizedFactors) linearFactors->remove(index); + } + + // Remove removed factors from the variable index so we do not attempt to + // relinearize them + variableIndex->remove(updateParams_.removeFactorIndices.begin(), + updateParams_.removeFactorIndices.end(), + removedFactors); + result->keysWithRemovedFactors = removedFactors.keys(); + + // Compute unused keys and indices + // Get keys from removed factors and new factors, and compute unused keys, + // i.e., keys that are empty now and do not appear in the new factors. + KeySet removedAndEmpty; + for (Key key : result->keysWithRemovedFactors) { + if (variableIndex->empty(key)) + removedAndEmpty.insert(removedAndEmpty.end(), key); + } + KeySet newFactorSymbKeys = newFactors.keys(); + std::set_difference( + removedAndEmpty.begin(), removedAndEmpty.end(), + newFactorSymbKeys.begin(), newFactorSymbKeys.end(), + std::inserter(result->unusedKeys, result->unusedKeys.end())); + + // Get indices for unused keys + for (Key key : result->unusedKeys) { + result->unusedIndices.insert(result->unusedIndices.end(), key); + } + } + + // 3. Mark linear update + void gatherInvolvedKeys(const NonlinearFactorGraph& newFactors, + const NonlinearFactorGraph& nonlinearFactors, + ISAM2Result* result) const { + gttic(gatherInvolvedKeys); + result->markedKeys = newFactors.keys(); // Get keys from new factors + // Also mark keys involved in removed factors + result->markedKeys.insert(result->keysWithRemovedFactors.begin(), + result->keysWithRemovedFactors.end()); + + // Also mark any provided extra re-eliminate keys + if (updateParams_.extraReelimKeys) { + for (Key key : *updateParams_.extraReelimKeys) { + result->markedKeys.insert(key); + } + } + // Also, keys that were not observed in existing factors, but whose affected + // keys have been extended now (e.g. smart factors) + if (updateParams_.newAffectedKeys) { + for (const auto& factorAddedKeys : *updateParams_.newAffectedKeys) { + const auto factorIdx = factorAddedKeys.first; + const auto& affectedKeys = nonlinearFactors.at(factorIdx)->keys(); + result->markedKeys.insert(affectedKeys.begin(), affectedKeys.end()); + } + } + + // Observed keys for detailed results + if (params_.enableDetailedResults) { + for (Key key : result->markedKeys) { + result->detail->variableStatus[key].isObserved = true; + } + } + + for (Key index : result->markedKeys) { + // Only add if not unused + if (result->unusedIndices.find(index) == result->unusedIndices.end()) + // Make a copy of these, as we'll soon add to them + result->observedKeys.push_back(index); + } + } + + static void CheckRelinearizationRecursiveMap( + const FastMap& thresholds, const VectorValues& delta, + const ISAM2::sharedClique& clique, KeySet* relinKeys) { + // Check the current clique for relinearization + bool relinearize = false; + for (Key var : *clique->conditional()) { + // Find the threshold for this variable type + const Vector& threshold = thresholds.find(Symbol(var).chr())->second; + + const Vector& deltaVar = delta[var]; + + // Verify the threshold vector matches the actual variable size + if (threshold.rows() != deltaVar.rows()) + throw std::invalid_argument( + "Relinearization threshold vector dimensionality for '" + + std::string(1, Symbol(var).chr()) + + "' passed into iSAM2 parameters does not match actual variable " + "dimensionality."); + + // Check for relinearization + if ((deltaVar.array().abs() > threshold.array()).any()) { + relinKeys->insert(var); + relinearize = true; + } + } + + // If this node was relinearized, also check its children + if (relinearize) { + for (const ISAM2::sharedClique& child : clique->children) { + CheckRelinearizationRecursiveMap(thresholds, delta, child, relinKeys); + } + } + } + + static void CheckRelinearizationRecursiveDouble( + double threshold, const VectorValues& delta, + const ISAM2::sharedClique& clique, KeySet* relinKeys) { + // Check the current clique for relinearization + bool relinearize = false; + for (Key var : *clique->conditional()) { + double maxDelta = delta[var].lpNorm(); + if (maxDelta >= threshold) { + relinKeys->insert(var); + relinearize = true; + } + } + + // If this node was relinearized, also check its children + if (relinearize) { + for (const ISAM2::sharedClique& child : clique->children) { + CheckRelinearizationRecursiveDouble(threshold, delta, child, relinKeys); + } + } + } + + /** + * Find the set of variables to be relinearized according to + * relinearizeThreshold. This check is performed recursively, starting at the + * top of the tree. Once a variable in the tree does not need to be + * relinearized, no further checks in that branch are performed. This is an + * approximation of the Full version, designed to save time at the expense of + * accuracy. + * @param delta The linear delta to check against the threshold + * @param keyFormatter Formatter for printing nonlinear keys during debugging + * @return The set of variable indices in delta whose magnitude is greater + * than or equal to relinearizeThreshold + */ + static KeySet CheckRelinearizationPartial( + const ISAM2::Roots& roots, const VectorValues& delta, + const ISAM2Params::RelinearizationThreshold& relinearizeThreshold) { + KeySet relinKeys; + for (const ISAM2::sharedClique& root : roots) { + if (relinearizeThreshold.type() == typeid(double)) + CheckRelinearizationRecursiveDouble( + boost::get(relinearizeThreshold), delta, root, &relinKeys); + else if (relinearizeThreshold.type() == typeid(FastMap)) + CheckRelinearizationRecursiveMap( + boost::get >(relinearizeThreshold), delta, + root, &relinKeys); + } + return relinKeys; + } + + /** + * Find the set of variables to be relinearized according to + * relinearizeThreshold. Any variables in the VectorValues delta whose vector + * magnitude is greater than or equal to relinearizeThreshold are returned. + * @param delta The linear delta to check against the threshold + * @param keyFormatter Formatter for printing nonlinear keys during debugging + * @return The set of variable indices in delta whose magnitude is greater + * than or equal to relinearizeThreshold + */ + static KeySet CheckRelinearizationFull( + const VectorValues& delta, + const ISAM2Params::RelinearizationThreshold& relinearizeThreshold) { + KeySet relinKeys; + + if (const double* threshold = boost::get(&relinearizeThreshold)) { + for (const VectorValues::KeyValuePair& key_delta : delta) { + double maxDelta = key_delta.second.lpNorm(); + if (maxDelta >= *threshold) relinKeys.insert(key_delta.first); + } + } else if (const FastMap* thresholds = + boost::get >(&relinearizeThreshold)) { + for (const VectorValues::KeyValuePair& key_delta : delta) { + const Vector& threshold = + thresholds->find(Symbol(key_delta.first).chr())->second; + if (threshold.rows() != key_delta.second.rows()) + throw std::invalid_argument( + "Relinearization threshold vector dimensionality for '" + + std::string(1, Symbol(key_delta.first).chr()) + + "' passed into iSAM2 parameters does not match actual variable " + "dimensionality."); + if ((key_delta.second.array().abs() > threshold.array()).any()) + relinKeys.insert(key_delta.first); + } + } + + return relinKeys; + } + + /** + * Apply expmap to the given values, but only for indices appearing in + * \c mask. Values are expmapped in-place. + * \param mask Mask on linear indices, only \c true entries are expmapped + */ + void expmapMasked(const VectorValues& delta, const KeySet& mask, + Values* theta) const { + assert(theta->size() == delta.size()); + Values::iterator key_value; + VectorValues::const_iterator key_delta; +#ifdef GTSAM_USE_TBB + for (key_value = theta->begin(); key_value != theta->end(); ++key_value) { + key_delta = delta.find(key_value->key); +#else + for (key_value = theta->begin(), key_delta = delta.begin(); + key_value != theta->end(); ++key_value, ++key_delta) { + assert(key_value->key == key_delta->first); +#endif + Key var = key_value->key; + assert(static_cast(delta[var].size()) == key_value->value.dim()); + assert(delta[var].allFinite()); + if (mask.exists(var)) { + Value* retracted = key_value->value.retract_(delta[var]); + key_value->value = *retracted; + retracted->deallocate_(); + } + } + } + + KeySet relinearize(const ISAM2::Roots& roots, const VectorValues& delta, + const KeySet& fixedVariables, Values* theta, + ISAM2Result* result) const { + KeySet relinKeys; + gttic(gather_relinearize_keys); + // 4. Mark keys in \Delta above threshold \beta: + // J=\{\Delta_{j}\in\Delta|\Delta_{j}\geq\beta\}. + if (params_.enablePartialRelinearizationCheck) + relinKeys = CheckRelinearizationPartial(roots, delta, + params_.relinearizeThreshold); + else + relinKeys = CheckRelinearizationFull(delta, params_.relinearizeThreshold); + if (updateParams_.forceFullSolve) + relinKeys = + CheckRelinearizationFull(delta, 0.0); // This is used for debugging + + // Remove from relinKeys any keys whose linearization points are fixed + for (Key key : fixedVariables) { + relinKeys.erase(key); + } + if (updateParams_.noRelinKeys) { + for (Key key : *updateParams_.noRelinKeys) { + relinKeys.erase(key); + } + } + + // Above relin threshold keys for detailed results + if (params_.enableDetailedResults) { + for (Key key : relinKeys) { + result->detail->variableStatus[key].isAboveRelinThreshold = true; + result->detail->variableStatus[key].isRelinearized = true; + } + } + + // Add the variables being relinearized to the marked keys + KeySet markedRelinMask; + for (const Key key : relinKeys) markedRelinMask.insert(key); + result->markedKeys.insert(relinKeys.begin(), relinKeys.end()); + gttoc(gather_relinearize_keys); + + gttic(fluid_find_all); + // 5. Mark all cliques that involve marked variables \Theta_{J} and all + // their ancestors. + if (!relinKeys.empty()) { + for (const auto& root : roots) + // add other cliques that have the marked ones in the separator + root->findAll(markedRelinMask, &result->markedKeys); + + // Relin involved keys for detailed results + if (params_.enableDetailedResults) { + KeySet involvedRelinKeys; + for (const auto& root : roots) + root->findAll(markedRelinMask, &involvedRelinKeys); + for (Key key : involvedRelinKeys) { + if (!result->detail->variableStatus[key].isAboveRelinThreshold) { + result->detail->variableStatus[key].isRelinearizeInvolved = true; + result->detail->variableStatus[key].isRelinearized = true; + } + } + } + } + gttoc(fluid_find_all); + + gttic(expmap); + // 6. Update linearization point for marked variables: + // \Theta_{J}:=\Theta_{J}+\Delta_{J}. + if (!relinKeys.empty()) expmapMasked(delta, markedRelinMask, theta); + gttoc(expmap); + + result->variablesRelinearized = result->markedKeys.size(); + return relinKeys; + } + + // 7. Linearize new factors + void linearizeNewFactors(const NonlinearFactorGraph& newFactors, + const Values& theta, size_t numNonlinearFactors, + const FactorIndices& newFactorsIndices, + GaussianFactorGraph* linearFactors) const { + gttic(linearizeNewFactors); + auto linearized = newFactors.linearize(theta); + if (params_.findUnusedFactorSlots) { + linearFactors->resize(numNonlinearFactors); + for (size_t i = 0; i < newFactors.size(); ++i) + (*linearFactors)[newFactorsIndices[i]] = (*linearized)[i]; + } else { + linearFactors->push_back(*linearized); + } + assert(linearFactors->size() == numNonlinearFactors); + } + + void augmentVariableIndex(const NonlinearFactorGraph& newFactors, + const FactorIndices& newFactorsIndices, + VariableIndex* variableIndex) const { + gttic(augmentVariableIndex); + // Augment the variable index with the new factors + if (params_.findUnusedFactorSlots) + variableIndex->augment(newFactors, newFactorsIndices); + else + variableIndex->augment(newFactors); + + // Augment it with existing factors which now affect to more variables: + if (updateParams_.newAffectedKeys) { + for (const auto& factorAddedKeys : *updateParams_.newAffectedKeys) { + const auto factorIdx = factorAddedKeys.first; + variableIndex->augmentExistingFactor(factorIdx, factorAddedKeys.second); + } + } + } + + void logRecalculateKeys(const ISAM2Result& result) const { + const bool debug = ISDEBUG("ISAM2 recalculate"); + + if (debug) { + std::cout << "markedKeys: "; + for (const Key key : result.markedKeys) { + std::cout << key << " "; + } + std::cout << std::endl; + std::cout << "observedKeys: "; + for (const Key key : result.observedKeys) { + std::cout << key << " "; + } + std::cout << std::endl; + } + } + + FactorIndexSet getAffectedFactors(const KeyList& keys, + const VariableIndex& variableIndex) const { + FactorIndexSet indices; + for (const Key key : keys) { + const FactorIndices& factors(variableIndex[key]); + indices.insert(factors.begin(), factors.end()); + } + return indices; + } + + // find intermediate (linearized) factors from cache that are passed into the + // affected area + GaussianFactorGraph getCachedBoundaryFactors( + const ISAM2::Cliques& orphans) const { + GaussianFactorGraph cachedBoundary; + + for (const auto& orphan : orphans) { + // retrieve the cached factor and add to boundary + cachedBoundary.push_back(orphan->cachedFactor()); + } + + return cachedBoundary; + } + + // retrieve all factors that ONLY contain the affected variables + // (note that the remaining stuff is summarized in the cached factors) + GaussianFactorGraph relinearizeAffectedFactors( + const FastList& affectedKeys, const KeySet& relinKeys, + const NonlinearFactorGraph& nonlinearFactors, + const VariableIndex& variableIndex, const Values& theta, + GaussianFactorGraph* linearFactors) const { + gttic(getAffectedFactors); + FactorIndexSet candidates = getAffectedFactors(affectedKeys, variableIndex); + gttoc(getAffectedFactors); + + gttic(affectedKeysSet); + // for fast lookup below + KeySet affectedKeysSet; + affectedKeysSet.insert(affectedKeys.begin(), affectedKeys.end()); + gttoc(affectedKeysSet); + + gttic(check_candidates_and_linearize); + GaussianFactorGraph linearized; + for (const FactorIndex idx : candidates) { + bool inside = true; + bool useCachedLinear = params_.cacheLinearizedFactors; + for (Key key : nonlinearFactors[idx]->keys()) { + if (affectedKeysSet.find(key) == affectedKeysSet.end()) { + inside = false; + break; + } + if (useCachedLinear && relinKeys.find(key) != relinKeys.end()) + useCachedLinear = false; + } + if (inside) { + if (useCachedLinear) { +#ifdef GTSAM_EXTRA_CONSISTENCY_CHECKS + assert((*linearFactors)[idx]); + assert((*linearFactors)[idx]->keys() == + nonlinearFactors[idx]->keys()); +#endif + linearized.push_back((*linearFactors)[idx]); + } else { + auto linearFactor = nonlinearFactors[idx]->linearize(theta); + linearized.push_back(linearFactor); + if (params_.cacheLinearizedFactors) { +#ifdef GTSAM_EXTRA_CONSISTENCY_CHECKS + assert((*linearFactors)[idx]->keys() == linearFactor->keys()); +#endif + (*linearFactors)[idx] = linearFactor; + } + } + } + } + gttoc(check_candidates_and_linearize); + + return linearized; + } + + KeySet recalculate(const Values& theta, const VariableIndex& variableIndex, + const NonlinearFactorGraph& nonlinearFactors, + const GaussianBayesNet& affectedBayesNet, + const ISAM2::Cliques& orphans, const KeySet& relinKeys, + GaussianFactorGraph* linearFactors, ISAM2::Roots* roots, + ISAM2::Nodes* nodes, ISAM2Result* result) const { + gttic(recalculate); + logRecalculateKeys(*result); + + // FactorGraph factors(affectedBayesNet); + // bug was here: we cannot reuse the original factors, because then the + // cached factors get messed up [all the necessary data is actually + // contained in the affectedBayesNet, including what was passed in from the + // boundaries, + // so this would be correct; however, in the process we also generate new + // cached_ entries that will be wrong (ie. they don't contain what would be + // passed up at a certain point if batch elimination was done, but that's + // what we need); we could choose not to update cached_ from here, but then + // the new information (and potentially different variable ordering) is not + // reflected in the cached_ values which again will be wrong] + // so instead we have to retrieve the original linearized factors AND add + // the cached factors from the boundary + + // BEGIN OF COPIED CODE + + // ordering provides all keys in conditionals, there cannot be others + // because path to root included + gttic(affectedKeys); + FastList affectedKeys; + for (const auto& conditional : affectedBayesNet) + affectedKeys.insert(affectedKeys.end(), conditional->beginFrontals(), + conditional->endFrontals()); + gttoc(affectedKeys); + + KeySet affectedKeysSet; // Will return this result + + static const double kBatchThreshold = 0.65; + + if (affectedKeys.size() >= theta.size() * kBatchThreshold) { + // Do a batch step - reorder and relinearize all variables + gttic(batch); + + gttic(add_keys); + br::copy(variableIndex | br::map_keys, + std::inserter(affectedKeysSet, affectedKeysSet.end())); + + // Removed unused keys: + VariableIndex affectedFactorsVarIndex = variableIndex; + + affectedFactorsVarIndex.removeUnusedVariables( + result->unusedIndices.begin(), result->unusedIndices.end()); + + for (const Key key : result->unusedIndices) { + affectedKeysSet.erase(key); + } + gttoc(add_keys); + + gttic(ordering); + Ordering order; + if (updateParams_.constrainedKeys) { + order = Ordering::ColamdConstrained(affectedFactorsVarIndex, + *updateParams_.constrainedKeys); + } else { + if (theta.size() > result->observedKeys.size()) { + // Only if some variables are unconstrained + FastMap constraintGroups; + for (Key var : result->observedKeys) constraintGroups[var] = 1; + order = Ordering::ColamdConstrained(affectedFactorsVarIndex, + constraintGroups); + } else { + order = Ordering::Colamd(affectedFactorsVarIndex); + } + } + gttoc(ordering); + + gttic(linearize); + GaussianFactorGraph linearized = *nonlinearFactors.linearize(theta); + if (params_.cacheLinearizedFactors) *linearFactors = linearized; + gttoc(linearize); + + gttic(eliminate); + ISAM2BayesTree::shared_ptr bayesTree = + ISAM2JunctionTree(GaussianEliminationTree( + linearized, affectedFactorsVarIndex, order)) + .eliminate(params_.getEliminationFunction()) + .first; + gttoc(eliminate); + + gttic(insert); + roots->clear(); + roots->insert(roots->end(), bayesTree->roots().begin(), + bayesTree->roots().end()); + nodes->clear(); + nodes->insert(bayesTree->nodes().begin(), bayesTree->nodes().end()); + gttoc(insert); + + result->variablesReeliminated = affectedKeysSet.size(); + result->factorsRecalculated = nonlinearFactors.size(); + + // Reeliminated keys for detailed results + if (params_.enableDetailedResults) { + for (Key key : theta.keys()) { + result->detail->variableStatus[key].isReeliminated = true; + } + } + + gttoc(batch); + + } else { + gttic(incremental); + const bool debug = ISDEBUG("ISAM2 recalculate"); + + // 2. Add the new factors \Factors' into the resulting factor graph + FastList affectedAndNewKeys; + affectedAndNewKeys.insert(affectedAndNewKeys.end(), affectedKeys.begin(), + affectedKeys.end()); + affectedAndNewKeys.insert(affectedAndNewKeys.end(), + result->observedKeys.begin(), + result->observedKeys.end()); + gttic(relinearizeAffected); + GaussianFactorGraph factors = relinearizeAffectedFactors( + affectedAndNewKeys, relinKeys, nonlinearFactors, variableIndex, theta, + linearFactors); + gttoc(relinearizeAffected); + + if (debug) { + factors.print("Relinearized factors: "); + std::cout << "Affected keys: "; + for (const Key key : affectedKeys) { + std::cout << key << " "; + } + std::cout << std::endl; + } + + // Reeliminated keys for detailed results + if (params_.enableDetailedResults) { + for (Key key : affectedAndNewKeys) { + result->detail->variableStatus[key].isReeliminated = true; + } + } + + result->variablesReeliminated = affectedAndNewKeys.size(); + result->factorsRecalculated = factors.size(); + + gttic(cached); + // add the cached intermediate results from the boundary of the orphans + // ... + GaussianFactorGraph cachedBoundary = getCachedBoundaryFactors(orphans); + if (debug) cachedBoundary.print("Boundary factors: "); + factors.push_back(cachedBoundary); + gttoc(cached); + + gttic(orphans); + // Add the orphaned subtrees + for (const auto& orphan : orphans) + factors += + boost::make_shared >(orphan); + gttoc(orphans); + + // END OF COPIED CODE + + // 3. Re-order and eliminate the factor graph into a Bayes net (Algorithm + // [alg:eliminate]), and re-assemble into a new Bayes tree (Algorithm + // [alg:BayesTree]) + + gttic(reorder_and_eliminate); + + gttic(list_to_set); + // create a partial reordering for the new and contaminated factors + // result->markedKeys are passed in: those variables will be forced to the + // end in the ordering + affectedKeysSet.insert(result->markedKeys.begin(), + result->markedKeys.end()); + affectedKeysSet.insert(affectedKeys.begin(), affectedKeys.end()); + gttoc(list_to_set); + + VariableIndex affectedFactorsVarIndex(factors); + + gttic(ordering_constraints); + // Create ordering constraints + FastMap constraintGroups; + if (updateParams_.constrainedKeys) { + constraintGroups = *updateParams_.constrainedKeys; + } else { + constraintGroups = FastMap(); + const int group = + result->observedKeys.size() < affectedFactorsVarIndex.size() ? 1 + : 0; + for (Key var : result->observedKeys) + constraintGroups.insert(std::make_pair(var, group)); + } + + // Remove unaffected keys from the constraints + for (FastMap::iterator iter = constraintGroups.begin(); + iter != constraintGroups.end(); + /*Incremented in loop ++iter*/) { + if (result->unusedIndices.exists(iter->first) || + !affectedKeysSet.exists(iter->first)) + constraintGroups.erase(iter++); + else + ++iter; + } + gttoc(ordering_constraints); + + // Generate ordering + gttic(Ordering); + Ordering ordering = Ordering::ColamdConstrained(affectedFactorsVarIndex, + constraintGroups); + gttoc(Ordering); + + ISAM2BayesTree::shared_ptr bayesTree = + ISAM2JunctionTree(GaussianEliminationTree( + factors, affectedFactorsVarIndex, ordering)) + .eliminate(params_.getEliminationFunction()) + .first; + + gttoc(reorder_and_eliminate); + + gttic(reassemble); + roots->insert(roots->end(), bayesTree->roots().begin(), + bayesTree->roots().end()); + nodes->insert(bayesTree->nodes().begin(), bayesTree->nodes().end()); + gttoc(reassemble); + + // 4. The orphans have already been inserted during elimination + + gttoc(incremental); + } + + // Root clique variables for detailed results + if (params_.enableDetailedResults) { + for (const auto& root : *roots) + for (Key var : *root->conditional()) + result->detail->variableStatus[var].inRootClique = true; + } + + return affectedKeysSet; + } +}; +/* ************************************************************************* */ + +} // namespace gtsam diff --git a/gtsam/nonlinear/ISAM2.cpp b/gtsam/nonlinear/ISAM2.cpp index 988e6779f..0ce60fd6f 100644 --- a/gtsam/nonlinear/ISAM2.cpp +++ b/gtsam/nonlinear/ISAM2.cpp @@ -16,26 +16,16 @@ * @author Michael Kaess, Richard Roberts, Frank Dellaert */ +#include #include +#include #include #include #include -#include // We need the inst file because we'll make a special JT templated on ISAM2 -#include #include -#include -#include -namespace br { -using namespace boost::range; -using namespace boost::adaptors; -} // namespace br - -#include -#include #include -#include using namespace std; @@ -44,35 +34,6 @@ namespace gtsam { // Instantiate base class template class BayesTree; -static const bool kDisableReordering = false; -static const double kBatchThreshold = 0.65; - -/* ************************************************************************* */ -// Special BayesTree class that uses ISAM2 cliques - this is the result of -// reeliminating ISAM2 subtrees. -class ISAM2BayesTree : public ISAM2::Base { - public: - typedef ISAM2::Base Base; - typedef ISAM2BayesTree This; - typedef boost::shared_ptr shared_ptr; - - ISAM2BayesTree() {} -}; - -/* ************************************************************************* */ -// Special JunctionTree class that produces ISAM2 BayesTree cliques, used for -// reeliminating ISAM2 subtrees. -class ISAM2JunctionTree - : public JunctionTree { - public: - typedef JunctionTree Base; - typedef ISAM2JunctionTree This; - typedef boost::shared_ptr shared_ptr; - - explicit ISAM2JunctionTree(const GaussianEliminationTree& eliminationTree) - : Base(eliminationTree) {} -}; - /* ************************************************************************* */ ISAM2::ISAM2(const ISAM2Params& params) : params_(params), update_count_(0) { if (params_.optimizationParams.type() == typeid(ISAM2DoglegParams)) @@ -96,392 +57,11 @@ bool ISAM2::equals(const ISAM2& other, double tol) const { } /* ************************************************************************* */ -FactorIndexSet ISAM2::getAffectedFactors(const KeyList& keys) const { - static const bool debug = false; - if (debug) cout << "Getting affected factors for "; - if (debug) { - for (const Key key : keys) { - cout << key << " "; - } - } - if (debug) cout << endl; - - FactorIndexSet indices; - for (const Key key : keys) { - const VariableIndex::Factors& factors(variableIndex_[key]); - indices.insert(factors.begin(), factors.end()); - } - if (debug) cout << "Affected factors are: "; - if (debug) { - for (const auto index : indices) { - cout << index << " "; - } - } - if (debug) cout << endl; - return indices; -} - -/* ************************************************************************* */ -// retrieve all factors that ONLY contain the affected variables -// (note that the remaining stuff is summarized in the cached factors) - -GaussianFactorGraph::shared_ptr ISAM2::relinearizeAffectedFactors( - const FastList& affectedKeys, const KeySet& relinKeys) const { - gttic(getAffectedFactors); - FactorIndexSet candidates = getAffectedFactors(affectedKeys); - gttoc(getAffectedFactors); - - gttic(affectedKeysSet); - // for fast lookup below - KeySet affectedKeysSet; - affectedKeysSet.insert(affectedKeys.begin(), affectedKeys.end()); - gttoc(affectedKeysSet); - - gttic(check_candidates_and_linearize); - auto linearized = boost::make_shared(); - for (const FactorIndex idx : candidates) { - bool inside = true; - bool useCachedLinear = params_.cacheLinearizedFactors; - for (Key key : nonlinearFactors_[idx]->keys()) { - if (affectedKeysSet.find(key) == affectedKeysSet.end()) { - inside = false; - break; - } - if (useCachedLinear && relinKeys.find(key) != relinKeys.end()) - useCachedLinear = false; - } - if (inside) { - if (useCachedLinear) { -#ifdef GTSAM_EXTRA_CONSISTENCY_CHECKS - assert(linearFactors_[idx]); - assert(linearFactors_[idx]->keys() == nonlinearFactors_[idx]->keys()); -#endif - linearized->push_back(linearFactors_[idx]); - } else { - auto linearFactor = nonlinearFactors_[idx]->linearize(theta_); - linearized->push_back(linearFactor); - if (params_.cacheLinearizedFactors) { -#ifdef GTSAM_EXTRA_CONSISTENCY_CHECKS - assert(linearFactors_[idx]->keys() == linearFactor->keys()); -#endif - linearFactors_[idx] = linearFactor; - } - } - } - } - gttoc(check_candidates_and_linearize); - - return linearized; -} - -/* ************************************************************************* */ -// find intermediate (linearized) factors from cache that are passed into the -// affected area - -GaussianFactorGraph ISAM2::getCachedBoundaryFactors(const Cliques& orphans) { - GaussianFactorGraph cachedBoundary; - - for (sharedClique orphan : orphans) { - // retrieve the cached factor and add to boundary - cachedBoundary.push_back(orphan->cachedFactor()); - } - - return cachedBoundary; -} - -/* ************************************************************************* */ -boost::shared_ptr ISAM2::recalculate( - const KeySet& markedKeys, const KeySet& relinKeys, - const KeyVector& observedKeys, const KeySet& unusedIndices, - const boost::optional >& constrainKeys, - ISAM2Result* result) { - // TODO(dellaert): new factors are linearized twice, - // the newFactors passed in are not used. - - const bool debug = ISDEBUG("ISAM2 recalculate"); - - // Input: BayesTree(this), newFactors - -// figures for paper, disable for timing -#ifdef PRINT_STATS - static int counter = 0; - int maxClique = 0; - double avgClique = 0; - int numCliques = 0; - int nnzR = 0; - if (counter > 0) { // cannot call on empty tree - GaussianISAM2_P::CliqueData cdata = this->getCliqueData(); - GaussianISAM2_P::CliqueStats cstats = cdata.getStats(); - maxClique = cstats.maxCONDITIONALSize; - avgClique = cstats.avgCONDITIONALSize; - numCliques = cdata.conditionalSizes.size(); - nnzR = calculate_nnz(this->root()); - } - counter++; -#endif - - if (debug) { - cout << "markedKeys: "; - for (const Key key : markedKeys) { - cout << key << " "; - } - cout << endl; - cout << "observedKeys: "; - for (const Key key : observedKeys) { - cout << key << " "; - } - cout << endl; - } - - // 1. Remove top of Bayes tree and convert to a factor graph: - // (a) For each affected variable, remove the corresponding clique and all - // parents up to the root. (b) Store orphaned sub-trees \BayesTree_{O} of - // removed cliques. - gttic(removetop); - Cliques orphans; - GaussianBayesNet affectedBayesNet; - this->removeTop(KeyVector(markedKeys.begin(), markedKeys.end()), - affectedBayesNet, orphans); - gttoc(removetop); - - // FactorGraph factors(affectedBayesNet); - // bug was here: we cannot reuse the original factors, because then the cached - // factors get messed up [all the necessary data is actually contained in the - // affectedBayesNet, including what was passed in from the boundaries, - // so this would be correct; however, in the process we also generate new - // cached_ entries that will be wrong (ie. they don't contain what would be - // passed up at a certain point if batch elimination was done, but that's - // what we need); we could choose not to update cached_ from here, but then - // the new information (and potentially different variable ordering) is not - // reflected in the cached_ values which again will be wrong] - // so instead we have to retrieve the original linearized factors AND add the - // cached factors from the boundary - - // BEGIN OF COPIED CODE - - // ordering provides all keys in conditionals, there cannot be others because - // path to root included - gttic(affectedKeys); - FastList affectedKeys; - for (const auto& conditional : affectedBayesNet) - affectedKeys.insert(affectedKeys.end(), conditional->beginFrontals(), - conditional->endFrontals()); - gttoc(affectedKeys); - - boost::shared_ptr affectedKeysSet( - new KeySet()); // Will return this result - - if (affectedKeys.size() >= theta_.size() * kBatchThreshold) { - // Do a batch step - reorder and relinearize all variables - gttic(batch); - - gttic(add_keys); - br::copy(variableIndex_ | br::map_keys, - std::inserter(*affectedKeysSet, affectedKeysSet->end())); - - // Removed unused keys: - VariableIndex affectedFactorsVarIndex = variableIndex_; - - affectedFactorsVarIndex.removeUnusedVariables(unusedIndices.begin(), - unusedIndices.end()); - - for (const Key key : unusedIndices) { - affectedKeysSet->erase(key); - } - gttoc(add_keys); - - gttic(ordering); - Ordering order; - if (constrainKeys) { - order = - Ordering::ColamdConstrained(affectedFactorsVarIndex, *constrainKeys); - } else { - if (theta_.size() > observedKeys.size()) { - // Only if some variables are unconstrained - FastMap constraintGroups; - for (Key var : observedKeys) constraintGroups[var] = 1; - order = Ordering::ColamdConstrained(affectedFactorsVarIndex, - constraintGroups); - } else { - order = Ordering::Colamd(affectedFactorsVarIndex); - } - } - gttoc(ordering); - - gttic(linearize); - GaussianFactorGraph linearized = *nonlinearFactors_.linearize(theta_); - if (params_.cacheLinearizedFactors) linearFactors_ = linearized; - gttoc(linearize); - - gttic(eliminate); - ISAM2BayesTree::shared_ptr bayesTree = - ISAM2JunctionTree( - GaussianEliminationTree(linearized, affectedFactorsVarIndex, order)) - .eliminate(params_.getEliminationFunction()) - .first; - gttoc(eliminate); - - gttic(insert); - this->clear(); - this->roots_.insert(this->roots_.end(), bayesTree->roots().begin(), - bayesTree->roots().end()); - this->nodes_.insert(bayesTree->nodes().begin(), bayesTree->nodes().end()); - gttoc(insert); - - result->variablesReeliminated = affectedKeysSet->size(); - result->factorsRecalculated = nonlinearFactors_.size(); - - lastAffectedMarkedCount = markedKeys.size(); - lastAffectedVariableCount = affectedKeysSet->size(); - lastAffectedFactorCount = linearized.size(); - - // Reeliminated keys for detailed results - if (params_.enableDetailedResults) { - for (Key key : theta_.keys()) { - result->detail->variableStatus[key].isReeliminated = true; - } - } - - gttoc(batch); - - } else { - gttic(incremental); - - // 2. Add the new factors \Factors' into the resulting factor graph - FastList affectedAndNewKeys; - affectedAndNewKeys.insert(affectedAndNewKeys.end(), affectedKeys.begin(), - affectedKeys.end()); - affectedAndNewKeys.insert(affectedAndNewKeys.end(), observedKeys.begin(), - observedKeys.end()); - gttic(relinearizeAffected); - GaussianFactorGraph factors( - *relinearizeAffectedFactors(affectedAndNewKeys, relinKeys)); - if (debug) factors.print("Relinearized factors: "); - gttoc(relinearizeAffected); - - if (debug) { - cout << "Affected keys: "; - for (const Key key : affectedKeys) { - cout << key << " "; - } - cout << endl; - } - - // Reeliminated keys for detailed results - if (params_.enableDetailedResults) { - for (Key key : affectedAndNewKeys) { - result->detail->variableStatus[key].isReeliminated = true; - } - } - - result->variablesReeliminated = affectedAndNewKeys.size(); - result->factorsRecalculated = factors.size(); - lastAffectedMarkedCount = markedKeys.size(); - lastAffectedVariableCount = affectedKeys.size(); - lastAffectedFactorCount = factors.size(); - -#ifdef PRINT_STATS - // output for generating figures - cout << "linear: #markedKeys: " << markedKeys.size() - << " #affectedVariables: " << affectedKeys.size() - << " #affectedFactors: " << factors.size() - << " maxCliqueSize: " << maxClique << " avgCliqueSize: " << avgClique - << " #Cliques: " << numCliques << " nnzR: " << nnzR << endl; -#endif - - gttic(cached); - // add the cached intermediate results from the boundary of the orphans ... - GaussianFactorGraph cachedBoundary = getCachedBoundaryFactors(orphans); - if (debug) cachedBoundary.print("Boundary factors: "); - factors.push_back(cachedBoundary); - gttoc(cached); - - gttic(orphans); - // Add the orphaned subtrees - for (const sharedClique& orphan : orphans) - factors += boost::make_shared >(orphan); - gttoc(orphans); - - // END OF COPIED CODE - - // 3. Re-order and eliminate the factor graph into a Bayes net (Algorithm - // [alg:eliminate]), and re-assemble into a new Bayes tree (Algorithm - // [alg:BayesTree]) - - gttic(reorder_and_eliminate); - - gttic(list_to_set); - // create a partial reordering for the new and contaminated factors - // markedKeys are passed in: those variables will be forced to the end in - // the ordering - affectedKeysSet->insert(markedKeys.begin(), markedKeys.end()); - affectedKeysSet->insert(affectedKeys.begin(), affectedKeys.end()); - gttoc(list_to_set); - - VariableIndex affectedFactorsVarIndex(factors); - - gttic(ordering_constraints); - // Create ordering constraints - FastMap constraintGroups; - if (constrainKeys) { - constraintGroups = *constrainKeys; - } else { - constraintGroups = FastMap(); - const int group = - observedKeys.size() < affectedFactorsVarIndex.size() ? 1 : 0; - for (Key var : observedKeys) - constraintGroups.insert(make_pair(var, group)); - } - - // Remove unaffected keys from the constraints - for (FastMap::iterator iter = constraintGroups.begin(); - iter != constraintGroups.end(); - /*Incremented in loop ++iter*/) { - if (unusedIndices.exists(iter->first) || - !affectedKeysSet->exists(iter->first)) - constraintGroups.erase(iter++); - else - ++iter; - } - gttoc(ordering_constraints); - - // Generate ordering - gttic(Ordering); - Ordering ordering = - Ordering::ColamdConstrained(affectedFactorsVarIndex, constraintGroups); - gttoc(Ordering); - - ISAM2BayesTree::shared_ptr bayesTree = - ISAM2JunctionTree( - GaussianEliminationTree(factors, affectedFactorsVarIndex, ordering)) - .eliminate(params_.getEliminationFunction()) - .first; - - gttoc(reorder_and_eliminate); - - gttic(reassemble); - this->roots_.insert(this->roots_.end(), bayesTree->roots().begin(), - bayesTree->roots().end()); - this->nodes_.insert(bayesTree->nodes().begin(), bayesTree->nodes().end()); - gttoc(reassemble); - - // 4. The orphans have already been inserted during elimination - - gttoc(incremental); - } - - // Root clique variables for detailed results - if (params_.enableDetailedResults) { - for (const sharedNode& root : this->roots()) - for (Key var : *root->conditional()) - result->detail->variableStatus[var].inRootClique = true; - } - - return affectedKeysSet; -} - -/* ************************************************************************* */ -void ISAM2::addVariables(const Values& newTheta) { +void ISAM2::addVariables( + const Values& newTheta, + ISAM2Result::DetailedResults::StatusMap* variableStatus) { + gttic(addNewVariables); + // \Theta:=\Theta\cup\Theta_{new}. const bool debug = ISDEBUG("ISAM2 AddVariables"); theta_.insert(newTheta); @@ -490,6 +70,13 @@ void ISAM2::addVariables(const Values& newTheta) { delta_.insert(newTheta.zeroVectors()); deltaNewton_.insert(newTheta.zeroVectors()); RgProd_.insert(newTheta.zeroVectors()); + + // New keys for detailed results + if (variableStatus && params_.enableDetailedResults) { + for (Key key : newTheta.keys()) { + (*variableStatus)[key].isNew = true; + } + } } /* ************************************************************************* */ @@ -506,36 +93,6 @@ void ISAM2::removeVariables(const KeySet& unusedKeys) { } } -/* ************************************************************************* */ -void ISAM2::expmapMasked(const KeySet& mask) { - assert(theta_.size() == delta_.size()); - Values::iterator key_value; - VectorValues::const_iterator key_delta; -#ifdef GTSAM_USE_TBB - for (key_value = theta_.begin(); key_value != theta_.end(); ++key_value) { - key_delta = delta_.find(key_value->key); -#else - for (key_value = theta_.begin(), key_delta = delta_.begin(); - key_value != theta_.end(); ++key_value, ++key_delta) { - assert(key_value->key == key_delta->first); -#endif - Key var = key_value->key; - assert(static_cast(delta_[var].size()) == key_value->value.dim()); - assert(delta_[var].allFinite()); - if (mask.exists(var)) { - Value* retracted = key_value->value.retract_(delta_[var]); - key_value->value = *retracted; - retracted->deallocate_(); -#ifndef NDEBUG - // If debugging, invalidate delta_ entries to Inf, to trigger assertions - // if we try to re-use them. - delta_[var] = Vector::Constant(delta_[var].rows(), - numeric_limits::infinity()); -#endif - } - } -} - /* ************************************************************************* */ ISAM2Result ISAM2::update( const NonlinearFactorGraph& newFactors, const Values& newTheta, @@ -544,7 +101,6 @@ ISAM2Result ISAM2::update( const boost::optional >& noRelinKeys, const boost::optional >& extraReelimKeys, bool force_relinearize) { - ISAM2UpdateParams params; params.constrainedKeys = constrainedKeys; params.extraReelimKeys = extraReelimKeys; @@ -557,28 +113,21 @@ ISAM2Result ISAM2::update( /* ************************************************************************* */ ISAM2Result ISAM2::update(const NonlinearFactorGraph& newFactors, - const Values& newTheta, - const ISAM2UpdateParams& updateParams) { - const bool debug = ISDEBUG("ISAM2 update"); - const bool verbose = ISDEBUG("ISAM2 update verbose"); - + const Values& newTheta, + const ISAM2UpdateParams& updateParams) { gttic(ISAM2_update); this->update_count_++; - lastAffectedVariableCount = 0; - lastAffectedFactorCount = 0; - lastAffectedCliqueCount = 0; - lastAffectedMarkedCount = 0; - lastBacksubVariableCount = 0; - lastNnzTop = 0; ISAM2Result result; if (params_.enableDetailedResults) result.detail = ISAM2Result::DetailedResults(); const bool relinearizeThisStep = - updateParams.force_relinearize || (params_.enableRelinearization && - update_count_ % params_.relinearizeSkip == 0); + updateParams.force_relinearize || + (params_.enableRelinearization && + update_count_ % params_.relinearizeSkip == 0); + const bool verbose = ISDEBUG("ISAM2 update verbose"); if (verbose) { cout << "ISAM2::update\n"; this->print("ISAM2: "); @@ -586,243 +135,66 @@ ISAM2Result ISAM2::update(const NonlinearFactorGraph& newFactors, // Update delta if we need it to check relinearization later if (relinearizeThisStep) { - gttic(updateDelta); - updateDelta(kDisableReordering); - gttoc(updateDelta); + updateDelta(updateParams.forceFullSolve); } - gttic(push_back_factors); + UpdateImpl update(params_, updateParams); + // 1. Add any new factors \Factors:=\Factors\cup\Factors'. - // Add the new factor indices to the result struct - if (debug || verbose) newFactors.print("The new factors are: "); - Impl::AddFactorsStep1(newFactors, params_.findUnusedFactorSlots, - &nonlinearFactors_, &result.newFactorsIndices); + update.pushBackFactors(newFactors, &nonlinearFactors_, &linearFactors_, + &variableIndex_, &result); - // Remove the removed factors - NonlinearFactorGraph removeFactors; - removeFactors.reserve(updateParams.removeFactorIndices.size()); - for (const auto index : updateParams.removeFactorIndices) { - removeFactors.push_back(nonlinearFactors_[index]); - nonlinearFactors_.remove(index); - if (params_.cacheLinearizedFactors) linearFactors_.remove(index); - } - - // Remove removed factors from the variable index so we do not attempt to - // relinearize them - variableIndex_.remove(updateParams.removeFactorIndices.begin(), - updateParams.removeFactorIndices.end(), - removeFactors); - - // Compute unused keys and indices - KeySet unusedKeys; - KeySet unusedIndices; - { - // Get keys from removed factors and new factors, and compute unused keys, - // i.e., keys that are empty now and do not appear in the new factors. - KeySet removedAndEmpty; - for (Key key : removeFactors.keys()) { - if (variableIndex_[key].empty()) - removedAndEmpty.insert(removedAndEmpty.end(), key); - } - KeySet newFactorSymbKeys = newFactors.keys(); - std::set_difference(removedAndEmpty.begin(), removedAndEmpty.end(), - newFactorSymbKeys.begin(), newFactorSymbKeys.end(), - std::inserter(unusedKeys, unusedKeys.end())); - - // Get indices for unused keys - for (Key key : unusedKeys) { - unusedIndices.insert(unusedIndices.end(), key); - } - } - gttoc(push_back_factors); - - gttic(add_new_variables); // 2. Initialize any new variables \Theta_{new} and add - // \Theta:=\Theta\cup\Theta_{new}. - addVariables(newTheta); - // New keys for detailed results - if (params_.enableDetailedResults) { - for (Key key : newTheta.keys()) { - result.detail->variableStatus[key].isNew = true; - } - } - gttoc(add_new_variables); + addVariables(newTheta, result.detail ? &result.detail->variableStatus : 0); gttic(evaluate_error_before); if (params_.evaluateNonlinearError) result.errorBefore.reset(nonlinearFactors_.error(calculateEstimate())); gttoc(evaluate_error_before); - gttic(gather_involved_keys); // 3. Mark linear update - KeySet markedKeys = newFactors.keys(); // Get keys from new factors - // Also mark keys involved in removed factors - { - KeySet markedRemoveKeys = - removeFactors.keys(); // Get keys involved in removed factors - markedKeys.insert( - markedRemoveKeys.begin(), - markedRemoveKeys.end()); // Add to the overall set of marked keys - } - // Also mark any provided extra re-eliminate keys - if (updateParams.extraReelimKeys) { - for (Key key : *updateParams.extraReelimKeys) { - markedKeys.insert(key); - } - } - // Also, keys that were not observed in existing factors, but whose affected - // keys have been extended now (e.g. smart factors) - if (updateParams.newAffectedKeys) { - for (const auto &factorAddedKeys : *updateParams.newAffectedKeys) { - const auto factorIdx = factorAddedKeys.first; - const auto& affectedKeys = nonlinearFactors_.at(factorIdx)->keys(); - markedKeys.insert(affectedKeys.begin(),affectedKeys.end()); - } - } - - // Observed keys for detailed results - if (params_.enableDetailedResults) { - for (Key key : markedKeys) { - result.detail->variableStatus[key].isObserved = true; - } - } - - KeyVector observedKeys; - for (Key index : markedKeys) { - // Only add if not unused - if (unusedIndices.find(index) == unusedIndices.end()) - // Make a copy of these, as we'll soon add to them - observedKeys.push_back(index); - } - gttoc(gather_involved_keys); + update.gatherInvolvedKeys(newFactors, nonlinearFactors_, &result); // Check relinearization if we're at the nth step, or we are using a looser // loop relin threshold KeySet relinKeys; if (relinearizeThisStep) { - gttic(gather_relinearize_keys); - // 4. Mark keys in \Delta above threshold \beta: - // J=\{\Delta_{j}\in\Delta|\Delta_{j}\geq\beta\}. - if (params_.enablePartialRelinearizationCheck) - relinKeys = Impl::CheckRelinearizationPartial( - roots_, delta_, params_.relinearizeThreshold); - else - relinKeys = - Impl::CheckRelinearizationFull(delta_, params_.relinearizeThreshold); - if (kDisableReordering) - relinKeys = Impl::CheckRelinearizationFull( - delta_, 0.0); // This is used for debugging - - // Remove from relinKeys any keys whose linearization points are fixed - for (Key key : fixedVariables_) { - relinKeys.erase(key); - } - if (updateParams.noRelinKeys) { - for (Key key : *updateParams.noRelinKeys) { - relinKeys.erase(key); - } - } - - // Above relin threshold keys for detailed results - if (params_.enableDetailedResults) { - for (Key key : relinKeys) { - result.detail->variableStatus[key].isAboveRelinThreshold = true; - result.detail->variableStatus[key].isRelinearized = true; - } - } - - // Add the variables being relinearized to the marked keys - KeySet markedRelinMask; - for (const Key key : relinKeys) markedRelinMask.insert(key); - markedKeys.insert(relinKeys.begin(), relinKeys.end()); - gttoc(gather_relinearize_keys); - - gttic(fluid_find_all); - // 5. Mark all cliques that involve marked variables \Theta_{J} and all - // their ancestors. - if (!relinKeys.empty()) { - for (const sharedClique& root : roots_) - // add other cliques that have the marked ones in the separator - root->findAll(markedRelinMask, &markedKeys); - - // Relin involved keys for detailed results - if (params_.enableDetailedResults) { - KeySet involvedRelinKeys; - for (const sharedClique& root : roots_) - root->findAll(markedRelinMask, &involvedRelinKeys); - for (Key key : involvedRelinKeys) { - if (!result.detail->variableStatus[key].isAboveRelinThreshold) { - result.detail->variableStatus[key].isRelinearizeInvolved = true; - result.detail->variableStatus[key].isRelinearized = true; - } - } - } - } - gttoc(fluid_find_all); - - gttic(expmap); - // 6. Update linearization point for marked variables: - // \Theta_{J}:=\Theta_{J}+\Delta_{J}. - if (!relinKeys.empty()) expmapMasked(markedRelinMask); - gttoc(expmap); - - result.variablesRelinearized = markedKeys.size(); + relinKeys = + update.relinearize(roots_, delta_, fixedVariables_, &theta_, &result); } else { result.variablesRelinearized = 0; } - gttic(linearize_new); // 7. Linearize new factors - if (params_.cacheLinearizedFactors) { - gttic(linearize); - auto linearFactors = newFactors.linearize(theta_); - if (params_.findUnusedFactorSlots) { - linearFactors_.resize(nonlinearFactors_.size()); - for (size_t newFactorI = 0; newFactorI < newFactors.size(); ++newFactorI) - linearFactors_[result.newFactorsIndices[newFactorI]] = - (*linearFactors)[newFactorI]; - } else { - linearFactors_.push_back(*linearFactors); - } - assert(nonlinearFactors_.size() == linearFactors_.size()); - gttoc(linearize); - } - gttoc(linearize_new); + update.linearizeNewFactors(newFactors, theta_, nonlinearFactors_.size(), + result.newFactorsIndices, &linearFactors_); + update.augmentVariableIndex(newFactors, result.newFactorsIndices, + &variableIndex_); - gttic(augment_VI); - // Augment the variable index with the new factors - if (params_.findUnusedFactorSlots) - variableIndex_.augment(newFactors, result.newFactorsIndices); - else - variableIndex_.augment(newFactors); - - // Augment it with existing factors which now affect to more variables: - if (updateParams.newAffectedKeys) { - for (const auto &factorAddedKeys : *updateParams.newAffectedKeys) { - const auto factorIdx = factorAddedKeys.first; - variableIndex_.augmentExistingFactor( - factorIdx, factorAddedKeys.second); - } - } - gttoc(augment_VI); - - gttic(recalculate); // 8. Redo top of Bayes tree - boost::shared_ptr replacedKeys; - if (!markedKeys.empty() || !observedKeys.empty()) - replacedKeys = recalculate( - markedKeys, relinKeys, observedKeys, unusedIndices, - updateParams.constrainedKeys, &result); + if (!result.markedKeys.empty() || !result.observedKeys.empty()) { + // Remove top of Bayes tree and convert to a factor graph: + // (a) For each affected variable, remove the corresponding clique and all + // parents up to the root. (b) Store orphaned sub-trees \BayesTree_{O} of + // removed cliques. + GaussianBayesNet affectedBayesNet; + Cliques orphans; + this->removeTop( + KeyVector(result.markedKeys.begin(), result.markedKeys.end()), + affectedBayesNet, orphans); - // Update replaced keys mask (accumulates until back-substitution takes place) - if (replacedKeys) - deltaReplacedMask_.insert(replacedKeys->begin(), replacedKeys->end()); - gttoc(recalculate); + KeySet affectedKeysSet = update.recalculate( + theta_, variableIndex_, nonlinearFactors_, affectedBayesNet, orphans, + relinKeys, &linearFactors_, &roots_, &nodes_, &result); + // Update replaced keys mask (accumulates until back-substitution takes + // place) + deltaReplacedMask_.insert(affectedKeysSet.begin(), affectedKeysSet.end()); + } // Update data structures to remove unused keys - if (!unusedKeys.empty()) { + if (!result.unusedKeys.empty()) { gttic(remove_variables); - removeVariables(unusedKeys); + removeVariables(result.unusedKeys); gttoc(remove_variables); } result.cliques = this->nodes().size(); @@ -966,7 +338,7 @@ void ISAM2::marginalizeLeaves( } } // Create factor graph from factor indices - for (const auto index: factorsFromMarginalizedInClique_step1) { + for (const auto index : factorsFromMarginalizedInClique_step1) { graph.push_back(nonlinearFactors_[index]->linearize(theta_)); } @@ -1040,7 +412,7 @@ void ISAM2::marginalizeLeaves( // Remove the factors to remove that have been summarized in the newly-added // marginal factors NonlinearFactorGraph removedFactors; - for (const auto index: factorIndicesToRemove) { + for (const auto index : factorIndicesToRemove) { removedFactors.push_back(nonlinearFactors_[index]); nonlinearFactors_.remove(index); if (params_.cacheLinearizedFactors) linearFactors_.remove(index); @@ -1057,6 +429,7 @@ void ISAM2::marginalizeLeaves( } /* ************************************************************************* */ +// Marked const but actually changes mutable delta void ISAM2::updateDelta(bool forceFullSolve) const { gttic(updateDelta); if (params_.optimizationParams.type() == typeid(ISAM2GaussNewtonParams)) { @@ -1066,8 +439,8 @@ void ISAM2::updateDelta(bool forceFullSolve) const { const double effectiveWildfireThreshold = forceFullSolve ? 0.0 : gaussNewtonParams.wildfireThreshold; gttic(Wildfire_update); - lastBacksubVariableCount = Impl::UpdateGaussNewtonDelta( - roots_, deltaReplacedMask_, effectiveWildfireThreshold, &delta_); + DeltaImpl::UpdateGaussNewtonDelta(roots_, deltaReplacedMask_, + effectiveWildfireThreshold, &delta_); deltaReplacedMask_.clear(); gttoc(Wildfire_update); @@ -1083,15 +456,15 @@ void ISAM2::updateDelta(bool forceFullSolve) const { // Compute Newton's method step gttic(Wildfire_update); - lastBacksubVariableCount = Impl::UpdateGaussNewtonDelta( + DeltaImpl::UpdateGaussNewtonDelta( roots_, deltaReplacedMask_, effectiveWildfireThreshold, &deltaNewton_); gttoc(Wildfire_update); // Compute steepest descent step const VectorValues gradAtZero = this->gradientAtZero(); // Compute gradient - Impl::UpdateRgProd(roots_, deltaReplacedMask_, gradAtZero, - &RgProd_); // Update RgProd - const VectorValues dx_u = Impl::ComputeGradientSearch( + DeltaImpl::UpdateRgProd(roots_, deltaReplacedMask_, gradAtZero, + &RgProd_); // Update RgProd + const VectorValues dx_u = DeltaImpl::ComputeGradientSearch( gradAtZero, RgProd_); // Compute gradient search point // Clear replaced keys mask because now we've updated deltaNewton_ and @@ -1163,8 +536,7 @@ VectorValues ISAM2::gradientAtZero() const { VectorValues g; // Sum up contributions for each clique - for (const auto& root : this->roots()) - root->addGradientAtZero(&g); + for (const auto& root : this->roots()) root->addGradientAtZero(&g); return g; } diff --git a/gtsam/nonlinear/ISAM2.h b/gtsam/nonlinear/ISAM2.h index 60cdc45fe..b0eb4693b 100644 --- a/gtsam/nonlinear/ISAM2.h +++ b/gtsam/nonlinear/ISAM2.h @@ -20,12 +20,12 @@ #pragma once +#include +#include #include #include -#include #include #include -#include #include @@ -73,8 +73,8 @@ class GTSAM_EXPORT ISAM2 : public BayesTree { * This is \c mutable because it is used internally to not update delta_ * until it is needed. */ - mutable KeySet - deltaReplacedMask_; // TODO(dellaert): Make sure accessed in the right way + mutable KeySet deltaReplacedMask_; // TODO(dellaert): Make sure accessed in + // the right way /** All original nonlinear factors are stored here to use during * relinearization */ @@ -175,9 +175,9 @@ class GTSAM_EXPORT ISAM2 : public BayesTree { * @return An ISAM2Result struct containing information about the update * @note No default parameters to avoid ambiguous call errors. */ - virtual ISAM2Result update( - const NonlinearFactorGraph& newFactors, const Values& newTheta, - const ISAM2UpdateParams& updateParams); + virtual ISAM2Result update(const NonlinearFactorGraph& newFactors, + const Values& newTheta, + const ISAM2UpdateParams& updateParams); /** Marginalize out variables listed in leafKeys. These keys must be leaves * in the BayesTree. Throws MarginalizeNonleafException if non-leaves are @@ -226,7 +226,6 @@ class GTSAM_EXPORT ISAM2 : public BayesTree { return traits::Retract(theta_.at(key), delta); } - /** Compute an estimate for a single variable using its incomplete linear * delta computed during the last update. This is faster than calling the * no-argument version of calculateEstimate, which operates on all variables. @@ -243,9 +242,6 @@ class GTSAM_EXPORT ISAM2 : public BayesTree { /// @name Public members for non-typical usage /// @{ - /** Internal implementation functions */ - struct Impl; - /** Compute an estimate using a complete delta computed by a full * back-substitution. */ @@ -268,13 +264,6 @@ class GTSAM_EXPORT ISAM2 : public BayesTree { /** Access the nonlinear variable index */ const KeySet& getFixedVariables() const { return fixedVariables_; } - size_t lastAffectedVariableCount; - size_t lastAffectedFactorCount; - size_t lastAffectedCliqueCount; - size_t lastAffectedMarkedCount; - mutable size_t lastBacksubVariableCount; - size_t lastNnzTop; - const ISAM2Params& params() const { return params_; } /** prints out clique statistics */ @@ -295,37 +284,17 @@ class GTSAM_EXPORT ISAM2 : public BayesTree { /** * Add new variables to the ISAM2 system. * @param newTheta Initial values for new variables - * @param theta Current solution to be augmented with new initialization - * @param delta Current linear delta to be augmented with zeros - * @param deltaNewton - * @param RgProd - * @param keyFormatter Formatter for printing nonlinear keys during debugging + * @param variableStatus optional detailed result structure */ - void addVariables(const Values& newTheta); + void addVariables( + const Values& newTheta, + ISAM2Result::DetailedResults::StatusMap* variableStatus = 0); /** * Remove variables from the ISAM2 system. */ void removeVariables(const KeySet& unusedKeys); - /** - * Apply expmap to the given values, but only for indices appearing in - * \c mask. Values are expmapped in-place. - * \param mask Mask on linear indices, only \c true entries are expmapped - */ - void expmapMasked(const KeySet& mask); - - FactorIndexSet getAffectedFactors(const FastList& keys) const; - GaussianFactorGraph::shared_ptr relinearizeAffectedFactors( - const FastList& affectedKeys, const KeySet& relinKeys) const; - GaussianFactorGraph getCachedBoundaryFactors(const Cliques& orphans); - - virtual boost::shared_ptr recalculate( - const KeySet& markedKeys, const KeySet& relinKeys, - const KeyVector& observedKeys, const KeySet& unusedIndices, - const boost::optional >& constrainKeys, - ISAM2Result* result); - void updateDelta(bool forceFullSolve = false) const; }; // ISAM2 @@ -334,5 +303,3 @@ template <> struct traits : public Testable {}; } // namespace gtsam - -#include diff --git a/gtsam/nonlinear/ISAM2Result.h b/gtsam/nonlinear/ISAM2Result.h index 763a5e198..d49c92627 100644 --- a/gtsam/nonlinear/ISAM2Result.h +++ b/gtsam/nonlinear/ISAM2Result.h @@ -24,8 +24,8 @@ #include #include -#include #include +#include #include @@ -96,6 +96,20 @@ struct GTSAM_EXPORT ISAM2Result { */ FactorIndices newFactorsIndices; + /** Unused keys, and indices for unused keys. TODO(frank): the same?? + * i.e., keys that are empty now and do not appear in the new factors. + */ + KeySet unusedKeys, unusedIndices; + + /** keys for variables that were observed, i.e., not unused. */ + KeyVector observedKeys; + + /** Keys of variables that had factors removed. */ + KeySet keysWithRemovedFactors; + + /** All keys that were marked during the update process. */ + KeySet markedKeys; + /** A struct holding detailed results, which must be enabled with * ISAM2Params::enableDetailedResults. */ @@ -132,9 +146,10 @@ struct GTSAM_EXPORT ISAM2Result { inRootClique(false) {} }; - /** The status of each variable during this update, see VariableStatus. - */ - FastMap variableStatus; + typedef FastMap StatusMap; + + /// The status of each variable during this update, see VariableStatus. + StatusMap variableStatus; }; /** Detailed results, if enabled by ISAM2Params::enableDetailedResults. See diff --git a/gtsam/nonlinear/ISAM2UpdateParams.h b/gtsam/nonlinear/ISAM2UpdateParams.h index 77f722b12..884ac7922 100644 --- a/gtsam/nonlinear/ISAM2UpdateParams.h +++ b/gtsam/nonlinear/ISAM2UpdateParams.h @@ -16,11 +16,11 @@ #pragma once -#include #include -#include // GTSAM_EXPORT -#include // Key, KeySet -#include //FactorIndices +#include // GTSAM_EXPORT +#include // Key, KeySet +#include //FactorIndices +#include namespace gtsam { @@ -63,8 +63,12 @@ struct GTSAM_EXPORT ISAM2UpdateParams { * depend on Keys `X(2)`, `X(3)`. Next call to ISAM2::update() must include * its `newAffectedKeys` field with the map `13 -> {X(2), X(3)}`. */ - boost::optional> newAffectedKeys{boost::none}; + boost::optional> newAffectedKeys{boost::none}; + /** By default, iSAM2 uses a wildfire update scheme that stops updating when + * the deltas become too small down in the tree. This flagg forces a full + * solve instead. */ + bool forceFullSolve{false}; }; -} // namespace gtsam +} // namespace gtsam diff --git a/tests/testGaussianISAM2.cpp b/tests/testGaussianISAM2.cpp index 2b6809ada..d8b829872 100644 --- a/tests/testGaussianISAM2.cpp +++ b/tests/testGaussianISAM2.cpp @@ -4,7 +4,8 @@ * @author Michael Kaess */ -#include +#include +#include #include #include @@ -14,7 +15,6 @@ #include #include #include -#include #include #include #include @@ -23,10 +23,13 @@ #include #include #include -#include #include -using namespace boost::assign; + +#include + +#include #include +using namespace boost::assign; namespace br { using namespace boost::adaptors; using namespace boost::range; } using namespace std; @@ -304,7 +307,7 @@ TEST(ISAM2, AddFactorsStep1) FactorIndices actualNewFactorIndices; - ISAM2::Impl::AddFactorsStep1(newFactors, true, &nonlinearFactors, &actualNewFactorIndices); + UpdateImpl::AddFactorsStep1(newFactors, true, &nonlinearFactors, &actualNewFactorIndices); EXPECT(assert_equal(expectedNonlinearFactors, nonlinearFactors)); EXPECT(assert_container_equality(expectedNewFactorIndices, actualNewFactorIndices));