/* ---------------------------------------------------------------------------- * 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 */ #pragma once #include #include namespace gtsam { struct ISAM2::Impl { struct PartialSolveResult { 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, VectorValues& delta, VectorValues& deltaNewton, VectorValues& deltaGradSearch, std::vector& replacedKeys, Ordering& ordering, 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 NonlinearFactorGraph& 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 CheckRelinearizationFull(const VectorValues& delta, const Ordering& ordering, const ISAM2Params::RelinearizationThreshold& relinearizeThreshold, const KeyFormatter& keyFormatter = DefaultKeyFormatter); /** * Find the set of variables to be relinearized according to relinearizeThreshold. * This check is performed recursively, starting at the top of the tree. Once a * variable in the tree does not need to be relinearized, no further checks in * that branch are performed. This is an approximation of the Full version, designed * to save time at the expense of accuracy. * @param delta The linear delta to check against the threshold * @param keyFormatter Formatter for printing nonlinear keys during debugging * @return The set of variable indices in delta whose magnitude is greater than or * equal to relinearizeThreshold */ static FastSet CheckRelinearizationPartial(const ISAM2Clique::shared_ptr& root, const VectorValues& 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(ISAM2Clique::shared_ptr clique, FastSet& keys, const std::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 VectorValues& delta, const Ordering& ordering, const std::vector& mask, boost::optional invalidateIfDebug = boost::none, 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. * \param useQR Whether to use QR (if true), or Cholesky (if false). * \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, bool useQR); static size_t UpdateDelta(const boost::shared_ptr& root, std::vector& replacedKeys, VectorValues& delta, double wildfireThreshold); static size_t UpdateDoglegDeltas(const ISAM2& isam, double wildfireThreshold, std::vector& replacedKeys, VectorValues& deltaNewton, VectorValues& RgProd); }; }