diff --git a/gtsam/nonlinear/ISAM2-impl.cpp b/gtsam/nonlinear/ISAM2-impl.cpp index 3b5156535..aa201138c 100644 --- a/gtsam/nonlinear/ISAM2-impl.cpp +++ b/gtsam/nonlinear/ISAM2-impl.cpp @@ -11,33 +11,35 @@ /** * @file ISAM2-impl.cpp - * @brief Incremental update functionality (ISAM2) for BayesTree, with fluid relinearization. + * @brief Incremental update functionality (ISAM2) for BayesTree, with fluid + * relinearization. * @author Michael Kaess * @author Richard Roberts */ -#include -#include // for selective linearization thresholds #include -#include // for GTSAM_USE_TBB +#include // for GTSAM_USE_TBB +#include // for selective linearization thresholds +#include -#include #include +#include +#include +#include using namespace std; namespace gtsam { /* ************************************************************************* */ -void ISAM2::Impl::AddVariables( - const Values& newTheta, Values& theta, VectorValues& delta, - VectorValues& deltaNewton, VectorValues& RgProd, - const KeyFormatter& keyFormatter) -{ +void ISAM2::Impl::AddVariables(const Values& newTheta, Values& theta, + VectorValues& delta, VectorValues& deltaNewton, + VectorValues& RgProd, + const KeyFormatter& keyFormatter) { const bool debug = ISDEBUG("ISAM2 AddVariables"); theta.insert(newTheta); - if(debug) newTheta.print("The new variables are: "); + if (debug) newTheta.print("The new variables are: "); // Add zeros into the VectorValues delta.insert(newTheta.zeroVectors()); deltaNewton.insert(newTheta.zeroVectors()); @@ -45,55 +47,55 @@ void ISAM2::Impl::AddVariables( } /* ************************************************************************* */ -void ISAM2::Impl::AddFactorsStep1(const NonlinearFactorGraph& newFactors, bool useUnusedSlots, - NonlinearFactorGraph& nonlinearFactors, FactorIndices& newFactorIndices) -{ +void ISAM2::Impl::AddFactorsStep1(const NonlinearFactorGraph& newFactors, + bool useUnusedSlots, + NonlinearFactorGraph& nonlinearFactors, + FactorIndices& newFactorIndices) { newFactorIndices.resize(newFactors.size()); - if(useUnusedSlots) - { + if (useUnusedSlots) { size_t globalFactorIndex = 0; - for(size_t newFactorIndex = 0; newFactorIndex < newFactors.size(); ++newFactorIndex) - { + 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; + 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); + } while (true); // Use the current slot, updating nonlinearFactors and newFactorSlots. nonlinearFactors[globalFactorIndex] = newFactors[newFactorIndex]; newFactorIndices[newFactorIndex] = globalFactorIndex; } - } - else - { + } 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) + for (size_t i = 0; i < newFactors.size(); ++i) newFactorIndices[i] = i + nonlinearFactors.size(); nonlinearFactors.push_back(newFactors); } } /* ************************************************************************* */ -void ISAM2::Impl::RemoveVariables(const KeySet& unusedKeys, const FastVector& roots, +void ISAM2::Impl::RemoveVariables(const KeySet& unusedKeys, + const FastVector& roots, Values& theta, VariableIndex& variableIndex, - VectorValues& delta, VectorValues& deltaNewton, VectorValues& RgProd, - KeySet& replacedKeys, Base::Nodes& nodes, - KeySet& fixedVariables) -{ + VectorValues& delta, + VectorValues& deltaNewton, + VectorValues& RgProd, KeySet& replacedKeys, + Base::Nodes& nodes, KeySet& fixedVariables) { variableIndex.removeUnusedVariables(unusedKeys.begin(), unusedKeys.end()); - for(Key key: unusedKeys) { + for (Key key : unusedKeys) { delta.erase(key); deltaNewton.erase(key); RgProd.erase(key); @@ -105,26 +107,28 @@ void ISAM2::Impl::RemoveVariables(const KeySet& unusedKeys, const FastVector(&relinearizeThreshold)) - { - for(const VectorValues::KeyValuePair& key_delta: delta) { + 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); + 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()) + } 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); } } @@ -133,81 +137,86 @@ KeySet ISAM2::Impl::CheckRelinearizationFull(const VectorValues& delta, } /* ************************************************************************* */ -void CheckRelinearizationRecursiveDouble(KeySet& relinKeys, double threshold, - const VectorValues& delta, const ISAM2Clique::shared_ptr& clique) -{ +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()) { + for (Key var : *clique->conditional()) { double maxDelta = delta[var].lpNorm(); - if(maxDelta >= threshold) { - relinKeys.insert(var); + if (maxDelta >= threshold) { + relinKeys->insert(var); relinearize = true; } } // If this node was relinearized, also check its children - if(relinearize) { - for(const ISAM2Clique::shared_ptr& child: clique->children) { - CheckRelinearizationRecursiveDouble(relinKeys, threshold, delta, child); + if (relinearize) { + for (const ISAM2::sharedClique& child : clique->children) { + CheckRelinearizationRecursiveDouble(threshold, delta, child, relinKeys); } } } /* ************************************************************************* */ -void CheckRelinearizationRecursiveMap(KeySet& relinKeys, const FastMap& thresholds, - const VectorValues& delta, - const ISAM2Clique::shared_ptr& clique) -{ +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()) { + 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."); + 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); + 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 ISAM2Clique::shared_ptr& child: clique->children) { - CheckRelinearizationRecursiveMap(relinKeys, thresholds, delta, child); + if (relinearize) { + for (const ISAM2::sharedClique& child : clique->children) { + CheckRelinearizationRecursiveMap(thresholds, delta, child, relinKeys); } } } /* ************************************************************************* */ -KeySet ISAM2::Impl::CheckRelinearizationPartial(const FastVector& roots, - const VectorValues& delta, - const ISAM2Params::RelinearizationThreshold& relinearizeThreshold) -{ +KeySet ISAM2::Impl::CheckRelinearizationPartial( + const FastVector& roots, const VectorValues& delta, + const ISAM2Params::RelinearizationThreshold& relinearizeThreshold) { KeySet relinKeys; - for(const ISAM2::sharedClique& root: roots) { - if(relinearizeThreshold.type() == typeid(double)) - CheckRelinearizationRecursiveDouble(relinKeys, boost::get(relinearizeThreshold), delta, root); - else if(relinearizeThreshold.type() == typeid(FastMap)) - CheckRelinearizationRecursiveMap(relinKeys, boost::get >(relinearizeThreshold), delta, root); + 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; } /* ************************************************************************* */ -void ISAM2::Impl::FindAll(ISAM2Clique::shared_ptr clique, KeySet& keys, const KeySet& markedMask) -{ +void ISAM2::Impl::FindAll(ISAM2::sharedClique clique, KeySet& keys, + const KeySet& markedMask) { static const bool debug = false; // does the separator contain any of the variables? bool found = false; - for(Key key: clique->conditional()->parents()) { + for (Key key : clique->conditional()->parents()) { if (markedMask.exists(key)) { found = true; break; @@ -215,19 +224,22 @@ void ISAM2::Impl::FindAll(ISAM2Clique::shared_ptr clique, KeySet& keys, const Ke } if (found) { // then add this clique - keys.insert(clique->conditional()->beginFrontals(), clique->conditional()->endFrontals()); - if(debug) clique->print("Key(s) marked in clique "); - if(debug) cout << "so marking key " << clique->conditional()->front() << endl; + keys.insert(clique->conditional()->beginFrontals(), + clique->conditional()->endFrontals()); + if (debug) clique->print("Key(s) marked in clique "); + if (debug) + cout << "so marking key " << clique->conditional()->front() << endl; } - for(const ISAM2Clique::shared_ptr& child: clique->children) { + for (const ISAM2::sharedClique& child : clique->children) { FindAll(child, keys, markedMask); } } /* ************************************************************************* */ -void ISAM2::Impl::ExpmapMasked(Values& values, const VectorValues& delta, - const KeySet& mask, boost::optional invalidateIfDebug, const KeyFormatter& keyFormatter) -{ +void ISAM2::Impl::ExpmapMasked(const VectorValues& delta, const KeySet& mask, + Values* values, + boost::optional invalidateIfDebug, + const KeyFormatter& keyFormatter) { // If debugging, invalidate if requested, otherwise do not invalidate. // Invalidating means setting expmapped entries to Inf, to trigger assertions // if we try to re-use them. @@ -235,65 +247,70 @@ void ISAM2::Impl::ExpmapMasked(Values& values, const VectorValues& delta, invalidateIfDebug = boost::none; #endif - assert(values.size() == delta.size()); + assert(values->size() == delta.size()); Values::iterator key_value; VectorValues::const_iterator key_delta; #ifdef GTSAM_USE_TBB - for(key_value = values.begin(); key_value != values.end(); ++key_value) - { + for (key_value = values->begin(); key_value != values->end(); ++key_value) { key_delta = delta.find(key_value->key); #else - for(key_value = values.begin(), key_delta = delta.begin(); key_value != values.end(); ++key_value, ++key_delta) - { + for (key_value = values->begin(), key_delta = delta.begin(); + key_value != values->end(); ++key_value, ++key_delta) { assert(key_value->key == key_delta->first); #endif Key var = key_value->key; - assert(delta[var].size() == (int)key_value->value.dim()); + assert(static_cast(delta[var].size()) == key_value->value.dim()); assert(delta[var].allFinite()); - if(mask.exists(var)) { + if (mask.exists(var)) { Value* retracted = key_value->value.retract_(delta[var]); key_value->value = *retracted; retracted->deallocate_(); - if(invalidateIfDebug) - (*invalidateIfDebug)[var].operator=(Vector::Constant(delta[var].rows(), numeric_limits::infinity())); // Strange syntax to work with clang++ (bug in clang?) + if (invalidateIfDebug) + (*invalidateIfDebug)[var].operator=(Vector::Constant( + delta[var].rows(), + numeric_limits::infinity())); // Strange syntax to work + // with clang++ (bug in + // clang?) } } } /* ************************************************************************* */ namespace internal { -inline static void optimizeInPlace(const boost::shared_ptr& clique, VectorValues& result) { +inline static void optimizeInPlace(const ISAM2::sharedClique& clique, + VectorValues* result) { // parents are assumed to already be solved and available in result - result.update(clique->conditional()->solve(result)); + result->update(clique->conditional()->solve(*result)); // starting from the root, call optimize on each conditional - for(const boost::shared_ptr& child: clique->children) + for (const ISAM2::sharedClique& child : clique->children) optimizeInPlace(child, result); } -} +} // namespace internal /* ************************************************************************* */ -size_t ISAM2::Impl::UpdateGaussNewtonDelta(const FastVector& roots, - const KeySet& replacedKeys, VectorValues& delta, double wildfireThreshold) { - +size_t ISAM2::Impl::UpdateGaussNewtonDelta( + const FastVector& roots, const KeySet& replacedKeys, + double wildfireThreshold, VectorValues* delta) { size_t lastBacksubVariableCount; if (wildfireThreshold <= 0.0) { // Threshold is zero or less, so do a full recalculation - for(const ISAM2::sharedClique& root: roots) + for (const ISAM2::sharedClique& root : roots) internal::optimizeInPlace(root, delta); - lastBacksubVariableCount = delta.size(); + lastBacksubVariableCount = delta->size(); } else { // Optimize with wildfire lastBacksubVariableCount = 0; - for(const ISAM2::sharedClique& root: roots) - lastBacksubVariableCount += optimizeWildfireNonRecursive( - root, wildfireThreshold, replacedKeys, delta); // modifies delta + for (const ISAM2::sharedClique& root : roots) + lastBacksubVariableCount += optimizeWildfire( + root, wildfireThreshold, replacedKeys, delta); // modifies delta #if !defined(NDEBUG) && defined(GTSAM_EXTRA_CONSISTENCY_CHECKS) - for (VectorValues::const_iterator key_delta = delta.begin(); key_delta != delta.end(); ++key_delta) { - assert(delta[key_delta->first].allFinite()); + for (VectorValues::const_iterator key_delta = delta->begin(); + key_delta != delta->end(); ++key_delta) { + assert((*delta)[key_delta->first].allFinite()); } #endif } @@ -303,69 +320,77 @@ size_t ISAM2::Impl::UpdateGaussNewtonDelta(const FastVector /* ************************************************************************* */ namespace internal { -void updateRgProd(const boost::shared_ptr& clique, const KeySet& replacedKeys, - const VectorValues& grad, VectorValues& RgProd, size_t& varsUpdated) { - +void updateRgProd(const ISAM2::sharedClique& clique, const KeySet& replacedKeys, + const VectorValues& grad, VectorValues* RgProd, + size_t* varsUpdated) { // Check if any frontal or separator keys were recalculated, if so, we need // update deltas and recurse to children, but if not, we do not need to // recurse further because of the running separator property. bool anyReplaced = false; - for(Key j: *clique->conditional()) { - if(replacedKeys.exists(j)) { + for (Key j : *clique->conditional()) { + if (replacedKeys.exists(j)) { anyReplaced = true; break; } } - if(anyReplaced) { + if (anyReplaced) { // Update the current variable // Get VectorValues slice corresponding to current variables - Vector gR = grad.vector(FastVector(clique->conditional()->beginFrontals(), clique->conditional()->endFrontals())); - Vector gS = grad.vector(FastVector(clique->conditional()->beginParents(), clique->conditional()->endParents())); + Vector gR = + grad.vector(FastVector(clique->conditional()->beginFrontals(), + clique->conditional()->endFrontals())); + Vector gS = + grad.vector(FastVector(clique->conditional()->beginParents(), + clique->conditional()->endParents())); // Compute R*g and S*g for this clique - Vector RSgProd = clique->conditional()->get_R() * gR + clique->conditional()->get_S() * gS; + Vector RSgProd = clique->conditional()->get_R() * gR + + clique->conditional()->get_S() * gS; // Write into RgProd vector DenseIndex vectorPosition = 0; - for(Key frontal: clique->conditional()->frontals()) { - Vector& RgProdValue = RgProd[frontal]; + for (Key frontal : clique->conditional()->frontals()) { + Vector& RgProdValue = (*RgProd)[frontal]; RgProdValue = RSgProd.segment(vectorPosition, RgProdValue.size()); vectorPosition += RgProdValue.size(); } - // Now solve the part of the Newton's method point for this clique (back-substitution) - //(*clique)->solveInPlace(deltaNewton); + // Now solve the part of the Newton's method point for this clique + // (back-substitution) + // (*clique)->solveInPlace(deltaNewton); - varsUpdated += clique->conditional()->nrFrontals(); + *varsUpdated += clique->conditional()->nrFrontals(); // Recurse to children - for(const ISAM2Clique::shared_ptr& child: clique->children) { - updateRgProd(child, replacedKeys, grad, RgProd, varsUpdated); } + for (const ISAM2::sharedClique& child : clique->children) { + updateRgProd(child, replacedKeys, grad, RgProd, varsUpdated); + } } } -} +} // namespace internal /* ************************************************************************* */ -size_t ISAM2::Impl::UpdateRgProd(const ISAM2::Roots& roots, const KeySet& replacedKeys, - const VectorValues& gradAtZero, VectorValues& RgProd) { - +size_t ISAM2::Impl::UpdateRgProd(const ISAM2::Roots& roots, + const KeySet& replacedKeys, + const VectorValues& gradAtZero, + VectorValues* RgProd) { // Update variables size_t varsUpdated = 0; - for(const ISAM2::sharedClique& root: roots) { - internal::updateRgProd(root, replacedKeys, gradAtZero, RgProd, varsUpdated); + for (const ISAM2::sharedClique& root : roots) { + internal::updateRgProd(root, replacedKeys, gradAtZero, RgProd, + &varsUpdated); } return varsUpdated; } - + /* ************************************************************************* */ VectorValues ISAM2::Impl::ComputeGradientSearch(const VectorValues& gradAtZero, - const VectorValues& RgProd) -{ + const VectorValues& RgProd) { // Compute gradient squared-magnitude const double gradientSqNorm = gradAtZero.dot(gradAtZero); - + // Compute minimizing step size double RgNormSq = RgProd.vector().squaredNorm(); double step = -gradientSqNorm / RgNormSq; @@ -374,4 +399,4 @@ VectorValues ISAM2::Impl::ComputeGradientSearch(const VectorValues& gradAtZero, return step * gradAtZero; } -} +} // namespace gtsam diff --git a/gtsam/nonlinear/ISAM2-impl.h b/gtsam/nonlinear/ISAM2-impl.h index 161932344..0e05ab453 100644 --- a/gtsam/nonlinear/ISAM2-impl.h +++ b/gtsam/nonlinear/ISAM2-impl.h @@ -108,18 +108,17 @@ struct GTSAM_EXPORT ISAM2::Impl { /** * Apply expmap to the given values, but only for indices appearing in * \c markedRelinMask. Values are expmapped in-place. - * \param [in, out] values The value to expmap in-place * \param delta The linear delta with which to expmap - * \param ordering The ordering * \param mask Mask on linear indices, only \c true entries are expmapped + * \param [in, out] values The value to expmap in-place * \param invalidateIfDebug If this is true, *and* NDEBUG is not defined, * expmapped deltas will be set to an invalid value (infinity) to catch bugs * where we might expmap something twice, or expmap it but then not * recalculate its delta. * @param keyFormatter Formatter for printing nonlinear keys during debugging */ - static void ExpmapMasked(Values& values, const VectorValues& delta, - const KeySet& mask, + static void ExpmapMasked( + const VectorValues& delta, const KeySet& mask, Values* values, boost::optional invalidateIfDebug = boost::none, const KeyFormatter& keyFormatter = DefaultKeyFormatter); @@ -127,14 +126,14 @@ struct GTSAM_EXPORT ISAM2::Impl { * Update the Newton's method step point, using wildfire */ static size_t UpdateGaussNewtonDelta(const FastVector& roots, - const KeySet& replacedKeys, VectorValues& delta, double wildfireThreshold); + 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); + const VectorValues& gradAtZero, VectorValues* RgProd); /** * Compute the gradient-search point. Only used in Dogleg. diff --git a/gtsam/nonlinear/ISAM2-inl.h b/gtsam/nonlinear/ISAM2-inl.h index fecefd2a5..fb63dc428 100644 --- a/gtsam/nonlinear/ISAM2-inl.h +++ b/gtsam/nonlinear/ISAM2-inl.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) @@ -11,301 +11,24 @@ /** * @file ISAM2-inl.h - * @brief + * @brief * @author Richard Roberts * @date Mar 16, 2012 */ - #pragma once -#include -#include -#include +#include namespace gtsam { /* ************************************************************************* */ -template +template VALUE ISAM2::calculateEstimate(Key key) const { const Vector& delta = getDelta()[key]; return traits::Retract(theta_.at(key), delta); } /* ************************************************************************* */ -namespace internal { -template -void optimizeWildfire(const boost::shared_ptr& clique, double threshold, - KeySet& changed, const KeySet& replaced, VectorValues& delta, size_t& count) -{ - // if none of the variables in this clique (frontal and separator!) changed - // significantly, then by the running intersection property, none of the - // cliques in the children need to be processed - - // Are any clique variables part of the tree that has been redone? - bool cliqueReplaced = replaced.exists((*clique)->frontals().front()); -#if !defined(NDEBUG) && defined(GTSAM_EXTRA_CONSISTENCY_CHECKS) - for(Key frontal: clique->conditional()->frontals()) { - assert(cliqueReplaced == replaced.exists(frontal)); - } -#endif - - // If not redone, then has one of the separator variables changed significantly? - bool recalculate = cliqueReplaced; - if(!recalculate) { - for(Key parent: clique->conditional()->parents()) { - if(changed.exists(parent)) { - recalculate = true; - break; - } - } - } - - // Solve clique if it was replaced, or if any parents were changed above the - // threshold or themselves replaced. - if(recalculate) { - - // Temporary copy of the original values, to check how much they change - FastVector originalValues(clique->conditional()->nrFrontals()); - GaussianConditional::const_iterator it; - for(it = clique->conditional()->beginFrontals(); it!=clique->conditional()->endFrontals(); it++) { - originalValues[it - clique->conditional()->beginFrontals()] = delta[*it]; - } - - // Back-substitute - delta.update(clique->conditional()->solve(delta)); - count += clique->conditional()->nrFrontals(); - - // Whether the values changed above a threshold, or always true if the - // clique was replaced. - bool valuesChanged = cliqueReplaced; - for(it = clique->conditional()->beginFrontals(); it != clique->conditional()->endFrontals(); it++) { - if(!valuesChanged) { - const Vector& oldValue(originalValues[it - clique->conditional()->beginFrontals()]); - const Vector& newValue(delta[*it]); - if((oldValue - newValue).lpNorm() >= threshold) { - valuesChanged = true; - break; - } - } else - break; - } - - // If the values were above the threshold or this clique was replaced - if(valuesChanged) { - // Set changed flag for each frontal variable and leave the new values - for(Key frontal: clique->conditional()->frontals()) { - changed.insert(frontal); - } - } else { - // Replace with the old values - for(it = clique->conditional()->beginFrontals(); it != clique->conditional()->endFrontals(); it++) { - delta[*it] = originalValues[it - clique->conditional()->beginFrontals()]; - } - } - - // Recurse to children - for(const typename CLIQUE::shared_ptr& child: clique->children) { - optimizeWildfire(child, threshold, changed, replaced, delta, count); - } - } -} - -template -bool optimizeWildfireNode(const boost::shared_ptr& clique, double threshold, - KeySet& changed, const KeySet& replaced, VectorValues& delta, size_t& count) -{ - // if none of the variables in this clique (frontal and separator!) changed - // significantly, then by the running intersection property, none of the - // cliques in the children need to be processed - - // Are any clique variables part of the tree that has been redone? - bool cliqueReplaced = replaced.exists(clique->conditional()->frontals().front()); -#if !defined(NDEBUG) && defined(GTSAM_EXTRA_CONSISTENCY_CHECKS) - for(Key frontal: clique->conditional()->frontals()) { - assert(cliqueReplaced == replaced.exists(frontal)); - } -#endif - - // If not redone, then has one of the separator variables changed significantly? - bool recalculate = cliqueReplaced; - if(!recalculate) { - for(Key parent: clique->conditional()->parents()) { - if(changed.exists(parent)) { - recalculate = true; - break; - } - } - } - - // Solve clique if it was replaced, or if any parents were changed above the - // threshold or themselves replaced. - // TODO(gareth): This code shares a lot of logic w/ linearAlgorithms-inst, potentially refactor - if(recalculate) - { - // Temporary copy of the original values, to check how much they change - FastVector originalValues(clique->conditional()->nrFrontals()); - GaussianConditional::const_iterator it; - for(it = clique->conditional()->beginFrontals(); it != clique->conditional()->endFrontals(); it++) { - originalValues[it - clique->conditional()->beginFrontals()] = delta[*it]; - } - - // Back-substitute - special version stores solution pointers in cliques for fast access. - { - // Create solution part pointers if necessary and possible - necessary if solnPointers_ is - // empty, and possible if either we're a root, or we have a parent with valid solnPointers_. - boost::shared_ptr parent = clique->parent_.lock(); - if(clique->solnPointers_.empty() && (clique->isRoot() || !parent->solnPointers_.empty())) - { - for(Key key: clique->conditional()->frontals()) - clique->solnPointers_.insert(std::make_pair(key, delta.find(key))); - for(Key key: clique->conditional()->parents()) - clique->solnPointers_.insert(std::make_pair(key, parent->solnPointers_.at(key))); - } - - // See if we can use solution part pointers - we can if they either already existed or were - // created above. - if(!clique->solnPointers_.empty()) - { - GaussianConditional& c = *clique->conditional(); - // Solve matrix - Vector xS; - { - // Count dimensions of vector - DenseIndex dim = 0; - FastVector parentPointers; - parentPointers.reserve(clique->conditional()->nrParents()); - for(Key parent: clique->conditional()->parents()) { - parentPointers.push_back(clique->solnPointers_.at(parent)); - dim += parentPointers.back()->second.size(); - } - - // Fill parent vector - xS.resize(dim); - DenseIndex vectorPos = 0; - for(const VectorValues::const_iterator& parentPointer: parentPointers) { - const Vector& parentVector = parentPointer->second; - xS.block(vectorPos,0,parentVector.size(),1) = parentVector.block(0,0,parentVector.size(),1); - vectorPos += parentVector.size(); - } - } - - // NOTE(gareth): We can no longer write: xS = b - S * xS - // This is because Eigen (as of 3.3) no longer evaluates S * xS into - // a temporary, and the operation trashes valus in xS. - // See: http://eigen.tuxfamily.org/index.php?title=3.3 - const Vector rhs = c.getb() - c.get_S() * xS; - const Vector solution = c.get_R().triangularView().solve(rhs); - - // Check for indeterminant solution - if(solution.hasNaN()) throw IndeterminantLinearSystemException(c.keys().front()); - - // Insert solution into a VectorValues - DenseIndex vectorPosition = 0; - for(GaussianConditional::const_iterator frontal = c.beginFrontals(); frontal != c.endFrontals(); ++frontal) { - clique->solnPointers_.at(*frontal)->second = solution.segment(vectorPosition, c.getDim(frontal)); - vectorPosition += c.getDim(frontal); - } - } - else - { - // Just call plain solve because we couldn't use solution pointers. - delta.update(clique->conditional()->solve(delta)); - } - } - count += clique->conditional()->nrFrontals(); - - // Whether the values changed above a threshold, or always true if the - // clique was replaced. - bool valuesChanged = cliqueReplaced; - for(it = clique->conditional()->beginFrontals(); it != clique->conditional()->endFrontals(); it++) { - if(!valuesChanged) { - const Vector& oldValue(originalValues[it - clique->conditional()->beginFrontals()]); - const Vector& newValue(delta[*it]); - if((oldValue - newValue).lpNorm() >= threshold) { - valuesChanged = true; - break; - } - } else - break; - } - - // If the values were above the threshold or this clique was replaced - if(valuesChanged) { - // Set changed flag for each frontal variable and leave the new values - for(Key frontal: clique->conditional()->frontals()) { - changed.insert(frontal); - } - } else { - // Replace with the old values - for(it = clique->conditional()->beginFrontals(); it != clique->conditional()->endFrontals(); it++) { - delta[*it] = originalValues[it - clique->conditional()->beginFrontals()]; - } - } - } - - return recalculate; -} - -} // namespace internal - -/* ************************************************************************* */ -template -size_t optimizeWildfire(const boost::shared_ptr& root, double threshold, const KeySet& keys, VectorValues& delta) { - KeySet changed; - int count = 0; - // starting from the root, call optimize on each conditional - if(root) - internal::optimizeWildfire(root, threshold, changed, keys, delta, count); - return count; -} - -/* ************************************************************************* */ -template -size_t optimizeWildfireNonRecursive(const boost::shared_ptr& root, double threshold, const KeySet& keys, VectorValues& delta) -{ - KeySet changed; - size_t count = 0; - - if (root) { - std::stack > travStack; - travStack.push(root); - boost::shared_ptr currentNode = root; - while (!travStack.empty()) { - currentNode = travStack.top(); - travStack.pop(); - bool recalculate = internal::optimizeWildfireNode(currentNode, threshold, changed, keys, delta, count); - if (recalculate) { - for(const typename CLIQUE::shared_ptr& child: currentNode->children) { - travStack.push(child); - } - } - } - } - - return count; -} - -/* ************************************************************************* */ -template -void nnz_internal(const boost::shared_ptr& clique, int& result) { - int dimR = (int)clique->conditional()->rows(); - int dimSep = (int)clique->conditional()->get_S().cols(); - result += ((dimR+1)*dimR)/2 + dimSep*dimR; - // traverse the children - for(const typename CLIQUE::shared_ptr& child: clique->children) { - nnz_internal(child, result); - } -} - -/* ************************************************************************* */ -template -int calculate_nnz(const boost::shared_ptr& clique) { - int result = 0; - // starting from the root, add up entries of frontal and conditional matrices of each conditional - nnz_internal(clique, result); - return result; -} - -} +} // namespace gtsam diff --git a/gtsam/nonlinear/ISAM2.cpp b/gtsam/nonlinear/ISAM2.cpp index d4df01a68..bdd6b42bb 100644 --- a/gtsam/nonlinear/ISAM2.cpp +++ b/gtsam/nonlinear/ISAM2.cpp @@ -11,31 +11,41 @@ /** * @file ISAM2-inl.h - * @brief Incremental update functionality (ISAM2) for BayesTree, with fluid relinearization. + * @brief Incremental update functionality (ISAM2) for BayesTree, with fluid + * relinearization. * @author Michael Kaess, Richard Roberts */ -#include // for operator += +#include + +#include +#include +#include +#include + +#include // for operator += using namespace boost::assign; +#include #include #include -#include -namespace br { using namespace boost::range; using namespace boost::adaptors; } +namespace br { +using namespace boost::range; +using namespace boost::adaptors; +} // namespace br -#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 // We need the inst file because we'll make a special JT templated on ISAM2 #include +#include +#include +#include -#include #include -#include #include +#include using namespace std; @@ -49,11 +59,10 @@ static const bool disableReordering = false; static const double batchThreshold = 0.65; /* ************************************************************************* */ -// Special BayesTree class that uses ISAM2 cliques - this is the result of reeliminating ISAM2 -// subtrees. -class ISAM2BayesTree : public ISAM2::Base -{ -public: +// 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; @@ -62,43 +71,58 @@ public: }; /* ************************************************************************* */ -// Special JunctionTree class that produces ISAM2 BayesTree cliques, used for reeliminating ISAM2 -// subtrees. -class ISAM2JunctionTree : public JunctionTree -{ -public: +// 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; - ISAM2JunctionTree(const GaussianEliminationTree& eliminationTree) : - Base(eliminationTree) {} + ISAM2JunctionTree(const GaussianEliminationTree& eliminationTree) + : Base(eliminationTree) {} }; /* ************************************************************************* */ -std::string ISAM2DoglegParams::adaptationModeTranslator(const DoglegOptimizerImpl::TrustRegionAdaptationMode& adaptationMode) const { +std::string ISAM2DoglegParams::adaptationModeTranslator( + const DoglegOptimizerImpl::TrustRegionAdaptationMode& adaptationMode) + const { std::string s; switch (adaptationMode) { - case DoglegOptimizerImpl::SEARCH_EACH_ITERATION: s = "SEARCH_EACH_ITERATION"; break; - case DoglegOptimizerImpl::ONE_STEP_PER_ITERATION: s = "ONE_STEP_PER_ITERATION"; break; - default: s = "UNDEFINED"; break; + case DoglegOptimizerImpl::SEARCH_EACH_ITERATION: + s = "SEARCH_EACH_ITERATION"; + break; + case DoglegOptimizerImpl::ONE_STEP_PER_ITERATION: + s = "ONE_STEP_PER_ITERATION"; + break; + default: + s = "UNDEFINED"; + break; } return s; } /* ************************************************************************* */ -DoglegOptimizerImpl::TrustRegionAdaptationMode ISAM2DoglegParams::adaptationModeTranslator(const std::string& adaptationMode) const { - std::string s = adaptationMode; boost::algorithm::to_upper(s); - if (s == "SEARCH_EACH_ITERATION") return DoglegOptimizerImpl::SEARCH_EACH_ITERATION; - if (s == "ONE_STEP_PER_ITERATION") return DoglegOptimizerImpl::ONE_STEP_PER_ITERATION; +DoglegOptimizerImpl::TrustRegionAdaptationMode +ISAM2DoglegParams::adaptationModeTranslator( + const std::string& adaptationMode) const { + std::string s = adaptationMode; + boost::algorithm::to_upper(s); + if (s == "SEARCH_EACH_ITERATION") + return DoglegOptimizerImpl::SEARCH_EACH_ITERATION; + if (s == "ONE_STEP_PER_ITERATION") + return DoglegOptimizerImpl::ONE_STEP_PER_ITERATION; /* default is SEARCH_EACH_ITERATION */ return DoglegOptimizerImpl::SEARCH_EACH_ITERATION; } /* ************************************************************************* */ -ISAM2Params::Factorization ISAM2Params::factorizationTranslator(const std::string& str) { - std::string s = str; boost::algorithm::to_upper(s); +ISAM2Params::Factorization ISAM2Params::factorizationTranslator( + const std::string& str) { + std::string s = str; + boost::algorithm::to_upper(s); if (s == "QR") return ISAM2Params::QR; if (s == "CHOLESKY") return ISAM2Params::CHOLESKY; @@ -107,84 +131,347 @@ ISAM2Params::Factorization ISAM2Params::factorizationTranslator(const std::strin } /* ************************************************************************* */ -std::string ISAM2Params::factorizationTranslator(const ISAM2Params::Factorization& value) { +std::string ISAM2Params::factorizationTranslator( + const ISAM2Params::Factorization& value) { std::string s; switch (value) { - case ISAM2Params::QR: s = "QR"; break; - case ISAM2Params::CHOLESKY: s = "CHOLESKY"; break; - default: s = "UNDEFINED"; break; + case ISAM2Params::QR: + s = "QR"; + break; + case ISAM2Params::CHOLESKY: + s = "CHOLESKY"; + break; + default: + s = "UNDEFINED"; + break; } return s; } /* ************************************************************************* */ -void ISAM2Clique::setEliminationResult(const FactorGraphType::EliminationResult& eliminationResult) -{ +void ISAM2Clique::setEliminationResult( + const FactorGraphType::EliminationResult& eliminationResult) { conditional_ = eliminationResult.first; cachedFactor_ = eliminationResult.second; // Compute gradient contribution gradientContribution_.resize(conditional_->cols() - 1); - // Rewrite -(R * P')'*d as -(d' * R * P')' for computational speed reasons - gradientContribution_ << -conditional_->get_R().transpose() * conditional_->get_d(), - -conditional_->get_S().transpose() * conditional_->get_d(); + // Rewrite -(R * P')'*d as -(d' * R * P')' for computational speed + // reasons + gradientContribution_ << -conditional_->get_R().transpose() * + conditional_->get_d(), + -conditional_->get_S().transpose() * conditional_->get_d(); } /* ************************************************************************* */ bool ISAM2Clique::equals(const This& other, double tol) const { return Base::equals(other) && - ((!cachedFactor_ && !other.cachedFactor_) - || (cachedFactor_ && other.cachedFactor_ - && cachedFactor_->equals(*other.cachedFactor_, tol))); + ((!cachedFactor_ && !other.cachedFactor_) || + (cachedFactor_ && other.cachedFactor_ && + cachedFactor_->equals(*other.cachedFactor_, tol))); } /* ************************************************************************* */ -void ISAM2Clique::print(const std::string& s, const KeyFormatter& formatter) const -{ - Base::print(s,formatter); - if(cachedFactor_) +void ISAM2Clique::print(const std::string& s, + const KeyFormatter& formatter) const { + Base::print(s, formatter); + if (cachedFactor_) cachedFactor_->print(s + "Cached: ", formatter); else std::cout << s << "Cached empty" << std::endl; - if(gradientContribution_.rows() != 0) + if (gradientContribution_.rows() != 0) gtsam::print(gradientContribution_, "Gradient contribution: "); } /* ************************************************************************* */ -ISAM2::ISAM2(const ISAM2Params& params): params_(params), update_count_(0) { - if(params_.optimizationParams.type() == typeid(ISAM2DoglegParams)) - doglegDelta_ = boost::get(params_.optimizationParams).initialDelta; +bool ISAM2Clique::isDirty(const KeySet& replaced, const KeySet& changed) const { + // if none of the variables in this clique (frontal and separator!) changed + // significantly, then by the running intersection property, none of the + // cliques in the children need to be processed + + // Are any clique variables part of the tree that has been redone? + bool dirty = replaced.exists(conditional_->frontals().front()); +#if !defined(NDEBUG) && defined(GTSAM_EXTRA_CONSISTENCY_CHECKS) + for (Key frontal : conditional_->frontals()) { + assert(dirty == replaced.exists(frontal)); + } +#endif + + // If not, then has one of the separator variables changed significantly? + if (!dirty) { + for (Key parent : conditional_->parents()) { + if (changed.exists(parent)) { + dirty = true; + break; + } + } + } + return dirty; +} +/* ************************************************************************* */ +FastVector ISAM2Clique::copyRelevantValues( + const VectorValues& delta) const { + FastVector values; + values.reserve(conditional_->nrFrontals()); + for (Key frontal : conditional_->frontals()) { + values.push_back(delta[frontal]); + } + return values; +} + +/* ************************************************************************* */ +bool ISAM2Clique::valuesChanged(const KeySet& replaced, + const FastVector& originalValues, + const VectorValues& delta, + double threshold) const { + if (replaced.exists(conditional_->frontals().front())) return true; + size_t i = 0; + for (Key frontal : conditional_->frontals()) { + const Vector diff = originalValues[i++] - delta[frontal]; + if (diff.lpNorm() >= threshold) { + return true; + } + } + return false; +} + +/* ************************************************************************* */ +/// Set changed flag for each frontal variable +void ISAM2Clique::markFrontalsAsChanged(KeySet* changed) const { + for (Key frontal : conditional_->frontals()) { + changed->insert(frontal); + } +} + +/* ************************************************************************* */ +void ISAM2Clique::restoreFromOriginals(const FastVector& originalValues, + VectorValues* delta) const { + size_t i = 0; + for (Key frontal : conditional_->frontals()) { + delta->at(frontal) = originalValues[i++]; + } +} + +/* ************************************************************************* */ +void ISAM2Clique::optimizeWildfire(const KeySet& replaced, double threshold, + KeySet* changed, VectorValues* delta, + size_t* count) const { + if (isDirty(replaced, *changed)) { + // Temporary copy of the original values, to check how much they change + auto originalValues = copyRelevantValues(*delta); + + // Back-substitute + delta->update(conditional_->solve(*delta)); + count += conditional_->nrFrontals(); + + if (valuesChanged(replaced, originalValues, *delta, threshold)) { + markFrontalsAsChanged(changed); + } else { + restoreFromOriginals(originalValues, delta); + } + + // Recurse to children + for (const auto& child : children) { + child->optimizeWildfire(replaced, threshold, changed, delta, count); + } + } +} + +/* ************************************************************************* */ +bool ISAM2Clique::optimizeWildfireNode(const KeySet& replaced, double threshold, + KeySet* changed, VectorValues* delta, + size_t* count) const { + // TODO(gareth): This code shares a lot of logic w/ linearAlgorithms-inst, + // potentially refactor + bool dirty = isDirty(replaced, *changed); + if (dirty) { + // Temporary copy of the original values, to check how much they change + auto originalValues = copyRelevantValues(*delta); + + // Back-substitute - special version stores solution pointers in cliques for + // fast access. + { + // Create solution part pointers if necessary and possible - necessary if + // solnPointers_ is empty, and possible if either we're a root, or we have + // a parent with valid solnPointers_. + ISAM2::sharedClique parent = parent_.lock(); + if (solnPointers_.empty() && + (isRoot() || !parent->solnPointers_.empty())) { + for (Key frontal : conditional_->frontals()) + solnPointers_.emplace(frontal, delta->find(frontal)); + for (Key parentKey : conditional_->parents()) { + assert(parent->solnPointers_.exists(parentKey)); + solnPointers_.emplace(parentKey, parent->solnPointers_.at(parentKey)); + } + } + + // See if we can use solution part pointers - we can if they either + // already existed or were created above. + if (!solnPointers_.empty()) { + GaussianConditional& c = *conditional_; + // Solve matrix + Vector xS; + { + // Count dimensions of vector + DenseIndex dim = 0; + FastVector parentPointers; + parentPointers.reserve(conditional_->nrParents()); + for (Key parent : conditional_->parents()) { + parentPointers.push_back(solnPointers_.at(parent)); + dim += parentPointers.back()->second.size(); + } + + // Fill parent vector + xS.resize(dim); + DenseIndex vectorPos = 0; + for (const VectorValues::const_iterator& parentPointer : + parentPointers) { + const Vector& parentVector = parentPointer->second; + xS.block(vectorPos, 0, parentVector.size(), 1) = + parentVector.block(0, 0, parentVector.size(), 1); + vectorPos += parentVector.size(); + } + } + + // NOTE(gareth): We can no longer write: xS = b - S * xS + // This is because Eigen (as of 3.3) no longer evaluates S * xS into + // a temporary, and the operation trashes valus in xS. + // See: http://eigen.tuxfamily.org/index.php?title=3.3 + const Vector rhs = c.getb() - c.get_S() * xS; + const Vector solution = + c.get_R().triangularView().solve(rhs); + + // Check for indeterminant solution + if (solution.hasNaN()) + throw IndeterminantLinearSystemException(c.keys().front()); + + // Insert solution into a VectorValues + DenseIndex vectorPosition = 0; + for (GaussianConditional::const_iterator frontal = c.beginFrontals(); + frontal != c.endFrontals(); ++frontal) { + solnPointers_.at(*frontal)->second = + solution.segment(vectorPosition, c.getDim(frontal)); + vectorPosition += c.getDim(frontal); + } + } else { + // Just call plain solve because we couldn't use solution pointers. + delta->update(conditional_->solve(*delta)); + } + } + count += conditional_->nrFrontals(); + + if (valuesChanged(replaced, originalValues, *delta, threshold)) { + markFrontalsAsChanged(changed); + } else { + restoreFromOriginals(originalValues, delta); + } + } + + return dirty; +} + +/* ************************************************************************* */ +void ISAM2Clique::nnz_internal(size_t* result) const { + size_t dimR = conditional_->rows(); + size_t dimSep = conditional_->get_S().cols(); + *result += ((dimR + 1) * dimR) / 2 + dimSep * dimR; + // traverse the children + for (const auto& child : children) { + child->nnz_internal(result); + } +} + +/* ************************************************************************* */ +size_t ISAM2Clique::calculate_nnz() const { + size_t result = 0; + nnz_internal(&result); + return result; +} + +/* ************************************************************************* */ +size_t optimizeWildfire(const ISAM2::sharedClique& root, double threshold, + const KeySet& keys, VectorValues* delta) { + KeySet changed; + size_t count = 0; + // starting from the root, call optimize on each conditional + if (root) root->optimizeWildfire(keys, threshold, &changed, delta, &count); + return count; +} + +/* ************************************************************************* */ +// This version is non-recursive version, but seems to have +// a bug, as was diagnosed with ISAM2Example_SmartFactor. Disabled for now. +size_t optimizeWildfireNonRecursive(const ISAM2::sharedClique& root, + double threshold, const KeySet& keys, + VectorValues* delta) { + KeySet changed; + size_t count = 0; + + if (root) { + std::stack travStack; + travStack.push(root); + ISAM2::sharedClique currentNode = root; + while (!travStack.empty()) { + currentNode = travStack.top(); + travStack.pop(); + bool dirty = currentNode->optimizeWildfireNode(keys, threshold, &changed, + delta, &count); + if (dirty) { + for (const auto& child : currentNode->children) { + travStack.push(child); + } + } + } + } + + return count; +} + +/* ************************************************************************* */ +ISAM2::ISAM2(const ISAM2Params& params) : params_(params), update_count_(0) { + if (params_.optimizationParams.type() == typeid(ISAM2DoglegParams)) + doglegDelta_ = + boost::get(params_.optimizationParams).initialDelta; } /* ************************************************************************* */ ISAM2::ISAM2() : update_count_(0) { - if(params_.optimizationParams.type() == typeid(ISAM2DoglegParams)) - doglegDelta_ = boost::get(params_.optimizationParams).initialDelta; + if (params_.optimizationParams.type() == typeid(ISAM2DoglegParams)) + doglegDelta_ = + boost::get(params_.optimizationParams).initialDelta; } /* ************************************************************************* */ bool ISAM2::equals(const ISAM2& other, double tol) const { - return Base::equals(other, tol) - && theta_.equals(other.theta_, tol) && variableIndex_.equals(other.variableIndex_, tol) - && nonlinearFactors_.equals(other.nonlinearFactors_, tol) - && fixedVariables_ == other.fixedVariables_; + return Base::equals(other, tol) && theta_.equals(other.theta_, tol) && + variableIndex_.equals(other.variableIndex_, tol) && + nonlinearFactors_.equals(other.nonlinearFactors_, tol) && + fixedVariables_ == other.fixedVariables_; } /* ************************************************************************* */ KeySet 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; + if (debug) cout << "Getting affected factors for "; + if (debug) { + for (const Key key : keys) { + cout << key << " "; + } + } + if (debug) cout << endl; NonlinearFactorGraph allAffected; KeySet indices; - for(const Key key: keys) { + 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 size_t index: indices) { cout << index << " "; } } - if(debug) cout << endl; + if (debug) cout << "Affected factors are: "; + if (debug) { + for (const size_t index : indices) { + cout << index << " "; + } + } + if (debug) cout << endl; return indices; } @@ -192,9 +479,8 @@ KeySet ISAM2::getAffectedFactors(const KeyList& keys) const { // 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 -{ +GaussianFactorGraph::shared_ptr ISAM2::relinearizeAffectedFactors( + const FastList& affectedKeys, const KeySet& relinKeys) const { gttic(getAffectedFactors); KeySet candidates = getAffectedFactors(affectedKeys); gttoc(getAffectedFactors); @@ -208,29 +494,31 @@ ISAM2::relinearizeAffectedFactors(const FastList& affectedKeys, const KeySe gttoc(affectedKeysSet); gttic(check_candidates_and_linearize); - GaussianFactorGraph::shared_ptr linearized = boost::make_shared(); - for(Key idx: candidates) { + GaussianFactorGraph::shared_ptr linearized = + boost::make_shared(); + for (Key idx : candidates) { bool inside = true; bool useCachedLinear = params_.cacheLinearizedFactors; - for(Key key: nonlinearFactors_[idx]->keys()) { - if(affectedKeysSet.find(key) == affectedKeysSet.end()) { + for (Key key : nonlinearFactors_[idx]->keys()) { + if (affectedKeysSet.find(key) == affectedKeysSet.end()) { inside = false; break; } - if(useCachedLinear && relinKeys.find(key) != relinKeys.end()) + if (useCachedLinear && relinKeys.find(key) != relinKeys.end()) useCachedLinear = false; } - if(inside) { - if(useCachedLinear) { + 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 { - GaussianFactor::shared_ptr linearFactor = nonlinearFactors_[idx]->linearize(theta_); + GaussianFactor::shared_ptr linearFactor = + nonlinearFactors_[idx]->linearize(theta_); linearized->push_back(linearFactor); - if(params_.cacheLinearizedFactors) { + if (params_.cacheLinearizedFactors) { #ifdef GTSAM_EXTRA_CONSISTENCY_CHECKS assert(linearFactors_[idx]->keys() == linearFactor->keys()); #endif @@ -245,12 +533,13 @@ ISAM2::relinearizeAffectedFactors(const FastList& affectedKeys, const KeySe } /* ************************************************************************* */ -// find intermediate (linearized) factors from cache that are passed into the affected area +// find intermediate (linearized) factors from cache that are passed into the +// affected area GaussianFactorGraph ISAM2::getCachedBoundaryFactors(const Cliques& orphans) { GaussianFactorGraph cachedBoundary; - for(sharedClique orphan: orphans) { + for (sharedClique orphan : orphans) { // retrieve the cached factor and add to boundary cachedBoundary.push_back(orphan->cachedFactor()); } @@ -259,27 +548,27 @@ GaussianFactorGraph ISAM2::getCachedBoundaryFactors(const Cliques& orphans) { } /* ************************************************************************* */ -boost::shared_ptr ISAM2::recalculate(const KeySet& markedKeys, const KeySet& relinKeys, - const vector& observedKeys, - const KeySet& unusedIndices, - const boost::optional >& constrainKeys, - ISAM2Result& result) -{ - // TODO: new factors are linearized twice, the newFactors passed in are not used. +boost::shared_ptr ISAM2::recalculate( + const KeySet& markedKeys, const KeySet& relinKeys, + const vector& 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 -//#define PRINT_STATS // figures for paper, disable for timing +// 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(); + 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; @@ -289,81 +578,89 @@ boost::shared_ptr ISAM2::recalculate(const KeySet& markedKeys, const Ke counter++; #endif - if(debug) { + if (debug) { cout << "markedKeys: "; - for(const Key key: markedKeys) { cout << key << " "; } + for (const Key key : markedKeys) { + cout << key << " "; + } cout << endl; cout << "observedKeys: "; - for(const Key key: observedKeys) { cout << key << " "; } + 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. + // (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(FastVector(markedKeys.begin(), markedKeys.end()), affectedBayesNet, orphans); + this->removeTop(FastVector(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 + // 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 + // ordering provides all keys in conditionals, there cannot be others because + // path to root included gttic(affectedKeys); FastList affectedKeys; - for(const ConditionalType::shared_ptr& conditional: affectedBayesNet) - affectedKeys.insert(affectedKeys.end(), conditional->beginFrontals(), conditional->endFrontals()); + for (const ConditionalType::shared_ptr& conditional : affectedBayesNet) + affectedKeys.insert(affectedKeys.end(), conditional->beginFrontals(), + conditional->endFrontals()); gttoc(affectedKeys); - boost::shared_ptr affectedKeysSet(new KeySet()); // Will return this result + boost::shared_ptr affectedKeysSet( + new KeySet()); // Will return this result - if(affectedKeys.size() >= theta_.size() * batchThreshold) - { + if (affectedKeys.size() >= theta_.size() * batchThreshold) { // 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())); + br::copy(variableIndex_ | br::map_keys, + std::inserter(*affectedKeysSet, affectedKeysSet->end())); // Removed unused keys: VariableIndex affectedFactorsVarIndex = variableIndex_; - affectedFactorsVarIndex.removeUnusedVariables(unusedIndices.begin(), unusedIndices.end()); + affectedFactorsVarIndex.removeUnusedVariables(unusedIndices.begin(), + unusedIndices.end()); - for (const Key key: unusedIndices) - { + 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()) - { + 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 - { + for (Key var : observedKeys) constraintGroups[var] = 1; + order = Ordering::ColamdConstrained(affectedFactorsVarIndex, + constraintGroups); + } else { order = Ordering::Colamd(affectedFactorsVarIndex); } } @@ -371,18 +668,21 @@ boost::shared_ptr ISAM2::recalculate(const KeySet& markedKeys, const Ke gttic(linearize); GaussianFactorGraph linearized = *nonlinearFactors_.linearize(theta_); - if(params_.cacheLinearizedFactors) - linearFactors_ = linearized; + if (params_.cacheLinearizedFactors) linearFactors_ = linearized; gttoc(linearize); gttic(eliminate); - ISAM2BayesTree::shared_ptr bayesTree = ISAM2JunctionTree(GaussianEliminationTree(linearized, affectedFactorsVarIndex, order)) - .eliminate(params_.getEliminationFunction()).first; + 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->roots_.insert(this->roots_.end(), bayesTree->roots().begin(), + bayesTree->roots().end()); this->nodes_.insert(bayesTree->nodes().begin(), bayesTree->nodes().end()); gttoc(insert); @@ -394,8 +694,8 @@ boost::shared_ptr ISAM2::recalculate(const KeySet& markedKeys, const Ke lastAffectedFactorCount = linearized.size(); // Reeliminated keys for detailed results - if(params_.enableDetailedResults) { - for(Key key: theta_.keys()) { + if (params_.enableDetailedResults) { + for (Key key : theta_.keys()) { result.detail->variableStatus[key].isReeliminated = true; } } @@ -403,23 +703,31 @@ boost::shared_ptr ISAM2::recalculate(const KeySet& markedKeys, const Ke 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()); + 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: "); + 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; } + 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) { + if (params_.enableDetailedResults) { + for (Key key : affectedAndNewKeys) { result.detail->variableStatus[key].isReeliminated = true; } } @@ -432,34 +740,38 @@ boost::shared_ptr ISAM2::recalculate(const KeySet& markedKeys, const Ke #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; + 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: "); + if (debug) cachedBoundary.print("Boundary factors: "); factors.push_back(cachedBoundary); gttoc(cached); gttic(orphans); // Add the orphaned subtrees - for(const sharedClique& orphan: orphans) + 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]) + // 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 + // 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); @@ -468,37 +780,46 @@ boost::shared_ptr ISAM2::recalculate(const KeySet& markedKeys, const Ke gttic(ordering_constraints); // Create ordering constraints - FastMap constraintGroups; - if(constrainKeys) { + FastMap constraintGroups; + if (constrainKeys) { constraintGroups = *constrainKeys; } else { - constraintGroups = FastMap(); - const int group = observedKeys.size() < affectedFactorsVarIndex.size() ? 1 : 0; - for (Key var: observedKeys) + 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 ++); + 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; + ++iter; } gttoc(ordering_constraints); // Generate ordering gttic(Ordering); - Ordering ordering = Ordering::ColamdConstrained(affectedFactorsVarIndex, constraintGroups); + Ordering ordering = + Ordering::ColamdConstrained(affectedFactorsVarIndex, constraintGroups); gttoc(Ordering); - ISAM2BayesTree::shared_ptr bayesTree = ISAM2JunctionTree(GaussianEliminationTree( - factors, affectedFactorsVarIndex, ordering)).eliminate(params_.getEliminationFunction()).first; + 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->roots_.insert(this->roots_.end(), bayesTree->roots().begin(), + bayesTree->roots().end()); this->nodes_.insert(bayesTree->nodes().begin(), bayesTree->nodes().end()); gttoc(reassemble); @@ -508,9 +829,9 @@ boost::shared_ptr ISAM2::recalculate(const KeySet& markedKeys, const Ke } // Root clique variables for detailed results - if(params_.enableDetailedResults) { - for(const sharedNode& root: this->roots()) - for(Key var: *root->conditional()) + if (params_.enableDetailedResults) { + for (const sharedNode& root : this->roots()) + for (Key var : *root->conditional()) result.detail->variableStatus[var].inRootClique = true; } @@ -519,11 +840,12 @@ boost::shared_ptr ISAM2::recalculate(const KeySet& markedKeys, const Ke /* ************************************************************************* */ ISAM2Result ISAM2::update( - const NonlinearFactorGraph& newFactors, const Values& newTheta, const FactorIndices& removeFactorIndices, - const boost::optional >& constrainedKeys, const boost::optional >& noRelinKeys, - const boost::optional >& extraReelimKeys, bool force_relinearize) -{ - + const NonlinearFactorGraph& newFactors, const Values& newTheta, + const FactorIndices& removeFactorIndices, + const boost::optional >& constrainedKeys, + const boost::optional >& noRelinKeys, + const boost::optional >& extraReelimKeys, + bool force_relinearize) { const bool debug = ISDEBUG("ISAM2 update"); const bool verbose = ISDEBUG("ISAM2 update verbose"); @@ -538,18 +860,19 @@ ISAM2Result ISAM2::update( lastBacksubVariableCount = 0; lastNnzTop = 0; ISAM2Result result; - if(params_.enableDetailedResults) + if (params_.enableDetailedResults) result.detail = ISAM2Result::DetailedResults(); - const bool relinearizeThisStep = force_relinearize - || (params_.enableRelinearization && update_count_ % params_.relinearizeSkip == 0); + const bool relinearizeThisStep = + force_relinearize || (params_.enableRelinearization && + update_count_ % params_.relinearizeSkip == 0); - if(verbose) { + if (verbose) { cout << "ISAM2::update\n"; this->print("ISAM2: "); } // Update delta if we need it to check relinearization later - if(relinearizeThisStep) { + if (relinearizeThisStep) { gttic(updateDelta); updateDelta(disableReordering); gttoc(updateDelta); @@ -558,20 +881,23 @@ ISAM2Result ISAM2::update( 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); + if (debug || verbose) newFactors.print("The new factors are: "); + Impl::AddFactorsStep1(newFactors, params_.findUnusedFactorSlots, + nonlinearFactors_, result.newFactorsIndices); // Remove the removed factors - NonlinearFactorGraph removeFactors; removeFactors.reserve(removeFactorIndices.size()); - for(size_t index: removeFactorIndices) { + NonlinearFactorGraph removeFactors; + removeFactors.reserve(removeFactorIndices.size()); + for (size_t index : removeFactorIndices) { removeFactors.push_back(nonlinearFactors_[index]); nonlinearFactors_.remove(index); - if(params_.cacheLinearizedFactors) - linearFactors_.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(removeFactorIndices.begin(), removeFactorIndices.end(), removeFactors); + // Remove removed factors from the variable index so we do not attempt to + // relinearize them + variableIndex_.remove(removeFactorIndices.begin(), removeFactorIndices.end(), + removeFactors); // Compute unused keys and indices KeySet unusedKeys; @@ -580,123 +906,145 @@ ISAM2Result ISAM2::update( // 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()) + 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())); + newFactorSymbKeys.begin(), newFactorSymbKeys.end(), + std::inserter(unusedKeys, unusedKeys.end())); // Get indices for unused keys - for(Key key: unusedKeys) { + 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}. + // 2. Initialize any new variables \Theta_{new} and add + // \Theta:=\Theta\cup\Theta_{new}. Impl::AddVariables(newTheta, theta_, delta_, deltaNewton_, RgProd_); // New keys for detailed results - if(params_.enableDetailedResults) { - for(Key key: newTheta.keys()) { result.detail->variableStatus[key].isNew = true; } } + if (params_.enableDetailedResults) { + for (Key key : newTheta.keys()) { + result.detail->variableStatus[key].isNew = true; + } + } gttoc(add_new_variables); gttic(evaluate_error_before); - if(params_.evaluateNonlinearError) + 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 + 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 + 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(extraReelimKeys) { - for(Key key: *extraReelimKeys) { + if (extraReelimKeys) { + for (Key key : *extraReelimKeys) { markedKeys.insert(key); } } // Observed keys for detailed results - if(params_.enableDetailedResults) { - for(Key key: markedKeys) { + if (params_.enableDetailedResults) { + for (Key key : markedKeys) { result.detail->variableStatus[key].isObserved = true; } } // NOTE: we use assign instead of the iterator constructor here because this // is a vector of size_t, so the constructor unintentionally resolves to // vector(size_t count, Key value) instead of the iterator constructor. - FastVector observedKeys; observedKeys.reserve(markedKeys.size()); - for(Key index: markedKeys) { - if(unusedIndices.find(index) == unusedIndices.end()) // Only add if not unused - observedKeys.push_back(index); // Make a copy of these, as we'll soon add to them + FastVector observedKeys; + observedKeys.reserve(markedKeys.size()); + for (Key index : markedKeys) { + if (unusedIndices.find(index) == + unusedIndices.end()) // Only add if not unused + observedKeys.push_back( + index); // Make a copy of these, as we'll soon add to them } gttoc(gather_involved_keys); - // Check relinearization if we're at the nth step, or we are using a looser loop relin threshold + // 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); + // 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(disableReordering) relinKeys = Impl::CheckRelinearizationFull(delta_, 0.0); // This is used for debugging + relinKeys = + Impl::CheckRelinearizationFull(delta_, params_.relinearizeThreshold); + if (disableReordering) + 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_) { + for (Key key : fixedVariables_) { relinKeys.erase(key); } - if(noRelinKeys) { - for(Key key: *noRelinKeys) { + if (noRelinKeys) { + for (Key key : *noRelinKeys) { relinKeys.erase(key); } } // Above relin threshold keys for detailed results - if(params_.enableDetailedResults) { - for(Key key: relinKeys) { + if (params_.enableDetailedResults) { + for (Key key : relinKeys) { result.detail->variableStatus[key].isAboveRelinThreshold = true; - result.detail->variableStatus[key].isRelinearized = 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); + 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. + // 5. Mark all cliques that involve marked variables \Theta_{J} and all + // their ancestors. if (!relinKeys.empty()) { - for(const sharedClique& root: roots_) + for (const sharedClique& root : roots_) // add other cliques that have the marked ones in the separator Impl::FindAll(root, markedKeys, markedRelinMask); // Relin involved keys for detailed results - if(params_.enableDetailedResults) { + if (params_.enableDetailedResults) { KeySet involvedRelinKeys; - for(const sharedClique& root: roots_) + for (const sharedClique& root : roots_) Impl::FindAll(root, involvedRelinKeys, markedRelinMask); - for(Key key: involvedRelinKeys) { - if(!result.detail->variableStatus[key].isAboveRelinThreshold) { + for (Key key : involvedRelinKeys) { + if (!result.detail->variableStatus[key].isAboveRelinThreshold) { result.detail->variableStatus[key].isRelinearizeInvolved = true; - result.detail->variableStatus[key].isRelinearized = 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}. + // 6. Update linearization point for marked variables: + // \Theta_{J}:=\Theta_{J}+\Delta_{J}. if (!relinKeys.empty()) - Impl::ExpmapMasked(theta_, delta_, markedRelinMask, delta_); + Impl::ExpmapMasked(delta_, markedRelinMask, &theta_, delta_); gttoc(expmap); result.variablesRelinearized = markedKeys.size(); @@ -706,17 +1054,16 @@ ISAM2Result ISAM2::update( gttic(linearize_new); // 7. Linearize new factors - if(params_.cacheLinearizedFactors) { + if (params_.cacheLinearizedFactors) { gttic(linearize); - GaussianFactorGraph::shared_ptr linearFactors = newFactors.linearize(theta_); - if(params_.findUnusedFactorSlots) - { + GaussianFactorGraph::shared_ptr 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 - { + 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()); @@ -726,7 +1073,7 @@ ISAM2Result ISAM2::update( gttic(augment_VI); // Augment the variable index with the new factors - if(params_.findUnusedFactorSlots) + if (params_.findUnusedFactorSlots) variableIndex_.augment(newFactors, result.newFactorsIndices); else variableIndex_.augment(newFactors); @@ -734,26 +1081,28 @@ ISAM2Result ISAM2::update( gttic(recalculate); // 8. Redo top of Bayes tree - boost::shared_ptr replacedKeys; - if(!markedKeys.empty() || !observedKeys.empty()) - replacedKeys = recalculate(markedKeys, relinKeys, observedKeys, unusedIndices, constrainedKeys, result); + boost::shared_ptr replacedKeys; + if (!markedKeys.empty() || !observedKeys.empty()) + replacedKeys = recalculate(markedKeys, relinKeys, observedKeys, + unusedIndices, constrainedKeys, result); // Update replaced keys mask (accumulates until back-substitution takes place) - if(replacedKeys) + if (replacedKeys) deltaReplacedMask_.insert(replacedKeys->begin(), replacedKeys->end()); gttoc(recalculate); // Update data structures to remove unused keys - if(!unusedKeys.empty()) { + if (!unusedKeys.empty()) { gttic(remove_variables); - Impl::RemoveVariables(unusedKeys, roots_, theta_, variableIndex_, delta_, deltaNewton_, RgProd_, - deltaReplacedMask_, Base::nodes_, fixedVariables_); + Impl::RemoveVariables(unusedKeys, roots_, theta_, variableIndex_, delta_, + deltaNewton_, RgProd_, deltaReplacedMask_, + Base::nodes_, fixedVariables_); gttoc(remove_variables); } result.cliques = this->nodes().size(); gttic(evaluate_error_after); - if(params_.evaluateNonlinearError) + if (params_.evaluateNonlinearError) result.errorAfter.reset(nonlinearFactors_.error(calculateEstimate())); gttoc(evaluate_error_after); @@ -761,16 +1110,16 @@ ISAM2Result ISAM2::update( } /* ************************************************************************* */ -void ISAM2::marginalizeLeaves(const FastList& leafKeysList, - boost::optional marginalFactorsIndices, - boost::optional deletedFactorsIndices) -{ +void ISAM2::marginalizeLeaves( + const FastList& leafKeysList, + boost::optional marginalFactorsIndices, + boost::optional deletedFactorsIndices) { // Convert to ordered set KeySet leafKeys(leafKeysList.begin(), leafKeysList.end()); // Keep track of marginal factors - map from clique to the marginal factors // that should be incorporated into it, passed up from it's children. -// multimap marginalFactors; + // multimap marginalFactors; map > marginalFactors; // Keep track of factors that get summarized by removing cliques @@ -780,17 +1129,17 @@ void ISAM2::marginalizeLeaves(const FastList& leafKeysList, KeySet leafKeysRemoved; // Remove each variable and its subtrees - for(Key j: leafKeys) { - if(!leafKeysRemoved.exists(j)) { // If the index was not already removed by removing another subtree + for (Key j : leafKeys) { + if (!leafKeysRemoved.exists(j)) { // If the index was not already removed + // by removing another subtree // Traverse up the tree to find the root of the marginalized subtree sharedClique clique = nodes_[j]; - while(!clique->parent_._empty()) - { - // Check if parent contains a marginalized leaf variable. Only need to check the first - // variable because it is the closest to the leaves. + while (!clique->parent_._empty()) { + // Check if parent contains a marginalized leaf variable. Only need to + // check the first variable because it is the closest to the leaves. sharedClique parent = clique->parent(); - if(leafKeys.exists(parent->conditional()->front())) + if (leafKeys.exists(parent->conditional()->front())) clique = parent; else break; @@ -798,39 +1147,48 @@ void ISAM2::marginalizeLeaves(const FastList& leafKeysList, // See if we should remove the whole clique bool marginalizeEntireClique = true; - for(Key frontal: clique->conditional()->frontals()) { - if(!leafKeys.exists(frontal)) { + for (Key frontal : clique->conditional()->frontals()) { + if (!leafKeys.exists(frontal)) { marginalizeEntireClique = false; - break; } } + break; + } + } // Remove either the whole clique or part of it - if(marginalizeEntireClique) { - // Remove the whole clique and its subtree, and keep the marginal factor. + if (marginalizeEntireClique) { + // Remove the whole clique and its subtree, and keep the marginal + // factor. GaussianFactor::shared_ptr marginalFactor = clique->cachedFactor(); // We do not need the marginal factors associated with this clique // because their information is already incorporated in the new // marginal factor. So, now associate this marginal factor with the // parent of this clique. - marginalFactors[clique->parent()->conditional()->front()].push_back(marginalFactor); + marginalFactors[clique->parent()->conditional()->front()].push_back( + marginalFactor); // Now remove this clique and its subtree - all of its marginal // information has been stored in marginalFactors. - const Cliques removedCliques = this->removeSubtree(clique); // Remove the subtree and throw away the cliques - for(const sharedClique& removedClique: removedCliques) { + const Cliques removedCliques = this->removeSubtree( + clique); // Remove the subtree and throw away the cliques + for (const sharedClique& removedClique : removedCliques) { marginalFactors.erase(removedClique->conditional()->front()); - leafKeysRemoved.insert(removedClique->conditional()->beginFrontals(), removedClique->conditional()->endFrontals()); - for(Key frontal: removedClique->conditional()->frontals()) - { + leafKeysRemoved.insert(removedClique->conditional()->beginFrontals(), + removedClique->conditional()->endFrontals()); + for (Key frontal : removedClique->conditional()->frontals()) { // Add to factors to remove - const VariableIndex::Factors& involvedFactors = variableIndex_[frontal]; - factorIndicesToRemove.insert(involvedFactors.begin(), involvedFactors.end()); + const VariableIndex::Factors& involvedFactors = + variableIndex_[frontal]; + factorIndicesToRemove.insert(involvedFactors.begin(), + involvedFactors.end()); // Check for non-leaf keys - if(!leafKeys.exists(frontal)) - throw runtime_error("Requesting to marginalize variables that are not leaves, the ISAM2 object is now in an inconsistent state so should no longer be used."); + if (!leafKeys.exists(frontal)) + throw runtime_error( + "Requesting to marginalize variables that are not leaves, " + "the ISAM2 object is now in an inconsistent state so should " + "no longer be used."); } } - } - else { + } else { // Reeliminate the current clique and the marginals from its children, // then keep only the marginal on the non-marginalized variables. We // get the childrens' marginals from any existing children, plus @@ -841,181 +1199,220 @@ void ISAM2::marginalizeLeaves(const FastList& leafKeysList, GaussianFactorGraph graph; KeySet factorsInSubtreeRoot; Cliques subtreesToRemove; - for(const sharedClique& child: clique->children) { + for (const sharedClique& child : clique->children) { // Remove subtree if child depends on any marginalized keys - for(Key parent: child->conditional()->parents()) { - if(leafKeys.exists(parent)) { + for (Key parent : child->conditional()->parents()) { + if (leafKeys.exists(parent)) { subtreesToRemove.push_back(child); - graph.push_back(child->cachedFactor()); // Add child marginal + graph.push_back(child->cachedFactor()); // Add child marginal break; } } } Cliques childrenRemoved; - for(const sharedClique& childToRemove: subtreesToRemove) { - const Cliques removedCliques = this->removeSubtree(childToRemove); // Remove the subtree and throw away the cliques - childrenRemoved.insert(childrenRemoved.end(), removedCliques.begin(), removedCliques.end()); - for(const sharedClique& removedClique: removedCliques) { + for (const sharedClique& childToRemove : subtreesToRemove) { + const Cliques removedCliques = this->removeSubtree( + childToRemove); // Remove the subtree and throw away the cliques + childrenRemoved.insert(childrenRemoved.end(), removedCliques.begin(), + removedCliques.end()); + for (const sharedClique& removedClique : removedCliques) { marginalFactors.erase(removedClique->conditional()->front()); - leafKeysRemoved.insert(removedClique->conditional()->beginFrontals(), removedClique->conditional()->endFrontals()); - for(Key frontal: removedClique->conditional()->frontals()) - { + leafKeysRemoved.insert( + removedClique->conditional()->beginFrontals(), + removedClique->conditional()->endFrontals()); + for (Key frontal : removedClique->conditional()->frontals()) { // Add to factors to remove - const VariableIndex::Factors& involvedFactors = variableIndex_[frontal]; - factorIndicesToRemove.insert(involvedFactors.begin(), involvedFactors.end()); + const VariableIndex::Factors& involvedFactors = + variableIndex_[frontal]; + factorIndicesToRemove.insert(involvedFactors.begin(), + involvedFactors.end()); // Check for non-leaf keys - if(!leafKeys.exists(frontal)) - throw runtime_error("Requesting to marginalize variables that are not leaves, the ISAM2 object is now in an inconsistent state so should no longer be used."); + if (!leafKeys.exists(frontal)) + throw runtime_error( + "Requesting to marginalize variables that are not leaves, " + "the ISAM2 object is now in an inconsistent state so " + "should no longer be used."); } } } - // Add the factors that are pulled into the current clique by the marginalized variables. - // These are the factors that involve *marginalized* frontal variables in this clique - // but do not involve frontal variables of any of its children. - // TODO: reuse cached linear factors + // Add the factors that are pulled into the current clique by the + // marginalized variables. These are the factors that involve + // *marginalized* frontal variables in this clique but do not involve + // frontal variables of any of its children. + // TODO(dellaert): reuse cached linear factors KeySet factorsFromMarginalizedInClique_step1; - for(Key frontal: clique->conditional()->frontals()) { - if(leafKeys.exists(frontal)) - factorsFromMarginalizedInClique_step1.insert(variableIndex_[frontal].begin(), variableIndex_[frontal].end()); } + for (Key frontal : clique->conditional()->frontals()) { + if (leafKeys.exists(frontal)) + factorsFromMarginalizedInClique_step1.insert( + variableIndex_[frontal].begin(), variableIndex_[frontal].end()); + } // Remove any factors in subtrees that we're removing at this step - for(const sharedClique& removedChild: childrenRemoved) { - for(Key indexInClique: removedChild->conditional()->frontals()) { - for(Key factorInvolving: variableIndex_[indexInClique]) { - factorsFromMarginalizedInClique_step1.erase(factorInvolving); } } } + for (const sharedClique& removedChild : childrenRemoved) { + for (Key indexInClique : removedChild->conditional()->frontals()) { + for (Key factorInvolving : variableIndex_[indexInClique]) { + factorsFromMarginalizedInClique_step1.erase(factorInvolving); + } + } + } // Create factor graph from factor indices - for(size_t i: factorsFromMarginalizedInClique_step1) { - graph.push_back(nonlinearFactors_[i]->linearize(theta_)); } + for (size_t i : factorsFromMarginalizedInClique_step1) { + graph.push_back(nonlinearFactors_[i]->linearize(theta_)); + } - // Reeliminate the linear graph to get the marginal and discard the conditional - const KeySet cliqueFrontals(clique->conditional()->beginFrontals(), clique->conditional()->endFrontals()); + // Reeliminate the linear graph to get the marginal and discard the + // conditional + const KeySet cliqueFrontals(clique->conditional()->beginFrontals(), + clique->conditional()->endFrontals()); FastVector cliqueFrontalsToEliminate; - std::set_intersection(cliqueFrontals.begin(), cliqueFrontals.end(), leafKeys.begin(), leafKeys.end(), - std::back_inserter(cliqueFrontalsToEliminate)); - pair eliminationResult1 = - params_.getEliminationFunction()(graph, Ordering(cliqueFrontalsToEliminate)); + std::set_intersection(cliqueFrontals.begin(), cliqueFrontals.end(), + leafKeys.begin(), leafKeys.end(), + std::back_inserter(cliqueFrontalsToEliminate)); + pair + eliminationResult1 = params_.getEliminationFunction()( + graph, Ordering(cliqueFrontalsToEliminate)); // Add the resulting marginal - if(eliminationResult1.second) - marginalFactors[clique->conditional()->front()].push_back(eliminationResult1.second); + if (eliminationResult1.second) + marginalFactors[clique->conditional()->front()].push_back( + eliminationResult1.second); // Split the current clique // Find the position of the last leaf key in this clique DenseIndex nToRemove = 0; - while(leafKeys.exists(clique->conditional()->keys()[nToRemove])) - ++ nToRemove; + while (leafKeys.exists(clique->conditional()->keys()[nToRemove])) + ++nToRemove; // Make the clique's matrix appear as a subset - const DenseIndex dimToRemove = clique->conditional()->matrixObject().offset(nToRemove); + const DenseIndex dimToRemove = + clique->conditional()->matrixObject().offset(nToRemove); clique->conditional()->matrixObject().firstBlock() = nToRemove; clique->conditional()->matrixObject().rowStart() = dimToRemove; // Change the keys in the clique - FastVector originalKeys; originalKeys.swap(clique->conditional()->keys()); - clique->conditional()->keys().assign(originalKeys.begin() + nToRemove, originalKeys.end()); + FastVector originalKeys; + originalKeys.swap(clique->conditional()->keys()); + clique->conditional()->keys().assign(originalKeys.begin() + nToRemove, + originalKeys.end()); clique->conditional()->nrFrontals() -= nToRemove; - // Add to factors to remove factors involved in frontals of current clique - for(Key frontal: cliqueFrontalsToEliminate) - { - const VariableIndex::Factors& involvedFactors = variableIndex_[frontal]; - factorIndicesToRemove.insert(involvedFactors.begin(), involvedFactors.end()); + // Add to factors to remove factors involved in frontals of current + // clique + for (Key frontal : cliqueFrontalsToEliminate) { + const VariableIndex::Factors& involvedFactors = + variableIndex_[frontal]; + factorIndicesToRemove.insert(involvedFactors.begin(), + involvedFactors.end()); } // Add removed keys - leafKeysRemoved.insert(cliqueFrontalsToEliminate.begin(), cliqueFrontalsToEliminate.end()); + leafKeysRemoved.insert(cliqueFrontalsToEliminate.begin(), + cliqueFrontalsToEliminate.end()); } } } - // At this point we have updated the BayesTree, now update the remaining iSAM2 data structures + // At this point we have updated the BayesTree, now update the remaining iSAM2 + // data structures // Gather factors to add - the new marginal factors GaussianFactorGraph factorsToAdd; typedef pair > Key_Factors; - for(const Key_Factors& key_factors: marginalFactors) { - for(const GaussianFactor::shared_ptr& factor: key_factors.second) { - if(factor) { + for (const Key_Factors& key_factors : marginalFactors) { + for (const GaussianFactor::shared_ptr& factor : key_factors.second) { + if (factor) { factorsToAdd.push_back(factor); - if(marginalFactorsIndices) + if (marginalFactorsIndices) marginalFactorsIndices->push_back(nonlinearFactors_.size()); - nonlinearFactors_.push_back(boost::make_shared( - factor)); - if(params_.cacheLinearizedFactors) - linearFactors_.push_back(factor); - for(Key factorKey: *factor) { - fixedVariables_.insert(factorKey); } + nonlinearFactors_.push_back( + boost::make_shared(factor)); + if (params_.cacheLinearizedFactors) linearFactors_.push_back(factor); + for (Key factorKey : *factor) { + fixedVariables_.insert(factorKey); + } } } } - variableIndex_.augment(factorsToAdd); // Augment the variable index + variableIndex_.augment(factorsToAdd); // Augment the variable index - // Remove the factors to remove that have been summarized in the newly-added marginal factors + // Remove the factors to remove that have been summarized in the newly-added + // marginal factors NonlinearFactorGraph removedFactors; - for(size_t i: factorIndicesToRemove) { + for (size_t i : factorIndicesToRemove) { removedFactors.push_back(nonlinearFactors_[i]); nonlinearFactors_.remove(i); - if(params_.cacheLinearizedFactors) - linearFactors_.remove(i); + if (params_.cacheLinearizedFactors) linearFactors_.remove(i); } - variableIndex_.remove(factorIndicesToRemove.begin(), factorIndicesToRemove.end(), removedFactors); + variableIndex_.remove(factorIndicesToRemove.begin(), + factorIndicesToRemove.end(), removedFactors); - if(deletedFactorsIndices) - deletedFactorsIndices->assign(factorIndicesToRemove.begin(), factorIndicesToRemove.end()); + if (deletedFactorsIndices) + deletedFactorsIndices->assign(factorIndicesToRemove.begin(), + factorIndicesToRemove.end()); // Remove the marginalized variables - Impl::RemoveVariables(KeySet(leafKeys.begin(), leafKeys.end()), roots_, theta_, variableIndex_, delta_, deltaNewton_, RgProd_, - deltaReplacedMask_, nodes_, fixedVariables_); + Impl::RemoveVariables(KeySet(leafKeys.begin(), leafKeys.end()), roots_, + theta_, variableIndex_, delta_, deltaNewton_, RgProd_, + deltaReplacedMask_, nodes_, fixedVariables_); } /* ************************************************************************* */ -void ISAM2::updateDelta(bool forceFullSolve) const -{ +void ISAM2::updateDelta(bool forceFullSolve) const { gttic(updateDelta); - if(params_.optimizationParams.type() == typeid(ISAM2GaussNewtonParams)) { + if (params_.optimizationParams.type() == typeid(ISAM2GaussNewtonParams)) { // If using Gauss-Newton, update with wildfireThreshold const ISAM2GaussNewtonParams& gaussNewtonParams = boost::get(params_.optimizationParams); - const double effectiveWildfireThreshold = forceFullSolve ? 0.0 : gaussNewtonParams.wildfireThreshold; + const double effectiveWildfireThreshold = + forceFullSolve ? 0.0 : gaussNewtonParams.wildfireThreshold; gttic(Wildfire_update); lastBacksubVariableCount = Impl::UpdateGaussNewtonDelta( - roots_, deltaReplacedMask_, delta_, effectiveWildfireThreshold); + roots_, deltaReplacedMask_, effectiveWildfireThreshold, &delta_); deltaReplacedMask_.clear(); gttoc(Wildfire_update); - } else if(params_.optimizationParams.type() == typeid(ISAM2DoglegParams)) { + } else if (params_.optimizationParams.type() == typeid(ISAM2DoglegParams)) { // If using Dogleg, do a Dogleg step const ISAM2DoglegParams& doglegParams = boost::get(params_.optimizationParams); - const double effectiveWildfireThreshold = forceFullSolve ? 0.0 : doglegParams.wildfireThreshold; + const double effectiveWildfireThreshold = + forceFullSolve ? 0.0 : doglegParams.wildfireThreshold; // Do one Dogleg iteration gttic(Dogleg_Iterate); // Compute Newton's method step gttic(Wildfire_update); - lastBacksubVariableCount = Impl::UpdateGaussNewtonDelta(roots_, deltaReplacedMask_, deltaNewton_, effectiveWildfireThreshold); + lastBacksubVariableCount = Impl::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(gradAtZero, RgProd_); // Compute gradient search point + const VectorValues gradAtZero = this->gradientAtZero(); // Compute gradient + Impl::UpdateRgProd(roots_, deltaReplacedMask_, gradAtZero, + &RgProd_); // Update RgProd + const VectorValues dx_u = Impl::ComputeGradientSearch( + gradAtZero, RgProd_); // Compute gradient search point - // Clear replaced keys mask because now we've updated deltaNewton_ and RgProd_ + // Clear replaced keys mask because now we've updated deltaNewton_ and + // RgProd_ deltaReplacedMask_.clear(); // Compute dogleg point - DoglegOptimizerImpl::IterationResult doglegResult(DoglegOptimizerImpl::Iterate( - *doglegDelta_, doglegParams.adaptationMode, dx_u, deltaNewton_, *this, nonlinearFactors_, - theta_, nonlinearFactors_.error(theta_), doglegParams.verbose)); + DoglegOptimizerImpl::IterationResult doglegResult( + DoglegOptimizerImpl::Iterate( + *doglegDelta_, doglegParams.adaptationMode, dx_u, deltaNewton_, + *this, nonlinearFactors_, theta_, nonlinearFactors_.error(theta_), + doglegParams.verbose)); gttoc(Dogleg_Iterate); gttic(Copy_dx_d); // Update Delta and linear step doglegDelta_ = doglegResult.delta; - delta_ = doglegResult.dx_d; // Copy the VectorValues containing with the linear solution + delta_ = + doglegResult + .dx_d; // Copy the VectorValues containing with the linear solution gttoc(Copy_dx_d); } } @@ -1037,60 +1434,62 @@ const Value& ISAM2::calculateEstimate(Key key) const { /* ************************************************************************* */ Values ISAM2::calculateBestEstimate() const { - updateDelta(true); // Force full solve when updating delta_ + updateDelta(true); // Force full solve when updating delta_ return theta_.retract(delta_); } /* ************************************************************************* */ Matrix ISAM2::marginalCovariance(Key key) const { - return marginalFactor(key, params_.getEliminationFunction())->information().inverse(); + return marginalFactor(key, params_.getEliminationFunction()) + ->information() + .inverse(); } /* ************************************************************************* */ const VectorValues& ISAM2::getDelta() const { - if(!deltaReplacedMask_.empty()) - updateDelta(); + if (!deltaReplacedMask_.empty()) updateDelta(); return delta_; } /* ************************************************************************* */ -double ISAM2::error(const VectorValues& x) const -{ +double ISAM2::error(const VectorValues& x) const { return GaussianFactorGraph(*this).error(x); } /* ************************************************************************* */ -static void gradientAtZeroTreeAdder(const boost::shared_ptr& root, VectorValues& g) { +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) { + 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); + 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) { + for (const sharedClique& child : root->children) { gradientAtZeroTreeAdder(child, g); } } /* ************************************************************************* */ -VectorValues ISAM2::gradientAtZero() const -{ +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 ISAM2::sharedClique& root : this->roots()) + gradientAtZeroTreeAdder(root, &g); return g; } -} +} // namespace gtsam /// namespace gtsam diff --git a/gtsam/nonlinear/ISAM2.h b/gtsam/nonlinear/ISAM2.h index 235cb65a5..d3904067c 100644 --- a/gtsam/nonlinear/ISAM2.h +++ b/gtsam/nonlinear/ISAM2.h @@ -509,7 +509,7 @@ class GTSAM_EXPORT ISAM2Clique Base::FactorType::shared_ptr cachedFactor_; Vector gradientContribution_; - FastMap solnPointers_; + mutable FastMap solnPointers_; /// Default constructor ISAM2Clique() : Base() {} @@ -546,6 +546,45 @@ class GTSAM_EXPORT ISAM2Clique void print(const std::string& s = "", const KeyFormatter& formatter = DefaultKeyFormatter) const; + /** + * Check if clique was replaced, or if any parents were changed above the + * threshold or themselves replaced. + */ + bool isDirty(const KeySet& replaced, const KeySet& changed) const; + + /// Copy values in delta pertaining to this clique. + FastVector copyRelevantValues(const VectorValues& delta) const; + + /* + * Check whether the values changed above a threshold, or always true if the + * clique was replaced. + */ + bool valuesChanged(const KeySet& replaced, + const FastVector& originalValues, + const VectorValues& delta, double threshold) const; + + /// Set changed flag for each frontal variable + void markFrontalsAsChanged(KeySet* changed) const; + + /// Restore delta to original values, guided by frontal keys. + void restoreFromOriginals(const FastVector& originalValues, + VectorValues* delta) const; + + void optimizeWildfire(const KeySet& replaced, double threshold, KeySet* changed, + VectorValues* delta, + size_t* count) const; + + bool optimizeWildfireNode(const KeySet& replaced, double threshold, KeySet* changed, + VectorValues* delta, + size_t* count) const; + + /** + * Starting from the root, add up entries of frontal and conditional matrices + * of each conditional + */ + void nnz_internal(size_t* result) const; + size_t calculate_nnz() const; + private: /** Serialization function */ friend class boost::serialization::access; @@ -810,30 +849,23 @@ class GTSAM_EXPORT ISAM2 : public BayesTree { template <> struct traits : public Testable {}; -/// Optimize the BayesTree, starting from the root. -/// @param replaced Needs to contain -/// all variables that are contained in the top of the Bayes tree that has been -/// redone. -/// @param delta The current solution, an offset from the linearization -/// point. -/// @param threshold The maximum change against the PREVIOUS delta for -/// non-replaced variables that can be ignored, ie. the old delta entry is kept -/// and recursive backsubstitution might eventually stop if none of the changed -/// variables are contained in the subtree. -/// @return The number of variables that were solved for -template -size_t optimizeWildfire(const boost::shared_ptr& root, double threshold, - const KeySet& replaced, VectorValues& delta); +/** + * Optimize the BayesTree, starting from the root. + * @param threshold The maximum change against the PREVIOUS delta for + * non-replaced variables that can be ignored, ie. the old delta entry is kept + * and recursive backsubstitution might eventually stop if none of the changed + * variables are contained in the subtree. + * @param replaced Needs to contain all variables that are contained in the top + * of the Bayes tree that has been redone. + * @return The number of variables that were solved for. + * @param delta The current solution, an offset from the linearization point. + */ +size_t optimizeWildfire(const ISAM2::sharedClique& root, double threshold, + const KeySet& replaced, VectorValues* delta); -template -size_t optimizeWildfireNonRecursive(const boost::shared_ptr& root, +size_t optimizeWildfireNonRecursive(const ISAM2::sharedClique& root, double threshold, const KeySet& replaced, - VectorValues& delta); - -/// calculate the number of non-zero entries for the tree starting at clique -/// (use root for complete matrix) -template -int calculate_nnz(const boost::shared_ptr& clique); + VectorValues* delta); } // namespace gtsam diff --git a/tests/testGaussianISAM2.cpp b/tests/testGaussianISAM2.cpp index f56b458be..681c30587 100644 --- a/tests/testGaussianISAM2.cpp +++ b/tests/testGaussianISAM2.cpp @@ -843,7 +843,7 @@ TEST(ISAM2, calculate_nnz) { ISAM2 isam = createSlamlikeISAM2(); int expected = 241; - int actual = calculate_nnz(isam.roots().front()); + int actual = isam.roots().front()->calculate_nnz(); EXPECT_LONGS_EQUAL(expected, actual); }