diff --git a/.cproject b/.cproject index 2032850b4..c6c10d6db 100644 --- a/.cproject +++ b/.cproject @@ -21,7 +21,7 @@ - + diff --git a/.project b/.project index 9856df2ea..c4d531b04 100644 --- a/.project +++ b/.project @@ -23,7 +23,7 @@ org.eclipse.cdt.make.core.buildArguments - -j5 + org.eclipse.cdt.make.core.buildCommand diff --git a/gtsam/nonlinear/GaussianISAM2-inl.h b/gtsam/nonlinear/GaussianISAM2-inl.h deleted file mode 100644 index 57801fbe4..000000000 --- a/gtsam/nonlinear/GaussianISAM2-inl.h +++ /dev/null @@ -1,176 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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) - - * See LICENSE for the license information - - * -------------------------------------------------------------------------- */ - -/** - * @file GaussianISAM2 - * @brief Full non-linear ISAM - * @author Michael Kaess - */ - -#include -#include - -#include - -namespace gtsam { - - using namespace std; - - /* ************************************************************************* */ - namespace internal { - template - void optimizeWildfire(const boost::shared_ptr& clique, double threshold, - vector& changed, const vector& replaced, Permuted& delta, int& 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[(*clique)->frontals().front()]; -#ifndef NDEBUG - BOOST_FOREACH(Index frontal, (*clique)->frontals()) { - assert(cliqueReplaced == replaced[frontal]); - } -#endif - - // If not redone, then has one of the separator variables changed significantly? - bool recalculate = cliqueReplaced; - if(!recalculate) { - BOOST_FOREACH(Index parent, (*clique)->parents()) { - if(changed[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 - vector originalValues((*clique)->nrFrontals()); - GaussianConditional::const_iterator it; - for(it = (*clique)->beginFrontals(); it!=(*clique)->endFrontals(); it++) { - originalValues[it - (*clique)->beginFrontals()] = delta[*it]; - } - - // Back-substitute - (*clique)->solveInPlace(delta); - count += (*clique)->nrFrontals(); - - // Whether the values changed above a threshold, or always true if the - // clique was replaced. - bool valuesChanged = cliqueReplaced; - for(it = (*clique)->beginFrontals(); it!=(*clique)->endFrontals(); it++) { - if(!valuesChanged) { - const Vector& oldValue(originalValues[it - (*clique)->beginFrontals()]); - const SubVector& 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 - BOOST_FOREACH(Index frontal, (*clique)->frontals()) { - changed[frontal] = true; - } - } else { - // Replace with the old values - for(it = (*clique)->beginFrontals(); it!=(*clique)->endFrontals(); it++) { - delta[*it] = originalValues[it - (*clique)->beginFrontals()]; - } - } - - // Recurse to children - BOOST_FOREACH(const typename CLIQUE::shared_ptr& child, clique->children_) { - optimizeWildfire(child, threshold, changed, replaced, delta, count); - } - } - } - } - - /* ************************************************************************* */ - template - int optimizeWildfire(const boost::shared_ptr& root, double threshold, const vector& keys, Permuted& delta) { - vector changed(keys.size(), false); - 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 - VectorValues optimizeGradientSearch(const ISAM2& isam) { - tic(0, "Allocate VectorValues"); - VectorValues grad = *allocateVectorValues(isam); - toc(0, "Allocate VectorValues"); - - optimizeGradientSearchInPlace(isam, grad); - - return grad; - } - - /* ************************************************************************* */ - template - void optimizeGradientSearchInPlace(const ISAM2& Rd, VectorValues& grad) { - tic(1, "Compute Gradient"); - // Compute gradient (call gradientAtZero function, which is defined for various linear systems) - gradientAtZero(Rd, grad); - double gradientSqNorm = grad.dot(grad); - toc(1, "Compute Gradient"); - - tic(2, "Compute R*g"); - // Compute R * g - FactorGraph Rd_jfg(Rd); - Errors Rg = Rd_jfg * grad; - toc(2, "Compute R*g"); - - tic(3, "Compute minimizing step size"); - // Compute minimizing step size - double step = -gradientSqNorm / dot(Rg, Rg); - toc(3, "Compute minimizing step size"); - - tic(4, "Compute point"); - // Compute steepest descent point - scal(step, grad); - toc(4, "Compute point"); - } - - /* ************************************************************************* */ - template - void nnz_internal(const boost::shared_ptr& clique, int& result) { - int dimR = (*clique)->dim(); - int dimSep = (*clique)->get_S().cols() - dimR; - result += ((dimR+1)*dimR)/2 + dimSep*dimR; - // traverse the children - BOOST_FOREACH(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/GaussianISAM2.cpp b/gtsam/nonlinear/GaussianISAM2.cpp deleted file mode 100644 index 624c5d4f0..000000000 --- a/gtsam/nonlinear/GaussianISAM2.cpp +++ /dev/null @@ -1,60 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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) - - * See LICENSE for the license information - - * -------------------------------------------------------------------------- */ - -/** - * @file GaussianISAM2 - * @brief Full non-linear ISAM - * @author Michael Kaess - */ - -#include -#include -#include - -using namespace std; -using namespace gtsam; - -#include - -namespace gtsam { - - /* ************************************************************************* */ - VectorValues gradient(const BayesTree >& bayesTree, const VectorValues& x0) { - return gradient(FactorGraph(bayesTree), x0); - } - - /* ************************************************************************* */ - static void gradientAtZeroTreeAdder(const boost::shared_ptr >& root, VectorValues& g) { - // Loop through variables in each clique, adding contributions - int variablePosition = 0; - for(GaussianConditional::const_iterator jit = root->conditional()->begin(); jit != root->conditional()->end(); ++jit) { - const int dim = root->conditional()->dim(jit); - g[*jit] += root->gradientContribution().segment(variablePosition, dim); - variablePosition += dim; - } - - // Recursively add contributions from children - typedef boost::shared_ptr > sharedClique; - BOOST_FOREACH(const sharedClique& child, root->children()) { - gradientAtZeroTreeAdder(child, g); - } - } - - /* ************************************************************************* */ - void gradientAtZero(const BayesTree >& bayesTree, VectorValues& g) { - // Zero-out gradient - g.setZero(); - - // Sum up contributions for each clique - gradientAtZeroTreeAdder(bayesTree.root(), g); - } - -} diff --git a/gtsam/nonlinear/GaussianISAM2.h b/gtsam/nonlinear/GaussianISAM2.h deleted file mode 100644 index a602d81a6..000000000 --- a/gtsam/nonlinear/GaussianISAM2.h +++ /dev/null @@ -1,153 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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) - - * See LICENSE for the license information - - * -------------------------------------------------------------------------- */ - -/** - * @file GaussianISAM - * @brief Full non-linear ISAM. - * @author Michael Kaess - */ - -// \callgraph - -#pragma once - -#include -#include - -namespace gtsam { - -/** - * @ingroup ISAM2 - * @brief The main ISAM2 class that is exposed to gtsam users, see ISAM2 for usage. - * - * This is a thin wrapper around an ISAM2 class templated on - * GaussianConditional, and the values on which that GaussianISAM2 is - * templated. - * - * @tparam VALUES The Values or TupleValues\Emph{N} that contains the - * variables. - * @tparam GRAPH The NonlinearFactorGraph structure to store factors. Defaults to standard NonlinearFactorGraph - */ -template -class GaussianISAM2 : public ISAM2 { - typedef ISAM2 Base; -public: - - /// @name Standard Constructors - /// @{ - - /** Create an empty ISAM2 instance */ - GaussianISAM2(const ISAM2Params& params) : ISAM2(params) {} - - /** Create an empty ISAM2 instance using the default set of parameters (see ISAM2Params) */ - GaussianISAM2() : ISAM2() {} - - /// @} - /// @name Advanced Interface - /// @{ - - void cloneTo(boost::shared_ptr& newGaussianISAM2) const { - boost::shared_ptr isam2 = boost::static_pointer_cast(newGaussianISAM2); - Base::cloneTo(isam2); - } - - /// @} - -}; - -/** Get the linear delta for the ISAM2 object, unpermuted the delta returned by ISAM2::getDelta() */ -template -VectorValues optimize(const ISAM2& isam) { - VectorValues delta = *allocateVectorValues(isam); - internal::optimizeInPlace(isam.root(), delta); - return delta; -} - -/// 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 -int optimizeWildfire(const boost::shared_ptr& root, - double threshold, const std::vector& replaced, Permuted& delta); - -/** - * Optimize along the gradient direction, with a closed-form computation to - * perform the line search. The gradient is computed about \f$ \delta x=0 \f$. - * - * This function returns \f$ \delta x \f$ that minimizes a reparametrized - * problem. The error function of a GaussianBayesNet is - * - * \f[ f(\delta x) = \frac{1}{2} |R \delta x - d|^2 = \frac{1}{2}d^T d - d^T R \delta x + \frac{1}{2} \delta x^T R^T R \delta x \f] - * - * with gradient and Hessian - * - * \f[ g(\delta x) = R^T(R\delta x - d), \qquad G(\delta x) = R^T R. \f] - * - * This function performs the line search in the direction of the - * gradient evaluated at \f$ g = g(\delta x = 0) \f$ with step size - * \f$ \alpha \f$ that minimizes \f$ f(\delta x = \alpha g) \f$: - * - * \f[ f(\alpha) = \frac{1}{2} d^T d + g^T \delta x + \frac{1}{2} \alpha^2 g^T G g \f] - * - * Optimizing by setting the derivative to zero yields - * \f$ \hat \alpha = (-g^T g) / (g^T G g) \f$. For efficiency, this function - * evaluates the denominator without computing the Hessian \f$ G \f$, returning - * - * \f[ \delta x = \hat\alpha g = \frac{-g^T g}{(R g)^T(R g)} \f] - */ -template -VectorValues optimizeGradientSearch(const ISAM2& isam); - -/** In-place version of optimizeGradientSearch requiring pre-allocated VectorValues \c x */ -template -void optimizeGradientSearchInPlace(const ISAM2& isam, VectorValues& grad); - -/// 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); - -/** - * Compute the gradient of the energy function, - * \f$ \nabla_{x=x_0} \left\Vert \Sigma^{-1} R x - d \right\Vert^2 \f$, - * centered around \f$ x = x_0 \f$. - * The gradient is \f$ R^T(Rx-d) \f$. - * This specialized version is used with ISAM2, where each clique stores its - * gradient contribution. - * @param bayesTree The Gaussian Bayes Tree $(R,d)$ - * @param x0 The center about which to compute the gradient - * @return The gradient as a VectorValues - */ -VectorValues gradient(const BayesTree >& bayesTree, const VectorValues& x0); - -/** - * Compute the gradient of the energy function, - * \f$ \nabla_{x=0} \left\Vert \Sigma^{-1} R x - d \right\Vert^2 \f$, - * centered around zero. - * The gradient about zero is \f$ -R^T d \f$. See also gradient(const GaussianBayesNet&, const VectorValues&). - * This specialized version is used with ISAM2, where each clique stores its - * gradient contribution. - * @param bayesTree The Gaussian Bayes Tree $(R,d)$ - * @param [output] g A VectorValues to store the gradient, which must be preallocated, see allocateVectorValues - * @return The gradient as a VectorValues - */ -void gradientAtZero(const BayesTree >& bayesTree, VectorValues& g); - -}/// namespace gtsam - -#include diff --git a/gtsam/nonlinear/ISAM2-impl-inl.h b/gtsam/nonlinear/ISAM2-impl.cpp similarity index 60% rename from gtsam/nonlinear/ISAM2-impl-inl.h rename to gtsam/nonlinear/ISAM2-impl.cpp index 40e87a09b..8dce6934a 100644 --- a/gtsam/nonlinear/ISAM2-impl-inl.h +++ b/gtsam/nonlinear/ISAM2-impl.cpp @@ -10,126 +10,17 @@ * -------------------------------------------------------------------------- */ /** - * @file ISAM2-impl-inl.h + * @file ISAM2-impl.cpp * @brief Incremental update functionality (ISAM2) for BayesTree, with fluid relinearization. * @author Michael Kaess, Richard Roberts */ -#include +#include namespace gtsam { -using namespace std; - -template -struct ISAM2::Impl { - - typedef ISAM2 ISAM2Type; - - struct PartialSolveResult { - typename ISAM2Type::sharedClique bayesTree; - Permutation fullReordering; - Permutation fullReorderingInverse; - }; - - struct ReorderingMode { - size_t nFullSystemVars; - enum { /*AS_ADDED,*/ COLAMD } algorithm; - enum { NO_CONSTRAINT, CONSTRAIN_LAST } constrain; - boost::optional&> constrainedKeys; - }; - - /** - * Add new variables to the ISAM2 system. - * @param newTheta Initial values for new variables - * @param theta Current solution to be augmented with new initialization - * @param delta Current linear delta to be augmented with zeros - * @param ordering Current ordering to be augmented with new variables - * @param nodes Current BayesTree::Nodes index to be augmented with slots for new variables - * @param keyFormatter Formatter for printing nonlinear keys during debugging - */ - static void AddVariables(const Values& newTheta, Values& theta, Permuted& delta, vector& replacedKeys, - Ordering& ordering, typename Base::Nodes& nodes, const KeyFormatter& keyFormatter = DefaultKeyFormatter); - - /** - * Extract the set of variable indices from a NonlinearFactorGraph. For each Symbol - * in each NonlinearFactor, obtains the index by calling ordering[symbol]. - * @param ordering The current ordering from which to obtain the variable indices - * @param factors The factors from which to extract the variables - * @return The set of variables indices from the factors - */ - static FastSet IndicesFromFactors(const Ordering& ordering, const GRAPH& factors); - - /** - * Find the set of variables to be relinearized according to relinearizeThreshold. - * Any variables in the VectorValues delta whose vector magnitude is greater than - * or equal to relinearizeThreshold are returned. - * @param delta The linear delta to check against the threshold - * @param keyFormatter Formatter for printing nonlinear keys during debugging - * @return The set of variable indices in delta whose magnitude is greater than or - * equal to relinearizeThreshold - */ - static FastSet CheckRelinearization(const Permuted& delta, const Ordering& ordering, - const ISAM2Params::RelinearizationThreshold& relinearizeThreshold, const KeyFormatter& keyFormatter = DefaultKeyFormatter); - - /** - * Recursively search this clique and its children for marked keys appearing - * in the separator, and add the *frontal* keys of any cliques whose - * separator contains any marked keys to the set \c keys. The purpose of - * this is to discover the cliques that need to be redone due to information - * propagating to them from cliques that directly contain factors being - * relinearized. - * - * The original comment says this finds all variables directly connected to - * the marked ones by measurements. Is this true, because it seems like this - * would also pull in variables indirectly connected through other frontal or - * separator variables? - * - * Alternatively could we trace up towards the root for each variable here? - */ - static void FindAll(typename ISAM2Clique::shared_ptr clique, FastSet& keys, const vector& markedMask); - - /** - * 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 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 Permuted& delta, - const Ordering& ordering, const std::vector& mask, - boost::optional&> invalidateIfDebug = boost::optional&>(), - const KeyFormatter& keyFormatter = DefaultKeyFormatter); - - /** - * Reorder and eliminate factors. These factors form a subset of the full - * problem, so along with the BayesTree we get a partial reordering of the - * problem that needs to be applied to the other data in ISAM2, which is the - * VariableIndex, the delta, the ordering, and the orphans (including cached - * factors). - * \param factors The factors to eliminate, representing part of the full - * problem. This is permuted during use and so is cleared when this function - * returns in order to invalidate it. - * \param keys The set of indices used in \c factors. - * \return The eliminated BayesTree and the permutation to be applied to the - * rest of the ISAM2 data. - */ - static PartialSolveResult PartialSolve(GaussianFactorGraph& factors, const FastSet& keys, - const ReorderingMode& reorderingMode); - - static size_t UpdateDelta(const boost::shared_ptr >& root, std::vector& replacedKeys, Permuted& delta, double wildfireThreshold); - -}; - /* ************************************************************************* */ -template -void ISAM2::Impl::AddVariables( +void ISAM2::Impl::AddVariables( const Values& newTheta, Values& theta, Permuted& delta, vector& replacedKeys, Ordering& ordering,typename Base::Nodes& nodes, const KeyFormatter& keyFormatter) { const bool debug = ISDEBUG("ISAM2 AddVariables"); @@ -163,8 +54,7 @@ void ISAM2::Impl::AddVariables( } /* ************************************************************************* */ -template -FastSet ISAM2::Impl::IndicesFromFactors(const Ordering& ordering, const GRAPH& factors) { +FastSet ISAM2::Impl::IndicesFromFactors(const Ordering& ordering, const GRAPH& factors) { FastSet indices; BOOST_FOREACH(const typename NonlinearFactor::shared_ptr& factor, factors) { BOOST_FOREACH(Key key, factor->keys()) { @@ -175,8 +65,7 @@ FastSet ISAM2::Impl::IndicesFromFactors(const Ordering } /* ************************************************************************* */ -template -FastSet ISAM2::Impl::CheckRelinearization(const Permuted& delta, const Ordering& ordering, +FastSet ISAM2::Impl::CheckRelinearization(const Permuted& delta, const Ordering& ordering, const ISAM2Params::RelinearizationThreshold& relinearizeThreshold, const KeyFormatter& keyFormatter) { FastSet relinKeys; @@ -204,8 +93,7 @@ FastSet ISAM2::Impl::CheckRelinearization(const Permut } /* ************************************************************************* */ -template -void ISAM2::Impl::FindAll(typename ISAM2Clique::shared_ptr clique, FastSet& keys, const vector& markedMask) { +void ISAM2::Impl::FindAll(typename ISAM2Clique::shared_ptr clique, FastSet& keys, const vector& markedMask) { static const bool debug = false; // does the separator contain any of the variables? bool found = false; @@ -219,14 +107,13 @@ void ISAM2::Impl::FindAll(typename ISAM2Clique:: if(debug) clique->print("Key(s) marked in clique "); if(debug) cout << "so marking key " << (*clique)->keys().front() << endl; } - BOOST_FOREACH(const typename ISAM2Clique::shared_ptr& child, clique->children_) { + BOOST_FOREACH(const typename ISAM2Clique::shared_ptr& child, clique->children_) { FindAll(child, keys, markedMask); } } /* ************************************************************************* */ -template -void ISAM2::Impl::ExpmapMasked(Values& values, const Permuted& delta, const Ordering& ordering, +void ISAM2::Impl::ExpmapMasked(Values& values, const Permuted& delta, const Ordering& ordering, const vector& mask, 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 @@ -262,9 +149,8 @@ void ISAM2::Impl::ExpmapMasked(Values& values, const Permute } /* ************************************************************************* */ -template -typename ISAM2::Impl::PartialSolveResult -ISAM2::Impl::PartialSolve(GaussianFactorGraph& factors, +ISAM2::Impl::PartialSolveResult +ISAM2::Impl::PartialSolve(GaussianFactorGraph& factors, const FastSet& keys, const ReorderingMode& reorderingMode) { static const bool debug = ISDEBUG("ISAM2 recalculate"); @@ -340,14 +226,8 @@ ISAM2::Impl::PartialSolve(GaussianFactorGraph& factors, // eliminate into a Bayes net tic(7,"eliminate"); - JunctionTree jt(factors, affectedFactorsIndex); + JunctionTree jt(factors, affectedFactorsIndex); result.bayesTree = jt.eliminate(EliminatePreferLDL); - if(debug && result.bayesTree) { - if(boost::dynamic_pointer_cast >(result.bayesTree)) - cout << "Is an ISAM2 clique" << endl; - cout << "Re-eliminated BT:\n"; - result.bayesTree->printTree(""); - } toc(7,"eliminate"); tic(8,"permute eliminated"); @@ -363,19 +243,18 @@ ISAM2::Impl::PartialSolve(GaussianFactorGraph& factors, /* ************************************************************************* */ namespace internal { -inline static void optimizeInPlace(const boost::shared_ptr >& clique, VectorValues& result) { +inline static void optimizeInPlace(const boost::shared_ptr& clique, VectorValues& result) { // parents are assumed to already be solved and available in result clique->conditional()->solveInPlace(result); // starting from the root, call optimize on each conditional - BOOST_FOREACH(const boost::shared_ptr >& child, clique->children_) + BOOST_FOREACH(const boost::shared_ptr& child, clique->children_) optimizeInPlace(child, result); } } /* ************************************************************************* */ -template -size_t ISAM2::Impl::UpdateDelta(const boost::shared_ptr >& root, std::vector& replacedKeys, Permuted& delta, double wildfireThreshold) { +size_t ISAM2::Impl::UpdateDelta(const boost::shared_ptr& root, std::vector& replacedKeys, Permuted& delta, double wildfireThreshold) { size_t lastBacksubVariableCount; diff --git a/gtsam/nonlinear/ISAM2-impl.h b/gtsam/nonlinear/ISAM2-impl.h new file mode 100644 index 000000000..3dcc15aa8 --- /dev/null +++ b/gtsam/nonlinear/ISAM2-impl.h @@ -0,0 +1,127 @@ +/* ---------------------------------------------------------------------------- + + * 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) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file ISAM2-impl.h + * @brief Incremental update functionality (ISAM2) for BayesTree, with fluid relinearization. + * @author Michael Kaess, Richard Roberts + */ + +#include + +namespace gtsam { + +using namespace std; + +struct ISAM2::Impl { + + struct PartialSolveResult { + typename ISAM2::sharedClique bayesTree; + Permutation fullReordering; + Permutation fullReorderingInverse; + }; + + struct ReorderingMode { + size_t nFullSystemVars; + enum { /*AS_ADDED,*/ COLAMD } algorithm; + enum { NO_CONSTRAINT, CONSTRAIN_LAST } constrain; + boost::optional&> constrainedKeys; + }; + + /** + * Add new variables to the ISAM2 system. + * @param newTheta Initial values for new variables + * @param theta Current solution to be augmented with new initialization + * @param delta Current linear delta to be augmented with zeros + * @param ordering Current ordering to be augmented with new variables + * @param nodes Current BayesTree::Nodes index to be augmented with slots for new variables + * @param keyFormatter Formatter for printing nonlinear keys during debugging + */ + static void AddVariables(const Values& newTheta, Values& theta, Permuted& delta, vector& replacedKeys, + Ordering& ordering, typename Base::Nodes& nodes, const KeyFormatter& keyFormatter = DefaultKeyFormatter); + + /** + * Extract the set of variable indices from a NonlinearFactorGraph. For each Symbol + * in each NonlinearFactor, obtains the index by calling ordering[symbol]. + * @param ordering The current ordering from which to obtain the variable indices + * @param factors The factors from which to extract the variables + * @return The set of variables indices from the factors + */ + static FastSet IndicesFromFactors(const Ordering& ordering, const GRAPH& factors); + + /** + * Find the set of variables to be relinearized according to relinearizeThreshold. + * Any variables in the VectorValues delta whose vector magnitude is greater than + * or equal to relinearizeThreshold are returned. + * @param delta The linear delta to check against the threshold + * @param keyFormatter Formatter for printing nonlinear keys during debugging + * @return The set of variable indices in delta whose magnitude is greater than or + * equal to relinearizeThreshold + */ + static FastSet CheckRelinearization(const Permuted& delta, const Ordering& ordering, + const ISAM2Params::RelinearizationThreshold& relinearizeThreshold, const KeyFormatter& keyFormatter = DefaultKeyFormatter); + + /** + * Recursively search this clique and its children for marked keys appearing + * in the separator, and add the *frontal* keys of any cliques whose + * separator contains any marked keys to the set \c keys. The purpose of + * this is to discover the cliques that need to be redone due to information + * propagating to them from cliques that directly contain factors being + * relinearized. + * + * The original comment says this finds all variables directly connected to + * the marked ones by measurements. Is this true, because it seems like this + * would also pull in variables indirectly connected through other frontal or + * separator variables? + * + * Alternatively could we trace up towards the root for each variable here? + */ + static void FindAll(typename ISAM2Clique::shared_ptr clique, FastSet& keys, const vector& markedMask); + + /** + * 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 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 Permuted& delta, + const Ordering& ordering, const std::vector& mask, + boost::optional&> invalidateIfDebug = boost::optional&>(), + const KeyFormatter& keyFormatter = DefaultKeyFormatter); + + /** + * Reorder and eliminate factors. These factors form a subset of the full + * problem, so along with the BayesTree we get a partial reordering of the + * problem that needs to be applied to the other data in ISAM2, which is the + * VariableIndex, the delta, the ordering, and the orphans (including cached + * factors). + * \param factors The factors to eliminate, representing part of the full + * problem. This is permuted during use and so is cleared when this function + * returns in order to invalidate it. + * \param keys The set of indices used in \c factors. + * \return The eliminated BayesTree and the permutation to be applied to the + * rest of the ISAM2 data. + */ + static PartialSolveResult PartialSolve(GaussianFactorGraph& factors, const FastSet& keys, + const ReorderingMode& reorderingMode); + + static size_t UpdateDelta(const boost::shared_ptr >& root, std::vector& replacedKeys, Permuted& delta, double wildfireThreshold); + +}; + +} diff --git a/gtsam/nonlinear/ISAM2-inl.h b/gtsam/nonlinear/ISAM2-inl.h index 0d5f190a3..89f727f8b 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) @@ -10,624 +10,136 @@ * -------------------------------------------------------------------------- */ /** - * @file ISAM2-inl.h - * @brief Incremental update functionality (ISAM2) for BayesTree, with fluid relinearization. - * @author Michael Kaess, Richard Roberts + * @file ISAM2-inl.h + * @brief + * @author Richard Roberts + * @date Mar 16, 2012 */ + #pragma once -#include -#include // for operator += -using namespace boost::assign; -#include -#include -#include -#include -#include - -#include -#include +#include +#include +#include namespace gtsam { using namespace std; -static const bool disableReordering = false; -static const double batchThreshold = 0.65; - /* ************************************************************************* */ -template -ISAM2::ISAM2(const ISAM2Params& params): - delta_(Permutation(), deltaUnpermuted_), deltaUptodate_(true), params_(params) { - // See note in gtsam/base/boost_variant_with_workaround.h - if(params_.optimizationParams.type() == typeid(ISAM2DoglegParams)) - doglegDelta_ = boost::get(params_.optimizationParams).initialDelta; -} +namespace internal { +template +void optimizeWildfire(const boost::shared_ptr& clique, double threshold, + vector& changed, const vector& replaced, Permuted& delta, int& 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 -/* ************************************************************************* */ -template -ISAM2::ISAM2(): - delta_(Permutation(), deltaUnpermuted_), deltaUptodate_(true) { - // See note in gtsam/base/boost_variant_with_workaround.h - if(params_.optimizationParams.type() == typeid(ISAM2DoglegParams)) - doglegDelta_ = boost::get(params_.optimizationParams).initialDelta; -} - -/* ************************************************************************* */ -template -FastList ISAM2::getAffectedFactors(const FastList& keys) const { - static const bool debug = false; - if(debug) cout << "Getting affected factors for "; - if(debug) { BOOST_FOREACH(const Index key, keys) { cout << key << " "; } } - if(debug) cout << endl; - - FactorGraph allAffected; - FastList indices; - BOOST_FOREACH(const Index key, keys) { -// const list l = nonlinearFactors_.factors(key); -// indices.insert(indices.begin(), l.begin(), l.end()); - const VariableIndex::Factors& factors(variableIndex_[key]); - BOOST_FOREACH(size_t factor, factors) { - if(debug) cout << "Variable " << key << " affects factor " << factor << endl; - indices.push_back(factor); - } + // Are any clique variables part of the tree that has been redone? + bool cliqueReplaced = replaced[(*clique)->frontals().front()]; +#ifndef NDEBUG + BOOST_FOREACH(Index frontal, (*clique)->frontals()) { + assert(cliqueReplaced == replaced[frontal]); } - indices.sort(); - indices.unique(); - if(debug) cout << "Affected factors are: "; - if(debug) { BOOST_FOREACH(const size_t index, indices) { cout << index << " "; } } - if(debug) cout << endl; - return indices; -} +#endif -/* ************************************************************************* */ -// retrieve all factors that ONLY contain the affected variables -// (note that the remaining stuff is summarized in the cached factors) -template -FactorGraph::shared_ptr -ISAM2::relinearizeAffectedFactors(const FastList& affectedKeys) const { - - tic(1,"getAffectedFactors"); - FastList candidates = getAffectedFactors(affectedKeys); - toc(1,"getAffectedFactors"); - - GRAPH nonlinearAffectedFactors; - - tic(2,"affectedKeysSet"); - // for fast lookup below - FastSet affectedKeysSet; - affectedKeysSet.insert(affectedKeys.begin(), affectedKeys.end()); - toc(2,"affectedKeysSet"); - - tic(3,"check candidates"); - BOOST_FOREACH(size_t idx, candidates) { - bool inside = true; - BOOST_FOREACH(Key key, nonlinearFactors_[idx]->keys()) { - Index var = ordering_[key]; - if (affectedKeysSet.find(var) == affectedKeysSet.end()) { - inside = false; + // If not redone, then has one of the separator variables changed significantly? + bool recalculate = cliqueReplaced; + if(!recalculate) { + BOOST_FOREACH(Index parent, (*clique)->parents()) { + if(changed[parent]) { + recalculate = true; break; } } - if (inside) - nonlinearAffectedFactors.push_back(nonlinearFactors_[idx]); } - toc(3,"check candidates"); - tic(4,"linearize"); - FactorGraph::shared_ptr linearized(nonlinearAffectedFactors.linearize(theta_, ordering_)); - toc(4,"linearize"); - return linearized; + // 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 + vector originalValues((*clique)->nrFrontals()); + GaussianConditional::const_iterator it; + for(it = (*clique)->beginFrontals(); it!=(*clique)->endFrontals(); it++) { + originalValues[it - (*clique)->beginFrontals()] = delta[*it]; + } + + // Back-substitute + (*clique)->solveInPlace(delta); + count += (*clique)->nrFrontals(); + + // Whether the values changed above a threshold, or always true if the + // clique was replaced. + bool valuesChanged = cliqueReplaced; + for(it = (*clique)->beginFrontals(); it!=(*clique)->endFrontals(); it++) { + if(!valuesChanged) { + const Vector& oldValue(originalValues[it - (*clique)->beginFrontals()]); + const SubVector& 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 + BOOST_FOREACH(Index frontal, (*clique)->frontals()) { + changed[frontal] = true; + } + } else { + // Replace with the old values + for(it = (*clique)->beginFrontals(); it!=(*clique)->endFrontals(); it++) { + delta[*it] = originalValues[it - (*clique)->beginFrontals()]; + } + } + + // Recurse to children + BOOST_FOREACH(const typename CLIQUE::shared_ptr& child, clique->children_) { + optimizeWildfire(child, threshold, changed, replaced, delta, count); + } + } +} } /* ************************************************************************* */ -// find intermediate (linearized) factors from cache that are passed into the affected area -template -FactorGraph::CacheFactor> -ISAM2::getCachedBoundaryFactors(Cliques& orphans) { - - static const bool debug = false; - - FactorGraph cachedBoundary; - - BOOST_FOREACH(sharedClique orphan, orphans) { - // find the last variable that was eliminated - Index key = (*orphan)->frontals().back(); -#ifndef NDEBUG -// typename BayesNet::const_iterator it = orphan->end(); -// const CONDITIONAL& lastCONDITIONAL = **(--it); -// typename CONDITIONAL::const_iterator keyit = lastCONDITIONAL.endParents(); -// const Index lastKey = *(--keyit); -// assert(key == lastKey); -#endif - // retrieve the cached factor and add to boundary - cachedBoundary.push_back(boost::dynamic_pointer_cast(orphan->cachedFactor())); - if(debug) { cout << "Cached factor for variable " << key; orphan->cachedFactor()->print(""); } - } - - return cachedBoundary; +template +int optimizeWildfire(const boost::shared_ptr& root, double threshold, const vector& keys, Permuted& delta) { + vector changed(keys.size(), false); + 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 -boost::shared_ptr > ISAM2::recalculate( - const FastSet& markedKeys, const FastVector& newKeys, const FactorGraph::shared_ptr newFactors, - const boost::optional >& constrainKeys, ISAM2Result& result) { - - // TODO: new factors are linearized twice, the newFactors passed in are not used. - - static const bool debug = ISDEBUG("ISAM2 recalculate"); - - // Input: BayesTree(this), newFactors - -//#define PRINT_STATS // figures for paper, disable for timing -#ifdef PRINT_STATS - static int counter = 0; - int maxClique = 0; - double avgClique = 0; - int numCliques = 0; - int nnzR = 0; - if (counter>0) { // cannot call on empty tree - GaussianISAM2_P::CliqueData cdata = this->getCliqueData(); - GaussianISAM2_P::CliqueStats cstats = cdata.getStats(); - maxClique = cstats.maxCONDITIONALSize; - avgClique = cstats.avgCONDITIONALSize; - numCliques = cdata.conditionalSizes.size(); - nnzR = calculate_nnz(this->root()); - } - counter++; -#endif - - if(debug) { - cout << "markedKeys: "; - BOOST_FOREACH(const Index key, markedKeys) { cout << key << " "; } - cout << endl; - cout << "newKeys: "; - BOOST_FOREACH(const Index key, newKeys) { 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. - tic(1, "removetop"); - Cliques orphans; - BayesNet affectedBayesNet; - this->removeTop(markedKeys, affectedBayesNet, orphans); - toc(1, "removetop"); - - if(debug) affectedBayesNet.print("Removed top: "); - if(debug) orphans.print("Orphans: "); - - // FactorGraph factors(affectedBayesNet); - // bug was here: we cannot reuse the original factors, because then the cached factors get messed up - // [all the necessary data is actually contained in the affectedBayesNet, including what was passed in from the boundaries, - // so this would be correct; however, in the process we also generate new cached_ entries that will be wrong (ie. they don't - // contain what would be passed up at a certain point if batch elimination was done, but that's what we need); we could choose - // not to update cached_ from here, but then the new information (and potentially different variable ordering) is not reflected - // in the cached_ values which again will be wrong] - // so instead we have to retrieve the original linearized factors AND add the cached factors from the boundary - - // BEGIN OF COPIED CODE - - // ordering provides all keys in conditionals, there cannot be others because path to root included - tic(2,"affectedKeys"); - FastList affectedKeys = affectedBayesNet.ordering(); - toc(2,"affectedKeys"); - - if(affectedKeys.size() >= theta_.size() * batchThreshold) { - - tic(3,"batch"); - - tic(0,"add keys"); - boost::shared_ptr > affectedKeysSet(new FastSet()); - BOOST_FOREACH(const Ordering::value_type& key_index, ordering_) { affectedKeysSet->insert(key_index.second); } - toc(0,"add keys"); - - tic(1,"reorder"); - tic(1,"CCOLAMD"); - // Do a batch step - reorder and relinearize all variables - vector cmember(theta_.size(), 0); - FastSet constrainedKeysSet; - if(constrainKeys) - constrainedKeysSet = *constrainKeys; - else - constrainedKeysSet.insert(newKeys.begin(), newKeys.end()); - if(theta_.size() > constrainedKeysSet.size()) { - BOOST_FOREACH(Index var, constrainedKeysSet) { cmember[var] = 1; } - } - Permutation::shared_ptr colamd(inference::PermutationCOLAMD_(variableIndex_, cmember)); - Permutation::shared_ptr colamdInverse(colamd->inverse()); - toc(1,"CCOLAMD"); - - // Reorder - tic(2,"permute global variable index"); - variableIndex_.permute(*colamd); - toc(2,"permute global variable index"); - tic(3,"permute delta"); - delta_.permute(*colamd); - toc(3,"permute delta"); - tic(4,"permute ordering"); - ordering_.permuteWithInverse(*colamdInverse); - toc(4,"permute ordering"); - toc(1,"reorder"); - - tic(2,"linearize"); - GaussianFactorGraph factors(*nonlinearFactors_.linearize(theta_, ordering_)); - toc(2,"linearize"); - - tic(5,"eliminate"); - JunctionTree jt(factors, variableIndex_); - sharedClique newRoot = jt.eliminate(EliminatePreferLDL); - if(debug) newRoot->print("Eliminated: "); - toc(5,"eliminate"); - - tic(6,"insert"); - this->clear(); - this->insert(newRoot); - toc(6,"insert"); - - toc(3,"batch"); - - result.variablesReeliminated = affectedKeysSet->size(); - - lastAffectedMarkedCount = markedKeys.size(); - lastAffectedVariableCount = affectedKeysSet->size(); - lastAffectedFactorCount = factors.size(); - - return affectedKeysSet; - - } else { - - tic(4,"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(), newKeys.begin(), newKeys.end()); - tic(1,"relinearizeAffected"); - GaussianFactorGraph factors(*relinearizeAffectedFactors(affectedAndNewKeys)); - if(debug) factors.print("Relinearized factors: "); - toc(1,"relinearizeAffected"); - - if(debug) { cout << "Affected keys: "; BOOST_FOREACH(const Index key, affectedKeys) { cout << key << " "; } cout << endl; } - - result.variablesReeliminated = affectedAndNewKeys.size(); - lastAffectedMarkedCount = markedKeys.size(); - lastAffectedVariableCount = affectedKeys.size(); - lastAffectedFactorCount = factors.size(); - -#ifdef PRINT_STATS - // output for generating figures - cout << "linear: #markedKeys: " << markedKeys.size() << " #affectedVariables: " << affectedKeys.size() - << " #affectedFactors: " << factors.size() << " maxCliqueSize: " << maxClique - << " avgCliqueSize: " << avgClique << " #Cliques: " << numCliques << " nnzR: " << nnzR << endl; -#endif - - tic(2,"cached"); - // add the cached intermediate results from the boundary of the orphans ... - FactorGraph cachedBoundary = getCachedBoundaryFactors(orphans); - if(debug) cachedBoundary.print("Boundary factors: "); - factors.reserve(factors.size() + cachedBoundary.size()); - // Copy so that we can later permute factors - BOOST_FOREACH(const CacheFactor::shared_ptr& cached, cachedBoundary) { - factors.push_back(GaussianFactor::shared_ptr(new CacheFactor(*cached))); - } - toc(2,"cached"); - - // 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]) - - tic(4,"reorder and eliminate"); - - tic(1,"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 - boost::shared_ptr > affectedKeysSet(new FastSet(markedKeys)); - affectedKeysSet->insert(affectedKeys.begin(), affectedKeys.end()); - toc(1,"list to set"); - - tic(2,"PartialSolve"); - typename Impl::ReorderingMode reorderingMode; - reorderingMode.nFullSystemVars = ordering_.nVars(); - reorderingMode.algorithm = Impl::ReorderingMode::COLAMD; - reorderingMode.constrain = Impl::ReorderingMode::CONSTRAIN_LAST; - if(constrainKeys) - reorderingMode.constrainedKeys = *constrainKeys; - else - reorderingMode.constrainedKeys = FastSet(newKeys.begin(), newKeys.end()); - typename Impl::PartialSolveResult partialSolveResult = - Impl::PartialSolve(factors, *affectedKeysSet, reorderingMode); - toc(2,"PartialSolve"); - - // We now need to permute everything according this partial reordering: the - // delta vector, the global ordering, and the factors we're about to - // re-eliminate. The reordered variables are also mentioned in the - // orphans and the leftover cached factors. - tic(3,"permute global variable index"); - variableIndex_.permute(partialSolveResult.fullReordering); - toc(3,"permute global variable index"); - tic(4,"permute delta"); - delta_.permute(partialSolveResult.fullReordering); - toc(4,"permute delta"); - tic(5,"permute ordering"); - ordering_.permuteWithInverse(partialSolveResult.fullReorderingInverse); - toc(5,"permute ordering"); - - toc(4,"reorder and eliminate"); - - tic(6,"re-assemble"); - if(partialSolveResult.bayesTree) { - assert(!this->root_); - this->insert(partialSolveResult.bayesTree); - } - toc(6,"re-assemble"); - - // 4. Insert the orphans back into the new Bayes tree. - tic(7,"orphans"); - tic(1,"permute"); - BOOST_FOREACH(sharedClique orphan, orphans) { - (void)orphan->permuteSeparatorWithInverse(partialSolveResult.fullReorderingInverse); - } - toc(1,"permute"); - tic(2,"insert"); - // add orphans to the bottom of the new tree - BOOST_FOREACH(sharedClique orphan, orphans) { - // Because the affectedKeysSelector is sorted, the orphan separator keys - // will be sorted correctly according to the new elimination order after - // applying the permutation, so findParentClique, which looks for the - // lowest-ordered parent, will still work. - Index parentRepresentative = Base::findParentClique((*orphan)->parents()); - sharedClique parent = (*this)[parentRepresentative]; - parent->children_ += orphan; - orphan->parent_ = parent; // set new parent! - } - toc(2,"insert"); - toc(7,"orphans"); - - toc(4,"incremental"); - - return affectedKeysSet; +/* ************************************************************************* */ +template +void nnz_internal(const boost::shared_ptr& clique, int& result) { + int dimR = (*clique)->dim(); + int dimSep = (*clique)->get_S().cols() - dimR; + result += ((dimR+1)*dimR)/2 + dimSep*dimR; + // traverse the children + BOOST_FOREACH(const typename CLIQUE::shared_ptr& child, clique->children_) { + nnz_internal(child, result); } } /* ************************************************************************* */ -template -ISAM2Result ISAM2::update( - const GRAPH& newFactors, const Values& newTheta, const FastVector& removeFactorIndices, - const boost::optional >& constrainedKeys, bool force_relinearize) { - - static const bool debug = ISDEBUG("ISAM2 update"); - static const bool verbose = ISDEBUG("ISAM2 update verbose"); - - static int count = 0; - count++; - - lastAffectedVariableCount = 0; - lastAffectedFactorCount = 0; - lastAffectedCliqueCount = 0; - lastAffectedMarkedCount = 0; - lastBacksubVariableCount = 0; - lastNnzTop = 0; - ISAM2Result result; - const bool relinearizeThisStep = force_relinearize || (params_.enableRelinearization && count % params_.relinearizeSkip == 0); - - if(verbose) { - cout << "ISAM2::update\n"; - this->print("ISAM2: "); - } - - // Update delta if we need it to check relinearization later - if(relinearizeThisStep) { - tic(0, "updateDelta"); - updateDelta(disableReordering); - toc(0, "updateDelta"); - } - - tic(1,"push_back factors"); - // Add the new factor indices to the result struct - result.newFactorsIndices.resize(newFactors.size()); - for(size_t i=0; i markedKeys = Impl::IndicesFromFactors(ordering_, newFactors); // Get keys from new factors - // Also mark keys involved in removed factors - { - FastSet markedRemoveKeys = Impl::IndicesFromFactors(ordering_, removeFactors); // Get keys involved in removed factors - markedKeys.insert(markedRemoveKeys.begin(), markedRemoveKeys.end()); // Add to the overall set of marked keys - } - // 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, Index value) instead of the iterator constructor. - FastVector newKeys; newKeys.assign(markedKeys.begin(), markedKeys.end()); // Make a copy of these, as we'll soon add to them - toc(4,"gather involved keys"); - - // Check relinearization if we're at the nth step, or we are using a looser loop relin threshold - if (relinearizeThisStep) { - tic(5,"gather relinearize keys"); - vector markedRelinMask(ordering_.nVars(), false); - // 4. Mark keys in \Delta above threshold \beta: J=\{\Delta_{j}\in\Delta|\Delta_{j}\geq\beta\}. - FastSet relinKeys = Impl::CheckRelinearization(delta_, ordering_, params_.relinearizeThreshold); - if(disableReordering) relinKeys = Impl::CheckRelinearization(delta_, ordering_, 0.0); // This is used for debugging - - // Add the variables being relinearized to the marked keys - BOOST_FOREACH(const Index j, relinKeys) { markedRelinMask[j] = true; } - markedKeys.insert(relinKeys.begin(), relinKeys.end()); - toc(5,"gather relinearize keys"); - - tic(6,"fluid find_all"); - // 5. Mark all cliques that involve marked variables \Theta_{J} and all their ancestors. - if (!relinKeys.empty() && this->root()) - Impl::FindAll(this->root(), markedKeys, markedRelinMask); // add other cliques that have the marked ones in the separator - toc(6,"fluid find_all"); - - tic(7,"expmap"); - // 6. Update linearization point for marked variables: \Theta_{J}:=\Theta_{J}+\Delta_{J}. - if (!relinKeys.empty()) - Impl::ExpmapMasked(theta_, delta_, ordering_, markedRelinMask, delta_); - toc(7,"expmap"); - - result.variablesRelinearized = markedKeys.size(); - -#ifndef NDEBUG - lastRelinVariables_ = markedRelinMask; -#endif - } else { - result.variablesRelinearized = 0; -#ifndef NDEBUG - lastRelinVariables_ = vector(ordering_.nVars(), false); -#endif - } - - tic(8,"linearize new"); - tic(1,"linearize"); - // 7. Linearize new factors - FactorGraph::shared_ptr linearFactors = newFactors.linearize(theta_, ordering_); - toc(1,"linearize"); - - tic(2,"augment VI"); - // Augment the variable index with the new factors - variableIndex_.augment(*linearFactors); - toc(2,"augment VI"); - toc(8,"linearize new"); - - tic(9,"recalculate"); - // 8. Redo top of Bayes tree - // Convert constrained symbols to indices - boost::optional > constrainedIndices; - if(constrainedKeys) { - constrainedIndices.reset(FastSet()); - BOOST_FOREACH(Key key, *constrainedKeys) { - constrainedIndices->insert(ordering_[key]); - } - } - boost::shared_ptr > replacedKeys; - if(!markedKeys.empty() || !newKeys.empty()) - replacedKeys = recalculate(markedKeys, newKeys, linearFactors, constrainedIndices, result); - - // Update replaced keys mask (accumulates until back-substitution takes place) - if(replacedKeys) { - BOOST_FOREACH(const Index var, *replacedKeys) { - deltaReplacedMask_[var] = true; } } - toc(9,"recalculate"); - - //tic(9,"solve"); - // 9. Solve - if(debug) delta_.print("delta_: "); - //toc(9,"solve"); - - tic(10,"evaluate error after"); - if(params_.evaluateNonlinearError) - result.errorAfter.reset(nonlinearFactors_.error(calculateEstimate())); - toc(10,"evaluate error after"); - - result.cliques = this->nodes().size(); - deltaUptodate_ = false; - +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; } -/* ************************************************************************* */ -template -void ISAM2::updateDelta(bool forceFullSolve) const { - - 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; - tic(0, "Wildfire update"); - lastBacksubVariableCount = Impl::UpdateDelta(this->root(), deltaReplacedMask_, delta_, effectiveWildfireThreshold); - toc(0, "Wildfire update"); - - } else if(params_.optimizationParams.type() == typeid(ISAM2DoglegParams)) { - // If using Dogleg, do a Dogleg step - const ISAM2DoglegParams& doglegParams = - boost::get(params_.optimizationParams); - - // Do one Dogleg iteration - tic(1, "Dogleg Iterate"); - DoglegOptimizerImpl::IterationResult doglegResult = DoglegOptimizerImpl::Iterate( - *doglegDelta_, doglegParams.adaptationMode, *this, nonlinearFactors_, theta_, ordering_, nonlinearFactors_.error(theta_), doglegParams.verbose); - toc(1, "Dogleg Iterate"); - - // Update Delta and linear step - doglegDelta_ = doglegResult.Delta; - delta_.permutation() = Permutation::Identity(delta_.size()); // Dogleg solves for the full delta so there is no permutation - delta_.container() = doglegResult.dx_d; // Copy the VectorValues containing with the linear solution - - // Clear replaced mask - deltaReplacedMask_.assign(deltaReplacedMask_.size(), false); - } - - deltaUptodate_ = true; } -/* ************************************************************************* */ -template -Values ISAM2::calculateEstimate() const { - // We use ExpmapMasked here instead of regular expmap because the former - // handles Permuted - Values ret(theta_); - vector mask(ordering_.nVars(), true); - Impl::ExpmapMasked(ret, getDelta(), ordering_, mask); - return ret; -} -/* ************************************************************************* */ -template -template -VALUE ISAM2::calculateEstimate(Key key) const { - const Index index = getOrdering()[key]; - const SubVector delta = getDelta()[index]; - return theta_.at(key).retract(delta); -} - -/* ************************************************************************* */ -template -Values ISAM2::calculateBestEstimate() const { - VectorValues delta(theta_.dims(ordering_)); - optimize2(this->root(), delta); - return theta_.retract(delta, ordering_); -} - -/* ************************************************************************* */ -template -const Permuted& ISAM2::getDelta() const { - if(!deltaUptodate_) - updateDelta(); - return delta_; -} - -} -/// namespace gtsam diff --git a/gtsam/nonlinear/ISAM2.cpp b/gtsam/nonlinear/ISAM2.cpp new file mode 100644 index 000000000..01d6bb2cf --- /dev/null +++ b/gtsam/nonlinear/ISAM2.cpp @@ -0,0 +1,687 @@ +/* ---------------------------------------------------------------------------- + + * 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) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file ISAM2-inl.h + * @brief Incremental update functionality (ISAM2) for BayesTree, with fluid relinearization. + * @author Michael Kaess, Richard Roberts + */ + +#include +#include // for operator += +using namespace boost::assign; + +#include +#include +#include +#include +#include + +#include +#include +#include + + +namespace gtsam { + +using namespace std; + +static const bool disableReordering = false; +static const double batchThreshold = 0.65; + +/* ************************************************************************* */ +ISAM2::ISAM2(const ISAM2Params& params): + delta_(Permutation(), deltaUnpermuted_), deltaUptodate_(true), params_(params) { + // See note in gtsam/base/boost_variant_with_workaround.h + if(params_.optimizationParams.type() == typeid(ISAM2DoglegParams)) + doglegDelta_ = boost::get(params_.optimizationParams).initialDelta; +} + +/* ************************************************************************* */ +ISAM2::ISAM2(): + delta_(Permutation(), deltaUnpermuted_), deltaUptodate_(true) { + // See note in gtsam/base/boost_variant_with_workaround.h + if(params_.optimizationParams.type() == typeid(ISAM2DoglegParams)) + doglegDelta_ = boost::get(params_.optimizationParams).initialDelta; +} + +/* ************************************************************************* */ +FastList ISAM2::getAffectedFactors(const FastList& keys) const { + static const bool debug = false; + if(debug) cout << "Getting affected factors for "; + if(debug) { BOOST_FOREACH(const Index key, keys) { cout << key << " "; } } + if(debug) cout << endl; + + FactorGraph allAffected; + FastList indices; + BOOST_FOREACH(const Index key, keys) { +// const list l = nonlinearFactors_.factors(key); +// indices.insert(indices.begin(), l.begin(), l.end()); + const VariableIndex::Factors& factors(variableIndex_[key]); + BOOST_FOREACH(size_t factor, factors) { + if(debug) cout << "Variable " << key << " affects factor " << factor << endl; + indices.push_back(factor); + } + } + indices.sort(); + indices.unique(); + if(debug) cout << "Affected factors are: "; + if(debug) { BOOST_FOREACH(const size_t index, indices) { cout << index << " "; } } + if(debug) cout << endl; + return indices; +} + +/* ************************************************************************* */ +// retrieve all factors that ONLY contain the affected variables +// (note that the remaining stuff is summarized in the cached factors) +FactorGraph::shared_ptr +ISAM2::relinearizeAffectedFactors(const FastList& affectedKeys) const { + + tic(1,"getAffectedFactors"); + FastList candidates = getAffectedFactors(affectedKeys); + toc(1,"getAffectedFactors"); + + NonlinearFactorGraph nonlinearAffectedFactors; + + tic(2,"affectedKeysSet"); + // for fast lookup below + FastSet affectedKeysSet; + affectedKeysSet.insert(affectedKeys.begin(), affectedKeys.end()); + toc(2,"affectedKeysSet"); + + tic(3,"check candidates"); + BOOST_FOREACH(size_t idx, candidates) { + bool inside = true; + BOOST_FOREACH(Key key, nonlinearFactors_[idx]->keys()) { + Index var = ordering_[key]; + if (affectedKeysSet.find(var) == affectedKeysSet.end()) { + inside = false; + break; + } + } + if (inside) + nonlinearAffectedFactors.push_back(nonlinearFactors_[idx]); + } + toc(3,"check candidates"); + + tic(4,"linearize"); + FactorGraph::shared_ptr linearized(nonlinearAffectedFactors.linearize(theta_, ordering_)); + toc(4,"linearize"); + return linearized; +} + +/* ************************************************************************* */ +// find intermediate (linearized) factors from cache that are passed into the affected area +FactorGraph +ISAM2::getCachedBoundaryFactors(Cliques& orphans) { + + static const bool debug = false; + + FactorGraph cachedBoundary; + + BOOST_FOREACH(sharedClique orphan, orphans) { + // find the last variable that was eliminated + Index key = (*orphan)->frontals().back(); +#ifndef NDEBUG +// typename BayesNet::const_iterator it = orphan->end(); +// const CONDITIONAL& lastCONDITIONAL = **(--it); +// typename CONDITIONAL::const_iterator keyit = lastCONDITIONAL.endParents(); +// const Index lastKey = *(--keyit); +// assert(key == lastKey); +#endif + // retrieve the cached factor and add to boundary + cachedBoundary.push_back(boost::dynamic_pointer_cast(orphan->cachedFactor())); + if(debug) { cout << "Cached factor for variable " << key; orphan->cachedFactor()->print(""); } + } + + return cachedBoundary; +} + +boost::shared_ptr > ISAM2::recalculate( + const FastSet& markedKeys, const FastVector& newKeys, const FactorGraph::shared_ptr newFactors, + const boost::optional >& constrainKeys, ISAM2Result& result) { + + // TODO: new factors are linearized twice, the newFactors passed in are not used. + + static const bool debug = ISDEBUG("ISAM2 recalculate"); + + // Input: BayesTree(this), newFactors + +//#define PRINT_STATS // figures for paper, disable for timing +#ifdef PRINT_STATS + static int counter = 0; + int maxClique = 0; + double avgClique = 0; + int numCliques = 0; + int nnzR = 0; + if (counter>0) { // cannot call on empty tree + GaussianISAM2_P::CliqueData cdata = this->getCliqueData(); + GaussianISAM2_P::CliqueStats cstats = cdata.getStats(); + maxClique = cstats.maxCONDITIONALSize; + avgClique = cstats.avgCONDITIONALSize; + numCliques = cdata.conditionalSizes.size(); + nnzR = calculate_nnz(this->root()); + } + counter++; +#endif + + if(debug) { + cout << "markedKeys: "; + BOOST_FOREACH(const Index key, markedKeys) { cout << key << " "; } + cout << endl; + cout << "newKeys: "; + BOOST_FOREACH(const Index key, newKeys) { 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. + tic(1, "removetop"); + Cliques orphans; + BayesNet affectedBayesNet; + this->removeTop(markedKeys, affectedBayesNet, orphans); + toc(1, "removetop"); + + if(debug) affectedBayesNet.print("Removed top: "); + if(debug) orphans.print("Orphans: "); + + // FactorGraph factors(affectedBayesNet); + // bug was here: we cannot reuse the original factors, because then the cached factors get messed up + // [all the necessary data is actually contained in the affectedBayesNet, including what was passed in from the boundaries, + // so this would be correct; however, in the process we also generate new cached_ entries that will be wrong (ie. they don't + // contain what would be passed up at a certain point if batch elimination was done, but that's what we need); we could choose + // not to update cached_ from here, but then the new information (and potentially different variable ordering) is not reflected + // in the cached_ values which again will be wrong] + // so instead we have to retrieve the original linearized factors AND add the cached factors from the boundary + + // BEGIN OF COPIED CODE + + // ordering provides all keys in conditionals, there cannot be others because path to root included + tic(2,"affectedKeys"); + FastList affectedKeys = affectedBayesNet.ordering(); + toc(2,"affectedKeys"); + + if(affectedKeys.size() >= theta_.size() * batchThreshold) { + + tic(3,"batch"); + + tic(0,"add keys"); + boost::shared_ptr > affectedKeysSet(new FastSet()); + BOOST_FOREACH(const Ordering::value_type& key_index, ordering_) { affectedKeysSet->insert(key_index.second); } + toc(0,"add keys"); + + tic(1,"reorder"); + tic(1,"CCOLAMD"); + // Do a batch step - reorder and relinearize all variables + vector cmember(theta_.size(), 0); + FastSet constrainedKeysSet; + if(constrainKeys) + constrainedKeysSet = *constrainKeys; + else + constrainedKeysSet.insert(newKeys.begin(), newKeys.end()); + if(theta_.size() > constrainedKeysSet.size()) { + BOOST_FOREACH(Index var, constrainedKeysSet) { cmember[var] = 1; } + } + Permutation::shared_ptr colamd(inference::PermutationCOLAMD_(variableIndex_, cmember)); + Permutation::shared_ptr colamdInverse(colamd->inverse()); + toc(1,"CCOLAMD"); + + // Reorder + tic(2,"permute global variable index"); + variableIndex_.permute(*colamd); + toc(2,"permute global variable index"); + tic(3,"permute delta"); + delta_.permute(*colamd); + toc(3,"permute delta"); + tic(4,"permute ordering"); + ordering_.permuteWithInverse(*colamdInverse); + toc(4,"permute ordering"); + toc(1,"reorder"); + + tic(2,"linearize"); + GaussianFactorGraph factors(*nonlinearFactors_.linearize(theta_, ordering_)); + toc(2,"linearize"); + + tic(5,"eliminate"); + JunctionTree jt(factors, variableIndex_); + sharedClique newRoot = jt.eliminate(EliminatePreferLDL); + if(debug) newRoot->print("Eliminated: "); + toc(5,"eliminate"); + + tic(6,"insert"); + this->clear(); + this->insert(newRoot); + toc(6,"insert"); + + toc(3,"batch"); + + result.variablesReeliminated = affectedKeysSet->size(); + + lastAffectedMarkedCount = markedKeys.size(); + lastAffectedVariableCount = affectedKeysSet->size(); + lastAffectedFactorCount = factors.size(); + + return affectedKeysSet; + + } else { + + tic(4,"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(), newKeys.begin(), newKeys.end()); + tic(1,"relinearizeAffected"); + GaussianFactorGraph factors(*relinearizeAffectedFactors(affectedAndNewKeys)); + if(debug) factors.print("Relinearized factors: "); + toc(1,"relinearizeAffected"); + + if(debug) { cout << "Affected keys: "; BOOST_FOREACH(const Index key, affectedKeys) { cout << key << " "; } cout << endl; } + + result.variablesReeliminated = affectedAndNewKeys.size(); + lastAffectedMarkedCount = markedKeys.size(); + lastAffectedVariableCount = affectedKeys.size(); + lastAffectedFactorCount = factors.size(); + +#ifdef PRINT_STATS + // output for generating figures + cout << "linear: #markedKeys: " << markedKeys.size() << " #affectedVariables: " << affectedKeys.size() + << " #affectedFactors: " << factors.size() << " maxCliqueSize: " << maxClique + << " avgCliqueSize: " << avgClique << " #Cliques: " << numCliques << " nnzR: " << nnzR << endl; +#endif + + tic(2,"cached"); + // add the cached intermediate results from the boundary of the orphans ... + FactorGraph cachedBoundary = getCachedBoundaryFactors(orphans); + if(debug) cachedBoundary.print("Boundary factors: "); + factors.reserve(factors.size() + cachedBoundary.size()); + // Copy so that we can later permute factors + BOOST_FOREACH(const CacheFactor::shared_ptr& cached, cachedBoundary) { + factors.push_back(GaussianFactor::shared_ptr(new CacheFactor(*cached))); + } + toc(2,"cached"); + + // 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]) + + tic(4,"reorder and eliminate"); + + tic(1,"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 + boost::shared_ptr > affectedKeysSet(new FastSet(markedKeys)); + affectedKeysSet->insert(affectedKeys.begin(), affectedKeys.end()); + toc(1,"list to set"); + + tic(2,"PartialSolve"); + typename Impl::ReorderingMode reorderingMode; + reorderingMode.nFullSystemVars = ordering_.nVars(); + reorderingMode.algorithm = Impl::ReorderingMode::COLAMD; + reorderingMode.constrain = Impl::ReorderingMode::CONSTRAIN_LAST; + if(constrainKeys) + reorderingMode.constrainedKeys = *constrainKeys; + else + reorderingMode.constrainedKeys = FastSet(newKeys.begin(), newKeys.end()); + typename Impl::PartialSolveResult partialSolveResult = + Impl::PartialSolve(factors, *affectedKeysSet, reorderingMode); + toc(2,"PartialSolve"); + + // We now need to permute everything according this partial reordering: the + // delta vector, the global ordering, and the factors we're about to + // re-eliminate. The reordered variables are also mentioned in the + // orphans and the leftover cached factors. + tic(3,"permute global variable index"); + variableIndex_.permute(partialSolveResult.fullReordering); + toc(3,"permute global variable index"); + tic(4,"permute delta"); + delta_.permute(partialSolveResult.fullReordering); + toc(4,"permute delta"); + tic(5,"permute ordering"); + ordering_.permuteWithInverse(partialSolveResult.fullReorderingInverse); + toc(5,"permute ordering"); + + toc(4,"reorder and eliminate"); + + tic(6,"re-assemble"); + if(partialSolveResult.bayesTree) { + assert(!this->root_); + this->insert(partialSolveResult.bayesTree); + } + toc(6,"re-assemble"); + + // 4. Insert the orphans back into the new Bayes tree. + tic(7,"orphans"); + tic(1,"permute"); + BOOST_FOREACH(sharedClique orphan, orphans) { + (void)orphan->permuteSeparatorWithInverse(partialSolveResult.fullReorderingInverse); + } + toc(1,"permute"); + tic(2,"insert"); + // add orphans to the bottom of the new tree + BOOST_FOREACH(sharedClique orphan, orphans) { + // Because the affectedKeysSelector is sorted, the orphan separator keys + // will be sorted correctly according to the new elimination order after + // applying the permutation, so findParentClique, which looks for the + // lowest-ordered parent, will still work. + Index parentRepresentative = Base::findParentClique((*orphan)->parents()); + sharedClique parent = (*this)[parentRepresentative]; + parent->children_ += orphan; + orphan->parent_ = parent; // set new parent! + } + toc(2,"insert"); + toc(7,"orphans"); + + toc(4,"incremental"); + + return affectedKeysSet; + } +} + +/* ************************************************************************* */ +ISAM2Result ISAM2::update( + const NonlinearFactorGraph& newFactors, const Values& newTheta, const FastVector& removeFactorIndices, + const boost::optional >& constrainedKeys, bool force_relinearize) { + + static const bool debug = ISDEBUG("ISAM2 update"); + static const bool verbose = ISDEBUG("ISAM2 update verbose"); + + static int count = 0; + count++; + + lastAffectedVariableCount = 0; + lastAffectedFactorCount = 0; + lastAffectedCliqueCount = 0; + lastAffectedMarkedCount = 0; + lastBacksubVariableCount = 0; + lastNnzTop = 0; + ISAM2Result result; + const bool relinearizeThisStep = force_relinearize || (params_.enableRelinearization && count % params_.relinearizeSkip == 0); + + if(verbose) { + cout << "ISAM2::update\n"; + this->print("ISAM2: "); + } + + // Update delta if we need it to check relinearization later + if(relinearizeThisStep) { + tic(0, "updateDelta"); + updateDelta(disableReordering); + toc(0, "updateDelta"); + } + + tic(1,"push_back factors"); + // Add the new factor indices to the result struct + result.newFactorsIndices.resize(newFactors.size()); + for(size_t i=0; i markedKeys = Impl::IndicesFromFactors(ordering_, newFactors); // Get keys from new factors + // Also mark keys involved in removed factors + { + FastSet markedRemoveKeys = Impl::IndicesFromFactors(ordering_, removeFactors); // Get keys involved in removed factors + markedKeys.insert(markedRemoveKeys.begin(), markedRemoveKeys.end()); // Add to the overall set of marked keys + } + // 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, Index value) instead of the iterator constructor. + FastVector newKeys; newKeys.assign(markedKeys.begin(), markedKeys.end()); // Make a copy of these, as we'll soon add to them + toc(4,"gather involved keys"); + + // Check relinearization if we're at the nth step, or we are using a looser loop relin threshold + if (relinearizeThisStep) { + tic(5,"gather relinearize keys"); + vector markedRelinMask(ordering_.nVars(), false); + // 4. Mark keys in \Delta above threshold \beta: J=\{\Delta_{j}\in\Delta|\Delta_{j}\geq\beta\}. + FastSet relinKeys = Impl::CheckRelinearization(delta_, ordering_, params_.relinearizeThreshold); + if(disableReordering) relinKeys = Impl::CheckRelinearization(delta_, ordering_, 0.0); // This is used for debugging + + // Add the variables being relinearized to the marked keys + BOOST_FOREACH(const Index j, relinKeys) { markedRelinMask[j] = true; } + markedKeys.insert(relinKeys.begin(), relinKeys.end()); + toc(5,"gather relinearize keys"); + + tic(6,"fluid find_all"); + // 5. Mark all cliques that involve marked variables \Theta_{J} and all their ancestors. + if (!relinKeys.empty() && this->root()) + Impl::FindAll(this->root(), markedKeys, markedRelinMask); // add other cliques that have the marked ones in the separator + toc(6,"fluid find_all"); + + tic(7,"expmap"); + // 6. Update linearization point for marked variables: \Theta_{J}:=\Theta_{J}+\Delta_{J}. + if (!relinKeys.empty()) + Impl::ExpmapMasked(theta_, delta_, ordering_, markedRelinMask, delta_); + toc(7,"expmap"); + + result.variablesRelinearized = markedKeys.size(); + +#ifndef NDEBUG + lastRelinVariables_ = markedRelinMask; +#endif + } else { + result.variablesRelinearized = 0; +#ifndef NDEBUG + lastRelinVariables_ = vector(ordering_.nVars(), false); +#endif + } + + tic(8,"linearize new"); + tic(1,"linearize"); + // 7. Linearize new factors + FactorGraph::shared_ptr linearFactors = newFactors.linearize(theta_, ordering_); + toc(1,"linearize"); + + tic(2,"augment VI"); + // Augment the variable index with the new factors + variableIndex_.augment(*linearFactors); + toc(2,"augment VI"); + toc(8,"linearize new"); + + tic(9,"recalculate"); + // 8. Redo top of Bayes tree + // Convert constrained symbols to indices + boost::optional > constrainedIndices; + if(constrainedKeys) { + constrainedIndices.reset(FastSet()); + BOOST_FOREACH(Key key, *constrainedKeys) { + constrainedIndices->insert(ordering_[key]); + } + } + boost::shared_ptr > replacedKeys; + if(!markedKeys.empty() || !newKeys.empty()) + replacedKeys = recalculate(markedKeys, newKeys, linearFactors, constrainedIndices, result); + + // Update replaced keys mask (accumulates until back-substitution takes place) + if(replacedKeys) { + BOOST_FOREACH(const Index var, *replacedKeys) { + deltaReplacedMask_[var] = true; } } + toc(9,"recalculate"); + + //tic(9,"solve"); + // 9. Solve + if(debug) delta_.print("delta_: "); + //toc(9,"solve"); + + tic(10,"evaluate error after"); + if(params_.evaluateNonlinearError) + result.errorAfter.reset(nonlinearFactors_.error(calculateEstimate())); + toc(10,"evaluate error after"); + + result.cliques = this->nodes().size(); + deltaUptodate_ = false; + + return result; +} + +/* ************************************************************************* */ +void ISAM2::updateDelta(bool forceFullSolve) const { + + 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; + tic(0, "Wildfire update"); + lastBacksubVariableCount = Impl::UpdateDelta(this->root(), deltaReplacedMask_, delta_, effectiveWildfireThreshold); + toc(0, "Wildfire update"); + + } else if(params_.optimizationParams.type() == typeid(ISAM2DoglegParams)) { + // If using Dogleg, do a Dogleg step + const ISAM2DoglegParams& doglegParams = + boost::get(params_.optimizationParams); + + // Do one Dogleg iteration + tic(1, "Dogleg Iterate"); + DoglegOptimizerImpl::IterationResult doglegResult = DoglegOptimizerImpl::Iterate( + *doglegDelta_, doglegParams.adaptationMode, *this, nonlinearFactors_, theta_, ordering_, nonlinearFactors_.error(theta_), doglegParams.verbose); + toc(1, "Dogleg Iterate"); + + // Update Delta and linear step + doglegDelta_ = doglegResult.Delta; + delta_.permutation() = Permutation::Identity(delta_.size()); // Dogleg solves for the full delta so there is no permutation + delta_.container() = doglegResult.dx_d; // Copy the VectorValues containing with the linear solution + + // Clear replaced mask + deltaReplacedMask_.assign(deltaReplacedMask_.size(), false); + } + + deltaUptodate_ = true; +} + +/* ************************************************************************* */ +Values ISAM2::calculateEstimate() const { + // We use ExpmapMasked here instead of regular expmap because the former + // handles Permuted + Values ret(theta_); + vector mask(ordering_.nVars(), true); + Impl::ExpmapMasked(ret, getDelta(), ordering_, mask); + return ret; +} + +/* ************************************************************************* */ +template +VALUE ISAM2::calculateEstimate(Key key) const { + const Index index = getOrdering()[key]; + const SubVector delta = getDelta()[index]; + return theta_.at(key).retract(delta); +} + +/* ************************************************************************* */ +Values ISAM2::calculateBestEstimate() const { + VectorValues delta(theta_.dims(ordering_)); + optimize2(this->root(), delta); + return theta_.retract(delta, ordering_); +} + +/* ************************************************************************* */ +const Permuted& ISAM2::getDelta() const { + if(!deltaUptodate_) + updateDelta(); + return delta_; +} + +/* ************************************************************************* */ +VectorValues optimizeGradientSearch(const ISAM2& isam) { + tic(0, "Allocate VectorValues"); + VectorValues grad = *allocateVectorValues(isam); + toc(0, "Allocate VectorValues"); + + optimizeGradientSearchInPlace(isam, grad); + + return grad; +} + +/* ************************************************************************* */ +void optimizeGradientSearchInPlace(const ISAM2& Rd, VectorValues& grad) { + tic(1, "Compute Gradient"); + // Compute gradient (call gradientAtZero function, which is defined for various linear systems) + gradientAtZero(Rd, grad); + double gradientSqNorm = grad.dot(grad); + toc(1, "Compute Gradient"); + + tic(2, "Compute R*g"); + // Compute R * g + FactorGraph Rd_jfg(Rd); + Errors Rg = Rd_jfg * grad; + toc(2, "Compute R*g"); + + tic(3, "Compute minimizing step size"); + // Compute minimizing step size + double step = -gradientSqNorm / dot(Rg, Rg); + toc(3, "Compute minimizing step size"); + + tic(4, "Compute point"); + // Compute steepest descent point + scal(step, grad); + toc(4, "Compute point"); +} + +/* ************************************************************************* */ +VectorValues gradient(const BayesTree& bayesTree, const VectorValues& x0) { + return gradient(FactorGraph(bayesTree), x0); +} + +/* ************************************************************************* */ +static void gradientAtZeroTreeAdder(const boost::shared_ptr& root, VectorValues& g) { + // Loop through variables in each clique, adding contributions + int variablePosition = 0; + for(GaussianConditional::const_iterator jit = root->conditional()->begin(); jit != root->conditional()->end(); ++jit) { + const int dim = root->conditional()->dim(jit); + g[*jit] += root->gradientContribution().segment(variablePosition, dim); + variablePosition += dim; + } + + // Recursively add contributions from children + typedef boost::shared_ptr sharedClique; + BOOST_FOREACH(const sharedClique& child, root->children()) { + gradientAtZeroTreeAdder(child, g); + } +} + +/* ************************************************************************* */ +void gradientAtZero(const BayesTree& bayesTree, VectorValues& g) { + // Zero-out gradient + g.setZero(); + + // Sum up contributions for each clique + gradientAtZeroTreeAdder(bayesTree.root(), g); +} + +} +/// namespace gtsam diff --git a/gtsam/nonlinear/ISAM2.h b/gtsam/nonlinear/ISAM2.h index 208f0748f..d5a1cac52 100644 --- a/gtsam/nonlinear/ISAM2.h +++ b/gtsam/nonlinear/ISAM2.h @@ -12,7 +12,7 @@ /** * @file ISAM2.h * @brief Incremental update functionality (ISAM2) for BayesTree, with fluid relinearization. - * @author Michael Kaess + * @author Michael Kaess, Richard Roberts */ // \callgraph @@ -23,8 +23,6 @@ #include #include -// Workaround for boost < 1.47, see note in file -//#include #include namespace gtsam { @@ -181,14 +179,13 @@ struct ISAM2Result { FastVector newFactorsIndices; }; -template -struct ISAM2Clique : public BayesTreeCliqueBase, CONDITIONAL> { +struct ISAM2Clique : public BayesTreeCliqueBase { - typedef ISAM2Clique This; - typedef BayesTreeCliqueBase Base; + typedef ISAM2Clique This; + typedef BayesTreeCliqueBase Base; typedef boost::shared_ptr shared_ptr; typedef boost::weak_ptr weak_ptr; - typedef CONDITIONAL ConditionalType; + typedef GaussianConditional ConditionalType; typedef typename ConditionalType::shared_ptr sharedConditional; typename Base::FactorType::shared_ptr cachedFactor_; @@ -269,8 +266,7 @@ private: * estimate of all variables. * */ -template -class ISAM2: public BayesTree > { +class ISAM2: public BayesTree { protected: @@ -316,7 +312,7 @@ protected: mutable std::vector deltaReplacedMask_; /** All original nonlinear factors are stored here to use during relinearization */ - GRAPH nonlinearFactors_; + NonlinearFactorGraph nonlinearFactors_; /** The current elimination ordering Symbols to Index (integer) keys. * @@ -339,9 +335,7 @@ private: public: - typedef BayesTree > Base; ///< The BayesTree base class - typedef ISAM2 This; ///< This class - typedef GRAPH Graph; + typedef BayesTree Base; ///< The BayesTree base class /** Create an empty ISAM2 instance */ ISAM2(const ISAM2Params& params); @@ -353,7 +347,7 @@ public: typedef typename Base::sharedClique sharedClique; ///< Shared pointer to a clique typedef typename Base::Cliques Cliques; ///< List of Clique typedef from base class - void cloneTo(boost::shared_ptr& newISAM2) const { + void cloneTo(boost::shared_ptr& newISAM2) const { boost::shared_ptr bayesTree = boost::static_pointer_cast(newISAM2); Base::cloneTo(bayesTree); newISAM2->theta_ = theta_; @@ -395,7 +389,7 @@ public: * (Params::relinearizeSkip). * @return An ISAM2Result struct containing information about the update */ - ISAM2Result update(const GRAPH& newFactors = GRAPH(), const Values& newTheta = Values(), + ISAM2Result update(const NonlinearFactorGraph& newFactors = NonlinearFactorGraph(), const Values& newTheta = Values(), const FastVector& removeFactorIndices = FastVector(), const boost::optional >& constrainedKeys = boost::none, bool force_relinearize = false); @@ -432,7 +426,7 @@ public: const Permuted& getDelta() const; /** Access the set of nonlinear factors */ - const GRAPH& getFactorsUnsafe() const { return nonlinearFactors_; } + const NonlinearFactorGraph& getFactorsUnsafe() const { return nonlinearFactors_; } /** Access the current ordering */ const Ordering& getOrdering() const { return ordering_; } @@ -462,6 +456,88 @@ private: }; // ISAM2 +/** Get the linear delta for the ISAM2 object, unpermuted the delta returned by ISAM2::getDelta() */ +VectorValues optimize(const ISAM2& isam) { + VectorValues delta = *allocateVectorValues(isam); + internal::optimizeInPlace(isam.root(), delta); + return delta; +} + +/// 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 +int optimizeWildfire(const boost::shared_ptr& root, + double threshold, const std::vector& replaced, Permuted& delta); + +/** + * Optimize along the gradient direction, with a closed-form computation to + * perform the line search. The gradient is computed about \f$ \delta x=0 \f$. + * + * This function returns \f$ \delta x \f$ that minimizes a reparametrized + * problem. The error function of a GaussianBayesNet is + * + * \f[ f(\delta x) = \frac{1}{2} |R \delta x - d|^2 = \frac{1}{2}d^T d - d^T R \delta x + \frac{1}{2} \delta x^T R^T R \delta x \f] + * + * with gradient and Hessian + * + * \f[ g(\delta x) = R^T(R\delta x - d), \qquad G(\delta x) = R^T R. \f] + * + * This function performs the line search in the direction of the + * gradient evaluated at \f$ g = g(\delta x = 0) \f$ with step size + * \f$ \alpha \f$ that minimizes \f$ f(\delta x = \alpha g) \f$: + * + * \f[ f(\alpha) = \frac{1}{2} d^T d + g^T \delta x + \frac{1}{2} \alpha^2 g^T G g \f] + * + * Optimizing by setting the derivative to zero yields + * \f$ \hat \alpha = (-g^T g) / (g^T G g) \f$. For efficiency, this function + * evaluates the denominator without computing the Hessian \f$ G \f$, returning + * + * \f[ \delta x = \hat\alpha g = \frac{-g^T g}{(R g)^T(R g)} \f] + */ +VectorValues optimizeGradientSearch(const ISAM2& isam); + +/** In-place version of optimizeGradientSearch requiring pre-allocated VectorValues \c x */ +void optimizeGradientSearchInPlace(const ISAM2& isam, VectorValues& grad); + +/// 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); + +/** + * Compute the gradient of the energy function, + * \f$ \nabla_{x=x_0} \left\Vert \Sigma^{-1} R x - d \right\Vert^2 \f$, + * centered around \f$ x = x_0 \f$. + * The gradient is \f$ R^T(Rx-d) \f$. + * This specialized version is used with ISAM2, where each clique stores its + * gradient contribution. + * @param bayesTree The Gaussian Bayes Tree $(R,d)$ + * @param x0 The center about which to compute the gradient + * @return The gradient as a VectorValues + */ +VectorValues gradient(const BayesTree& bayesTree, const VectorValues& x0); + +/** + * Compute the gradient of the energy function, + * \f$ \nabla_{x=0} \left\Vert \Sigma^{-1} R x - d \right\Vert^2 \f$, + * centered around zero. + * The gradient about zero is \f$ -R^T d \f$. See also gradient(const GaussianBayesNet&, const VectorValues&). + * This specialized version is used with ISAM2, where each clique stores its + * gradient contribution. + * @param bayesTree The Gaussian Bayes Tree $(R,d)$ + * @param [output] g A VectorValues to store the gradient, which must be preallocated, see allocateVectorValues + * @return The gradient as a VectorValues + */ +void gradientAtZero(const BayesTree& bayesTree, VectorValues& g); + } /// namespace gtsam #include