From 792c8ee55a67249e9f777b7e37491dc966df04d0 Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Thu, 12 Apr 2012 03:04:32 +0000 Subject: [PATCH] Detailed results optionally returned by ISAM2::update, with the status of each variable --- gtsam/nonlinear/ISAM2.cpp | 76 +++++++++++++++++++++++++++++---------- gtsam/nonlinear/ISAM2.h | 39 ++++++++++++++++++-- 2 files changed, 94 insertions(+), 21 deletions(-) diff --git a/gtsam/nonlinear/ISAM2.cpp b/gtsam/nonlinear/ISAM2.cpp index d8138366b..e00c93cae 100644 --- a/gtsam/nonlinear/ISAM2.cpp +++ b/gtsam/nonlinear/ISAM2.cpp @@ -206,7 +206,7 @@ GaussianFactorGraph ISAM2::getCachedBoundaryFactors(Cliques& orphans) { } boost::shared_ptr > ISAM2::recalculate( - const FastSet& markedKeys, const FastSet& relinKeys, const FastVector& newKeys, + const FastSet& markedKeys, const FastSet& relinKeys, const FastVector& observedKeys, const boost::optional >& constrainKeys, ISAM2Result& result) { // TODO: new factors are linearized twice, the newFactors passed in are not used. @@ -237,8 +237,8 @@ boost::shared_ptr > ISAM2::recalculate( cout << "markedKeys: "; BOOST_FOREACH(const Index key, markedKeys) { cout << key << " "; } cout << endl; - cout << "newKeys: "; - BOOST_FOREACH(const Index key, newKeys) { cout << key << " "; } + cout << "observedKeys: "; + BOOST_FOREACH(const Index key, observedKeys) { cout << key << " "; } cout << endl; } @@ -270,12 +270,13 @@ boost::shared_ptr > ISAM2::recalculate( FastList affectedKeys = affectedBayesNet.ordering(); toc(2,"affectedKeys"); + boost::shared_ptr > affectedKeysSet(new FastSet()); // Will return this result + if(affectedKeys.size() >= theta_.size() * batchThreshold) { tic(3,"batch"); tic(0,"add keys"); - boost::shared_ptr > affectedKeysSet(new FastSet()); BOOST_FOREACH(const Ordering::value_type& key_index, ordering_) { affectedKeysSet->insert(key_index.second); } toc(0,"add keys"); @@ -296,8 +297,8 @@ boost::shared_ptr > ISAM2::recalculate( } } } else { - if(theta_.size() > newKeys.size()) { // Only if some variables are unconstrained - BOOST_FOREACH(Index var, newKeys) { cmember[var] = 1; } + if(theta_.size() > observedKeys.size()) { // Only if some variables are unconstrained + BOOST_FOREACH(Index var, observedKeys) { cmember[var] = 1; } } } Permutation::shared_ptr colamd(inference::PermutationCOLAMD_(variableIndex_, cmember)); @@ -338,15 +339,17 @@ boost::shared_ptr > ISAM2::recalculate( this->insert(newRoot); toc(6,"insert"); - toc(3,"batch"); - result.variablesReeliminated = affectedKeysSet->size(); lastAffectedMarkedCount = markedKeys.size(); lastAffectedVariableCount = affectedKeysSet->size(); lastAffectedFactorCount = linearFactors_.size(); - return affectedKeysSet; + // Reeliminated keys for detailed results + if(params_.enableDetailedResults) + BOOST_FOREACH(Key key, theta_.keys()) { result.detail->variableStatus[key].isReeliminated = true; } + + toc(3,"batch"); } else { @@ -355,7 +358,7 @@ boost::shared_ptr > ISAM2::recalculate( // 2. Add the new factors \Factors' into the resulting factor graph FastList affectedAndNewKeys; affectedAndNewKeys.insert(affectedAndNewKeys.end(), affectedKeys.begin(), affectedKeys.end()); - affectedAndNewKeys.insert(affectedAndNewKeys.end(), newKeys.begin(), newKeys.end()); + affectedAndNewKeys.insert(affectedAndNewKeys.end(), observedKeys.begin(), observedKeys.end()); tic(1,"relinearizeAffected"); GaussianFactorGraph factors(*relinearizeAffectedFactors(affectedAndNewKeys, relinKeys)); if(debug) factors.print("Relinearized factors: "); @@ -363,6 +366,10 @@ boost::shared_ptr > ISAM2::recalculate( if(debug) { cout << "Affected keys: "; BOOST_FOREACH(const Index key, affectedKeys) { cout << key << " "; } cout << endl; } + // Reeliminated keys for detailed results + if(params_.enableDetailedResults) + BOOST_FOREACH(Index index, affectedAndNewKeys) { result.detail->variableStatus[inverseOrdering_->at(index)].isReeliminated = true; } + result.variablesReeliminated = affectedAndNewKeys.size(); lastAffectedMarkedCount = markedKeys.size(); lastAffectedVariableCount = affectedKeys.size(); @@ -391,7 +398,7 @@ boost::shared_ptr > ISAM2::recalculate( tic(1,"list to set"); // create a partial reordering for the new and contaminated factors // markedKeys are passed in: those variables will be forced to the end in the ordering - boost::shared_ptr > affectedKeysSet(new FastSet(markedKeys)); + affectedKeysSet->insert(markedKeys.begin(), markedKeys.end()); affectedKeysSet->insert(affectedKeys.begin(), affectedKeys.end()); toc(1,"list to set"); @@ -404,7 +411,7 @@ boost::shared_ptr > ISAM2::recalculate( reorderingMode.constrainedKeys = *constrainKeys; } else { reorderingMode.constrainedKeys = FastMap(); - BOOST_FOREACH(Index var, newKeys) { reorderingMode.constrainedKeys->insert(make_pair(var, 1)); } + BOOST_FOREACH(Index var, observedKeys) { reorderingMode.constrainedKeys->insert(make_pair(var, 1)); } } Impl::PartialSolveResult partialSolveResult = Impl::PartialSolve(factors, *affectedKeysSet, reorderingMode, (params_.factorization == ISAM2Params::QR)); @@ -467,9 +474,13 @@ boost::shared_ptr > ISAM2::recalculate( toc(7,"orphans"); toc(4,"incremental"); - - return affectedKeysSet; } + + // Root clique variables for detailed results + if(params_.enableDetailedResults) + BOOST_FOREACH(Index index, this->root()->conditional()->frontals()) { result.detail->variableStatus[inverseOrdering_->at(index)].inRootClique = true; } + + return affectedKeysSet; } /* ************************************************************************* */ @@ -490,6 +501,8 @@ ISAM2Result ISAM2::update( lastBacksubVariableCount = 0; lastNnzTop = 0; ISAM2Result result; + if(params_.enableDetailedResults) + result.detail = ISAM2Result::DetailedResults(); const bool relinearizeThisStep = force_relinearize || (params_.enableRelinearization && count % params_.relinearizeSkip == 0); if(verbose) { @@ -528,6 +541,10 @@ ISAM2Result ISAM2::update( tic(2,"add new variables"); // 2. Initialize any new variables \Theta_{new} and add \Theta:=\Theta\cup\Theta_{new}. Impl::AddVariables(newTheta, theta_, delta_, deltaNewton_, RgProd_, deltaReplacedMask_, ordering_, Base::nodes_); + // New keys for detailed results + if(params_.enableDetailedResults) { + inverseOrdering_ = ordering_.invert(); + BOOST_FOREACH(Key key, newTheta.keys()) { result.detail->variableStatus[key].isNew = true; } } toc(2,"add new variables"); tic(3,"evaluate error before"); @@ -543,10 +560,13 @@ ISAM2Result ISAM2::update( FastSet markedRemoveKeys = Impl::IndicesFromFactors(ordering_, removeFactors); // Get keys involved in removed factors markedKeys.insert(markedRemoveKeys.begin(), markedRemoveKeys.end()); // Add to the overall set of marked keys } + // Observed keys for detailed results + if(params_.enableDetailedResults) + BOOST_FOREACH(Index index, markedKeys) { result.detail->variableStatus[inverseOrdering_->at(index)].isObserved = true; } // NOTE: we use assign instead of the iterator constructor here because this // is a vector of size_t, so the constructor unintentionally resolves to // vector(size_t count, Index value) instead of the iterator constructor. - FastVector newKeys; newKeys.assign(markedKeys.begin(), markedKeys.end()); // Make a copy of these, as we'll soon add to them + FastVector observedKeys; observedKeys.assign(markedKeys.begin(), markedKeys.end()); // Make a copy of these, as we'll soon add to them toc(4,"gather involved keys"); // Check relinearization if we're at the nth step, or we are using a looser loop relin threshold @@ -558,6 +578,12 @@ ISAM2Result ISAM2::update( relinKeys = Impl::CheckRelinearization(delta_, ordering_, params_.relinearizeThreshold); if(disableReordering) relinKeys = Impl::CheckRelinearization(delta_, ordering_, 0.0); // This is used for debugging + // Above relin threshold keys for detailed results + if(params_.enableDetailedResults) { + BOOST_FOREACH(Index index, relinKeys) { + result.detail->variableStatus[inverseOrdering_->at(index)].isAboveRelinThreshold = true; + result.detail->variableStatus[inverseOrdering_->at(index)].isRelinearized = true; } } + // Add the variables being relinearized to the marked keys BOOST_FOREACH(const Index j, relinKeys) { markedRelinMask[j] = true; } markedKeys.insert(relinKeys.begin(), relinKeys.end()); @@ -565,8 +591,20 @@ ISAM2Result ISAM2::update( tic(6,"fluid find_all"); // 5. Mark all cliques that involve marked variables \Theta_{J} and all their ancestors. - if (!relinKeys.empty() && this->root()) - Impl::FindAll(this->root(), markedKeys, markedRelinMask); // add other cliques that have the marked ones in the separator + if (!relinKeys.empty() && this->root()) { + // add other cliques that have the marked ones in the separator + Impl::FindAll(this->root(), markedKeys, markedRelinMask); + + // Relin involved keys for detailed results + if(params_.enableDetailedResults) { + FastSet involvedRelinKeys; + Impl::FindAll(this->root(), involvedRelinKeys, markedRelinMask); + BOOST_FOREACH(Index index, involvedRelinKeys) { + if(!result.detail->variableStatus[inverseOrdering_->at(index)].isAboveRelinThreshold) { + result.detail->variableStatus[inverseOrdering_->at(index)].isRelinearizeInvolved = true; + result.detail->variableStatus[inverseOrdering_->at(index)].isRelinearized = true; } } + } + } toc(6,"fluid find_all"); tic(7,"expmap"); @@ -617,8 +655,8 @@ ISAM2Result ISAM2::update( } } boost::shared_ptr > replacedKeys; - if(!markedKeys.empty() || !newKeys.empty()) - replacedKeys = recalculate(markedKeys, relinKeys, newKeys, constrainedIndices, result); + if(!markedKeys.empty() || !observedKeys.empty()) + replacedKeys = recalculate(markedKeys, relinKeys, observedKeys, constrainedIndices, result); // Update replaced keys mask (accumulates until back-substitution takes place) if(replacedKeys) { diff --git a/gtsam/nonlinear/ISAM2.h b/gtsam/nonlinear/ISAM2.h index 21f929ed9..2d576ec5d 100644 --- a/gtsam/nonlinear/ISAM2.h +++ b/gtsam/nonlinear/ISAM2.h @@ -127,6 +127,8 @@ struct ISAM2Params { 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) + /** Specify parameters as constructor arguments */ ISAM2Params( OptimizationParams _optimizationParams = ISAM2GaussNewtonParams(), ///< see ISAM2Params::optimizationParams @@ -140,7 +142,8 @@ struct ISAM2Params { ) : optimizationParams(_optimizationParams), relinearizeThreshold(_relinearizeThreshold), relinearizeSkip(_relinearizeSkip), enableRelinearization(_enableRelinearization), evaluateNonlinearError(_evaluateNonlinearError), factorization(_factorization), - cacheLinearizedFactors(_cacheLinearizedFactors), keyFormatter(_keyFormatter) {} + cacheLinearizedFactors(_cacheLinearizedFactors), keyFormatter(_keyFormatter), + enableDetailedResults(false) {} }; /** @@ -201,6 +204,35 @@ struct ISAM2Result { * used later to refer to the factors in order to remove them. */ FastVector 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), 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; }; struct ISAM2Clique : public BayesTreeCliqueBase { @@ -364,6 +396,9 @@ protected: /** The current Dogleg Delta (trust region radius) */ mutable boost::optional doglegDelta_; + /** The inverse ordering, only used for creating ISAM2Result::DetailedResults */ + boost::optional inverseOrdering_; + private: #ifndef NDEBUG std::vector lastRelinVariables_; @@ -468,7 +503,7 @@ private: GaussianFactorGraph getCachedBoundaryFactors(Cliques& orphans); boost::shared_ptr > recalculate(const FastSet& markedKeys, const FastSet& relinKeys, - const FastVector& newKeys, + const FastVector& observedKeys, const boost::optional >& constrainKeys, ISAM2Result& result); // void linear_update(const GaussianFactorGraph& newFactors); void updateDelta(bool forceFullSolve = false) const;