diff --git a/gtsam/nonlinear/ISAM2-inl.h b/gtsam/nonlinear/ISAM2-inl.h deleted file mode 100644 index fb63dc428..000000000 --- a/gtsam/nonlinear/ISAM2-inl.h +++ /dev/null @@ -1,34 +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 ISAM2-inl.h - * @brief - * @author Richard Roberts - * @date Mar 16, 2012 - */ - -#pragma once - -#include - -namespace gtsam { - -/* ************************************************************************* */ -template -VALUE ISAM2::calculateEstimate(Key key) const { - const Vector& delta = getDelta()[key]; - return traits::Retract(theta_.at(key), delta); -} - -/* ************************************************************************* */ - -} // namespace gtsam diff --git a/gtsam/nonlinear/ISAM2.cpp b/gtsam/nonlinear/ISAM2.cpp index f2b154cc4..e543508b2 100644 --- a/gtsam/nonlinear/ISAM2.cpp +++ b/gtsam/nonlinear/ISAM2.cpp @@ -10,22 +10,21 @@ * -------------------------------------------------------------------------- */ /** - * @file ISAM2-inl.h + * @file ISAM2.cpp * @brief Incremental update functionality (ISAM2) for BayesTree, with fluid * relinearization. - * @author Michael Kaess, Richard Roberts + * @author Michael Kaess, Richard Roberts, Frank Dellaert */ #include -#include -#include -#include -#include +#include +#include +#include +#include // We need the inst file because we'll make a special JT templated on ISAM2 +#include +#include -#include // for operator += -using namespace boost::assign; -#include #include #include namespace br { @@ -33,30 +32,19 @@ using namespace boost::range; using namespace boost::adaptors; } // namespace br -#include -#include -#include -#include -#include // We need the inst file because we'll make a special JT templated on ISAM2 -#include -#include -#include -#include - -#include -#include -#include +#include +#include +#include using namespace std; namespace gtsam { -// Instantiate base classes -template class BayesTreeCliqueBase; +// Instantiate base class template class BayesTree; -static const bool disableReordering = false; -static const double batchThreshold = 0.65; +static const bool kDisableReordering = false; +static const double kBatchThreshold = 0.65; /* ************************************************************************* */ // Special BayesTree class that uses ISAM2 cliques - this is the result of @@ -80,345 +68,10 @@ class ISAM2JunctionTree typedef ISAM2JunctionTree This; typedef boost::shared_ptr shared_ptr; - ISAM2JunctionTree(const GaussianEliminationTree& eliminationTree) + explicit ISAM2JunctionTree(const GaussianEliminationTree& eliminationTree) : Base(eliminationTree) {} }; -/* ************************************************************************* */ -string ISAM2DoglegParams::adaptationModeTranslator( - const DoglegOptimizerImpl::TrustRegionAdaptationMode& adaptationMode) - const { - string s; - switch (adaptationMode) { - case DoglegOptimizerImpl::SEARCH_EACH_ITERATION: - s = "SEARCH_EACH_ITERATION"; - break; - case DoglegOptimizerImpl::ONE_STEP_PER_ITERATION: - s = "ONE_STEP_PER_ITERATION"; - break; - default: - s = "UNDEFINED"; - break; - } - return s; -} - -/* ************************************************************************* */ -DoglegOptimizerImpl::TrustRegionAdaptationMode -ISAM2DoglegParams::adaptationModeTranslator( - const string& adaptationMode) const { - string s = adaptationMode; - boost::algorithm::to_upper(s); - if (s == "SEARCH_EACH_ITERATION") - return DoglegOptimizerImpl::SEARCH_EACH_ITERATION; - if (s == "ONE_STEP_PER_ITERATION") - return DoglegOptimizerImpl::ONE_STEP_PER_ITERATION; - - /* default is SEARCH_EACH_ITERATION */ - return DoglegOptimizerImpl::SEARCH_EACH_ITERATION; -} - -/* ************************************************************************* */ -ISAM2Params::Factorization ISAM2Params::factorizationTranslator( - const string& str) { - string s = str; - boost::algorithm::to_upper(s); - if (s == "QR") return ISAM2Params::QR; - if (s == "CHOLESKY") return ISAM2Params::CHOLESKY; - - /* default is CHOLESKY */ - return ISAM2Params::CHOLESKY; -} - -/* ************************************************************************* */ -string ISAM2Params::factorizationTranslator( - const ISAM2Params::Factorization& value) { - string s; - switch (value) { - case ISAM2Params::QR: - s = "QR"; - break; - case ISAM2Params::CHOLESKY: - s = "CHOLESKY"; - break; - default: - s = "UNDEFINED"; - break; - } - return s; -} - -/* ************************************************************************* */ -void ISAM2Clique::setEliminationResult( - const FactorGraphType::EliminationResult& eliminationResult) { - conditional_ = eliminationResult.first; - cachedFactor_ = eliminationResult.second; - // Compute gradient contribution - gradientContribution_.resize(conditional_->cols() - 1); - // Rewrite -(R * P')'*d as -(d' * R * P')' for computational speed - // reasons - gradientContribution_ << -conditional_->get_R().transpose() * - conditional_->get_d(), - -conditional_->get_S().transpose() * conditional_->get_d(); -} - -/* ************************************************************************* */ -bool ISAM2Clique::equals(const This& other, double tol) const { - return Base::equals(other) && - ((!cachedFactor_ && !other.cachedFactor_) || - (cachedFactor_ && other.cachedFactor_ && - cachedFactor_->equals(*other.cachedFactor_, tol))); -} - -/* ************************************************************************* */ -void ISAM2Clique::print(const string& s, const KeyFormatter& formatter) const { - Base::print(s, formatter); - if (cachedFactor_) - cachedFactor_->print(s + "Cached: ", formatter); - else - cout << s << "Cached empty" << endl; - if (gradientContribution_.rows() != 0) - gtsam::print(gradientContribution_, "Gradient contribution: "); -} - -/* ************************************************************************* */ -bool ISAM2Clique::isDirty(const KeySet& replaced, const KeySet& changed) const { - // if none of the variables in this clique (frontal and separator!) changed - // significantly, then by the running intersection property, none of the - // cliques in the children need to be processed - - // Are any clique variables part of the tree that has been redone? - bool dirty = replaced.exists(conditional_->frontals().front()); -#if !defined(NDEBUG) && defined(GTSAM_EXTRA_CONSISTENCY_CHECKS) - for (Key frontal : conditional_->frontals()) { - assert(dirty == replaced.exists(frontal)); - } -#endif - - // If not, then has one of the separator variables changed significantly? - if (!dirty) { - for (Key parent : conditional_->parents()) { - if (changed.exists(parent)) { - dirty = true; - break; - } - } - } - return dirty; -} - -/* ************************************************************************* */ -/** - * Back-substitute - special version stores solution pointers in cliques for - * fast access. - */ -void ISAM2Clique::fastBackSubstitute(VectorValues* delta) const { -#ifdef USE_BROKEN_FAST_BACKSUBSTITUTE - // TODO(gareth): This code shares a lot of logic w/ linearAlgorithms-inst, - // potentially refactor - - // Create solution part pointers if necessary and possible - necessary if - // solnPointers_ is empty, and possible if either we're a root, or we have - // a parent with valid solnPointers_. - ISAM2::sharedClique parent = parent_.lock(); - if (solnPointers_.empty() && (isRoot() || !parent->solnPointers_.empty())) { - for (Key frontal : conditional_->frontals()) - solnPointers_.emplace(frontal, delta->find(frontal)); - for (Key parentKey : conditional_->parents()) { - assert(parent->solnPointers_.exists(parentKey)); - solnPointers_.emplace(parentKey, parent->solnPointers_.at(parentKey)); - } - } - - // See if we can use solution part pointers - we can if they either - // already existed or were created above. - if (!solnPointers_.empty()) { - GaussianConditional& c = *conditional_; - // Solve matrix - Vector xS; - { - // Count dimensions of vector - DenseIndex dim = 0; - FastVector parentPointers; - parentPointers.reserve(conditional_->nrParents()); - for (Key parent : conditional_->parents()) { - parentPointers.push_back(solnPointers_.at(parent)); - dim += parentPointers.back()->second.size(); - } - - // Fill parent vector - xS.resize(dim); - DenseIndex vectorPos = 0; - for (const VectorValues::const_iterator& parentPointer : parentPointers) { - const Vector& parentVector = parentPointer->second; - xS.block(vectorPos, 0, parentVector.size(), 1) = - parentVector.block(0, 0, parentVector.size(), 1); - vectorPos += parentVector.size(); - } - } - - // NOTE(gareth): We can no longer write: xS = b - S * xS - // This is because Eigen (as of 3.3) no longer evaluates S * xS into - // a temporary, and the operation trashes valus in xS. - // See: http://eigen.tuxfamily.org/index.php?title=3.3 - const Vector rhs = c.getb() - c.get_S() * xS; - const Vector solution = c.get_R().triangularView().solve(rhs); - - // Check for indeterminant solution - if (solution.hasNaN()) - throw IndeterminantLinearSystemException(c.keys().front()); - - // Insert solution into a VectorValues - DenseIndex vectorPosition = 0; - for (GaussianConditional::const_iterator frontal = c.beginFrontals(); - frontal != c.endFrontals(); ++frontal) { - solnPointers_.at(*frontal)->second = - solution.segment(vectorPosition, c.getDim(frontal)); - vectorPosition += c.getDim(frontal); - } - } else { - // Just call plain solve because we couldn't use solution pointers. - delta->update(conditional_->solve(*delta)); - } -#else - delta->update(conditional_->solve(*delta)); -#endif -} - -/* ************************************************************************* */ -bool ISAM2Clique::valuesChanged(const KeySet& replaced, - const Vector& originalValues, - const VectorValues& delta, - double threshold) const { - auto frontals = conditional_->frontals(); - if (replaced.exists(frontals.front())) return true; - auto diff = originalValues - delta.vector(frontals); - return diff.lpNorm() >= threshold; -} - -/* ************************************************************************* */ -/// Set changed flag for each frontal variable -void ISAM2Clique::markFrontalsAsChanged(KeySet* changed) const { - for (Key frontal : conditional_->frontals()) { - changed->insert(frontal); - } -} - -/* ************************************************************************* */ -void ISAM2Clique::restoreFromOriginals(const Vector& originalValues, - VectorValues* delta) const { - size_t pos = 0; - for (Key frontal : conditional_->frontals()) { - auto v = delta->at(frontal); - v = originalValues.segment(pos, v.size()); - pos += v.size(); - } -} - -/* ************************************************************************* */ -// Note: not being used right now in favor of non-recursive version below. -void ISAM2Clique::optimizeWildfire(const KeySet& replaced, double threshold, - KeySet* changed, VectorValues* delta, - size_t* count) const { - if (isDirty(replaced, *changed)) { - // Temporary copy of the original values, to check how much they change - auto originalValues = delta->vector(conditional_->frontals()); - - // Back-substitute - fastBackSubstitute(delta); - count += conditional_->nrFrontals(); - - if (valuesChanged(replaced, originalValues, *delta, threshold)) { - markFrontalsAsChanged(changed); - } else { - restoreFromOriginals(originalValues, delta); - } - - // Recurse to children - for (const auto& child : children) { - child->optimizeWildfire(replaced, threshold, changed, delta, count); - } - } -} - -size_t optimizeWildfire(const ISAM2::sharedClique& root, double threshold, - const KeySet& keys, VectorValues* delta) { - KeySet changed; - size_t count = 0; - // starting from the root, call optimize on each conditional - if (root) root->optimizeWildfire(keys, threshold, &changed, delta, &count); - return count; -} - -/* ************************************************************************* */ -bool ISAM2Clique::optimizeWildfireNode(const KeySet& replaced, double threshold, - KeySet* changed, VectorValues* delta, - size_t* count) const { - // TODO(gareth): This code shares a lot of logic w/ linearAlgorithms-inst, - // potentially refactor - bool dirty = isDirty(replaced, *changed); - if (dirty) { - // Temporary copy of the original values, to check how much they change - auto originalValues = delta->vector(conditional_->frontals()); - - // Back-substitute - fastBackSubstitute(delta); - count += conditional_->nrFrontals(); - - if (valuesChanged(replaced, originalValues, *delta, threshold)) { - markFrontalsAsChanged(changed); - } else { - restoreFromOriginals(originalValues, delta); - } - } - - return dirty; -} - -size_t optimizeWildfireNonRecursive(const ISAM2::sharedClique& root, - double threshold, const KeySet& keys, - VectorValues* delta) { - KeySet changed; - size_t count = 0; - - if (root) { - std::stack travStack; - travStack.push(root); - ISAM2::sharedClique currentNode = root; - while (!travStack.empty()) { - currentNode = travStack.top(); - travStack.pop(); - bool dirty = currentNode->optimizeWildfireNode(keys, threshold, &changed, - delta, &count); - if (dirty) { - for (const auto& child : currentNode->children) { - travStack.push(child); - } - } - } - } - - return count; -} - -/* ************************************************************************* */ -void ISAM2Clique::nnz_internal(size_t* result) const { - size_t dimR = conditional_->rows(); - size_t dimSep = conditional_->get_S().cols(); - *result += ((dimR + 1) * dimR) / 2 + dimSep * dimR; - // traverse the children - for (const auto& child : children) { - child->nnz_internal(result); - } -} - -/* ************************************************************************* */ -size_t ISAM2Clique::calculate_nnz() const { - size_t result = 0; - nnz_internal(&result); - return result; -} - /* ************************************************************************* */ ISAM2::ISAM2(const ISAM2Params& params) : params_(params), update_count_(0) { if (params_.optimizationParams.type() == typeid(ISAM2DoglegParams)) @@ -622,7 +275,7 @@ boost::shared_ptr ISAM2::recalculate( boost::shared_ptr affectedKeysSet( new KeySet()); // Will return this result - if (affectedKeys.size() >= theta_.size() * batchThreshold) { + if (affectedKeys.size() >= theta_.size() * kBatchThreshold) { // Do a batch step - reorder and relinearize all variables gttic(batch); @@ -867,7 +520,7 @@ ISAM2Result ISAM2::update( // Update delta if we need it to check relinearization later if (relinearizeThisStep) { gttic(updateDelta); - updateDelta(disableReordering); + updateDelta(kDisableReordering); gttoc(updateDelta); } @@ -982,7 +635,7 @@ ISAM2Result ISAM2::update( else relinKeys = Impl::CheckRelinearizationFull(delta_, params_.relinearizeThreshold); - if (disableReordering) + if (kDisableReordering) relinKeys = Impl::CheckRelinearizationFull( delta_, 0.0); // This is used for debugging @@ -1485,4 +1138,3 @@ VectorValues ISAM2::gradientAtZero() const { } } // namespace gtsam -/// namespace gtsam diff --git a/gtsam/nonlinear/ISAM2.h b/gtsam/nonlinear/ISAM2.h index eb33038a7..069d90cc8 100644 --- a/gtsam/nonlinear/ISAM2.h +++ b/gtsam/nonlinear/ISAM2.h @@ -13,593 +13,23 @@ * @file ISAM2.h * @brief Incremental update functionality (ISAM2) for BayesTree, with fluid * relinearization. - * @author Michael Kaess, Richard Roberts + * @author Michael Kaess, Richard Roberts, Frank Dellaert */ // \callgraph #pragma once -#include +#include +#include +#include +#include +#include + #include -#include -#include -#include - -#include - namespace gtsam { -/** - * @addtogroup ISAM2 - * Parameters for ISAM2 using Gauss-Newton optimization. Either this class or - * ISAM2DoglegParams should be specified as the optimizationParams in - * ISAM2Params, which should in turn be passed to ISAM2(const ISAM2Params&). - */ -struct GTSAM_EXPORT ISAM2GaussNewtonParams { - double - wildfireThreshold; ///< Continue updating the linear delta only when - ///< changes are above this threshold (default: 0.001) - - /** Specify parameters as constructor arguments */ - ISAM2GaussNewtonParams( - double _wildfireThreshold = - 0.001 ///< see ISAM2GaussNewtonParams public variables, - ///< ISAM2GaussNewtonParams::wildfireThreshold - ) - : wildfireThreshold(_wildfireThreshold) {} - - void print(const std::string str = "") const { - using std::cout; - cout << str << "type: ISAM2GaussNewtonParams\n"; - cout << str << "wildfireThreshold: " << wildfireThreshold << "\n"; - cout.flush(); - } - - double getWildfireThreshold() const { return wildfireThreshold; } - void setWildfireThreshold(double wildfireThreshold) { - this->wildfireThreshold = wildfireThreshold; - } -}; - -/** - * @addtogroup ISAM2 - * Parameters for ISAM2 using Dogleg optimization. Either this class or - * ISAM2GaussNewtonParams should be specified as the optimizationParams in - * ISAM2Params, which should in turn be passed to ISAM2(const ISAM2Params&). - */ -struct GTSAM_EXPORT ISAM2DoglegParams { - double initialDelta; ///< The initial trust region radius for Dogleg - double - wildfireThreshold; ///< Continue updating the linear delta only when - ///< changes are above this threshold (default: 1e-5) - DoglegOptimizerImpl::TrustRegionAdaptationMode - adaptationMode; ///< See description in - ///< DoglegOptimizerImpl::TrustRegionAdaptationMode - bool - verbose; ///< Whether Dogleg prints iteration and convergence information - - /** Specify parameters as constructor arguments */ - ISAM2DoglegParams( - double _initialDelta = 1.0, ///< see ISAM2DoglegParams::initialDelta - double _wildfireThreshold = - 1e-5, ///< see ISAM2DoglegParams::wildfireThreshold - DoglegOptimizerImpl::TrustRegionAdaptationMode _adaptationMode = - DoglegOptimizerImpl:: - SEARCH_EACH_ITERATION, ///< see ISAM2DoglegParams::adaptationMode - bool _verbose = false ///< see ISAM2DoglegParams::verbose - ) - : initialDelta(_initialDelta), - wildfireThreshold(_wildfireThreshold), - adaptationMode(_adaptationMode), - verbose(_verbose) {} - - void print(const std::string str = "") const { - using std::cout; - cout << str << "type: ISAM2DoglegParams\n"; - cout << str << "initialDelta: " << initialDelta << "\n"; - cout << str << "wildfireThreshold: " << wildfireThreshold << "\n"; - cout << str - << "adaptationMode: " << adaptationModeTranslator(adaptationMode) - << "\n"; - cout.flush(); - } - - double getInitialDelta() const { return initialDelta; } - double getWildfireThreshold() const { return wildfireThreshold; } - std::string getAdaptationMode() const { - return adaptationModeTranslator(adaptationMode); - } - bool isVerbose() const { return verbose; } - void setInitialDelta(double initialDelta) { - this->initialDelta = initialDelta; - } - void setWildfireThreshold(double wildfireThreshold) { - this->wildfireThreshold = wildfireThreshold; - } - void setAdaptationMode(const std::string& adaptationMode) { - this->adaptationMode = adaptationModeTranslator(adaptationMode); - } - void setVerbose(bool verbose) { this->verbose = verbose; } - - std::string adaptationModeTranslator( - const DoglegOptimizerImpl::TrustRegionAdaptationMode& adaptationMode) - const; - DoglegOptimizerImpl::TrustRegionAdaptationMode adaptationModeTranslator( - const std::string& adaptationMode) const; -}; - -/** - * @addtogroup ISAM2 - * Parameters for the ISAM2 algorithm. Default parameter values are listed - * below. - */ -typedef FastMap ISAM2ThresholdMap; -typedef ISAM2ThresholdMap::value_type ISAM2ThresholdMapValue; -struct GTSAM_EXPORT ISAM2Params { - typedef boost::variant - OptimizationParams; ///< Either ISAM2GaussNewtonParams or - ///< ISAM2DoglegParams - typedef boost::variant > - RelinearizationThreshold; ///< Either a constant relinearization - ///< threshold or a per-variable-type set of - ///< thresholds - - /** Optimization parameters, this both selects the nonlinear optimization - * method and specifies its parameters, either ISAM2GaussNewtonParams or - * ISAM2DoglegParams. In the former, Gauss-Newton optimization will be used - * with the specified parameters, and in the latter Powell's dog-leg - * algorithm will be used with the specified parameters. - */ - OptimizationParams optimizationParams; - - /** Only relinearize variables whose linear delta magnitude is greater than - * this threshold (default: 0.1). If this is a FastMap instead - * of a double, then the threshold is specified for each dimension of each - * variable type. This parameter then maps from a character indicating the - * variable type to a Vector of thresholds for each dimension of that - * variable. For example, if Pose keys are of type TypedSymbol<'x',Pose3>, - * and landmark keys are of type TypedSymbol<'l',Point3>, then appropriate - * entries would be added with: - * \code - FastMap thresholds; - thresholds['x'] = (Vector(6) << 0.1, 0.1, 0.1, 0.5, 0.5, 0.5).finished(); - // 0.1 rad rotation threshold, 0.5 m translation threshold thresholds['l'] = - Vector3(1.0, 1.0, 1.0); // 1.0 m landmark position threshold - params.relinearizeThreshold = thresholds; - \endcode - */ - RelinearizationThreshold relinearizeThreshold; - - int relinearizeSkip; ///< Only relinearize any variables every - ///< relinearizeSkip calls to ISAM2::update (default: - ///< 10) - - bool enableRelinearization; ///< Controls whether ISAM2 will ever relinearize - ///< any variables (default: true) - - bool evaluateNonlinearError; ///< Whether to evaluate the nonlinear error - ///< before and after the update, to return in - ///< ISAM2Result from update() - - enum Factorization { CHOLESKY, QR }; - /** Specifies whether to use QR or CHOESKY numerical factorization (default: - * CHOLESKY). Cholesky is faster but potentially numerically unstable for - * poorly-conditioned problems, which can occur when uncertainty is very low - * in some variables (or dimensions of variables) and very high in others. QR - * is slower but more numerically stable in poorly-conditioned problems. We - * suggest using the default of Cholesky unless gtsam sometimes throws - * IndefiniteLinearSystemException when your problem's Hessian is actually - * positive definite. For positive definite problems, numerical error - * accumulation can cause the problem to become numerically negative or - * indefinite as solving proceeds, especially when using Cholesky. - */ - Factorization factorization; - - /** Whether to cache linear factors (default: true). - * This can improve performance if linearization is expensive, but can hurt - * performance if linearization is very cleap due to computation to look up - * additional keys. - */ - bool cacheLinearizedFactors; - - KeyFormatter - keyFormatter; ///< A KeyFormatter for when keys are printed during - ///< debugging (default: DefaultKeyFormatter) - - bool enableDetailedResults; ///< Whether to compute and return - ///< ISAM2Result::detailedResults, this can - ///< increase running time (default: false) - - /** Check variables for relinearization in tree-order, stopping the check once - * a variable does not need to be relinearized (default: false). This can - * improve speed by only checking a small part of the top of the tree. - * However, variables below the check cut-off can accumulate significant - * deltas without triggering relinearization. This is particularly useful in - * exploration scenarios where real-time performance is desired over - * correctness. Use with caution. - */ - bool enablePartialRelinearizationCheck; - - /// When you will be removing many factors, e.g. when using ISAM2 as a - /// fixed-lag smoother, enable this option to add factors in the first - /// available factor slots, to avoid accumulating NULL factor slots, at the - /// cost of having to search for slots every time a factor is added. - bool findUnusedFactorSlots; - - /** - * Specify parameters as constructor arguments - * See the documentation of member variables above. - */ - ISAM2Params(OptimizationParams _optimizationParams = ISAM2GaussNewtonParams(), - RelinearizationThreshold _relinearizeThreshold = 0.1, - int _relinearizeSkip = 10, bool _enableRelinearization = true, - bool _evaluateNonlinearError = false, - Factorization _factorization = ISAM2Params::CHOLESKY, - bool _cacheLinearizedFactors = true, - const KeyFormatter& _keyFormatter = - DefaultKeyFormatter ///< see ISAM2::Params::keyFormatter - ) - : optimizationParams(_optimizationParams), - relinearizeThreshold(_relinearizeThreshold), - relinearizeSkip(_relinearizeSkip), - enableRelinearization(_enableRelinearization), - evaluateNonlinearError(_evaluateNonlinearError), - factorization(_factorization), - cacheLinearizedFactors(_cacheLinearizedFactors), - keyFormatter(_keyFormatter), - enableDetailedResults(false), - enablePartialRelinearizationCheck(false), - findUnusedFactorSlots(false) {} - - /// print iSAM2 parameters - void print(const std::string& str = "") const { - using std::cout; - cout << str << "\n"; - - static const std::string kStr("optimizationParams: "); - if (optimizationParams.type() == typeid(ISAM2GaussNewtonParams)) - boost::get(optimizationParams).print(); - else if (optimizationParams.type() == typeid(ISAM2DoglegParams)) - boost::get(optimizationParams).print(kStr); - else - cout << kStr << "{unknown type}\n"; - - cout << "relinearizeThreshold: "; - if (relinearizeThreshold.type() == typeid(double)) { - cout << boost::get(relinearizeThreshold) << "\n"; - } else { - cout << "{mapped}\n"; - for (const ISAM2ThresholdMapValue& value : - boost::get(relinearizeThreshold)) { - cout << " '" << value.first - << "' -> [" << value.second.transpose() << " ]\n"; - } - } - - cout << "relinearizeSkip: " << relinearizeSkip << "\n"; - cout << "enableRelinearization: " << enableRelinearization - << "\n"; - cout << "evaluateNonlinearError: " << evaluateNonlinearError - << "\n"; - cout << "factorization: " - << factorizationTranslator(factorization) << "\n"; - cout << "cacheLinearizedFactors: " << cacheLinearizedFactors - << "\n"; - cout << "enableDetailedResults: " << enableDetailedResults - << "\n"; - cout << "enablePartialRelinearizationCheck: " - << enablePartialRelinearizationCheck << "\n"; - cout << "findUnusedFactorSlots: " << findUnusedFactorSlots - << "\n"; - cout.flush(); - } - - /// @name Getters and Setters for all properties - /// @{ - - OptimizationParams getOptimizationParams() const { - return this->optimizationParams; - } - RelinearizationThreshold getRelinearizeThreshold() const { - return relinearizeThreshold; - } - int getRelinearizeSkip() const { return relinearizeSkip; } - bool isEnableRelinearization() const { return enableRelinearization; } - bool isEvaluateNonlinearError() const { return evaluateNonlinearError; } - std::string getFactorization() const { - return factorizationTranslator(factorization); - } - bool isCacheLinearizedFactors() const { return cacheLinearizedFactors; } - KeyFormatter getKeyFormatter() const { return keyFormatter; } - bool isEnableDetailedResults() const { return enableDetailedResults; } - bool isEnablePartialRelinearizationCheck() const { - return enablePartialRelinearizationCheck; - } - - void setOptimizationParams(OptimizationParams optimizationParams) { - this->optimizationParams = optimizationParams; - } - void setRelinearizeThreshold(RelinearizationThreshold relinearizeThreshold) { - this->relinearizeThreshold = relinearizeThreshold; - } - void setRelinearizeSkip(int relinearizeSkip) { - this->relinearizeSkip = relinearizeSkip; - } - void setEnableRelinearization(bool enableRelinearization) { - this->enableRelinearization = enableRelinearization; - } - void setEvaluateNonlinearError(bool evaluateNonlinearError) { - this->evaluateNonlinearError = evaluateNonlinearError; - } - void setFactorization(const std::string& factorization) { - this->factorization = factorizationTranslator(factorization); - } - void setCacheLinearizedFactors(bool cacheLinearizedFactors) { - this->cacheLinearizedFactors = cacheLinearizedFactors; - } - void setKeyFormatter(KeyFormatter keyFormatter) { - this->keyFormatter = keyFormatter; - } - void setEnableDetailedResults(bool enableDetailedResults) { - this->enableDetailedResults = enableDetailedResults; - } - void setEnablePartialRelinearizationCheck( - bool enablePartialRelinearizationCheck) { - this->enablePartialRelinearizationCheck = enablePartialRelinearizationCheck; - } - - GaussianFactorGraph::Eliminate getEliminationFunction() const { - return factorization == CHOLESKY - ? (GaussianFactorGraph::Eliminate)EliminatePreferCholesky - : (GaussianFactorGraph::Eliminate)EliminateQR; - } - - /// @} - - /// @name Some utilities - /// @{ - - static Factorization factorizationTranslator(const std::string& str); - static std::string factorizationTranslator(const Factorization& value); - - /// @} -}; - -typedef FastVector FactorIndices; - -/** - * @addtogroup ISAM2 - * This struct is returned from ISAM2::update() and contains information about - * the update that is useful for determining whether the solution is - * converging, and about how much work was required for the update. See member - * variables for details and information about each entry. - */ -struct GTSAM_EXPORT ISAM2Result { - /** The nonlinear error of all of the factors, \a including new factors and - * variables added during the current call to ISAM2::update(). This error is - * calculated using the following variable values: - * \li Pre-existing variables will be evaluated by combining their - * linearization point before this call to update, with their partial linear - * delta, as computed by ISAM2::calculateEstimate(). - * \li New variables will be evaluated at their initialization points passed - * into the current call to update. - * \par Note: This will only be computed if - * ISAM2Params::evaluateNonlinearError is set to \c true, because there is - * some cost to this computation. - */ - boost::optional errorBefore; - - /** The nonlinear error of all of the factors computed after the current - * update, meaning that variables above the relinearization threshold - * (ISAM2Params::relinearizeThreshold) have been relinearized and new - * variables have undergone one linear update. Variable values are - * again computed by combining their linearization points with their - * partial linear deltas, by ISAM2::calculateEstimate(). - * \par Note: This will only be computed if - * ISAM2Params::evaluateNonlinearError is set to \c true, because there is - * some cost to this computation. - */ - boost::optional errorAfter; - - /** The number of variables that were relinearized because their linear - * deltas exceeded the reslinearization threshold - * (ISAM2Params::relinearizeThreshold), combined with any additional - * variables that had to be relinearized because they were involved in - * the same factor as a variable above the relinearization threshold. - * On steps where no relinearization is considered - * (see ISAM2Params::relinearizeSkip), this count will be zero. - */ - size_t variablesRelinearized; - - /** The number of variables that were reeliminated as parts of the Bayes' - * Tree were recalculated, due to new factors. When loop closures occur, - * this count will be large as the new loop-closing factors will tend to - * involve variables far away from the root, and everything up to the root - * will be reeliminated. - */ - size_t variablesReeliminated; - - /** The number of factors that were included in reelimination of the Bayes' - * tree. */ - size_t factorsRecalculated; - - /** The number of cliques in the Bayes' Tree */ - size_t cliques; - - /** The indices of the newly-added factors, in 1-to-1 correspondence with the - * factors passed as \c newFactors to ISAM2::update(). These indices may be - * used later to refer to the factors in order to remove them. - */ - FactorIndices newFactorsIndices; - - /** A struct holding detailed results, which must be enabled with - * ISAM2Params::enableDetailedResults. - */ - struct DetailedResults { - /** The status of a single variable, this struct is stored in - * DetailedResults::variableStatus */ - struct VariableStatus { - /** Whether the variable was just reeliminated, due to being relinearized, - * observed, new, or on the path up to the root clique from another - * reeliminated variable. */ - bool isReeliminated; - bool isAboveRelinThreshold; ///< Whether the variable was just - ///< relinearized due to being above the - ///< relinearization threshold - bool isRelinearizeInvolved; ///< Whether the variable was below the - ///< relinearization threshold but was - ///< relinearized by being involved in a - ///< factor with a variable above the - ///< relinearization threshold - bool isRelinearized; /// Whether the variable was relinearized, either by - /// being above the relinearization threshold or by - /// involvement. - bool isObserved; ///< Whether the variable was just involved in new - ///< factors - bool isNew; ///< Whether the variable itself was just added - bool inRootClique; ///< Whether the variable is in the root clique - VariableStatus() - : isReeliminated(false), - isAboveRelinThreshold(false), - isRelinearizeInvolved(false), - isRelinearized(false), - isObserved(false), - isNew(false), - inRootClique(false) {} - }; - - /** The status of each variable during this update, see VariableStatus. - */ - FastMap variableStatus; - }; - - /** Detailed results, if enabled by ISAM2Params::enableDetailedResults. See - * Detail for information about the results data stored here. */ - boost::optional detail; - - void print(const std::string str = "") const { - using std::cout; - cout << str << " Reelimintated: " << variablesReeliminated - << " Relinearized: " << variablesRelinearized - << " Cliques: " << cliques << std::endl; - } - - /** Getters and Setters */ - size_t getVariablesRelinearized() const { return variablesRelinearized; } - size_t getVariablesReeliminated() const { return variablesReeliminated; } - size_t getCliques() const { return cliques; } -}; - -/** - * Specialized Clique structure for ISAM2, incorporating caching and gradient - * contribution - * TODO: more documentation - */ -class GTSAM_EXPORT ISAM2Clique - : public BayesTreeCliqueBase { - public: - typedef ISAM2Clique This; - typedef BayesTreeCliqueBase Base; - typedef boost::shared_ptr shared_ptr; - typedef boost::weak_ptr weak_ptr; - typedef GaussianConditional ConditionalType; - typedef ConditionalType::shared_ptr sharedConditional; - - Base::FactorType::shared_ptr cachedFactor_; - Vector gradientContribution_; -#ifdef USE_BROKEN_FAST_BACKSUBSTITUTE - mutable FastMap solnPointers_; -#endif - - /// Default constructor - ISAM2Clique() : Base() {} - - /// Copy constructor, does *not* copy solution pointers as these are invalid - /// in different trees. - ISAM2Clique(const ISAM2Clique& other) - : Base(other), - cachedFactor_(other.cachedFactor_), - gradientContribution_(other.gradientContribution_) {} - - /// Assignment operator, does *not* copy solution pointers as these are - /// invalid in different trees. - ISAM2Clique& operator=(const ISAM2Clique& other) { - Base::operator=(other); - cachedFactor_ = other.cachedFactor_; - gradientContribution_ = other.gradientContribution_; - return *this; - } - - /// Overridden to also store the remaining factor and gradient contribution - void setEliminationResult( - const FactorGraphType::EliminationResult& eliminationResult); - - /** Access the cached factor */ - Base::FactorType::shared_ptr& cachedFactor() { return cachedFactor_; } - - /** Access the gradient contribution */ - const Vector& gradientContribution() const { return gradientContribution_; } - - bool equals(const This& other, double tol = 1e-9) const; - - /** print this node */ - void print(const std::string& s = "", - const KeyFormatter& formatter = DefaultKeyFormatter) const; - - void optimizeWildfire(const KeySet& replaced, double threshold, - KeySet* changed, VectorValues* delta, - size_t* count) const; - - bool optimizeWildfireNode(const KeySet& replaced, double threshold, - KeySet* changed, VectorValues* delta, - size_t* count) const; - - /** - * Starting from the root, add up entries of frontal and conditional matrices - * of each conditional - */ - void nnz_internal(size_t* result) const; - size_t calculate_nnz() const; - - private: - /** - * Check if clique was replaced, or if any parents were changed above the - * threshold or themselves replaced. - */ - bool isDirty(const KeySet& replaced, const KeySet& changed) const; - - /** - * Back-substitute - special version stores solution pointers in cliques for - * fast access. - */ - void fastBackSubstitute(VectorValues* delta) const; - - /* - * Check whether the values changed above a threshold, or always true if the - * clique was replaced. - */ - bool valuesChanged(const KeySet& replaced, const Vector& originalValues, - const VectorValues& delta, double threshold) const; - - /// Set changed flag for each frontal variable - void markFrontalsAsChanged(KeySet* changed) const; - - /// Restore delta to original values, guided by frontal keys. - void restoreFromOriginals(const Vector& originalValues, - VectorValues* delta) const; - - /** Serialization function */ - friend class boost::serialization::access; - template - void serialize(ARCHIVE& ar, const unsigned int /*version*/) { - ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); - ar& BOOST_SERIALIZATION_NVP(cachedFactor_); - ar& BOOST_SERIALIZATION_NVP(gradientContribution_); - } -}; // \struct ISAM2Clique - /** * @addtogroup ISAM2 * Implementation of the full ISAM2 algorithm for incremental nonlinear @@ -643,7 +73,7 @@ class GTSAM_EXPORT ISAM2 : public BayesTree { * until it is needed. */ mutable KeySet - deltaReplacedMask_; // TODO: Make sure accessed in the right way + deltaReplacedMask_; // TODO(dellaert): Make sure accessed in the right way /** All original nonlinear factors are stored here to use during * relinearization */ @@ -768,7 +198,11 @@ class GTSAM_EXPORT ISAM2 : public BayesTree { * @return */ template - VALUE calculateEstimate(Key key) const; + VALUE calculateEstimate(Key key) const { + const Vector& delta = getDelta()[key]; + return traits::Retract(theta_.at(key), delta); + } + /** Compute an estimate for a single variable using its incomplete linear * delta computed during the last update. This is faster than calling the @@ -846,32 +280,12 @@ class GTSAM_EXPORT ISAM2 : public BayesTree { const boost::optional >& constrainKeys, ISAM2Result& result); void updateDelta(bool forceFullSolve = false) const; - }; // ISAM2 /// traits template <> struct traits : public Testable {}; -/** - * Optimize the BayesTree, starting from the root. - * @param threshold The maximum change against the PREVIOUS delta for - * non-replaced variables that can be ignored, ie. the old delta entry is kept - * and recursive backsubstitution might eventually stop if none of the changed - * variables are contained in the subtree. - * @param replaced Needs to contain all variables that are contained in the top - * of the Bayes tree that has been redone. - * @return The number of variables that were solved for. - * @param delta The current solution, an offset from the linearization point. - */ -size_t optimizeWildfire(const ISAM2::sharedClique& root, double threshold, - const KeySet& replaced, VectorValues* delta); - -size_t optimizeWildfireNonRecursive(const ISAM2::sharedClique& root, - double threshold, const KeySet& replaced, - VectorValues* delta); - } // namespace gtsam #include -#include diff --git a/gtsam/nonlinear/ISAM2Clique.cpp b/gtsam/nonlinear/ISAM2Clique.cpp new file mode 100644 index 000000000..6e03b0680 --- /dev/null +++ b/gtsam/nonlinear/ISAM2Clique.cpp @@ -0,0 +1,302 @@ +/* ---------------------------------------------------------------------------- + + * 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 ISAM2Clique.cpp + * @brief Specialized iSAM2 Clique + * @author Michael Kaess, Richard Roberts, Frank Dellaert + */ + +#include +#include +#include +#include +#include + +using namespace std; + +namespace gtsam { + +// Instantiate base class +template class BayesTreeCliqueBase; + +/* ************************************************************************* */ +void ISAM2Clique::setEliminationResult( + const FactorGraphType::EliminationResult& eliminationResult) { + conditional_ = eliminationResult.first; + cachedFactor_ = eliminationResult.second; + // Compute gradient contribution + gradientContribution_.resize(conditional_->cols() - 1); + // Rewrite -(R * P')'*d as -(d' * R * P')' for computational speed + // reasons + gradientContribution_ << -conditional_->get_R().transpose() * + conditional_->get_d(), + -conditional_->get_S().transpose() * conditional_->get_d(); +} + +/* ************************************************************************* */ +bool ISAM2Clique::equals(const This& other, double tol) const { + return Base::equals(other) && + ((!cachedFactor_ && !other.cachedFactor_) || + (cachedFactor_ && other.cachedFactor_ && + cachedFactor_->equals(*other.cachedFactor_, tol))); +} + +/* ************************************************************************* */ +void ISAM2Clique::print(const string& s, const KeyFormatter& formatter) const { + Base::print(s, formatter); + if (cachedFactor_) + cachedFactor_->print(s + "Cached: ", formatter); + else + cout << s << "Cached empty" << endl; + if (gradientContribution_.rows() != 0) + gtsam::print(gradientContribution_, "Gradient contribution: "); +} + +/* ************************************************************************* */ +bool ISAM2Clique::isDirty(const KeySet& replaced, const KeySet& changed) const { + // if none of the variables in this clique (frontal and separator!) changed + // significantly, then by the running intersection property, none of the + // cliques in the children need to be processed + + // Are any clique variables part of the tree that has been redone? + bool dirty = replaced.exists(conditional_->frontals().front()); +#if !defined(NDEBUG) && defined(GTSAM_EXTRA_CONSISTENCY_CHECKS) + for (Key frontal : conditional_->frontals()) { + assert(dirty == replaced.exists(frontal)); + } +#endif + + // If not, then has one of the separator variables changed significantly? + if (!dirty) { + for (Key parent : conditional_->parents()) { + if (changed.exists(parent)) { + dirty = true; + break; + } + } + } + return dirty; +} + +/* ************************************************************************* */ +/** + * Back-substitute - special version stores solution pointers in cliques for + * fast access. + */ +void ISAM2Clique::fastBackSubstitute(VectorValues* delta) const { +#ifdef USE_BROKEN_FAST_BACKSUBSTITUTE + // TODO(gareth): This code shares a lot of logic w/ linearAlgorithms-inst, + // potentially refactor + + // Create solution part pointers if necessary and possible - necessary if + // solnPointers_ is empty, and possible if either we're a root, or we have + // a parent with valid solnPointers_. + ISAM2Clique::shared_ptr parent = parent_.lock(); + if (solnPointers_.empty() && (isRoot() || !parent->solnPointers_.empty())) { + for (Key frontal : conditional_->frontals()) + solnPointers_.emplace(frontal, delta->find(frontal)); + for (Key parentKey : conditional_->parents()) { + assert(parent->solnPointers_.exists(parentKey)); + solnPointers_.emplace(parentKey, parent->solnPointers_.at(parentKey)); + } + } + + // See if we can use solution part pointers - we can if they either + // already existed or were created above. + if (!solnPointers_.empty()) { + GaussianConditional& c = *conditional_; + // Solve matrix + Vector xS; + { + // Count dimensions of vector + DenseIndex dim = 0; + FastVector parentPointers; + parentPointers.reserve(conditional_->nrParents()); + for (Key parent : conditional_->parents()) { + parentPointers.push_back(solnPointers_.at(parent)); + dim += parentPointers.back()->second.size(); + } + + // Fill parent vector + xS.resize(dim); + DenseIndex vectorPos = 0; + for (const VectorValues::const_iterator& parentPointer : parentPointers) { + const Vector& parentVector = parentPointer->second; + xS.block(vectorPos, 0, parentVector.size(), 1) = + parentVector.block(0, 0, parentVector.size(), 1); + vectorPos += parentVector.size(); + } + } + + // NOTE(gareth): We can no longer write: xS = b - S * xS + // This is because Eigen (as of 3.3) no longer evaluates S * xS into + // a temporary, and the operation trashes valus in xS. + // See: http://eigen.tuxfamily.org/index.php?title=3.3 + const Vector rhs = c.getb() - c.get_S() * xS; + const Vector solution = c.get_R().triangularView().solve(rhs); + + // Check for indeterminant solution + if (solution.hasNaN()) + throw IndeterminantLinearSystemException(c.keys().front()); + + // Insert solution into a VectorValues + DenseIndex vectorPosition = 0; + for (GaussianConditional::const_iterator frontal = c.beginFrontals(); + frontal != c.endFrontals(); ++frontal) { + solnPointers_.at(*frontal)->second = + solution.segment(vectorPosition, c.getDim(frontal)); + vectorPosition += c.getDim(frontal); + } + } else { + // Just call plain solve because we couldn't use solution pointers. + delta->update(conditional_->solve(*delta)); + } +#else + delta->update(conditional_->solve(*delta)); +#endif +} + +/* ************************************************************************* */ +bool ISAM2Clique::valuesChanged(const KeySet& replaced, + const Vector& originalValues, + const VectorValues& delta, + double threshold) const { + auto frontals = conditional_->frontals(); + if (replaced.exists(frontals.front())) return true; + auto diff = originalValues - delta.vector(frontals); + return diff.lpNorm() >= threshold; +} + +/* ************************************************************************* */ +/// Set changed flag for each frontal variable +void ISAM2Clique::markFrontalsAsChanged(KeySet* changed) const { + for (Key frontal : conditional_->frontals()) { + changed->insert(frontal); + } +} + +/* ************************************************************************* */ +void ISAM2Clique::restoreFromOriginals(const Vector& originalValues, + VectorValues* delta) const { + size_t pos = 0; + for (Key frontal : conditional_->frontals()) { + auto v = delta->at(frontal); + v = originalValues.segment(pos, v.size()); + pos += v.size(); + } +} + +/* ************************************************************************* */ +// Note: not being used right now in favor of non-recursive version below. +void ISAM2Clique::optimizeWildfire(const KeySet& replaced, double threshold, + KeySet* changed, VectorValues* delta, + size_t* count) const { + if (isDirty(replaced, *changed)) { + // Temporary copy of the original values, to check how much they change + auto originalValues = delta->vector(conditional_->frontals()); + + // Back-substitute + fastBackSubstitute(delta); + count += conditional_->nrFrontals(); + + if (valuesChanged(replaced, originalValues, *delta, threshold)) { + markFrontalsAsChanged(changed); + } else { + restoreFromOriginals(originalValues, delta); + } + + // Recurse to children + for (const auto& child : children) { + child->optimizeWildfire(replaced, threshold, changed, delta, count); + } + } +} + +size_t optimizeWildfire(const ISAM2Clique::shared_ptr& root, double threshold, + const KeySet& keys, VectorValues* delta) { + KeySet changed; + size_t count = 0; + // starting from the root, call optimize on each conditional + if (root) root->optimizeWildfire(keys, threshold, &changed, delta, &count); + return count; +} + +/* ************************************************************************* */ +bool ISAM2Clique::optimizeWildfireNode(const KeySet& replaced, double threshold, + KeySet* changed, VectorValues* delta, + size_t* count) const { + // TODO(gareth): This code shares a lot of logic w/ linearAlgorithms-inst, + // potentially refactor + bool dirty = isDirty(replaced, *changed); + if (dirty) { + // Temporary copy of the original values, to check how much they change + auto originalValues = delta->vector(conditional_->frontals()); + + // Back-substitute + fastBackSubstitute(delta); + count += conditional_->nrFrontals(); + + if (valuesChanged(replaced, originalValues, *delta, threshold)) { + markFrontalsAsChanged(changed); + } else { + restoreFromOriginals(originalValues, delta); + } + } + + return dirty; +} + +size_t optimizeWildfireNonRecursive(const ISAM2Clique::shared_ptr& root, + double threshold, const KeySet& keys, + VectorValues* delta) { + KeySet changed; + size_t count = 0; + + if (root) { + std::stack travStack; + travStack.push(root); + ISAM2Clique::shared_ptr currentNode = root; + while (!travStack.empty()) { + currentNode = travStack.top(); + travStack.pop(); + bool dirty = currentNode->optimizeWildfireNode(keys, threshold, &changed, + delta, &count); + if (dirty) { + for (const auto& child : currentNode->children) { + travStack.push(child); + } + } + } + } + + return count; +} + +/* ************************************************************************* */ +void ISAM2Clique::nnz_internal(size_t* result) const { + size_t dimR = conditional_->rows(); + size_t dimSep = conditional_->get_S().cols(); + *result += ((dimR + 1) * dimR) / 2 + dimSep * dimR; + // traverse the children + for (const auto& child : children) { + child->nnz_internal(result); + } +} + +/* ************************************************************************* */ +size_t ISAM2Clique::calculate_nnz() const { + size_t result = 0; + nnz_internal(&result); + return result; +} + +} // namespace gtsam diff --git a/gtsam/nonlinear/ISAM2Clique.h b/gtsam/nonlinear/ISAM2Clique.h new file mode 100644 index 000000000..fecc8d335 --- /dev/null +++ b/gtsam/nonlinear/ISAM2Clique.h @@ -0,0 +1,157 @@ +/* ---------------------------------------------------------------------------- + + * 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 ISAM2Clique.h + * @brief Specialized iSAM2 Clique + * @author Michael Kaess, Richard Roberts + */ + +// \callgraph + +#pragma once + +#include +#include +#include +#include +#include +#include + +namespace gtsam { + +/** + * Specialized Clique structure for ISAM2, incorporating caching and gradient + * contribution + * TODO: more documentation + */ +class GTSAM_EXPORT ISAM2Clique + : public BayesTreeCliqueBase { + public: + typedef ISAM2Clique This; + typedef BayesTreeCliqueBase Base; + typedef boost::shared_ptr shared_ptr; + typedef boost::weak_ptr weak_ptr; + typedef GaussianConditional ConditionalType; + typedef ConditionalType::shared_ptr sharedConditional; + + Base::FactorType::shared_ptr cachedFactor_; + Vector gradientContribution_; +#ifdef USE_BROKEN_FAST_BACKSUBSTITUTE + mutable FastMap solnPointers_; +#endif + + /// Default constructor + ISAM2Clique() : Base() {} + + /// Copy constructor, does *not* copy solution pointers as these are invalid + /// in different trees. + ISAM2Clique(const ISAM2Clique& other) + : Base(other), + cachedFactor_(other.cachedFactor_), + gradientContribution_(other.gradientContribution_) {} + + /// Assignment operator, does *not* copy solution pointers as these are + /// invalid in different trees. + ISAM2Clique& operator=(const ISAM2Clique& other) { + Base::operator=(other); + cachedFactor_ = other.cachedFactor_; + gradientContribution_ = other.gradientContribution_; + return *this; + } + + /// Overridden to also store the remaining factor and gradient contribution + void setEliminationResult( + const FactorGraphType::EliminationResult& eliminationResult); + + /** Access the cached factor */ + Base::FactorType::shared_ptr& cachedFactor() { return cachedFactor_; } + + /** Access the gradient contribution */ + const Vector& gradientContribution() const { return gradientContribution_; } + + bool equals(const This& other, double tol = 1e-9) const; + + /** print this node */ + void print(const std::string& s = "", + const KeyFormatter& formatter = DefaultKeyFormatter) const; + + void optimizeWildfire(const KeySet& replaced, double threshold, + KeySet* changed, VectorValues* delta, + size_t* count) const; + + bool optimizeWildfireNode(const KeySet& replaced, double threshold, + KeySet* changed, VectorValues* delta, + size_t* count) const; + + /** + * Starting from the root, add up entries of frontal and conditional matrices + * of each conditional + */ + void nnz_internal(size_t* result) const; + size_t calculate_nnz() const; + + private: + /** + * Check if clique was replaced, or if any parents were changed above the + * threshold or themselves replaced. + */ + bool isDirty(const KeySet& replaced, const KeySet& changed) const; + + /** + * Back-substitute - special version stores solution pointers in cliques for + * fast access. + */ + void fastBackSubstitute(VectorValues* delta) const; + + /* + * Check whether the values changed above a threshold, or always true if the + * clique was replaced. + */ + bool valuesChanged(const KeySet& replaced, const Vector& originalValues, + const VectorValues& delta, double threshold) const; + + /// Set changed flag for each frontal variable + void markFrontalsAsChanged(KeySet* changed) const; + + /// Restore delta to original values, guided by frontal keys. + void restoreFromOriginals(const Vector& originalValues, + VectorValues* delta) const; + + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE& ar, const unsigned int /*version*/) { + ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); + ar& BOOST_SERIALIZATION_NVP(cachedFactor_); + ar& BOOST_SERIALIZATION_NVP(gradientContribution_); + } +}; // \struct ISAM2Clique + +/** + * Optimize the BayesTree, starting from the root. + * @param threshold The maximum change against the PREVIOUS delta for + * non-replaced variables that can be ignored, ie. the old delta entry is kept + * and recursive backsubstitution might eventually stop if none of the changed + * variables are contained in the subtree. + * @param replaced Needs to contain all variables that are contained in the top + * of the Bayes tree that has been redone. + * @return The number of variables that were solved for. + * @param delta The current solution, an offset from the linearization point. + */ +size_t optimizeWildfire(const ISAM2Clique::shared_ptr& root, double threshold, + const KeySet& replaced, VectorValues* delta); + +size_t optimizeWildfireNonRecursive(const ISAM2Clique::shared_ptr& root, + double threshold, const KeySet& replaced, + VectorValues* delta); + +} // namespace gtsam diff --git a/gtsam/nonlinear/ISAM2Params.cpp b/gtsam/nonlinear/ISAM2Params.cpp new file mode 100644 index 000000000..c768ce427 --- /dev/null +++ b/gtsam/nonlinear/ISAM2Params.cpp @@ -0,0 +1,89 @@ +/* ---------------------------------------------------------------------------- + + * 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 ISAM2Params.cpp + * @brief Parameters for iSAM 2. + * @author Michael Kaess, Richard Roberts, Frank Dellaert + */ + +#include +#include + +using namespace std; + +namespace gtsam { + +/* ************************************************************************* */ +string ISAM2DoglegParams::adaptationModeTranslator( + const DoglegOptimizerImpl::TrustRegionAdaptationMode& adaptationMode) + const { + string s; + switch (adaptationMode) { + case DoglegOptimizerImpl::SEARCH_EACH_ITERATION: + s = "SEARCH_EACH_ITERATION"; + break; + case DoglegOptimizerImpl::ONE_STEP_PER_ITERATION: + s = "ONE_STEP_PER_ITERATION"; + break; + default: + s = "UNDEFINED"; + break; + } + return s; +} + +/* ************************************************************************* */ +DoglegOptimizerImpl::TrustRegionAdaptationMode +ISAM2DoglegParams::adaptationModeTranslator( + const string& adaptationMode) const { + string s = adaptationMode; + boost::algorithm::to_upper(s); + if (s == "SEARCH_EACH_ITERATION") + return DoglegOptimizerImpl::SEARCH_EACH_ITERATION; + if (s == "ONE_STEP_PER_ITERATION") + return DoglegOptimizerImpl::ONE_STEP_PER_ITERATION; + + /* default is SEARCH_EACH_ITERATION */ + return DoglegOptimizerImpl::SEARCH_EACH_ITERATION; +} + +/* ************************************************************************* */ +ISAM2Params::Factorization ISAM2Params::factorizationTranslator( + const string& str) { + string s = str; + boost::algorithm::to_upper(s); + if (s == "QR") return ISAM2Params::QR; + if (s == "CHOLESKY") return ISAM2Params::CHOLESKY; + + /* default is CHOLESKY */ + return ISAM2Params::CHOLESKY; +} + +/* ************************************************************************* */ +string ISAM2Params::factorizationTranslator( + const ISAM2Params::Factorization& value) { + string s; + switch (value) { + case ISAM2Params::QR: + s = "QR"; + break; + case ISAM2Params::CHOLESKY: + s = "CHOLESKY"; + break; + default: + s = "UNDEFINED"; + break; + } + return s; +} + +} // namespace gtsam diff --git a/gtsam/nonlinear/ISAM2Params.h b/gtsam/nonlinear/ISAM2Params.h new file mode 100644 index 000000000..afddd1f8e --- /dev/null +++ b/gtsam/nonlinear/ISAM2Params.h @@ -0,0 +1,365 @@ +/* ---------------------------------------------------------------------------- + + * 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 ISAM2Params.h + * @brief Parameters for iSAM 2. + * @author Michael Kaess, Richard Roberts, Frank Dellaert + */ + +// \callgraph + +#pragma once + +#include +#include +#include +#include + +namespace gtsam { + +/** + * @addtogroup ISAM2 + * Parameters for ISAM2 using Gauss-Newton optimization. Either this class or + * ISAM2DoglegParams should be specified as the optimizationParams in + * ISAM2Params, which should in turn be passed to ISAM2(const ISAM2Params&). + */ +struct GTSAM_EXPORT ISAM2GaussNewtonParams { + double + wildfireThreshold; ///< Continue updating the linear delta only when + ///< changes are above this threshold (default: 0.001) + + /** Specify parameters as constructor arguments */ + ISAM2GaussNewtonParams( + double _wildfireThreshold = + 0.001 ///< see ISAM2GaussNewtonParams public variables, + ///< ISAM2GaussNewtonParams::wildfireThreshold + ) + : wildfireThreshold(_wildfireThreshold) {} + + void print(const std::string str = "") const { + using std::cout; + cout << str << "type: ISAM2GaussNewtonParams\n"; + cout << str << "wildfireThreshold: " << wildfireThreshold << "\n"; + cout.flush(); + } + + double getWildfireThreshold() const { return wildfireThreshold; } + void setWildfireThreshold(double wildfireThreshold) { + this->wildfireThreshold = wildfireThreshold; + } +}; + +/** + * @addtogroup ISAM2 + * Parameters for ISAM2 using Dogleg optimization. Either this class or + * ISAM2GaussNewtonParams should be specified as the optimizationParams in + * ISAM2Params, which should in turn be passed to ISAM2(const ISAM2Params&). + */ +struct GTSAM_EXPORT ISAM2DoglegParams { + double initialDelta; ///< The initial trust region radius for Dogleg + double + wildfireThreshold; ///< Continue updating the linear delta only when + ///< changes are above this threshold (default: 1e-5) + DoglegOptimizerImpl::TrustRegionAdaptationMode + adaptationMode; ///< See description in + ///< DoglegOptimizerImpl::TrustRegionAdaptationMode + bool + verbose; ///< Whether Dogleg prints iteration and convergence information + + /** Specify parameters as constructor arguments */ + ISAM2DoglegParams( + double _initialDelta = 1.0, ///< see ISAM2DoglegParams::initialDelta + double _wildfireThreshold = + 1e-5, ///< see ISAM2DoglegParams::wildfireThreshold + DoglegOptimizerImpl::TrustRegionAdaptationMode _adaptationMode = + DoglegOptimizerImpl:: + SEARCH_EACH_ITERATION, ///< see ISAM2DoglegParams::adaptationMode + bool _verbose = false ///< see ISAM2DoglegParams::verbose + ) + : initialDelta(_initialDelta), + wildfireThreshold(_wildfireThreshold), + adaptationMode(_adaptationMode), + verbose(_verbose) {} + + void print(const std::string str = "") const { + using std::cout; + cout << str << "type: ISAM2DoglegParams\n"; + cout << str << "initialDelta: " << initialDelta << "\n"; + cout << str << "wildfireThreshold: " << wildfireThreshold << "\n"; + cout << str + << "adaptationMode: " << adaptationModeTranslator(adaptationMode) + << "\n"; + cout.flush(); + } + + double getInitialDelta() const { return initialDelta; } + double getWildfireThreshold() const { return wildfireThreshold; } + std::string getAdaptationMode() const { + return adaptationModeTranslator(adaptationMode); + } + bool isVerbose() const { return verbose; } + void setInitialDelta(double initialDelta) { + this->initialDelta = initialDelta; + } + void setWildfireThreshold(double wildfireThreshold) { + this->wildfireThreshold = wildfireThreshold; + } + void setAdaptationMode(const std::string& adaptationMode) { + this->adaptationMode = adaptationModeTranslator(adaptationMode); + } + void setVerbose(bool verbose) { this->verbose = verbose; } + + std::string adaptationModeTranslator( + const DoglegOptimizerImpl::TrustRegionAdaptationMode& adaptationMode) + const; + DoglegOptimizerImpl::TrustRegionAdaptationMode adaptationModeTranslator( + const std::string& adaptationMode) const; +}; + +/** + * @addtogroup ISAM2 + * Parameters for the ISAM2 algorithm. Default parameter values are listed + * below. + */ +typedef FastMap ISAM2ThresholdMap; +typedef ISAM2ThresholdMap::value_type ISAM2ThresholdMapValue; +struct GTSAM_EXPORT ISAM2Params { + typedef boost::variant + OptimizationParams; ///< Either ISAM2GaussNewtonParams or + ///< ISAM2DoglegParams + typedef boost::variant > + RelinearizationThreshold; ///< Either a constant relinearization + ///< threshold or a per-variable-type set of + ///< thresholds + + /** Optimization parameters, this both selects the nonlinear optimization + * method and specifies its parameters, either ISAM2GaussNewtonParams or + * ISAM2DoglegParams. In the former, Gauss-Newton optimization will be used + * with the specified parameters, and in the latter Powell's dog-leg + * algorithm will be used with the specified parameters. + */ + OptimizationParams optimizationParams; + + /** Only relinearize variables whose linear delta magnitude is greater than + * this threshold (default: 0.1). If this is a FastMap instead + * of a double, then the threshold is specified for each dimension of each + * variable type. This parameter then maps from a character indicating the + * variable type to a Vector of thresholds for each dimension of that + * variable. For example, if Pose keys are of type TypedSymbol<'x',Pose3>, + * and landmark keys are of type TypedSymbol<'l',Point3>, then appropriate + * entries would be added with: + * \code + FastMap thresholds; + thresholds['x'] = (Vector(6) << 0.1, 0.1, 0.1, 0.5, 0.5, 0.5).finished(); + // 0.1 rad rotation threshold, 0.5 m translation threshold thresholds['l'] = + Vector3(1.0, 1.0, 1.0); // 1.0 m landmark position threshold + params.relinearizeThreshold = thresholds; + \endcode + */ + RelinearizationThreshold relinearizeThreshold; + + int relinearizeSkip; ///< Only relinearize any variables every + ///< relinearizeSkip calls to ISAM2::update (default: + ///< 10) + + bool enableRelinearization; ///< Controls whether ISAM2 will ever relinearize + ///< any variables (default: true) + + bool evaluateNonlinearError; ///< Whether to evaluate the nonlinear error + ///< before and after the update, to return in + ///< ISAM2Result from update() + + enum Factorization { CHOLESKY, QR }; + /** Specifies whether to use QR or CHOESKY numerical factorization (default: + * CHOLESKY). Cholesky is faster but potentially numerically unstable for + * poorly-conditioned problems, which can occur when uncertainty is very low + * in some variables (or dimensions of variables) and very high in others. QR + * is slower but more numerically stable in poorly-conditioned problems. We + * suggest using the default of Cholesky unless gtsam sometimes throws + * IndefiniteLinearSystemException when your problem's Hessian is actually + * positive definite. For positive definite problems, numerical error + * accumulation can cause the problem to become numerically negative or + * indefinite as solving proceeds, especially when using Cholesky. + */ + Factorization factorization; + + /** Whether to cache linear factors (default: true). + * This can improve performance if linearization is expensive, but can hurt + * performance if linearization is very cleap due to computation to look up + * additional keys. + */ + bool cacheLinearizedFactors; + + KeyFormatter + keyFormatter; ///< A KeyFormatter for when keys are printed during + ///< debugging (default: DefaultKeyFormatter) + + bool enableDetailedResults; ///< Whether to compute and return + ///< ISAM2Result::detailedResults, this can + ///< increase running time (default: false) + + /** Check variables for relinearization in tree-order, stopping the check once + * a variable does not need to be relinearized (default: false). This can + * improve speed by only checking a small part of the top of the tree. + * However, variables below the check cut-off can accumulate significant + * deltas without triggering relinearization. This is particularly useful in + * exploration scenarios where real-time performance is desired over + * correctness. Use with caution. + */ + bool enablePartialRelinearizationCheck; + + /// When you will be removing many factors, e.g. when using ISAM2 as a + /// fixed-lag smoother, enable this option to add factors in the first + /// available factor slots, to avoid accumulating NULL factor slots, at the + /// cost of having to search for slots every time a factor is added. + bool findUnusedFactorSlots; + + /** + * Specify parameters as constructor arguments + * See the documentation of member variables above. + */ + ISAM2Params(OptimizationParams _optimizationParams = ISAM2GaussNewtonParams(), + RelinearizationThreshold _relinearizeThreshold = 0.1, + int _relinearizeSkip = 10, bool _enableRelinearization = true, + bool _evaluateNonlinearError = false, + Factorization _factorization = ISAM2Params::CHOLESKY, + bool _cacheLinearizedFactors = true, + const KeyFormatter& _keyFormatter = + DefaultKeyFormatter ///< see ISAM2::Params::keyFormatter + ) + : optimizationParams(_optimizationParams), + relinearizeThreshold(_relinearizeThreshold), + relinearizeSkip(_relinearizeSkip), + enableRelinearization(_enableRelinearization), + evaluateNonlinearError(_evaluateNonlinearError), + factorization(_factorization), + cacheLinearizedFactors(_cacheLinearizedFactors), + keyFormatter(_keyFormatter), + enableDetailedResults(false), + enablePartialRelinearizationCheck(false), + findUnusedFactorSlots(false) {} + + /// print iSAM2 parameters + void print(const std::string& str = "") const { + using std::cout; + cout << str << "\n"; + + static const std::string kStr("optimizationParams: "); + if (optimizationParams.type() == typeid(ISAM2GaussNewtonParams)) + boost::get(optimizationParams).print(); + else if (optimizationParams.type() == typeid(ISAM2DoglegParams)) + boost::get(optimizationParams).print(kStr); + else + cout << kStr << "{unknown type}\n"; + + cout << "relinearizeThreshold: "; + if (relinearizeThreshold.type() == typeid(double)) { + cout << boost::get(relinearizeThreshold) << "\n"; + } else { + cout << "{mapped}\n"; + for (const ISAM2ThresholdMapValue& value : + boost::get(relinearizeThreshold)) { + cout << " '" << value.first + << "' -> [" << value.second.transpose() << " ]\n"; + } + } + + cout << "relinearizeSkip: " << relinearizeSkip << "\n"; + cout << "enableRelinearization: " << enableRelinearization + << "\n"; + cout << "evaluateNonlinearError: " << evaluateNonlinearError + << "\n"; + cout << "factorization: " + << factorizationTranslator(factorization) << "\n"; + cout << "cacheLinearizedFactors: " << cacheLinearizedFactors + << "\n"; + cout << "enableDetailedResults: " << enableDetailedResults + << "\n"; + cout << "enablePartialRelinearizationCheck: " + << enablePartialRelinearizationCheck << "\n"; + cout << "findUnusedFactorSlots: " << findUnusedFactorSlots + << "\n"; + cout.flush(); + } + + /// @name Getters and Setters for all properties + /// @{ + + OptimizationParams getOptimizationParams() const { + return this->optimizationParams; + } + RelinearizationThreshold getRelinearizeThreshold() const { + return relinearizeThreshold; + } + int getRelinearizeSkip() const { return relinearizeSkip; } + bool isEnableRelinearization() const { return enableRelinearization; } + bool isEvaluateNonlinearError() const { return evaluateNonlinearError; } + std::string getFactorization() const { + return factorizationTranslator(factorization); + } + bool isCacheLinearizedFactors() const { return cacheLinearizedFactors; } + KeyFormatter getKeyFormatter() const { return keyFormatter; } + bool isEnableDetailedResults() const { return enableDetailedResults; } + bool isEnablePartialRelinearizationCheck() const { + return enablePartialRelinearizationCheck; + } + + void setOptimizationParams(OptimizationParams optimizationParams) { + this->optimizationParams = optimizationParams; + } + void setRelinearizeThreshold(RelinearizationThreshold relinearizeThreshold) { + this->relinearizeThreshold = relinearizeThreshold; + } + void setRelinearizeSkip(int relinearizeSkip) { + this->relinearizeSkip = relinearizeSkip; + } + void setEnableRelinearization(bool enableRelinearization) { + this->enableRelinearization = enableRelinearization; + } + void setEvaluateNonlinearError(bool evaluateNonlinearError) { + this->evaluateNonlinearError = evaluateNonlinearError; + } + void setFactorization(const std::string& factorization) { + this->factorization = factorizationTranslator(factorization); + } + void setCacheLinearizedFactors(bool cacheLinearizedFactors) { + this->cacheLinearizedFactors = cacheLinearizedFactors; + } + void setKeyFormatter(KeyFormatter keyFormatter) { + this->keyFormatter = keyFormatter; + } + void setEnableDetailedResults(bool enableDetailedResults) { + this->enableDetailedResults = enableDetailedResults; + } + void setEnablePartialRelinearizationCheck( + bool enablePartialRelinearizationCheck) { + this->enablePartialRelinearizationCheck = enablePartialRelinearizationCheck; + } + + GaussianFactorGraph::Eliminate getEliminationFunction() const { + return factorization == CHOLESKY + ? (GaussianFactorGraph::Eliminate)EliminatePreferCholesky + : (GaussianFactorGraph::Eliminate)EliminateQR; + } + + /// @} + + /// @name Some utilities + /// @{ + + static Factorization factorizationTranslator(const std::string& str); + static std::string factorizationTranslator(const Factorization& value); + + /// @} +}; + +} // namespace gtsam diff --git a/gtsam/nonlinear/ISAM2Result.h b/gtsam/nonlinear/ISAM2Result.h new file mode 100644 index 000000000..1539d90c4 --- /dev/null +++ b/gtsam/nonlinear/ISAM2Result.h @@ -0,0 +1,159 @@ +/* ---------------------------------------------------------------------------- + + * 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 ISAM2Result.h + * @brief Class that stores detailed iSAM2 result. + * @author Michael Kaess, Richard Roberts, Frank Dellaert + */ + +// \callgraph + +#pragma once + +#include +#include + +#include +#include +#include +#include + +#include + +namespace gtsam { + +typedef FastVector FactorIndices; + +/** + * @addtogroup ISAM2 + * This struct is returned from ISAM2::update() and contains information about + * the update that is useful for determining whether the solution is + * converging, and about how much work was required for the update. See member + * variables for details and information about each entry. + */ +struct GTSAM_EXPORT ISAM2Result { + /** The nonlinear error of all of the factors, \a including new factors and + * variables added during the current call to ISAM2::update(). This error is + * calculated using the following variable values: + * \li Pre-existing variables will be evaluated by combining their + * linearization point before this call to update, with their partial linear + * delta, as computed by ISAM2::calculateEstimate(). + * \li New variables will be evaluated at their initialization points passed + * into the current call to update. + * \par Note: This will only be computed if + * ISAM2Params::evaluateNonlinearError is set to \c true, because there is + * some cost to this computation. + */ + boost::optional errorBefore; + + /** The nonlinear error of all of the factors computed after the current + * update, meaning that variables above the relinearization threshold + * (ISAM2Params::relinearizeThreshold) have been relinearized and new + * variables have undergone one linear update. Variable values are + * again computed by combining their linearization points with their + * partial linear deltas, by ISAM2::calculateEstimate(). + * \par Note: This will only be computed if + * ISAM2Params::evaluateNonlinearError is set to \c true, because there is + * some cost to this computation. + */ + boost::optional errorAfter; + + /** The number of variables that were relinearized because their linear + * deltas exceeded the reslinearization threshold + * (ISAM2Params::relinearizeThreshold), combined with any additional + * variables that had to be relinearized because they were involved in + * the same factor as a variable above the relinearization threshold. + * On steps where no relinearization is considered + * (see ISAM2Params::relinearizeSkip), this count will be zero. + */ + size_t variablesRelinearized; + + /** The number of variables that were reeliminated as parts of the Bayes' + * Tree were recalculated, due to new factors. When loop closures occur, + * this count will be large as the new loop-closing factors will tend to + * involve variables far away from the root, and everything up to the root + * will be reeliminated. + */ + size_t variablesReeliminated; + + /** The number of factors that were included in reelimination of the Bayes' + * tree. */ + size_t factorsRecalculated; + + /** The number of cliques in the Bayes' Tree */ + size_t cliques; + + /** The indices of the newly-added factors, in 1-to-1 correspondence with the + * factors passed as \c newFactors to ISAM2::update(). These indices may be + * used later to refer to the factors in order to remove them. + */ + FactorIndices newFactorsIndices; + + /** A struct holding detailed results, which must be enabled with + * ISAM2Params::enableDetailedResults. + */ + struct DetailedResults { + /** The status of a single variable, this struct is stored in + * DetailedResults::variableStatus */ + struct VariableStatus { + /** Whether the variable was just reeliminated, due to being relinearized, + * observed, new, or on the path up to the root clique from another + * reeliminated variable. */ + bool isReeliminated; + bool isAboveRelinThreshold; ///< Whether the variable was just + ///< relinearized due to being above the + ///< relinearization threshold + bool isRelinearizeInvolved; ///< Whether the variable was below the + ///< relinearization threshold but was + ///< relinearized by being involved in a + ///< factor with a variable above the + ///< relinearization threshold + bool isRelinearized; /// Whether the variable was relinearized, either by + /// being above the relinearization threshold or by + /// involvement. + bool isObserved; ///< Whether the variable was just involved in new + ///< factors + bool isNew; ///< Whether the variable itself was just added + bool inRootClique; ///< Whether the variable is in the root clique + VariableStatus() + : isReeliminated(false), + isAboveRelinThreshold(false), + isRelinearizeInvolved(false), + isRelinearized(false), + isObserved(false), + isNew(false), + inRootClique(false) {} + }; + + /** The status of each variable during this update, see VariableStatus. + */ + FastMap variableStatus; + }; + + /** Detailed results, if enabled by ISAM2Params::enableDetailedResults. See + * Detail for information about the results data stored here. */ + boost::optional detail; + + void print(const std::string str = "") const { + using std::cout; + cout << str << " Reelimintated: " << variablesReeliminated + << " Relinearized: " << variablesRelinearized + << " Cliques: " << cliques << std::endl; + } + + /** Getters and Setters */ + size_t getVariablesRelinearized() const { return variablesRelinearized; } + size_t getVariablesReeliminated() const { return variablesReeliminated; } + size_t getCliques() const { return cliques; } +}; + +} // namespace gtsam