diff --git a/gtsam/nonlinear/ISAM2.cpp b/gtsam/nonlinear/ISAM2.cpp index 122bf8aeb..d4df01a68 100644 --- a/gtsam/nonlinear/ISAM2.cpp +++ b/gtsam/nonlinear/ISAM2.cpp @@ -247,7 +247,7 @@ ISAM2::relinearizeAffectedFactors(const FastList& affectedKeys, const KeySe /* ************************************************************************* */ // find intermediate (linearized) factors from cache that are passed into the affected area -GaussianFactorGraph ISAM2::getCachedBoundaryFactors(Cliques& orphans) { +GaussianFactorGraph ISAM2::getCachedBoundaryFactors(const Cliques& orphans) { GaussianFactorGraph cachedBoundary; for(sharedClique orphan: orphans) { diff --git a/gtsam/nonlinear/ISAM2.h b/gtsam/nonlinear/ISAM2.h index 114c04018..235cb65a5 100644 --- a/gtsam/nonlinear/ISAM2.h +++ b/gtsam/nonlinear/ISAM2.h @@ -11,7 +11,8 @@ /** * @file ISAM2.h - * @brief Incremental update functionality (ISAM2) for BayesTree, with fluid relinearization. + * @brief Incremental update functionality (ISAM2) for BayesTree, with fluid + * relinearization. * @author Michael Kaess, Richard Roberts */ @@ -19,9 +20,12 @@ #pragma once -#include -#include +#include +#include + #include +#include +#include #include @@ -34,21 +38,29 @@ namespace gtsam { * 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) + 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) {} + double _wildfireThreshold = + 0.001 ///< see ISAM2GaussNewtonParams public variables, + ///< ISAM2GaussNewtonParams::wildfireThreshold + ) + : wildfireThreshold(_wildfireThreshold) {} void print(const std::string str = "") const { - std::cout << str << "type: ISAM2GaussNewtonParams\n"; - std::cout << str << "wildfireThreshold: " << wildfireThreshold << "\n"; - std::cout.flush(); + 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; } + void setWildfireThreshold(double wildfireThreshold) { + this->wildfireThreshold = wildfireThreshold; + } }; /** @@ -58,51 +70,81 @@ struct GTSAM_EXPORT ISAM2GaussNewtonParams { * 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 + 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) {} + 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 { - std::cout << str << "type: ISAM2DoglegParams\n"; - std::cout << str << "initialDelta: " << initialDelta << "\n"; - std::cout << str << "wildfireThreshold: " << wildfireThreshold << "\n"; - std::cout << str << "adaptationMode: " << adaptationModeTranslator(adaptationMode) << "\n"; - std::cout.flush(); + 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; }; + 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; } - 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; + 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. + * Parameters for the ISAM2 algorithm. Default parameter values are listed + * below. */ -typedef FastMap ISAM2ThresholdMap; +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 + 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 @@ -122,27 +164,36 @@ struct GTSAM_EXPORT ISAM2Params { * 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 + 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) + 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 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() + 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. + /** 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; @@ -153,97 +204,156 @@ struct GTSAM_EXPORT ISAM2Params { */ bool cacheLinearizedFactors; - KeyFormatter keyFormatter; ///< A KeyFormatter for when keys are printed during debugging (default: DefaultKeyFormatter) + 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) + 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. + /** 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. + /// 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 */ - ISAM2Params( - OptimizationParams _optimizationParams = ISAM2GaussNewtonParams(), ///< see ISAM2Params::optimizationParams - RelinearizationThreshold _relinearizeThreshold = 0.1, ///< see ISAM2Params::relinearizeThreshold - int _relinearizeSkip = 10, ///< see ISAM2Params::relinearizeSkip - bool _enableRelinearization = true, ///< see ISAM2Params::enableRelinearization - bool _evaluateNonlinearError = false, ///< see ISAM2Params::evaluateNonlinearError - Factorization _factorization = ISAM2Params::CHOLESKY, ///< see ISAM2Params::factorization - bool _cacheLinearizedFactors = true, ///< see ISAM2Params::cacheLinearizedFactors - 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) {} + /** + * 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 { - std::cout << str << "\n"; - if(optimizationParams.type() == typeid(ISAM2GaussNewtonParams)) - boost::get(optimizationParams).print("optimizationParams: "); - else if(optimizationParams.type() == typeid(ISAM2DoglegParams)) - boost::get(optimizationParams).print("optimizationParams: "); + 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 - std::cout << "optimizationParams: " << "{unknown type}" << "\n"; - if(relinearizeThreshold.type() == typeid(double)) - std::cout << "relinearizeThreshold: " << boost::get(relinearizeThreshold) << "\n"; - else - { - std::cout << "relinearizeThreshold: " << "{mapped}" << "\n"; - for(const ISAM2ThresholdMapValue& value: boost::get(relinearizeThreshold)) { - std::cout << " '" << value.first << "' -> [" << value.second.transpose() << " ]\n"; + 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"; } } - std::cout << "relinearizeSkip: " << relinearizeSkip << "\n"; - std::cout << "enableRelinearization: " << enableRelinearization << "\n"; - std::cout << "evaluateNonlinearError: " << evaluateNonlinearError << "\n"; - std::cout << "factorization: " << factorizationTranslator(factorization) << "\n"; - std::cout << "cacheLinearizedFactors: " << cacheLinearizedFactors << "\n"; - std::cout << "enableDetailedResults: " << enableDetailedResults << "\n"; - std::cout << "enablePartialRelinearizationCheck: " << enablePartialRelinearizationCheck << "\n"; - std::cout << "findUnusedFactorSlots: " << findUnusedFactorSlots << "\n"; - std::cout.flush(); + + 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; } + 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); } + 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; } + 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; } + 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; + ? (GaussianFactorGraph::Eliminate)EliminatePreferCholesky + : (GaussianFactorGraph::Eliminate)EliminateQR; } /// @} @@ -275,8 +385,9 @@ struct GTSAM_EXPORT ISAM2Result { * 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. + * \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; @@ -286,8 +397,9 @@ struct GTSAM_EXPORT ISAM2Result { * 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. + * \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; @@ -309,7 +421,8 @@ struct GTSAM_EXPORT ISAM2Result { */ size_t variablesReeliminated; - /** The number of factors that were included in reelimination of the Bayes' tree. */ + /** The number of factors that were included in reelimination of the Bayes' + * tree. */ size_t factorsRecalculated; /** The number of cliques in the Bayes' Tree */ @@ -332,14 +445,29 @@ struct GTSAM_EXPORT ISAM2Result { * 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) {} + 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. @@ -351,24 +479,27 @@ struct GTSAM_EXPORT ISAM2Result { * Detail for information about the results data stored here. */ boost::optional detail; - void print(const std::string str = "") const { - std::cout << str << " Reelimintated: " << variablesReeliminated << " Relinearized: " << variablesRelinearized << " Cliques: " << cliques << std::endl; + 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; }; + 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 + * Specialized Clique structure for ISAM2, incorporating caching and gradient + * contribution * TODO: more documentation */ -class GTSAM_EXPORT ISAM2Clique : public BayesTreeCliqueBase -{ -public: +class GTSAM_EXPORT ISAM2Clique + : public BayesTreeCliqueBase { + public: typedef ISAM2Clique This; typedef BayesTreeCliqueBase Base; typedef boost::shared_ptr shared_ptr; @@ -383,13 +514,16 @@ public: /// 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_) {} + /// 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) - { + /// 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_; @@ -397,7 +531,8 @@ public: } /// Overridden to also store the remaining factor and gradient contribution - void setEliminationResult(const FactorGraphType::EliminationResult& eliminationResult); + void setEliminationResult( + const FactorGraphType::EliminationResult& eliminationResult); /** Access the cached factor */ Base::FactorType::shared_ptr& cachedFactor() { return cachedFactor_; } @@ -405,44 +540,45 @@ public: /** Access the gradient contribution */ const Vector& gradientContribution() const { return gradientContribution_; } - bool equals(const This& other, double tol=1e-9) const; + bool equals(const This& other, double tol = 1e-9) const; /** print this node */ - void print(const std::string& s = "", const KeyFormatter& formatter = DefaultKeyFormatter) const; - -private: + void print(const std::string& s = "", + const KeyFormatter& formatter = DefaultKeyFormatter) const; + private: /** 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_); + 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 +}; // \struct ISAM2Clique /** * @addtogroup ISAM2 - * Implementation of the full ISAM2 algorithm for incremental nonlinear optimization. + * Implementation of the full ISAM2 algorithm for incremental nonlinear + * optimization. * - * The typical cycle of using this class to create an instance by providing ISAM2Params - * to the constructor, then add measurements and variables as they arrive using the update() - * method. At any time, calculateEstimate() may be called to obtain the current - * estimate of all variables. + * The typical cycle of using this class to create an instance by providing + * ISAM2Params to the constructor, then add measurements and variables as they + * arrive using the update() method. At any time, calculateEstimate() may be + * called to obtain the current estimate of all variables. * */ -class GTSAM_EXPORT ISAM2: public BayesTree { - -protected: - +class GTSAM_EXPORT ISAM2 : public BayesTree { + protected: /** The current linearization point */ Values theta_; - /** VariableIndex lets us look up factors by involved variable and keeps track of dimensions */ + /** VariableIndex lets us look up factors by involved variable and keeps track + * of dimensions */ VariableIndex variableIndex_; - /** The linear delta from the last linear solution, an update to the estimate in theta + /** The linear delta from the last linear solution, an update to the estimate + * in theta * * This is \c mutable because it is a "cached" variable - it is not updated * until either requested with getDelta() or calculateEstimate(), or needed @@ -450,8 +586,10 @@ protected: */ mutable VectorValues delta_; - mutable VectorValues deltaNewton_; // Only used when using Dogleg - stores the Gauss-Newton update - mutable VectorValues RgProd_; // Only used when using Dogleg - stores R*g and is updated incrementally + mutable VectorValues deltaNewton_; // Only used when using Dogleg - stores + // the Gauss-Newton update + mutable VectorValues RgProd_; // Only used when using Dogleg - stores R*g and + // is updated incrementally /** A cumulative mask for the variables that were replaced and have not yet * been updated in the linear solution delta_, this is only used internally, @@ -461,9 +599,11 @@ protected: * This is \c mutable because it is used internally to not update delta_ * until it is needed. */ - mutable KeySet deltaReplacedMask_; // TODO: Make sure accessed in the right way + mutable KeySet + deltaReplacedMask_; // TODO: Make sure accessed in the right way - /** All original nonlinear factors are stored here to use during relinearization */ + /** All original nonlinear factors are stored here to use during + * relinearization */ NonlinearFactorGraph nonlinearFactors_; /** The current linear factors, which are only updated as needed */ @@ -479,20 +619,21 @@ protected: * variables and thus cannot have their linearization points changed. */ KeySet fixedVariables_; - int update_count_; ///< Counter incremented every update(), used to determine periodic relinearization + int update_count_; ///< Counter incremented every update(), used to determine + ///< periodic relinearization -public: - - typedef ISAM2 This; ///< This class - typedef BayesTree Base; ///< The BayesTree base class - typedef Base::Clique Clique; ///< A clique - typedef Base::sharedClique sharedClique; ///< Shared pointer to a clique - typedef Base::Cliques Cliques; ///< List of Clique typedef from base class + public: + typedef ISAM2 This; ///< This class + typedef BayesTree Base; ///< The BayesTree base class + typedef Base::Clique Clique; ///< A clique + typedef Base::sharedClique sharedClique; ///< Shared pointer to a clique + typedef Base::Cliques Cliques; ///< List of Clique typedef from base class /** Create an empty ISAM2 instance */ - ISAM2(const ISAM2Params& params); + explicit ISAM2(const ISAM2Params& params); - /** Create an empty ISAM2 instance using the default set of parameters (see ISAM2Params) */ + /** Create an empty ISAM2 instance using the default set of parameters (see + * ISAM2Params) */ ISAM2(); /** default virtual destructor */ @@ -513,26 +654,31 @@ public: * thresholds. * * @param newFactors The new factors to be added to the system - * @param newTheta Initialization points for new variables to be added to the system. - * You must include here all new variables occuring in newFactors (which were not already - * in the system). There must not be any variables here that do not occur in newFactors, - * and additionally, variables that were already in the system must not be included here. + * @param newTheta Initialization points for new variables to be added to the + * system. You must include here all new variables occuring in newFactors + * (which were not already in the system). There must not be any variables + * here that do not occur in newFactors, and additionally, variables that were + * already in the system must not be included here. * @param removeFactorIndices Indices of factors to remove from system - * @param force_relinearize Relinearize any variables whose delta magnitude is sufficiently - * large (Params::relinearizeThreshold), regardless of the relinearization interval - * (Params::relinearizeSkip). - * @param constrainedKeys is an optional map of keys to group labels, such that a variable can - * be constrained to a particular grouping in the BayesTree - * @param noRelinKeys is an optional set of nonlinear keys that iSAM2 will hold at a constant linearization - * point, regardless of the size of the linear delta - * @param extraReelimKeys is an optional set of nonlinear keys that iSAM2 will re-eliminate, regardless - * of the size of the linear delta. This allows the provided keys to be reordered. + * @param force_relinearize Relinearize any variables whose delta magnitude is + * sufficiently large (Params::relinearizeThreshold), regardless of the + * relinearization interval (Params::relinearizeSkip). + * @param constrainedKeys is an optional map of keys to group labels, such + * that a variable can be constrained to a particular grouping in the + * BayesTree + * @param noRelinKeys is an optional set of nonlinear keys that iSAM2 will + * hold at a constant linearization point, regardless of the size of the + * linear delta + * @param extraReelimKeys is an optional set of nonlinear keys that iSAM2 will + * re-eliminate, regardless of the size of the linear delta. This allows the + * provided keys to be reordered. * @return An ISAM2Result struct containing information about the update */ - virtual ISAM2Result update(const NonlinearFactorGraph& newFactors = NonlinearFactorGraph(), + virtual ISAM2Result update( + const NonlinearFactorGraph& newFactors = NonlinearFactorGraph(), const Values& newTheta = Values(), const FactorIndices& removeFactorIndices = FactorIndices(), - const boost::optional >& constrainedKeys = boost::none, + const boost::optional >& constrainedKeys = boost::none, const boost::optional >& noRelinKeys = boost::none, const boost::optional >& extraReelimKeys = boost::none, bool force_relinearize = false); @@ -542,48 +688,50 @@ public: * requested to be marginalized. Marginalization leaves a linear * approximation of the marginal in the system, and the linearization points * of any variables involved in this linear marginal become fixed. The set - * fixed variables will include any key involved with the marginalized variables - * in the original factors, and possibly additional ones due to fill-in. + * fixed variables will include any key involved with the marginalized + * variables in the original factors, and possibly additional ones due to + * fill-in. * - * If provided, 'marginalFactorsIndices' will be augmented with the factor graph - * indices of the marginal factors added during the 'marginalizeLeaves' call + * If provided, 'marginalFactorsIndices' will be augmented with the factor + * graph indices of the marginal factors added during the 'marginalizeLeaves' + * call * - * If provided, 'deletedFactorsIndices' will be augmented with the factor graph - * indices of any factor that was removed during the 'marginalizeLeaves' call + * If provided, 'deletedFactorsIndices' will be augmented with the factor + * graph indices of any factor that was removed during the 'marginalizeLeaves' + * call */ - void marginalizeLeaves(const FastList& leafKeys, - boost::optional marginalFactorsIndices = boost::none, - boost::optional deletedFactorsIndices = boost::none); + void marginalizeLeaves( + const FastList& leafKeys, + boost::optional marginalFactorsIndices = boost::none, + boost::optional deletedFactorsIndices = boost::none); /// Access the current linearization point - const Values& getLinearizationPoint() const { - return theta_; - } + const Values& getLinearizationPoint() const { return theta_; } /// Check whether variable with given key exists in linearization point - bool valueExists(Key key) const { - return theta_.exists(key); - } + bool valueExists(Key key) const { return theta_.exists(key); } - /** Compute an estimate from the incomplete linear delta computed during the last update. - * This delta is incomplete because it was not updated below wildfire_threshold. If only - * a single variable is needed, it is faster to call calculateEstimate(const KEY&). + /** Compute an estimate from the incomplete linear delta computed during the + * last update. This delta is incomplete because it was not updated below + * wildfire_threshold. If only a single variable is needed, it is faster to + * call calculateEstimate(const KEY&). */ Values calculateEstimate() const; - /** Compute an estimate for a single variable using its incomplete linear delta computed - * during the last update. This is faster than calling the no-argument version of - * calculateEstimate, which operates on all variables. + /** Compute an estimate for a single variable using its incomplete linear + * delta computed during the last update. This is faster than calling the + * no-argument version of calculateEstimate, which operates on all variables. * @param key * @return */ - template + template VALUE calculateEstimate(Key key) const; - /** Compute an estimate for a single variable using its incomplete linear delta computed - * during the last update. This is faster than calling the no-argument version of - * calculateEstimate, which operates on all variables. This is a non-templated version - * that returns a Value base class for use with the MATLAB wrapper. + /** Compute an estimate for a single variable using its incomplete linear + * delta computed during the last update. This is faster than calling the + * no-argument version of calculateEstimate, which operates on all variables. + * This is a non-templated version that returns a Value base class for use + * with the MATLAB wrapper. * @param key * @return */ @@ -598,7 +746,8 @@ public: /** Internal implementation functions */ struct Impl; - /** Compute an estimate using a complete delta computed by a full back-substitution. + /** Compute an estimate using a complete delta computed by a full + * back-substitution. */ Values calculateBestEstimate() const; @@ -609,7 +758,9 @@ public: double error(const VectorValues& x) const; /** Access the set of nonlinear factors */ - const NonlinearFactorGraph& getFactorsUnsafe() const { return nonlinearFactors_; } + const NonlinearFactorGraph& getFactorsUnsafe() const { + return nonlinearFactors_; + } /** Access the nonlinear variable index */ const VariableIndex& getVariableIndex() const { return variableIndex_; } @@ -628,31 +779,36 @@ public: /** prints out clique statistics */ void printStats() const { getCliqueData().getStats().print(); } - - /** 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&). + + /** 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&). * * @return A VectorValues storing the gradient. */ VectorValues gradientAtZero() const; - + /// @} -protected: - + protected: FastSet getAffectedFactors(const FastList& keys) const; - GaussianFactorGraph::shared_ptr relinearizeAffectedFactors(const FastList& affectedKeys, const KeySet& relinKeys) const; - GaussianFactorGraph getCachedBoundaryFactors(Cliques& orphans); + GaussianFactorGraph::shared_ptr relinearizeAffectedFactors( + const FastList& affectedKeys, const KeySet& relinKeys) const; + GaussianFactorGraph getCachedBoundaryFactors(const Cliques& orphans); - virtual boost::shared_ptr recalculate(const KeySet& markedKeys, const KeySet& relinKeys, - const std::vector& observedKeys, const KeySet& unusedIndices, const boost::optional >& constrainKeys, ISAM2Result& result); + virtual boost::shared_ptr recalculate( + const KeySet& markedKeys, const KeySet& relinKeys, + const std::vector& observedKeys, const KeySet& unusedIndices, + const boost::optional >& constrainKeys, + ISAM2Result& result); void updateDelta(bool forceFullSolve = false) const; -}; // ISAM2 +}; // ISAM2 /// traits -template<> struct traits : public Testable {}; +template <> +struct traits : public Testable {}; /// Optimize the BayesTree, starting from the root. /// @param replaced Needs to contain @@ -665,19 +821,21 @@ template<> struct traits : public Testable {}; /// and recursive backsubstitution might eventually stop if none of the changed /// variables are contained in the subtree. /// @return The number of variables that were solved for -template -size_t optimizeWildfire(const boost::shared_ptr& root, - double threshold, const KeySet& replaced, VectorValues& delta); +template +size_t optimizeWildfire(const boost::shared_ptr& root, double threshold, + const KeySet& replaced, VectorValues& delta); -template +template size_t optimizeWildfireNonRecursive(const boost::shared_ptr& root, - double threshold, const KeySet& replaced, VectorValues& delta); + double threshold, const KeySet& replaced, + VectorValues& delta); -/// calculate the number of non-zero entries for the tree starting at clique (use root for complete matrix) -template +/// 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); -} /// namespace gtsam +} // namespace gtsam -#include #include +#include