diff --git a/gtsam/nonlinear/ISAM2-inl.h b/gtsam/nonlinear/ISAM2-inl.h index 2949fb5e9..383951e6d 100644 --- a/gtsam/nonlinear/ISAM2-inl.h +++ b/gtsam/nonlinear/ISAM2-inl.h @@ -156,7 +156,7 @@ ISAM2::getCachedBoundaryFactors(Cliques& orphans) { template boost::shared_ptr > ISAM2::recalculate( - const FastSet& markedKeys, const FastSet& structuralKeys, const FastVector& newKeys, const FactorGraph::shared_ptr newFactors) { + const FastSet& markedKeys, const FastSet& structuralKeys, const FastVector& newKeys, const FactorGraph::shared_ptr newFactors, ISAM2Result& result) { // TODO: new factors are linearized twice, the newFactors passed in are not used. @@ -298,6 +298,8 @@ boost::shared_ptr > ISAM2::recalculate( toc(3,"batch"); + result.variablesReeliminated = affectedKeysSet->size(); + lastAffectedMarkedCount = markedKeys.size(); lastAffectedVariableCount = affectedKeysSet->size(); lastAffectedFactorCount = factors.size(); @@ -318,6 +320,7 @@ boost::shared_ptr > ISAM2::recalculate( if(debug) { cout << "Affected keys: "; BOOST_FOREACH(const Index key, affectedKeys) { cout << key << " "; } cout << endl; } + result.variablesReeliminated = affectedAndNewKeys.size(); lastAffectedMarkedCount = markedKeys.size(); lastAffectedVariableCount = affectedKeys.size(); lastAffectedFactorCount = factors.size(); @@ -416,7 +419,7 @@ boost::shared_ptr > ISAM2::recalculate( /* ************************************************************************* */ template -void ISAM2::update( +ISAM2Result ISAM2::update( const NonlinearFactorGraph& newFactors, const Values& newTheta, bool force_relinearize) { static const bool debug = ISDEBUG("ISAM2 update"); @@ -431,22 +434,28 @@ void ISAM2::update( lastAffectedMarkedCount = 0; lastBacksubVariableCount = 0; lastNnzTop = 0; + ISAM2Result result; if(verbose) { cout << "ISAM2::update\n"; this->print("ISAM2: "); } - tic(1,"push_back factors"); + tic(0,"push_back factors"); // 1. Add any new factors \Factors:=\Factors\cup\Factors'. if(debug || verbose) newFactors.print("The new factors are: "); nonlinearFactors_.push_back(newFactors); - toc(1,"push_back factors"); + toc(0,"push_back factors"); - tic(2,"add new variables"); + tic(1,"add new variables"); // 2. Initialize any new variables \Theta_{new} and add \Theta:=\Theta\cup\Theta_{new}. Impl::AddVariables(newTheta, theta_, delta_, ordering_, Base::nodes_); - toc(2,"add new variables"); + toc(1,"add new variables"); + + tic(2,"evaluate error before"); + if(params_.evaluateNonlinearError) + result.errorBefore.reset(nonlinearFactors_.error(calculateEstimate())); + toc(2,"evaluate error before"); tic(3,"gather involved keys"); // 3. Mark linear update @@ -481,11 +490,14 @@ void ISAM2::update( Impl::ExpmapMasked(theta_, delta_, ordering_, markedRelinMask, delta_); toc(6,"expmap"); + result.variablesRelinearized = markedRelinMask.size(); + #ifndef NDEBUG lastRelinVariables_ = markedRelinMask; #endif } else { #ifndef NDEBUG + result.variablesRelinearized = 0; lastRelinVariables_ = vector(ordering_.nVars(), false); #endif } @@ -506,7 +518,7 @@ void ISAM2::update( // 8. Redo top of Bayes tree boost::shared_ptr > replacedKeys; if(!markedKeys.empty() || !newKeys.empty()) - replacedKeys = recalculate(markedKeys, structuralKeys, newKeys, linearFactors); + replacedKeys = recalculate(markedKeys, structuralKeys, newKeys, linearFactors, result); toc(8,"recalculate"); tic(9,"solve"); @@ -532,6 +544,13 @@ void ISAM2::update( #endif } toc(9,"solve"); + + tic(10,"evaluate error after"); + if(params_.evaluateNonlinearError) + result.errorAfter.reset(nonlinearFactors_.error(calculateEstimate())); + toc(10,"evaluate error after"); + + return result; } /* ************************************************************************* */ diff --git a/gtsam/nonlinear/ISAM2.h b/gtsam/nonlinear/ISAM2.h index 812bc340b..8159ebfb5 100644 --- a/gtsam/nonlinear/ISAM2.h +++ b/gtsam/nonlinear/ISAM2.h @@ -50,15 +50,69 @@ struct ISAM2Params { double relinearizeThreshold; ///< Only relinearize variables whose linear delta magnitude is greater than this threshold (default: 0.1) 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() /** Specify parameters as constructor arguments */ ISAM2Params( double _wildfireThreshold = 0.001, ///< ISAM2Params::wildfireThreshold double _relinearizeThreshold = 0.1, ///< ISAM2Params::relinearizeThreshold int _relinearizeSkip = 10, ///< ISAM2Params::relinearizeSkip - bool _enableRelinearization = true ///< ISAM2Params::enableRelinearization + bool _enableRelinearization = true, ///< ISAM2Params::enableRelinearization + bool _evaluateNonlinearError = false ///< ISAM2Params::evaluateNonlinearError ) : wildfireThreshold(_wildfireThreshold), relinearizeThreshold(_relinearizeThreshold), - relinearizeSkip(_relinearizeSkip), enableRelinearization(_enableRelinearization) {} + relinearizeSkip(_relinearizeSkip), enableRelinearization(_enableRelinearization), + evaluateNonlinearError(_evaluateNonlinearError) {} +}; + +/** + * @ingroup 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 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; }; /** @@ -142,8 +196,9 @@ public: * @param force_relinearize Relinearize any variables whose delta magnitude is sufficiently * large (Params::relinearizeThreshold), regardless of the relinearization interval * (Params::relinearizeSkip). + * @return An ISAM2Result struct containing information about the update */ - void update(const NonlinearFactorGraph& newFactors, const VALUES& newTheta, + ISAM2Result update(const NonlinearFactorGraph& newFactors, const VALUES& newTheta, bool force_relinearize = false); /** Access the current linearization point */ @@ -189,7 +244,7 @@ private: FactorGraph getCachedBoundaryFactors(Cliques& orphans); boost::shared_ptr > recalculate(const FastSet& markedKeys, const FastSet& structuralKeys, - const FastVector& newKeys, const FactorGraph::shared_ptr newFactors = FactorGraph::shared_ptr()); + const FastVector& newKeys, const FactorGraph::shared_ptr newFactors, ISAM2Result& result); // void linear_update(const GaussianFactorGraph& newFactors); }; // ISAM2